Improvements to servo calibration and usage

drivers/servo/servo.cpp

@@ -10,9 +10,9 @@ namespace servo {
     : pulse(pulse), value(value) {
   }
 
-  Calibration::Calibration()
+  Calibration::Calibration(Type type)
     : calibration(nullptr), calibration_points(0), limit_lower(true), limit_upper(true) {
-    create_default_calibration();
+    create_default_calibration(type);
   }
 
   Calibration::~Calibration() {
@@ -22,8 +22,22 @@ namespace servo {
     }
   }
 
-  void Calibration::create_default_calibration() {
+  void Calibration::create_default_calibration(Type type) {
-    create_three_point_calibration(DEFAULT_MIN_PULSE, DEFAULT_MID_PULSE, DEFAULT_MAX_PULSE);
+    switch(type) {
+    default:
+    case ANGULAR:
+      create_three_point_calibration(DEFAULT_MIN_PULSE, DEFAULT_MID_PULSE, DEFAULT_MAX_PULSE,
+                                     -90.0f,            0.0f,              +90.0f);
+      break;
+    case LINEAR:
+      create_two_point_calibration(DEFAULT_MIN_PULSE, DEFAULT_MAX_PULSE,
+                                   0.0f,              1.0f);
+      break;
+    case CONTINUOUS:
+      create_three_point_calibration(DEFAULT_MIN_PULSE, DEFAULT_MID_PULSE, DEFAULT_MAX_PULSE,
+                                     -1.0f,            0.0f,              +1.0f);
+      break;
+    }
   }
 
   bool Calibration::create_blank_calibration(uint num_points) {
@@ -39,11 +53,17 @@ namespace servo {
     return success;
   }
 
-  void Calibration::create_three_point_calibration(float minus_pulse, float zero_pulse, float plus_pulse, float value_extent) {
+  void Calibration::create_two_point_calibration(float min_pulse, float max_pulse, float min_value, float max_value) {
+    create_blank_calibration(2);
+    calibration[0] = CalibrationPoint(min_pulse, min_value);
+    calibration[1] = CalibrationPoint(max_pulse, max_value);
+  }
+
+  void Calibration::create_three_point_calibration(float min_pulse, float mid_pulse, float max_pulse, float min_value, float mid_value, float max_value) {
     create_blank_calibration(3);
-    calibration[0] = CalibrationPoint(minus_pulse, -value_extent);
+    calibration[0] = CalibrationPoint(min_pulse, min_value);
-    calibration[1] = CalibrationPoint(zero_pulse,           0.0f);
+    calibration[1] = CalibrationPoint(mid_pulse, mid_value);
-    calibration[2] = CalibrationPoint(plus_pulse,  +value_extent);
+    calibration[2] = CalibrationPoint(max_pulse, max_value);
   }
 
   bool Calibration::create_uniform_calibration(uint num_points, float min_pulse, float min_value, float max_pulse, float max_value) {
@@ -85,25 +105,32 @@ namespace servo {
     limit_upper = upper;
   }
 
-
+  float Converter::min_value() {
-  uint32_t Converter::pulse_to_level(float pulse, uint32_t resolution) {
+    float value = 0.0f;
-    if(pulse != 0) {
+    if(calibration_points >= 2) {
-      // Constrain the level to hardcoded limits to protect the servo
+      value = calibration[0].value;
-      pulse = MIN(MAX(pulse, LOWER_HARD_LIMIT), UPPER_HARD_LIMIT);
     }
-    return (uint32_t)((pulse * (float)resolution) / SERVO_PERIOD);
+    return value;
   }
 
-  uint32_t Converter::pulse_to_level(uint16_t pulse, uint32_t resolution) {
+  float Converter::mid_value() {
-    if(pulse != 0) {
+    float value = 0.0f;
-      // Constrain the level to hardcoded limits to protect the servo
+    if(calibration_points >= 2) {
-      pulse = MIN(MAX(pulse, LOWER_HARD_LIMIT_I), UPPER_HARD_LIMIT_I);
+      value = (calibration[0].value + calibration[calibration_points - 1].value) / 2.0f;
     }
-    return (uint32_t)(((uint64_t)pulse * (uint64_t)resolution) / SERVO_PERIOD);
+    return value;
+  }
+
+  float Converter::max_value() {
+    float value = 0.0f;
+    if(calibration_points >= 2) {
+      value = calibration[calibration_points - 1].value;
+    }
+    return value;
   }
 
   float Converter::value_to_pulse(float value) {
-    float pulse = 0;
+    float pulse = 0.0f;
     if(calibration_points >= 2) {
       uint8_t last = calibration_points - 1;
 
@@ -113,7 +140,7 @@ namespace servo {
         if(limit_lower)
           pulse = calibration[0].pulse;
         else
-          pulse = map_pulse(value, calibration[0].value, calibration[1].value,
+          pulse = map_float(value, calibration[0].value, calibration[1].value,
                                    calibration[0].pulse, calibration[1].pulse);
       }
       // Is the value above the top most calibration point?
@@ -122,14 +149,14 @@ namespace servo {
         if(limit_upper)
           pulse = calibration[last].pulse;
         else
-          pulse = map_pulse(value, calibration[last - 1].value, calibration[last].value,
+          pulse = map_float(value, calibration[last - 1].value, calibration[last].value,
                                    calibration[last - 1].pulse, calibration[last].pulse);
       }
       else {
         // The value must between two calibration points, so iterate through them to find which ones
         for(uint8_t i = 0; i < last; i++) {
           if(value <= calibration[i + 1].value) {
-            pulse = map_pulse(value, calibration[i].value, calibration[i + 1].value,
+            pulse = map_float(value, calibration[i].value, calibration[i + 1].value,
                                      calibration[i].pulse, calibration[i + 1].pulse);
             break; // No need to continue checking so break out of the loop
           }
@@ -140,13 +167,57 @@ namespace servo {
     return pulse;
   }
 
-  float Converter::map_pulse(float value, float min_value, float max_value, float min_pulse, float max_pulse) {
+  float Converter::value_from_pulse(float pulse) {
-    return (((value - min_value) * (max_pulse - min_pulse)) / (max_value - min_value)) + min_pulse;
+    float value = 0.0f;
+    if(calibration_points >= 2) {
+      uint8_t last = calibration_points - 1;
+
+      // Is the pulse below the bottom most calibration point?
+      if(pulse < calibration[0].pulse) {
+        // Should the pulse be limited to the calibration or projected below it?
+        if(limit_lower)
+          value = calibration[0].value;
+        else
+          value = map_float(pulse, calibration[0].pulse, calibration[1].pulse,
+                                   calibration[0].value, calibration[1].value);
+      }
+      // Is the pulse above the top most calibration point?
+      else if(pulse > calibration[last].pulse) {
+        // Should the pulse be limited to the calibration or projected above it?
+        if(limit_upper)
+          value = calibration[last].value;
+        else
+          value = map_float(pulse, calibration[last - 1].pulse, calibration[last].pulse,
+                                   calibration[last - 1].value, calibration[last].value);
+      }
+      else {
+        // The pulse must between two calibration points, so iterate through them to find which ones
+        for(uint8_t i = 0; i < last; i++) {
+          if(pulse <= calibration[i + 1].pulse) {
+            value = map_float(pulse, calibration[i].pulse, calibration[i + 1].pulse,
+                                     calibration[i].value, calibration[i + 1].value);
+            break; // No need to continue checking so break out of the loop
+          }
+        }
+      }
     }
 
-  Servo::Servo(uint pin)
+    return value;
-    : pin(pin) {
+  }
-  };
+
+  uint32_t Converter::pulse_to_level(float pulse, uint32_t resolution) {
+    // Constrain the level to hardcoded limits to protect the servo
+    pulse = MIN(MAX(pulse, LOWER_HARD_LIMIT), UPPER_HARD_LIMIT);
+    return (uint32_t)((pulse * (float)resolution) / SERVO_PERIOD);
+  }
+
+  float Converter::map_float(float in, float in_min, float in_max, float out_min, float out_max) {
+    return (((in - in_min) * (out_max - out_min)) / (in_max - in_min)) + out_min;
+  }
+
+  Servo::Servo(uint pin, Type type)
+    : pin(pin), converter(type) {
+  }
 
   Servo::~Servo() {
     gpio_set_function(pin, GPIO_FUNC_NULL);
@@ -162,54 +233,85 @@ namespace servo {
     pwm_init(pwm_gpio_to_slice_num(pin), &pwm_cfg, true);
     gpio_set_function(pin, GPIO_FUNC_PWM);
 
+    pwm_set_gpio_level(pin, 0);
+
     return true;
   }
 
-  void Servo::set_value(float value) {
+  bool Servo::is_enabled() {
-    float pulse = converter.value_to_pulse(value);
+    return enabled;
-    uint16_t level = (uint16_t)converter.pulse_to_level(pulse, 20000);
-    pwm_set_gpio_level(pin, level);
   }
 
-  // void RGBLED::set_brightness(uint8_t brightness) {
+  void Servo::enable() {
-  //   led_brightness = brightness;
+    if(last_enabled_pulse < MIN_VALID_PULSE) {
-  //   update_pwm();
+      servo_value = converter.mid_value();
-  // }
+      last_enabled_pulse = converter.value_to_pulse(servo_value);
+    }
+    pwm_set_gpio_level(pin, (uint16_t)converter.pulse_to_level(last_enabled_pulse, 20000));
+    enabled = true;
+  }
 
-  // void RGBLED::set_hsv(float h, float s, float v) {
+  void Servo::disable() {
-  //   float i = floor(h * 6.0f);
+    pwm_set_gpio_level(pin, 0);
-  //   float f = h * 6.0f - i;
+    enabled = false;
-  //   v *= 255.0f;
+  }
-  //   uint8_t p = v * (1.0f - s);
-  //   uint8_t q = v * (1.0f - f * s);
-  //   uint8_t t = v * (1.0f - (1.0f - f) * s);
 
-  //   switch (int(i) % 6) {
+  float Servo::get_value() {
-  //     case 0: led_r = v; led_g = t; led_b = p; break;
+    return servo_value;
-  //     case 1: led_r = q; led_g = v; led_b = p; break;
+  }
-  //     case 2: led_r = p; led_g = v; led_b = t; break;
-  //     case 3: led_r = p; led_g = q; led_b = v; break;
-  //     case 4: led_r = t; led_g = p; led_b = v; break;
-  //     case 5: led_r = v; led_g = p; led_b = q; break;
-  //   }
 
-  //   update_pwm();
+  void Servo::set_value(float value) {
-  // }
+    servo_value = value;
+    float pulse = converter.value_to_pulse(value);
+    if(pulse >= MIN_VALID_PULSE) {
+      last_enabled_pulse = pulse;
+      pwm_set_gpio_level(pin, (uint16_t)converter.pulse_to_level(last_enabled_pulse, 20000));
+      enabled = true;
+    }
+    else {
+      disable();
+    }
+  }
 
-  // void RGBLED::update_pwm() {
+  float Servo::get_pulse() {
-  //   uint16_t r16 = GAMMA[led_r];
+    return last_enabled_pulse;
-  //   uint16_t g16 = GAMMA[led_g];
+  }
-  //   uint16_t b16 = GAMMA[led_b];
+
-  //   r16 *= led_brightness;
+  void Servo::set_pulse(float pulse) {
-  //   g16 *= led_brightness;
+    if(pulse >= MIN_VALID_PULSE) {
-  //   b16 *= led_brightness;
+      servo_value = converter.value_from_pulse(pulse);
-  //   if(polarity == Polarity::ACTIVE_LOW) {
+      last_enabled_pulse = pulse;
-  //     r16 = UINT16_MAX - r16;
+      pwm_set_gpio_level(pin, (uint16_t)converter.pulse_to_level(last_enabled_pulse, 20000));
-  //     g16 = UINT16_MAX - g16;
+      enabled = true;
-  //     b16 = UINT16_MAX - b16;
+    }
-  //   }
+    else {
-  //   pwm_set_gpio_level(pin_r, r16);
+      disable();
-  //   pwm_set_gpio_level(pin_g, g16);
+    }
-  //   pwm_set_gpio_level(pin_b, b16);
+  }
-  // }
+
+  void Servo::to_min() {
+    set_value(converter.min_value());
+  }
+
+  void Servo::to_mid() {
+    set_value(converter.mid_value());
+  }
+
+  void Servo::to_max() {
+    set_value(converter.max_value());
+  }
+
+  void Servo::to_percent(float in, float in_min, float in_max) {
+    float value = Converter::map_float(in, in_min, in_max, converter.min_value(), converter.max_value());
+    set_value(value);
+  }
+
+  void Servo::to_percent(float in, float in_min, float in_max, float value_min, float value_max) {
+    float value = Converter::map_float(in, in_min, in_max, value_min, value_max);
+    set_value(value);
+  }
+
+  Calibration& Servo::calibration() {
+    return converter;
+  }
 };

drivers/servo/servo.hpp

@@ -10,6 +10,12 @@
 
 namespace servo {
 
+  enum Type {
+    ANGULAR = 0,
+    LINEAR,
+    CONTINUOUS
+  };
+
   class Calibration {
     //--------------------------------------------------
     // Constants
@@ -45,7 +51,7 @@ namespace servo {
     // Constructors/Destructor
     //--------------------------------------------------
   protected:
-    Calibration();
+    Calibration(Type type);
     virtual ~Calibration();
 
 
@@ -53,9 +59,10 @@ namespace servo {
     // Methods
     //--------------------------------------------------
   public:
-    void create_default_calibration();
+    void create_default_calibration(Type type);
     bool create_blank_calibration(uint num_points); // Must have at least two points
-    void create_three_point_calibration(float minus_pulse, float zero_pulse, float plus_pulse, float value_extent = DEFAULT_VALUE_EXTENT);
+    void create_two_point_calibration(float min_pulse, float max_pulse, float min_value, float max_value);
+    void create_three_point_calibration(float min_pulse, float mid_pulse, float max_pulse, float min_value, float mid_value, float max_value);
     bool create_uniform_calibration(uint num_points, float min_pulse, float min_value, float max_pulse, float max_value); // Must have at least two points
 
     uint points();
@@ -84,17 +91,12 @@ namespace servo {
     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
 
-    //Integer equivalents
-    static const uint16_t LOWER_HARD_LIMIT_I = (uint16_t)LOWER_HARD_LIMIT;
-    static const uint16_t UPPER_HARD_LIMIT_I = (uint16_t)UPPER_HARD_LIMIT;
-    static const uint64_t SERVO_PERIOD_I = (uint64_t)SERVO_PERIOD;
-
 
     //--------------------------------------------------
     // Constructors/Destructor
     //--------------------------------------------------
   public:
-    Converter() : Calibration() {}
+    Converter(Type type) : Calibration(type) {}
     virtual ~Converter() {}
 
 
@@ -102,11 +104,14 @@ namespace servo {
     // Methods
     //--------------------------------------------------
   public:
-    static uint32_t pulse_to_level(float pulse, uint32_t resolution);
+    float min_value();
-    static uint32_t pulse_to_level(uint16_t pulse, uint32_t resolution);
+    float mid_value();
+    float max_value();
     float value_to_pulse(float value);
-  private:
+    float value_from_pulse(float pulse);
-    static float map_pulse(float value, float min_value, float max_value, float min_pulse, float max_pulse);
+
+    static uint32_t pulse_to_level(float pulse, uint32_t resolution);
+    static float map_float(float in, float in_min, float in_max, float out_min, float out_max);
   };
 
   class Servo {
@@ -120,6 +125,8 @@ namespace servo {
     static const uint32_t MAX_PWM_WRAP = UINT16_MAX;
     static constexpr uint16_t MAX_PWM_DIVIDER = (1 << 7);
 
+    static constexpr float MIN_VALID_PULSE = 1.0f;
+
 
     //--------------------------------------------------
     // Variables
@@ -130,7 +137,9 @@ namespace servo {
     uint16_t pwm_period;
     float pwm_frequency = DEFAULT_PWM_FREQUENCY;
 
-    float servo_angle = 0.0f;
+    float servo_value = 0.0f;
+    float last_enabled_pulse = 0.0f;
+    bool enabled = false;
 
     Converter converter;
 
@@ -139,7 +148,7 @@ namespace servo {
     // Constructors/Destructor
     //--------------------------------------------------
   public:
-    Servo(uint pin);
+    Servo(uint pin, Type type = ANGULAR);
     ~Servo();
 
     //--------------------------------------------------
@@ -148,18 +157,23 @@ namespace servo {
   public:
     bool init();
 
-    void enable_servo();
+    bool is_enabled();
-    void disable_servo();
+    void enable();
+    void disable();
 
-    void set_pulse();
+    float get_value();
     void set_value(float value);
 
-    Calibration& calibration() {
+    float get_pulse();
-        return converter;
+    void set_pulse(float pulse);
-    }
 
-  private:
+    void to_min();
-    //void update_pwm();
+    void to_mid();
+    void to_max();
+    void to_percent(float in, float in_min = 0.0f, float in_max = 1.0f);
+    void to_percent(float in, float in_min, float in_max, float value_min, float value_max);
+
+    Calibration& calibration();
   };
 
 }

examples/servo2040/servo2040_functions.cpp

@@ -85,7 +85,7 @@ int main() {
       //pwms.set_chan_level(servo_seq, 2000);//toggle ? 2000 : 1000);
       //pwms.set_chan_polarity(servo_seq, toggle);
       //pwms.set_chan_offset(servo_seq, toggle ? 19000 : 0);
-      simple_servo.set_value(servo_seq);
+      simple_servo.set_pulse(servo_seq + 1500);
       servo_seq++;
       if(servo_seq >= 4) {
         servo_seq = 0;
@@ -93,7 +93,9 @@ int main() {
         //pwms.set_wrap(toggle ? 30000 : 20000);
         //float div = clock_get_hz(clk_sys) / (toggle ? 500000 : 5000000);
         //pwms.set_clkdiv(div);
+        simple_servo.disable();
       }
+      printf("Angle = %f, Pulse = %f, Enabled = %s\n", simple_servo.get_value(), simple_servo.get_pulse(), simple_servo.is_enabled() ? "true" : "false");
 
       //pwms.load_pwm();
     }