Raised servo functions to a base class, and added ServoCluster which uses the PIO PWM.

drivers/servo/calibration.cpp

@@ -0,0 +1,225 @@
+#include "calibration.hpp"
+
+namespace servo {
+  Calibration::CalibrationPoint::CalibrationPoint()
+    : pulse(0.0f), value(0.0f) {
+  }
+
+  Calibration::CalibrationPoint::CalibrationPoint(uint16_t pulse, float value)
+    : pulse(pulse), value(value) {
+  }
+
+  Calibration::Calibration()
+    : calibration(nullptr), calibration_points(0), limit_lower(true), limit_upper(true) {
+    create_default_calibration(ANGULAR);
+  }
+
+  Calibration::Calibration(Type type)
+    : calibration(nullptr), calibration_points(0), limit_lower(true), limit_upper(true) {
+    create_default_calibration(type);
+  }
+
+  Calibration::~Calibration() {
+    if(calibration != nullptr) {
+      delete[] calibration;
+      calibration = nullptr;
+    }
+  }
+
+  void Calibration::create_default_calibration(Type type) {
+    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) {
+    bool success = false;
+    if(num_points >= 2) {
+      if(calibration != nullptr)
+        delete[] calibration;
+      
+      calibration = new CalibrationPoint[num_points];
+      calibration_points = num_points;
+      success = true;
+    }
+    return success;
+  }
+
+  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(min_pulse, min_value);
+    calibration[1] = CalibrationPoint(mid_pulse, mid_value);
+    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) {
+    bool success = false;
+    if(create_blank_calibration(num_points)) {
+      float points_minus_one = (float)(num_points - 1);
+      for(uint i = 0; i < num_points; i++) {
+        float pulse = ((max_pulse - min_pulse) * (float)i) / points_minus_one;
+        float value = ((max_value - min_value) * (float)i) / points_minus_one;
+        calibration[i] = CalibrationPoint(pulse, value);
+      }
+      success = true;
+    }
+    return success;
+  }
+
+  uint Calibration::points() {
+    return calibration_points;
+  }
+
+  bool Calibration::get_point(uint8_t index, CalibrationPoint& point_out) {
+    bool success = false;
+    if(index < calibration_points) {
+      point_out = CalibrationPoint(calibration[index]);
+      success = true;
+    }
+
+    return success;
+  }
+
+  void Calibration::set_point(uint8_t index, const CalibrationPoint& point) {
+    if(index < calibration_points) {
+      calibration[index] = CalibrationPoint(point);
+    }
+  }
+
+  void Calibration::limit_to_calibration(bool lower, bool upper) {
+    limit_lower = lower;
+    limit_upper = upper;
+  }
+
+  float Converter::min_value() {
+    float value = 0.0f;
+    if(calibration_points >= 2) {
+      value = calibration[0].value;
+    }
+    return value;
+  }
+
+  float Converter::mid_value() {
+    float value = 0.0f;
+    if(calibration_points >= 2) {
+      value = (calibration[0].value + calibration[calibration_points - 1].value) / 2.0f;
+    }
+    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.0f;
+    if(calibration_points >= 2) {
+      uint8_t last = calibration_points - 1;
+
+      // Is the value below the bottom most calibration point?
+      if(value < calibration[0].value) {
+        // Should the value be limited to the calibration or projected below it?
+        if(limit_lower)
+          pulse = calibration[0].pulse;
+        else
+          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?
+      else if(value > calibration[last].value) {
+        // Should the value be limited to the calibration or projected above it?
+        if(limit_upper)
+          pulse = calibration[last].pulse;
+        else
+          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_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
+          }
+        }
+      }
+    }
+
+    return pulse;
+  }
+
+  float Converter::value_from_pulse(float 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
+          }
+        }
+      }
+    }
+
+    return value;
+  }
+
+  uint32_t Converter::pulse_to_level(float pulse, uint32_t resolution) {
+    uint32_t level = 0;
+    if(pulse >= Converter::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);
+    }
+    return level;
+  }
+
+  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;
+  }
+};

