13 pulseMin_(pulseMinUs),
14 pulseMax_(pulseMaxUs),
18 gear_ratio_(gear_ratio),
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;
30 HAL_TIM_PWM_Start(htim_, ch_);
38 if (minDeg > maxDeg) {
float t = minDeg; minDeg = maxDeg; maxDeg = t; }
41 const float span = (angleMax_ - angleMin_);
42 span_inv_ = (span != 0.0f) ? (1.0f / span) : 0.0f;
47 if (ratio <= 0.0f) ratio = 1.0f;
55 __HAL_TIM_SET_COMPARE(htim_, ch_, angleToPulse(deg));
60 if (us < pulseMin_) us = pulseMin_;
61 if (us > pulseMax_) us = pulseMax_;
62 __HAL_TIM_SET_COMPARE(htim_, ch_, us);
68 handClose_ = closeDeg;
73 const float target = open ? handOpen_ : handClose_;
82uint32_t Servo::angleToPulse(
float deg)
const {
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_;
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;
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);
102float Servo::clamp(
float v,
float lo,
float hi) {
103 return v < lo ? lo : (v > hi ? hi : v);
108 return __HAL_TIM_GET_COMPARE(htim_, ch_);
void begin(float start_ang=60.0f)
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)
void setGearRatio(float ratio=1.0f)
void setHandAngles(float openDeg, float closeDeg)
void setPulse(uint32_t us)
uint32_t getPulse() const
void setAngleRange(float minDeg, float maxDeg)