kopia lustrzana https://github.com/pimoroni/pimoroni-pico
				
				
				
			Added frequency adjustment to Servo
							rodzic
							
								
									93eafc4694
								
							
						
					
					
						commit
						334ff4e9f9
					
				| 
						 | 
				
			
			@ -11,18 +11,28 @@ namespace servo {
 | 
			
		|||
  }
 | 
			
		||||
 | 
			
		||||
  bool Servo::init() {
 | 
			
		||||
    pwm_cfg = pwm_get_default_config();
 | 
			
		||||
    pwm_config_set_wrap(&pwm_cfg, 20000 - 1);
 | 
			
		||||
    bool success = false;
 | 
			
		||||
 | 
			
		||||
    float div = clock_get_hz(clk_sys) / 1000000;
 | 
			
		||||
    pwm_config_set_clkdiv(&pwm_cfg, div);
 | 
			
		||||
    uint16_t period; uint16_t div16;
 | 
			
		||||
    if(Servo::calculate_pwm_factors(pwm_frequency, period, div16)) {
 | 
			
		||||
      pwm_period = period;
 | 
			
		||||
 | 
			
		||||
    pwm_init(pwm_gpio_to_slice_num(pin), &pwm_cfg, true);
 | 
			
		||||
    gpio_set_function(pin, GPIO_FUNC_PWM);
 | 
			
		||||
      pwm_cfg = pwm_get_default_config();
 | 
			
		||||
 | 
			
		||||
    pwm_set_gpio_level(pin, 0);
 | 
			
		||||
      // Set the new wrap (should be 1 less than the period to get full 0 to 100%)
 | 
			
		||||
      pwm_config_set_wrap(&pwm_cfg, pwm_period - 1);
 | 
			
		||||
 | 
			
		||||
    return true;
 | 
			
		||||
      // Apply the divider
 | 
			
		||||
      pwm_config_set_clkdiv(&pwm_cfg, (float)div16 / 16.0f); // There's no 'pwm_config_set_clkdiv_int_frac' for some reason...
 | 
			
		||||
 | 
			
		||||
      pwm_init(pwm_gpio_to_slice_num(pin), &pwm_cfg, true);
 | 
			
		||||
      gpio_set_function(pin, GPIO_FUNC_PWM);
 | 
			
		||||
 | 
			
		||||
      pwm_set_gpio_level(pin, 0);
 | 
			
		||||
 | 
			
		||||
      success = true;
 | 
			
		||||
    }
 | 
			
		||||
    return success;
 | 
			
		||||
  }
 | 
			
		||||
 | 
			
		||||
  uint Servo::get_pin() const {
 | 
			
		||||
| 
						 | 
				
			
			@ -31,12 +41,12 @@ namespace servo {
 | 
			
		|||
 | 
			
		||||
  void Servo::enable() {
 | 
			
		||||
    float new_pulse = state.enable();
 | 
			
		||||
    pwm_set_gpio_level(pin, (uint16_t)ServoState::pulse_to_level(new_pulse, 20000));
 | 
			
		||||
    pwm_set_gpio_level(pin, (uint16_t)ServoState::pulse_to_level(new_pulse, pwm_period, pwm_frequency));
 | 
			
		||||
  }
 | 
			
		||||
 | 
			
		||||
  void Servo::disable() {
 | 
			
		||||
    float new_pulse = state.disable();
 | 
			
		||||
    pwm_set_gpio_level(pin, (uint16_t)ServoState::pulse_to_level(new_pulse, 20000));
 | 
			
		||||
    pwm_set_gpio_level(pin, (uint16_t)ServoState::pulse_to_level(new_pulse, pwm_period, pwm_frequency));
 | 
			
		||||
  }
 | 
			
		||||
 | 
			
		||||
  bool Servo::is_enabled() const {
 | 
			
		||||
| 
						 | 
				
			
			@ -49,7 +59,7 @@ namespace servo {
 | 
			
		|||
 | 
			
		||||
  void Servo::set_value(float value) {
 | 
			
		||||
    float new_pulse = state.set_value(value);
 | 
			
		||||
    pwm_set_gpio_level(pin, (uint16_t)ServoState::pulse_to_level(new_pulse, 20000));
 | 
			
		||||
    pwm_set_gpio_level(pin, (uint16_t)ServoState::pulse_to_level(new_pulse, pwm_period, pwm_frequency));
 | 
			
		||||
  }
 | 
			
		||||
 | 
			
		||||
  float Servo::get_pulse() const {
 | 
			
		||||
| 
						 | 
				
			
			@ -58,7 +68,54 @@ namespace servo {
 | 
			
		|||
 | 
			
		||||
  void Servo::set_pulse(float pulse) {
 | 
			
		||||
    float new_pulse = state.set_pulse(pulse);
 | 
			
		||||
    pwm_set_gpio_level(pin, (uint16_t)ServoState::pulse_to_level(new_pulse, 20000));
 | 
			
		||||
    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;
 | 
			
		||||
  }
 | 
			
		||||
 | 
			
		||||
  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)) {
 | 
			
		||||
 | 
			
		||||
      // 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;
 | 
			
		||||
 | 
			
		||||
      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);
 | 
			
		||||
 | 
			
		||||
      // 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;
 | 
			
		||||
    }
 | 
			
		||||
    return success;
 | 
			
		||||
  }
 | 
			
		||||
 | 
			
		||||
  float Servo::get_min_value() const {
 | 
			
		||||
| 
						 | 
				
			
			@ -75,27 +132,27 @@ namespace servo {
 | 
			
		|||
 | 
			
		||||
  void Servo::to_min() {
 | 
			
		||||
    float new_pulse = state.to_min();
 | 
			
		||||
    pwm_set_gpio_level(pin, (uint16_t)ServoState::pulse_to_level(new_pulse, 20000));
 | 
			
		||||
    pwm_set_gpio_level(pin, (uint16_t)ServoState::pulse_to_level(new_pulse, pwm_period, pwm_frequency));
 | 
			
		||||
  }
 | 
			
		||||
 | 
			
		||||
  void Servo::to_mid() {
 | 
			
		||||
    float new_pulse = state.to_mid();
 | 
			
		||||
    pwm_set_gpio_level(pin, (uint16_t)ServoState::pulse_to_level(new_pulse, 20000));
 | 
			
		||||
    pwm_set_gpio_level(pin, (uint16_t)ServoState::pulse_to_level(new_pulse, pwm_period, pwm_frequency));
 | 
			
		||||
  }
 | 
			
		||||
 | 
			
		||||
  void Servo::to_max() {
 | 
			
		||||
    float new_pulse = state.to_max();
 | 
			
		||||
    pwm_set_gpio_level(pin, (uint16_t)ServoState::pulse_to_level(new_pulse, 20000));
 | 
			
		||||
    pwm_set_gpio_level(pin, (uint16_t)ServoState::pulse_to_level(new_pulse, pwm_period, pwm_frequency));
 | 
			
		||||
  }
 | 
			
		||||
 | 
			
		||||
  void Servo::to_percent(float in, float in_min, float in_max) {
 | 
			
		||||
    float new_pulse = state.to_percent(in, in_min, in_max);
 | 
			
		||||
    pwm_set_gpio_level(pin, (uint16_t)ServoState::pulse_to_level(new_pulse, 20000));
 | 
			
		||||
    pwm_set_gpio_level(pin, (uint16_t)ServoState::pulse_to_level(new_pulse, pwm_period, pwm_frequency));
 | 
			
		||||
  }
 | 
			
		||||
 | 
			
		||||
  void Servo::to_percent(float in, float in_min, float in_max, float value_min, float value_max) {
 | 
			
		||||
    float new_pulse = state.to_percent(in, in_min, in_max, value_min, value_max);
 | 
			
		||||
    pwm_set_gpio_level(pin, (uint16_t)ServoState::pulse_to_level(new_pulse, 20000));
 | 
			
		||||
    pwm_set_gpio_level(pin, (uint16_t)ServoState::pulse_to_level(new_pulse, pwm_period, pwm_frequency));
 | 
			
		||||
  }
 | 
			
		||||
 | 
			
		||||
  Calibration& Servo::calibration() {
 | 
			
		||||
| 
						 | 
				
			
			@ -105,4 +162,43 @@ namespace servo {
 | 
			
		|||
  const Calibration& Servo::calibration() const {
 | 
			
		||||
    return state.calibration();
 | 
			
		||||
  }
 | 
			
		||||
 | 
			
		||||
  // Derived from the rp2 Micropython implementation: https://github.com/micropython/micropython/blob/master/ports/rp2/machine_pwm.c
 | 
			
		||||
  bool Servo::calculate_pwm_factors(float freq, uint16_t& top_out, uint16_t& div16_out) {
 | 
			
		||||
    bool success = false;
 | 
			
		||||
    uint32_t source_hz = clock_get_hz(clk_sys);
 | 
			
		||||
 | 
			
		||||
    // Check the provided frequency is valid
 | 
			
		||||
    if((freq >= 1.0f) && (freq <= (float)(source_hz >> 1))) {
 | 
			
		||||
      uint32_t div16_top = (uint32_t)((float)(source_hz << 4) / freq);
 | 
			
		||||
      uint32_t top = 1;
 | 
			
		||||
 | 
			
		||||
      while(true) {
 | 
			
		||||
          // Try a few small prime factors to get close to the desired frequency.
 | 
			
		||||
          if((div16_top >= (5 << 4)) && (div16_top % 5 == 0) && (top * 5 <= MAX_PWM_WRAP)) {
 | 
			
		||||
              div16_top /= 5;
 | 
			
		||||
              top *= 5;
 | 
			
		||||
          }
 | 
			
		||||
          else if((div16_top >= (3 << 4)) && (div16_top % 3 == 0) && (top * 3 <= MAX_PWM_WRAP)) {
 | 
			
		||||
              div16_top /= 3;
 | 
			
		||||
              top *= 3;
 | 
			
		||||
          }
 | 
			
		||||
          else if((div16_top >= (2 << 4)) && (top * 2 <= MAX_PWM_WRAP)) {
 | 
			
		||||
              div16_top /= 2;
 | 
			
		||||
              top *= 2;
 | 
			
		||||
          }
 | 
			
		||||
          else {
 | 
			
		||||
              break;
 | 
			
		||||
          }
 | 
			
		||||
      }
 | 
			
		||||
      if(div16_top >= 16 && div16_top <= (UINT8_MAX << 4)) {
 | 
			
		||||
        top_out = top;
 | 
			
		||||
        div16_out = div16_top;
 | 
			
		||||
 | 
			
		||||
        success = true;
 | 
			
		||||
      }
 | 
			
		||||
    }
 | 
			
		||||
    return success;
 | 
			
		||||
  }
 | 
			
		||||
 | 
			
		||||
};
 | 
			
		||||
| 
						 | 
				
			
			@ -11,11 +11,10 @@ namespace servo {
 | 
			
		|||
    // Constants
 | 
			
		||||
    //--------------------------------------------------
 | 
			
		||||
  public:
 | 
			
		||||
    static const uint16_t DEFAULT_PWM_FREQUENCY = 50;       //The standard servo update rate
 | 
			
		||||
    static const uint16_t DEFAULT_FREQUENCY = 50;       //The standard servo update rate
 | 
			
		||||
 | 
			
		||||
  private:
 | 
			
		||||
    static const uint32_t MAX_PWM_WRAP = UINT16_MAX;
 | 
			
		||||
    static constexpr uint16_t MAX_PWM_DIVIDER = (1 << 7);
 | 
			
		||||
 | 
			
		||||
 | 
			
		||||
    //--------------------------------------------------
 | 
			
		||||
| 
						 | 
				
			
			@ -25,8 +24,7 @@ namespace servo {
 | 
			
		|||
    uint pin;
 | 
			
		||||
    pwm_config pwm_cfg;
 | 
			
		||||
    uint16_t pwm_period;
 | 
			
		||||
    float pwm_frequency = DEFAULT_PWM_FREQUENCY;
 | 
			
		||||
 | 
			
		||||
    float pwm_frequency = DEFAULT_FREQUENCY;
 | 
			
		||||
    ServoState state;
 | 
			
		||||
 | 
			
		||||
 | 
			
		||||
| 
						 | 
				
			
			@ -56,6 +54,9 @@ namespace servo {
 | 
			
		|||
    float get_pulse() const;
 | 
			
		||||
    void set_pulse(float pulse);
 | 
			
		||||
 | 
			
		||||
    float get_frequency() const;
 | 
			
		||||
    bool set_frequency(float freq);
 | 
			
		||||
 | 
			
		||||
    float get_min_value() const;
 | 
			
		||||
    float get_mid_value() const;
 | 
			
		||||
    float get_max_value() const;
 | 
			
		||||
| 
						 | 
				
			
			@ -68,6 +69,8 @@ namespace servo {
 | 
			
		|||
 | 
			
		||||
    Calibration& calibration();
 | 
			
		||||
    const Calibration& calibration() const;
 | 
			
		||||
  private:
 | 
			
		||||
    static bool calculate_pwm_factors(float freq, uint16_t& top_out, uint16_t& div16_out);
 | 
			
		||||
  };
 | 
			
		||||
 | 
			
		||||
}
 | 
			
		||||
| 
						 | 
				
			
			@ -31,14 +31,14 @@ namespace servo {
 | 
			
		|||
  void ServoCluster::enable(uint servo, bool load) {
 | 
			
		||||
    if(servo < NUM_BANK0_GPIOS) {
 | 
			
		||||
      float new_pulse = servos[servo].enable();
 | 
			
		||||
      multi_pwm.set_chan_level(servo, ServoState::pulse_to_level(new_pulse, 20000), load);
 | 
			
		||||
      multi_pwm.set_chan_level(servo, ServoState::pulse_to_level(new_pulse, 20000, 1000000 / 50), load);
 | 
			
		||||
    }
 | 
			
		||||
  }
 | 
			
		||||
 | 
			
		||||
  void ServoCluster::disable(uint servo, bool load) {
 | 
			
		||||
    if(servo < NUM_BANK0_GPIOS) {
 | 
			
		||||
      float new_pulse = servos[servo].disable();
 | 
			
		||||
      multi_pwm.set_chan_level(servo, ServoState::pulse_to_level(new_pulse, 20000), load);
 | 
			
		||||
      multi_pwm.set_chan_level(servo, ServoState::pulse_to_level(new_pulse, 20000, 1000000 / 50), load);
 | 
			
		||||
    }
 | 
			
		||||
  }
 | 
			
		||||
 | 
			
		||||
| 
						 | 
				
			
			@ -59,7 +59,7 @@ namespace servo {
 | 
			
		|||
  void ServoCluster::set_value(uint servo, float value, bool load) {
 | 
			
		||||
    if(servo < NUM_BANK0_GPIOS) {
 | 
			
		||||
      float new_pulse = servos[servo].set_value(value);
 | 
			
		||||
      multi_pwm.set_chan_level(servo, ServoState::pulse_to_level(new_pulse, 20000), load);
 | 
			
		||||
      multi_pwm.set_chan_level(servo, ServoState::pulse_to_level(new_pulse, 20000, 1000000 / 50), load);
 | 
			
		||||
    }
 | 
			
		||||
  }
 | 
			
		||||
 | 
			
		||||
| 
						 | 
				
			
			@ -73,7 +73,7 @@ namespace servo {
 | 
			
		|||
  void ServoCluster::set_pulse(uint servo, float pulse, bool load) {
 | 
			
		||||
    if(servo < NUM_BANK0_GPIOS) {
 | 
			
		||||
      float new_pulse = servos[servo].set_pulse(pulse);
 | 
			
		||||
      multi_pwm.set_chan_level(servo, ServoState::pulse_to_level(new_pulse, 20000), load);
 | 
			
		||||
      multi_pwm.set_chan_level(servo, ServoState::pulse_to_level(new_pulse, 20000, 1000000 / 50), load);
 | 
			
		||||
    }
 | 
			
		||||
  }
 | 
			
		||||
 | 
			
		||||
| 
						 | 
				
			
			@ -101,35 +101,35 @@ namespace servo {
 | 
			
		|||
  void ServoCluster::to_min(uint servo, bool load) {
 | 
			
		||||
    if(servo < NUM_BANK0_GPIOS) {
 | 
			
		||||
      float new_pulse = servos[servo].to_min();
 | 
			
		||||
      multi_pwm.set_chan_level(servo, ServoState::pulse_to_level(new_pulse, 20000), load);
 | 
			
		||||
      multi_pwm.set_chan_level(servo, ServoState::pulse_to_level(new_pulse, 20000, 1000000 / 50), load);
 | 
			
		||||
    }
 | 
			
		||||
  }
 | 
			
		||||
 | 
			
		||||
  void ServoCluster::to_mid(uint servo, bool load) {
 | 
			
		||||
    if(servo < NUM_BANK0_GPIOS) {
 | 
			
		||||
      float new_pulse = servos[servo].to_mid();
 | 
			
		||||
      multi_pwm.set_chan_level(servo, ServoState::pulse_to_level(new_pulse, 20000), load);
 | 
			
		||||
      multi_pwm.set_chan_level(servo, ServoState::pulse_to_level(new_pulse, 20000, 1000000 / 50), load);
 | 
			
		||||
    }
 | 
			
		||||
  }
 | 
			
		||||
 | 
			
		||||
  void ServoCluster::to_max(uint servo, bool load) {
 | 
			
		||||
    if(servo < NUM_BANK0_GPIOS) {
 | 
			
		||||
      float new_pulse = servos[servo].to_max();
 | 
			
		||||
      multi_pwm.set_chan_level(servo, ServoState::pulse_to_level(new_pulse, 20000), load);
 | 
			
		||||
      multi_pwm.set_chan_level(servo, ServoState::pulse_to_level(new_pulse, 20000, 1000000 / 50), load);
 | 
			
		||||
    }
 | 
			
		||||
  }
 | 
			
		||||
 | 
			
		||||
  void ServoCluster::to_percent(uint servo, float in, float in_min, float in_max, bool load) {
 | 
			
		||||
    if(servo < NUM_BANK0_GPIOS) {
 | 
			
		||||
      float new_pulse = servos[servo].to_percent(in, in_min, in_max);
 | 
			
		||||
      multi_pwm.set_chan_level(servo, ServoState::pulse_to_level(new_pulse, 20000), load);
 | 
			
		||||
      multi_pwm.set_chan_level(servo, ServoState::pulse_to_level(new_pulse, 20000, 1000000 / 50), load);
 | 
			
		||||
    }
 | 
			
		||||
  }
 | 
			
		||||
 | 
			
		||||
  void ServoCluster::to_percent(uint servo, float in, float in_min, float in_max, float value_min, float value_max, bool load) {
 | 
			
		||||
    if(servo < NUM_BANK0_GPIOS) {
 | 
			
		||||
      float new_pulse = servos[servo].to_percent(in, in_min, in_max, value_min, value_max);
 | 
			
		||||
      multi_pwm.set_chan_level(servo, ServoState::pulse_to_level(new_pulse, 20000), load);
 | 
			
		||||
      multi_pwm.set_chan_level(servo, ServoState::pulse_to_level(new_pulse, 20000, 1000000 / 50), load);
 | 
			
		||||
    }
 | 
			
		||||
  }
 | 
			
		||||
 | 
			
		||||
| 
						 | 
				
			
			
 | 
			
		|||
| 
						 | 
				
			
			@ -114,12 +114,12 @@ namespace servo {
 | 
			
		|||
    return table;
 | 
			
		||||
  }
 | 
			
		||||
 | 
			
		||||
  uint32_t ServoState::pulse_to_level(float pulse, uint32_t resolution) {
 | 
			
		||||
  uint32_t ServoState::pulse_to_level(float pulse, uint32_t resolution, float freq) {
 | 
			
		||||
    uint32_t level = 0;
 | 
			
		||||
    if(pulse >= MIN_VALID_PULSE) {
 | 
			
		||||
        // Constrain the level to hardcoded limits to protect the servo
 | 
			
		||||
        pulse = MIN(MAX(pulse, LOWER_HARD_LIMIT), UPPER_HARD_LIMIT);
 | 
			
		||||
        level = (uint32_t)((pulse * (float)resolution) / SERVO_PERIOD);
 | 
			
		||||
        level = (uint32_t)((pulse * (float)resolution * freq) / 1000000);
 | 
			
		||||
    }
 | 
			
		||||
    return level;
 | 
			
		||||
  }
 | 
			
		||||
| 
						 | 
				
			
			
 | 
			
		|||
| 
						 | 
				
			
			@ -16,7 +16,7 @@ namespace servo {
 | 
			
		|||
  private:
 | 
			
		||||
    static constexpr float LOWER_HARD_LIMIT = 500.0f;   // The minimum microsecond pulse to send
 | 
			
		||||
    static constexpr float UPPER_HARD_LIMIT = 2500.0f;  // The maximum microsecond pulse to send
 | 
			
		||||
    static constexpr float SERVO_PERIOD = 1000000 / 50;    // This is hardcoded as all servos *should* run at this frequency
 | 
			
		||||
    static constexpr float SERVO_PERIOD = 20000;    // This is hardcoded as all servos *should* run at this frequency
 | 
			
		||||
    static constexpr float MIN_VALID_PULSE = 1.0f;
 | 
			
		||||
 | 
			
		||||
 | 
			
		||||
| 
						 | 
				
			
			@ -67,7 +67,7 @@ namespace servo {
 | 
			
		|||
    const Calibration& calibration() const;
 | 
			
		||||
 | 
			
		||||
    //--------------------------------------------------
 | 
			
		||||
    static uint32_t pulse_to_level(float pulse, uint32_t resolution);
 | 
			
		||||
    static uint32_t pulse_to_level(float pulse, uint32_t resolution, float freq);
 | 
			
		||||
  };
 | 
			
		||||
 | 
			
		||||
}
 | 
			
		||||
| 
						 | 
				
			
			@ -23,6 +23,7 @@ MP_DEFINE_CONST_FUN_OBJ_1(Servo_disable_obj, Servo_disable);
 | 
			
		|||
MP_DEFINE_CONST_FUN_OBJ_1(Servo_is_enabled_obj, Servo_is_enabled);
 | 
			
		||||
MP_DEFINE_CONST_FUN_OBJ_KW(Servo_value_obj, 1, Servo_value);
 | 
			
		||||
MP_DEFINE_CONST_FUN_OBJ_KW(Servo_pulse_obj, 1, Servo_pulse);
 | 
			
		||||
MP_DEFINE_CONST_FUN_OBJ_KW(Servo_frequency_obj, 1, Servo_frequency);
 | 
			
		||||
MP_DEFINE_CONST_FUN_OBJ_1(Servo_min_value_obj, Servo_min_value);
 | 
			
		||||
MP_DEFINE_CONST_FUN_OBJ_1(Servo_mid_value_obj, Servo_mid_value);
 | 
			
		||||
MP_DEFINE_CONST_FUN_OBJ_1(Servo_max_value_obj, Servo_max_value);
 | 
			
		||||
| 
						 | 
				
			
			@ -73,6 +74,7 @@ STATIC const mp_rom_map_elem_t Servo_locals_dict_table[] = {
 | 
			
		|||
    { MP_ROM_QSTR(MP_QSTR_is_enabled), MP_ROM_PTR(&Servo_is_enabled_obj) },
 | 
			
		||||
    { MP_ROM_QSTR(MP_QSTR_value), MP_ROM_PTR(&Servo_value_obj) },
 | 
			
		||||
    { MP_ROM_QSTR(MP_QSTR_pulse), MP_ROM_PTR(&Servo_pulse_obj) },
 | 
			
		||||
    { MP_ROM_QSTR(MP_QSTR_frequency), MP_ROM_PTR(&Servo_frequency_obj) },
 | 
			
		||||
    { MP_ROM_QSTR(MP_QSTR_min_value), MP_ROM_PTR(&Servo_min_value_obj) },
 | 
			
		||||
    { MP_ROM_QSTR(MP_QSTR_mid_value), MP_ROM_PTR(&Servo_mid_value_obj) },
 | 
			
		||||
    { MP_ROM_QSTR(MP_QSTR_max_value), MP_ROM_PTR(&Servo_max_value_obj) },
 | 
			
		||||
| 
						 | 
				
			
			
 | 
			
		|||
| 
						 | 
				
			
			@ -495,12 +495,14 @@ void Servo_print(const mp_print_t *print, mp_obj_t self_in, mp_print_kind_t kind
 | 
			
		|||
 | 
			
		||||
    mp_print_str(print, "pin = ");
 | 
			
		||||
    mp_obj_print_helper(print, mp_obj_new_int(self->servo->get_pin()), PRINT_REPR);
 | 
			
		||||
    mp_print_str(print, ", enabled = ");
 | 
			
		||||
    mp_obj_print_helper(print, self->servo->is_enabled() ? mp_const_true : mp_const_false, PRINT_REPR);
 | 
			
		||||
    mp_print_str(print, ", pulse = ");
 | 
			
		||||
    mp_obj_print_helper(print, mp_obj_new_float(self->servo->get_pulse()), PRINT_REPR);
 | 
			
		||||
    mp_print_str(print, ", value = ");
 | 
			
		||||
    mp_obj_print_helper(print, mp_obj_new_float(self->servo->get_value()), PRINT_REPR);
 | 
			
		||||
    mp_print_str(print, ", enabled = ");
 | 
			
		||||
    mp_obj_print_helper(print, self->servo->is_enabled() ? mp_const_true : mp_const_false, PRINT_REPR);
 | 
			
		||||
    mp_print_str(print, ", freq = ");
 | 
			
		||||
    mp_obj_print_helper(print, mp_obj_new_float(self->servo->get_frequency()), PRINT_REPR);
 | 
			
		||||
 | 
			
		||||
    mp_print_str(print, ")");
 | 
			
		||||
}
 | 
			
		||||
| 
						 | 
				
			
			@ -631,6 +633,43 @@ extern mp_obj_t Servo_pulse(size_t n_args, const mp_obj_t *pos_args, mp_map_t *k
 | 
			
		|||
    }
 | 
			
		||||
}
 | 
			
		||||
 | 
			
		||||
extern mp_obj_t Servo_frequency(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) {
 | 
			
		||||
    if(n_args <= 1) {
 | 
			
		||||
        enum { ARG_self };
 | 
			
		||||
        static const mp_arg_t allowed_args[] = {
 | 
			
		||||
            { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ },
 | 
			
		||||
        };
 | 
			
		||||
 | 
			
		||||
        // Parse args.
 | 
			
		||||
        mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)];
 | 
			
		||||
        mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args);
 | 
			
		||||
 | 
			
		||||
        _Servo_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Servo_obj_t);
 | 
			
		||||
 | 
			
		||||
        return mp_obj_new_float(self->servo->get_frequency());
 | 
			
		||||
    }
 | 
			
		||||
    else {
 | 
			
		||||
        enum { ARG_self, ARG_freq };
 | 
			
		||||
        static const mp_arg_t allowed_args[] = {
 | 
			
		||||
            { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ },
 | 
			
		||||
            { MP_QSTR_freq, MP_ARG_REQUIRED | MP_ARG_OBJ },
 | 
			
		||||
        };
 | 
			
		||||
 | 
			
		||||
        // Parse args.
 | 
			
		||||
        mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)];
 | 
			
		||||
        mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args);
 | 
			
		||||
 | 
			
		||||
        _Servo_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Servo_obj_t);
 | 
			
		||||
 | 
			
		||||
        float freq = mp_obj_get_float(args[ARG_freq].u_obj);
 | 
			
		||||
 | 
			
		||||
        if(!self->servo->set_frequency(freq))
 | 
			
		||||
            mp_raise_ValueError("freq out of range");
 | 
			
		||||
        else
 | 
			
		||||
            return mp_const_none;
 | 
			
		||||
    }
 | 
			
		||||
}
 | 
			
		||||
 | 
			
		||||
extern mp_obj_t Servo_min_value(mp_obj_t self_in) {
 | 
			
		||||
    _Servo_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Servo_obj_t);
 | 
			
		||||
    return mp_obj_new_float(self->servo->get_min_value());
 | 
			
		||||
| 
						 | 
				
			
			
 | 
			
		|||
| 
						 | 
				
			
			@ -32,6 +32,7 @@ extern mp_obj_t Servo_disable(mp_obj_t self_in);
 | 
			
		|||
extern mp_obj_t Servo_is_enabled(mp_obj_t self_in);
 | 
			
		||||
extern mp_obj_t Servo_value(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
 | 
			
		||||
extern mp_obj_t Servo_pulse(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
 | 
			
		||||
extern mp_obj_t Servo_frequency(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
 | 
			
		||||
extern mp_obj_t Servo_min_value(mp_obj_t self_in);
 | 
			
		||||
extern mp_obj_t Servo_mid_value(mp_obj_t self_in);
 | 
			
		||||
extern mp_obj_t Servo_max_value(mp_obj_t self_in);
 | 
			
		||||
| 
						 | 
				
			
			@ -57,7 +58,4 @@ extern mp_obj_t ServoCluster_to_min(size_t n_args, const mp_obj_t *pos_args, mp_
 | 
			
		|||
extern mp_obj_t ServoCluster_to_mid(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
 | 
			
		||||
extern mp_obj_t ServoCluster_to_max(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
 | 
			
		||||
extern mp_obj_t ServoCluster_to_percent(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
 | 
			
		||||
extern mp_obj_t ServoCluster_calibration(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
 | 
			
		||||
 | 
			
		||||
 | 
			
		||||
extern bool Pimoroni_mp_obj_to_calibration(mp_obj_t in, void *out);
 | 
			
		||||
extern mp_obj_t ServoCluster_calibration(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
 | 
			
		||||
		Ładowanie…
	
		Reference in New Issue