drivers/servo/calibration.hpp

@@ -0,0 +1,121 @@
+#pragma once
+
+#include <stdint.h>
+#include <math.h>
+
+#include "pico/stdlib.h"
+#include "hardware/pwm.h"
+#include "hardware/clocks.h"
+#include "common/pimoroni_common.hpp"
+
+namespace servo {
+
+  enum Type {
+    ANGULAR = 0,
+    LINEAR,
+    CONTINUOUS
+  };
+
+  class Calibration {
+    //--------------------------------------------------
+    // Constants
+    //--------------------------------------------------
+  public:
+    static constexpr float DEFAULT_MIN_PULSE = 500.0f;   // in microseconds
+    static constexpr float DEFAULT_MID_PULSE = 1500.0f;  // in microseconds
+    static constexpr float DEFAULT_MAX_PULSE = 2500.0f;  // in microseconds
+    static constexpr float DEFAULT_VALUE_EXTENT = 90.0f; // a range of -90 to +90
+
+
+    //--------------------------------------------------
+    // Substructures
+    //--------------------------------------------------
+  public:
+    struct CalibrationPoint {
+        //--------------------------------------------------
+        // Constructors/Destructor
+        //--------------------------------------------------
+        CalibrationPoint();
+        CalibrationPoint(uint16_t pulse, float value);
+
+
+        //--------------------------------------------------
+        // Variables
+        //--------------------------------------------------
+        float pulse;
+        float value;
+    };
+
+    
+    //--------------------------------------------------
+    // Constructors/Destructor
+    //--------------------------------------------------
+  protected:
+    Calibration();
+    Calibration(Type type);
+    virtual ~Calibration();
+
+
+    //--------------------------------------------------
+    // Methods
+    //--------------------------------------------------
+  public:
+    void create_default_calibration(Type type);
+    bool create_blank_calibration(uint num_points); // Must have at least two points
+    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();
+    bool get_point(uint8_t index, CalibrationPoint& point_out);
+    void set_point(uint8_t index, const CalibrationPoint& point); // Ensure the points are entered in ascending value order
+
+    void limit_to_calibration(bool lower, bool upper);
+
+
+    //--------------------------------------------------
+    // Variables
+    //--------------------------------------------------
+  protected:
+    CalibrationPoint* calibration;
+    uint calibration_points;
+    bool limit_lower;
+    bool limit_upper;
+  };
+
+  class Converter : public Calibration {
+    //--------------------------------------------------
+    // Constants
+    //--------------------------------------------------
+  public:
+    static constexpr float MIN_VALID_PULSE = 1.0f;
+  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
+
+
+    //--------------------------------------------------
+    // Constructors/Destructor
+    //--------------------------------------------------
+  public:
+    Converter() : Calibration() {}
+    Converter(Type type) : Calibration(type) {}
+    virtual ~Converter() {}
+
+
+    //--------------------------------------------------
+    // Methods
+    //--------------------------------------------------
+  public:
+    float min_value();
+    float mid_value();
+    float max_value();
+    float value_to_pulse(float value);
+    float value_from_pulse(float 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);
+  };
+
+}

drivers/servo/servo.cmake

@@ -3,6 +3,9 @@ add_library(${DRIVER_NAME} INTERFACE)
 
 target_sources(${DRIVER_NAME} INTERFACE
   ${CMAKE_CURRENT_LIST_DIR}/servo.cpp
+  ${CMAKE_CURRENT_LIST_DIR}/servo_cluster.cpp
+  ${CMAKE_CURRENT_LIST_DIR}/calibration.cpp
+  ${CMAKE_CURRENT_LIST_DIR}/servo_state.cpp
 )
 
 target_include_directories(${DRIVER_NAME} INTERFACE ${CMAKE_CURRENT_LIST_DIR})
