diff --git a/drivers/servo/servo.cpp b/drivers/servo/servo.cpp index 4e06344b..b9ac5e5c 100644 --- a/drivers/servo/servo.cpp +++ b/drivers/servo/servo.cpp @@ -71,7 +71,6 @@ namespace servo { pwm_set_gpio_level(pin, (uint16_t)ServoState::pulse_to_level(new_pulse, pwm_period, pwm_frequency)); } - float Servo::get_frequency() const { return pwm_frequency; } @@ -79,41 +78,43 @@ namespace servo { bool Servo::set_frequency(float freq) { bool success = false; - // Calculate a suitable pwm wrap period for this frequency - uint16_t period; uint16_t div16; - if(Servo::calculate_pwm_factors(freq, period, div16)) { + if((freq >= MIN_FREQUENCY) && (freq <= MAX_FREQUENCY)) { + // Calculate a suitable pwm wrap period for this frequency + uint16_t period; uint16_t div16; + if(Servo::calculate_pwm_factors(freq, period, div16)) { - // Record if the new period will be larger or smaller. - // This is used to apply new pwm values either before or after the wrap is applied, - // to avoid momentary blips in PWM output on SLOW_DECAY - bool pre_update_pwm = (period > pwm_period); + // Record if the new period will be larger or smaller. + // This is used to apply new pwm values either before or after the wrap is applied, + // to avoid momentary blips in PWM output on SLOW_DECAY + bool pre_update_pwm = (period > pwm_period); - pwm_period = period; - pwm_frequency = freq; + pwm_period = period; + pwm_frequency = freq; - uint pin_num = pwm_gpio_to_slice_num(pin); + uint pin_num = pwm_gpio_to_slice_num(pin); - // Apply the new divider - uint8_t div = div16 >> 4; - uint8_t mod = div16 % 16; - pwm_set_clkdiv_int_frac(pin_num, div, mod); + // Apply the new divider + uint8_t div = div16 >> 4; + uint8_t mod = div16 % 16; + pwm_set_clkdiv_int_frac(pin_num, div, mod); - // If the the period is larger, update the pwm before setting the new wraps - if(pre_update_pwm) { - float current_pulse = get_pulse(); - pwm_set_gpio_level(pin, (uint16_t)ServoState::pulse_to_level(current_pulse, pwm_period, pwm_frequency)); + // If the the period is larger, update the pwm before setting the new wraps + if(pre_update_pwm) { + float current_pulse = get_pulse(); + pwm_set_gpio_level(pin, (uint16_t)ServoState::pulse_to_level(current_pulse, pwm_period, pwm_frequency)); + } + + // Set the new wrap (should be 1 less than the period to get full 0 to 100%) + pwm_set_wrap(pin_num, pwm_period - 1); + + // If the the period is smaller, update the pwm after setting the new wraps + if(!pre_update_pwm) { + float current_pulse = get_pulse(); + pwm_set_gpio_level(pin, (uint16_t)ServoState::pulse_to_level(current_pulse, pwm_period, pwm_frequency)); + } + + success = true; } - - // Set the new wrap (should be 1 less than the period to get full 0 to 100%) - pwm_set_wrap(pin_num, pwm_period - 1); - - // If the the period is smaller, update the pwm after setting the new wraps - if(!pre_update_pwm) { - float current_pulse = get_pulse(); - pwm_set_gpio_level(pin, (uint16_t)ServoState::pulse_to_level(current_pulse, pwm_period, pwm_frequency)); - } - - success = true; } return success; } diff --git a/drivers/servo/servo.hpp b/drivers/servo/servo.hpp index 0534b7d0..aaabbe57 100644 --- a/drivers/servo/servo.hpp +++ b/drivers/servo/servo.hpp @@ -11,9 +11,12 @@ namespace servo { // Constants //-------------------------------------------------- public: - static const uint16_t DEFAULT_FREQUENCY = 50; //The standard servo update rate + static constexpr float DEFAULT_FREQUENCY = 50.0f; // The standard servo update rate private: + static constexpr float MIN_FREQUENCY = 10.0f; // Lowest achievable with hardware PWM with good resolution + static constexpr float MAX_FREQUENCY = 350.0f; // Highest nice value that still allows the full uS pulse range + // Some servos are rated for 333Hz for instance static const uint32_t MAX_PWM_WRAP = UINT16_MAX; diff --git a/micropython/modules/servo/servo.cpp b/micropython/modules/servo/servo.cpp index ffd3e804..5c1964d8 100644 --- a/micropython/modules/servo/servo.cpp +++ b/micropython/modules/servo/servo.cpp @@ -664,7 +664,7 @@ extern mp_obj_t Servo_frequency(size_t n_args, const mp_obj_t *pos_args, mp_map_ float freq = mp_obj_get_float(args[ARG_freq].u_obj); if(!self->servo->set_frequency(freq)) - mp_raise_ValueError("freq out of range"); + mp_raise_ValueError("freq out of range. Expected 10Hz to 350Hz"); else return mp_const_none; }