RogiStandard-hal
STM32 向けサークル共通ライブラリ
読み取り中…
検索中…
一致する文字列を見つけられません
Servo.cpp
[詳解]
1#include "Servo.hpp"
2
3// サーボPWMの初期化(範囲/反転/開閉角を設定)
4Servo::Servo(TIM_HandleTypeDef* htim,
5 uint32_t channel,
6 uint16_t pulseMinUs,
7 uint16_t pulseMaxUs,
8 bool invert,
9 float gear_ratio,
10 uint8_t device_id)
11 : htim_(htim),
12 ch_(channel),
13 pulseMin_(pulseMinUs),
14 pulseMax_(pulseMaxUs),
15 invert_(invert),
16 angleMin_(-90.0f),
17 angleMax_( 90.0f),
18 gear_ratio_(gear_ratio),
19 handOpen_(10.0f), // true
20 handClose_(72.0f) // false
21{
22 device_id_ = device_id;
23 pulse_span_ = static_cast<float>(pulseMax_) - static_cast<float>(pulseMin_);
24 const float span = (angleMax_ - angleMin_);
25 span_inv_ = (span != 0.0f) ? (1.0f / span) : 0.0f;
26}
27
28// PWM出力開始と初期角度(open)適用
29void Servo::begin(float start_ang) {
30 HAL_TIM_PWM_Start(htim_, ch_);
31 // 初期は open=true → 10°
32 setAngle(start_ang);
33 //setPulse(1500);
34}
35
36// 入力角度レンジ[deg]を設定
37void Servo::setAngleRange(float minDeg, float maxDeg) {
38 if (minDeg > maxDeg) { float t = minDeg; minDeg = maxDeg; maxDeg = t; }
39 angleMin_ = minDeg;
40 angleMax_ = maxDeg;
41 const float span = (angleMax_ - angleMin_);
42 span_inv_ = (span != 0.0f) ? (1.0f / span) : 0.0f;
43}
44
45// 減速比を設定(<=0 は 1.0 とみなす)
46void Servo::setGearRatio(float ratio) {
47 if (ratio <= 0.0f) ratio = 1.0f;
48 gear_ratio_ = ratio;
49}
50
51// 角度[deg]を指定して出力(clamp+CCR更新)
52void Servo::setAngle(float deg) {
53 // 事前クランプは行わず、angleToPulse 内の飽和に一任する
54 // (外部角度→サーボ角度の変換後に正しく端で飽和させるため)
55 __HAL_TIM_SET_COMPARE(htim_, ch_, angleToPulse(deg));
56}
57
58// パルス幅[us]を直接指定
59void Servo::setPulse(uint32_t us) {
60 if (us < pulseMin_) us = pulseMin_;
61 if (us > pulseMax_) us = pulseMax_;
62 __HAL_TIM_SET_COMPARE(htim_, ch_, us);
63}
64
65// ハンドの開閉角度を設定
66void Servo::setHandAngles(float openDeg, float closeDeg) {
67 handOpen_ = openDeg;
68 handClose_ = closeDeg;
69}
70
71// ハンド開閉(true=open, false=close)
72void Servo::setHand(bool open) {
73 const float target = open ? handOpen_ : handClose_;
74 setAngle(target);
75}
76
77// ショートカット:開/閉
78void Servo::openHand() { setHand(true); }
79void Servo::closeHand() { setHand(false); }
80
81// 角度[deg]→パルス幅[us]へ線形変換(反転対応)
82uint32_t Servo::angleToPulse(float deg) const {
83 // 外部角度[deg] → サーボ角度[deg]
84 // - 外部角度の中心を基準に、(deg-center)*gear_ratio_ でサーボ角度を拡大/縮小
85 // - マッピング自体は [angleMin_, angleMax_] をサーボ角度レンジとみなして線形変換
86 if (span_inv_ == 0.0f) return pulseMin_;
87 const float center = 0.5f * (angleMin_ + angleMax_);
88 const float servo_deg = center + (deg - center) * gear_ratio_;
89 // 除算削減: 事前計算した1/spanで乗算
90 float t = (servo_deg - angleMin_) * span_inv_;
91 if (t < 0.f) t = 0.f; else if (t > 1.f) t = 1.f;
92 if (invert_) t = 1.f - t;
93
94 // 除算は無く、掛け算と加算のみ
95 float us = static_cast<float>(pulseMin_) + t * pulse_span_;
96 if (us < pulseMin_) us = static_cast<float>(pulseMin_);
97 if (us > pulseMax_) us = static_cast<float>(pulseMax_);
98 return static_cast<uint32_t>(us + 0.5f);
99}
100
101// 値を[min,max]に収めるヘルパ
102float Servo::clamp(float v, float lo, float hi) {
103 return v < lo ? lo : (v > hi ? hi : v);
104}
105
106// 現在のCCR(パルス幅[us])を返す
107uint32_t Servo::getPulse() const {
108 return __HAL_TIM_GET_COMPARE(htim_, ch_);
109}
void begin(float start_ang=60.0f)
Definition Servo.cpp:29
Servo(TIM_HandleTypeDef *htim, uint32_t channel, uint16_t pulseMinUs=500, uint16_t pulseMaxUs=2500, bool invert=false, float gear_ratio=1.0f, uint8_t device_id=0)
Definition Servo.cpp:4
void setAngle(float deg)
Definition Servo.cpp:52
void openHand()
Definition Servo.cpp:78
void setGearRatio(float ratio=1.0f)
Definition Servo.cpp:46
void setHandAngles(float openDeg, float closeDeg)
Definition Servo.cpp:66
void setHand(bool open)
Definition Servo.cpp:72
void setPulse(uint32_t us)
Definition Servo.cpp:59
void closeHand()
Definition Servo.cpp:79
uint32_t getPulse() const
Definition Servo.cpp:107
void setAngleRange(float minDeg, float maxDeg)
Definition Servo.cpp:37