@@ -10,4 +13,5 @@ target_include_directories(${DRIVER_NAME} INTERFACE ${CMAKE_CURRENT_LIST_DIR})
 target_link_libraries(${DRIVER_NAME} INTERFACE
     pico_stdlib
     hardware_pwm
+    multi_pwm
     )

drivers/servo/servo.cpp

@@ -1,222 +1,8 @@
 #include "servo.hpp"
-#include <cstdio>
 
 namespace servo {
-  Calibration::CalibrationPoint::CalibrationPoint()
-    : pulse(0.0f), value(0.0f) {
-  }
-
-  Calibration::CalibrationPoint::CalibrationPoint(uint16_t pulse, float value)
-    : pulse(pulse), value(value) {
-  }
-
-  Calibration::Calibration(Type type)
-    : calibration(nullptr), calibration_points(0), limit_lower(true), limit_upper(true) {
-    create_default_calibration(type);
-  }
-
-  Calibration::~Calibration() {
-    if(calibration != nullptr) {
-      delete[] calibration;
-      calibration = nullptr;
-    }
-  }
-
-  void Calibration::create_default_calibration(Type type) {
-    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) {
-    bool success = false;
-    if(num_points >= 2) {
-      if(calibration != nullptr)
-        delete[] calibration;
-      
-      calibration = new CalibrationPoint[num_points];
-      calibration_points = num_points;
-      success = true;
-    }
-    return success;
-  }
-
-  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(min_pulse, min_value);
-    calibration[1] = CalibrationPoint(mid_pulse, mid_value);
-    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) {
-    bool success = false;
-    if(create_blank_calibration(num_points)) {
-      float points_minus_one = (float)(num_points - 1);
-      for(uint i = 0; i < num_points; i++) {
-        float pulse = ((max_pulse - min_pulse) * (float)i) / points_minus_one;
-        float value = ((max_value - min_value) * (float)i) / points_minus_one;
-        calibration[i] = CalibrationPoint(pulse, value);
-      }
-      success = true;
-    }
-    return success;
-  }
-
-  uint Calibration::points() {
-    return calibration_points;
-  }
-
-  bool Calibration::get_point(uint8_t index, CalibrationPoint& point_out) {
-    bool success = false;
-    if(index < calibration_points) {
-      point_out = CalibrationPoint(calibration[index]);
-      success = true;
-    }
-
-    return success;
-  }
-
-  void Calibration::set_point(uint8_t index, const CalibrationPoint& point) {
-    if(index < calibration_points) {
-      calibration[index] = CalibrationPoint(point);
-    }
-  }
-
-  void Calibration::limit_to_calibration(bool lower, bool upper) {
-    limit_lower = lower;
-    limit_upper = upper;
-  }
-
-  float Converter::min_value() {
-    float value = 0.0f;
-    if(calibration_points >= 2) {
-      value = calibration[0].value;
-    }
-    return value;
-  }
-
-  float Converter::mid_value() {
-    float value = 0.0f;
-    if(calibration_points >= 2) {
-      value = (calibration[0].value + calibration[calibration_points - 1].value) / 2.0f;
-    }
-    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.0f;
-    if(calibration_points >= 2) {
-      uint8_t last = calibration_points - 1;
-
-      // Is the value below the bottom most calibration point?
-      if(value < calibration[0].value) {
-        // Should the value be limited to the calibration or projected below it?
-        if(limit_lower)
-          pulse = calibration[0].pulse;
-        else
-          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?
-      else if(value > calibration[last].value) {
-        // Should the value be limited to the calibration or projected above it?
-        if(limit_upper)
-          pulse = calibration[last].pulse;
-        else
-          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_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
-          }
-        }
-      }
-    }
-
-    return pulse;
-  }
-
-  float Converter::value_from_pulse(float 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
-          }
-        }
-      }
-    }
-
-    return value;
-  }
-
-  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) {
+    : pin(pin), state(type) {
   }
 
   Servo::~Servo() {
@@ -239,79 +25,63 @@ namespace servo {
   }
 
   bool Servo::is_enabled() {
-    return enabled;
+    return state.is_enabled();
   }
 
   void Servo::enable() {
-    if(last_enabled_pulse < MIN_VALID_PULSE) {
+    float new_pulse = state.enable();
-      servo_value = converter.mid_value();
+    pwm_set_gpio_level(pin, (uint16_t)Converter::pulse_to_level(new_pulse, 20000));
-      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 Servo::disable() {
-    pwm_set_gpio_level(pin, 0);
+    float new_pulse = state.disable();
-    enabled = false;
+    pwm_set_gpio_level(pin, (uint16_t)Converter::pulse_to_level(new_pulse, 20000));
   }
 
   float Servo::get_value() {
-    return servo_value;
+    return state.get_value();
   }
 
   void Servo::set_value(float value) {
-    servo_value = value;
+    float new_pulse = state.set_value(value);
-    float pulse = converter.value_to_pulse(value);
+    pwm_set_gpio_level(pin, (uint16_t)Converter::pulse_to_level(new_pulse, 20000));
-    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();
-    }
   }
 
   float Servo::get_pulse() {
-    return last_enabled_pulse;
+    return state.get_pulse();
   }
 
   void Servo::set_pulse(float pulse) {
-    if(pulse >= MIN_VALID_PULSE) {
+    float new_pulse = state.set_pulse(pulse);
-      servo_value = converter.value_from_pulse(pulse);
+    pwm_set_gpio_level(pin, (uint16_t)Converter::pulse_to_level(new_pulse, 20000));
-      last_enabled_pulse = pulse;
-      pwm_set_gpio_level(pin, (uint16_t)converter.pulse_to_level(last_enabled_pulse, 20000));
-      enabled = true;
-    }
-    else {
-      disable();
-    }
   }
 
   void Servo::to_min() {
-    set_value(converter.min_value());
+    float new_pulse = state.to_min();
+    pwm_set_gpio_level(pin, (uint16_t)Converter::pulse_to_level(new_pulse, 20000));
   }
 
   void Servo::to_mid() {
-    set_value(converter.mid_value());
+    float new_pulse = state.to_mid();
+    pwm_set_gpio_level(pin, (uint16_t)Converter::pulse_to_level(new_pulse, 20000));
   }
 
   void Servo::to_max() {
-    set_value(converter.max_value());
+    float new_pulse = state.to_max();
+    pwm_set_gpio_level(pin, (uint16_t)Converter::pulse_to_level(new_pulse, 20000));
   }
 
   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());
+    float new_pulse = state.to_percent(in, in_min, in_max);
-    set_value(value);
+    pwm_set_gpio_level(pin, (uint16_t)Converter::pulse_to_level(new_pulse, 20000));
   }
 
   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);
+    float new_pulse = state.to_percent(in, in_min, in_max, value_min, value_max);
-    set_value(value);
+    pwm_set_gpio_level(pin, (uint16_t)Converter::pulse_to_level(new_pulse, 20000));
   }
 
   Calibration& Servo::calibration() {
-    return converter;
+    return state.calibration();
   }
 };

drivers/servo/servo.hpp

@@ -7,113 +7,11 @@
 #include "hardware/pwm.h"
 #include "hardware/clocks.h"
 #include "common/pimoroni_common.hpp"
+#include "calibration.hpp"
+#include "servo_state.hpp"
 
 namespace servo {
 
-  enum Type {
-    ANGULAR = 0,
-    LINEAR,
-    CONTINUOUS
-  };
-
-  class Calibration {
-    //--------------------------------------------------
-    // Constants
-    //--------------------------------------------------
-  public:
-    static constexpr float DEFAULT_MIN_PULSE = 500.0f;   // in microseconds
-    static constexpr float DEFAULT_MID_PULSE = 1500.0f;  // in microseconds
-    static constexpr float DEFAULT_MAX_PULSE = 2500.0f;  // in microseconds
-    static constexpr float DEFAULT_VALUE_EXTENT = 90.0f; // a range of -90 to +90
-
-
-    //--------------------------------------------------
-    // Substructures
-    //--------------------------------------------------
-  public:
-    struct CalibrationPoint {
-        //--------------------------------------------------
-        // Constructors/Destructor
-        //--------------------------------------------------
-        CalibrationPoint();
-        CalibrationPoint(uint16_t pulse, float value);
-
-
-        //--------------------------------------------------
-        // Variables
-        //--------------------------------------------------
-        float pulse;
-        float value;
-    };
-
-    
-    //--------------------------------------------------
-    // Constructors/Destructor
-    //--------------------------------------------------
-  protected:
-    Calibration(Type type);
-    virtual ~Calibration();
-
-
-    //--------------------------------------------------
-    // Methods
-    //--------------------------------------------------
-  public:
-    void create_default_calibration(Type type);
-    bool create_blank_calibration(uint num_points); // Must have at least two points
-    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();
-    bool get_point(uint8_t index, CalibrationPoint& point_out);
-    void set_point(uint8_t index, const CalibrationPoint& point); // Ensure the points are entered in ascending value order
-
-    void limit_to_calibration(bool lower, bool upper);
-
-
-    //--------------------------------------------------
-    // Variables
-    //--------------------------------------------------
-  protected:
-    CalibrationPoint* calibration;
-    uint calibration_points;
-    bool limit_lower;
-    bool limit_upper;
-  };
-
-  class Converter : public Calibration {
-    //--------------------------------------------------
-    // Constants
-    //--------------------------------------------------
-  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
-
-
-    //--------------------------------------------------
-    // Constructors/Destructor
-    //--------------------------------------------------
-  public:
-    Converter(Type type) : Calibration(type) {}
-    virtual ~Converter() {}
-
-
-    //--------------------------------------------------
-    // Methods
-    //--------------------------------------------------
-  public:
-    float min_value();
-    float mid_value();
-    float max_value();
-    float value_to_pulse(float value);
-    float value_from_pulse(float 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 {
     //--------------------------------------------------
     // Constants
@@ -137,11 +35,7 @@ namespace servo {
     uint16_t pwm_period;
     float pwm_frequency = DEFAULT_PWM_FREQUENCY;
 
-    float servo_value = 0.0f;
+    ServoState state;
-    float last_enabled_pulse = 0.0f;
-    bool enabled = false;
-
-    Converter converter;
 
 
     //--------------------------------------------------

drivers/servo/servo_cluster.cpp

@@ -0,0 +1,117 @@
+#include "servo_cluster.hpp"
+
+namespace servo {
+  ServoCluster::ServoCluster(PIO pio, uint sm, uint channel_mask)
+    : multi_pwm(pio, sm, channel_mask) {
+    multi_pwm.set_wrap(20000);
+  }
+
+  ServoCluster::~ServoCluster() {
+  }
+
+  bool ServoCluster::init() {
+    // pwm_cfg = pwm_get_default_config();
+    // pwm_config_set_wrap(&pwm_cfg, 20000 - 1);
+
+    // float div = clock_get_hz(clk_sys) / 1000000;
+    // pwm_config_set_clkdiv(&pwm_cfg, div);
+
+    // 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;
+  }
+
+  bool ServoCluster::is_enabled(uint servo) {
+    if(servo < NUM_BANK0_GPIOS)
+      return servos[servo].is_enabled();
+    else
+      return false;
+  }
+
+  void ServoCluster::enable(uint servo, bool load) {
+    if(servo < NUM_BANK0_GPIOS) {
+      float new_pulse = servos[servo].enable();
+      multi_pwm.set_chan_level(servo, Converter::pulse_to_level(new_pulse, 20000), 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, Converter::pulse_to_level(new_pulse, 20000), load);
+    }
+  }
+
+  float ServoCluster::get_value(uint servo) {
+    if(servo < NUM_BANK0_GPIOS)
+      return servos[servo].get_value();
+    else
+      return 0.0f;
+  }
+
+  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, Converter::pulse_to_level(new_pulse, 20000), load);
+    }
+  }
+
+  float ServoCluster::get_pulse(uint servo) {
+    if(servo < NUM_BANK0_GPIOS)
+      return servos[servo].get_pulse();
+    else
+      return 0.0f;
+  }
+
+  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, Converter::pulse_to_level(new_pulse, 20000), load);
+    }
+  }
+
+  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, Converter::pulse_to_level(new_pulse, 20000), 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, Converter::pulse_to_level(new_pulse, 20000), 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, Converter::pulse_to_level(new_pulse, 20000), 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, Converter::pulse_to_level(new_pulse, 20000), 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, Converter::pulse_to_level(new_pulse, 20000), load);
+    }
+  }
+
+  Calibration* ServoCluster::calibration(uint servo) {
+    if(servo < NUM_BANK0_GPIOS)
+        return &servos[servo].calibration();
+    else
+        return nullptr;
+  }
+};

drivers/servo/servo_cluster.hpp

@@ -0,0 +1,69 @@
+#pragma once
+
+#include <stdint.h>
+#include <math.h>
+
+#include "pico/stdlib.h"
+#include "hardware/clocks.h"
+#include "common/pimoroni_common.hpp"
+#include "calibration.hpp"
+#include "multi_pwm.hpp"
+#include "servo_state.hpp"
+
+namespace servo {
+
+  class ServoCluster {
+    //--------------------------------------------------
+    // Constants
+    //--------------------------------------------------
+  public:
+    static const uint16_t DEFAULT_PWM_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);
+
+    static constexpr float MIN_VALID_PULSE = 1.0f;
+
+
+    //--------------------------------------------------
+    // Variables
+    //--------------------------------------------------
+  private:
+    MultiPWM multi_pwm;
+    ServoState servos[NUM_BANK0_GPIOS];
+
+
+    //--------------------------------------------------
+    // Constructors/Destructor
+    //--------------------------------------------------
+  public:
+    ServoCluster(PIO pio, uint sm, uint channel_mask);
+    ~ServoCluster();
+
+    //--------------------------------------------------
+    // Methods
+    //--------------------------------------------------
+  public:
+    bool init();
+
+    bool is_enabled(uint servo);
+    void enable(uint servo, bool load = true);
+    void disable(uint servo, bool load = true);
+
+    float get_value(uint servo);
+    void set_value(uint servo, float value, bool load = true);
+
+    float get_pulse(uint servo);
+    void set_pulse(uint servo, float pulse, bool load = true);
+
+    void to_min(uint servo, bool load = true);
+    void to_mid(uint servo, bool load = true);
+    void to_max(uint servo, bool load = true);
+    void to_percent(uint servo, float in, float in_min = 0.0f, float in_max = 1.0f, bool load = true);
+    void to_percent(uint servo, float in, float in_min, float in_max, float value_min, float value_max, bool load = true);
+
+    Calibration* calibration(uint servo);
+  };
+
+}

drivers/servo/servo_state.cpp

@@ -0,0 +1,84 @@
+#include "servo_state.hpp"
+
+namespace servo {
+  ServoState::ServoState(/*uint pin, */Type type)
+    : /*pin(pin), */converter(type) {
+  }
+
+  bool ServoState::is_enabled() {
+    return enabled;
+  }
+
+  float ServoState::enable() {
+    if(last_enabled_pulse < Converter::MIN_VALID_PULSE) {
+      servo_value = converter.mid_value();
+      last_enabled_pulse = converter.value_to_pulse(servo_value);
+    }
+    enabled = true;
+    return last_enabled_pulse;
+  }
+
+  float ServoState::disable() {
+    enabled = false;
+    return 0.0f; // A zero pulse
+  }
+
+  float ServoState::get_value() {
+    return servo_value;
+  }
+
+  float ServoState::set_value(float value) {
+    servo_value = value;
+    float pulse = converter.value_to_pulse(value);
+    if(pulse >= Converter::MIN_VALID_PULSE) {
+      last_enabled_pulse = pulse;
+      enabled = true;
+    }
+    else {
+      pulse = disable();
+    }
+    return pulse;
+  }
+
+  float ServoState::get_pulse() {
+    return last_enabled_pulse;
+  }
+
+  float ServoState::set_pulse(float pulse) {
+    if(pulse >= Converter::MIN_VALID_PULSE) {
+      servo_value = converter.value_from_pulse(pulse);
+      last_enabled_pulse = pulse;
+      enabled = true;
+    }
+    else {
+      pulse = disable();
+    }
+    return pulse;
+  }
+
+  float ServoState::to_min() {
+    return set_value(converter.min_value());
+  }
+
+  float ServoState::to_mid() {
+    return set_value(converter.mid_value());
+  }
+
+  float ServoState::to_max() {
+    return set_value(converter.max_value());
+  }
+
+  float ServoState::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());
+    return set_value(value);
+  }
+
+  float ServoState::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);
+    return set_value(value);
+  }
+
+  Calibration& ServoState::calibration() {
+    return converter;
+  }
+};

drivers/servo/servo_state.hpp

@@ -0,0 +1,57 @@
+#pragma once
+
+#include <stdint.h>
+#include <math.h>
+
+#include "pico/stdlib.h"
+#include "hardware/clocks.h"
+#include "common/pimoroni_common.hpp"
+#include "calibration.hpp"
+
+namespace servo {
+
+  class ServoState {
+    //--------------------------------------------------
+    // Variables
+    //--------------------------------------------------
+  private:
+    //uint pin;
+    float servo_value = 0.0f;
+    float last_enabled_pulse = 0.0f;
+    bool enabled = false;
+
+    Converter converter;
+
+
+    //--------------------------------------------------
+    // Constructors/Destructor
+    //--------------------------------------------------
+  public:
+    ServoState(/*uint pin, */Type type = ANGULAR);
+
+    //--------------------------------------------------
+    // Methods
+    //--------------------------------------------------
+  public:
+    bool init();
+
+    bool is_enabled();
+    float enable();
+    float disable();
+
+    float get_value();
+    float set_value(float value);
+
+    float get_pulse();
+    float set_pulse(float pulse);
+
+    float to_min();
+    float to_mid();
+    float to_max();
+    float to_percent(float in, float in_min, float in_max);
+    float to_percent(float in, float in_min, float in_max, float value_min, float value_max);
+
+    Calibration& calibration();
+  };
+
+}

examples/servo2040/servo2040_functions.cpp

@@ -58,6 +58,7 @@ int main() {
 
   //MultiPWM pwms(pio1, 0, 0b111111111111111);
   //pwms.set_wrap(20000);
+  ServoCluster cluster(pio1, 0, 0b111100);
 
   int speed = DEFAULT_SPEED;
   float offset = 0.0f;
@@ -83,9 +84,11 @@ int main() {
       count = 0;
 
       //pwms.set_chan_level(servo_seq, 2000);//toggle ? 2000 : 1000);
+      cluster.set_pulse(servo_seq + 2, toggle ? 2000 : 1000);
       //pwms.set_chan_polarity(servo_seq, toggle);
       //pwms.set_chan_offset(servo_seq, toggle ? 19000 : 0);
       simple_servo.set_pulse(servo_seq + 1500);
+      //cluster.set_pulse(0, servo_seq + 1500);
       servo_seq++;
       if(servo_seq >= 4) {
         servo_seq = 0;
@@ -95,7 +98,7 @@ int main() {
         //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");
+      //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();
     }

libraries/servo2040/servo2040.hpp

@@ -4,6 +4,7 @@
 #include "ws2812.hpp"
 #include "multi_pwm.hpp"
 #include "servo.hpp"
+#include "servo_cluster.hpp"
 
 namespace servo2040 {
   const uint SERVO_1 = 0;