Added initial structure of new motor classes

drivers/motor/CMakeLists.txt

@@ -1 +1,3 @@
-include(motor.cmake)
+include(motor.cmake)
+include(motor2.cmake)
+include(motor_cluster.cmake)

drivers/motor/calibration.cpp

@@ -0,0 +1,338 @@
+#include "calibration.hpp"
+
+namespace motor {
+  Calibration::Pair::Pair()
+  : duty(0.0f), speed(0.0f) {
+  }
+
+  Calibration::Pair::Pair(float duty, float speed)
+  : duty(duty), speed(speed) {
+  }
+
+  Calibration::Calibration()
+    : calibration(nullptr), calibration_size(0), limit_lower(true), limit_upper(true) {
+  }
+
+  Calibration::Calibration(CalibrationType default_type)
+    : Calibration() {
+    apply_default_pairs(default_type);
+  }
+
+  Calibration::Calibration(const Calibration &other)
+    : calibration(nullptr), calibration_size(0), limit_lower(other.limit_lower), limit_upper(other.limit_upper) {
+    uint size = other.size();
+    apply_blank_pairs(size);
+    for(uint i = 0; i < size; i++) {
+      calibration[i] = other.calibration[i];
+    }
+  }
+
+  Calibration::~Calibration() {
+    if(calibration != nullptr) {
+      delete[] calibration;
+      calibration = nullptr;
+    }
+  }
+
+  Calibration &Calibration::operator=(const Calibration &other) {
+    uint size = other.size();
+    apply_blank_pairs(size);
+    for(uint i = 0; i < size; i++) {
+      calibration[i] = other.calibration[i];
+    }
+    limit_lower = other.limit_lower;
+    limit_upper = other.limit_upper;
+
+    return *this;
+  }
+
+  Calibration::Pair &Calibration::operator[](uint8_t index) {
+    assert(index < calibration_size);
+    return calibration[index];
+  }
+
+  const Calibration::Pair &Calibration::operator[](uint8_t index) const {
+    assert(index < calibration_size);
+    return calibration[index];
+  }
+
+  void Calibration::apply_blank_pairs(uint size) {
+    if(calibration != nullptr) {
+      delete[] calibration;
+    }
+
+    if(size > 0) {
+      calibration = new Pair[size];
+      calibration_size = size;
+    }
+    else {
+      calibration = nullptr;
+      calibration_size = 0;
+    }
+  }
+
+  void Calibration::apply_two_pairs(float min_duty, float max_duty, float min_speed, float max_speed) {
+    apply_blank_pairs(2);
+    calibration[0] = Pair(min_duty, min_speed);
+    calibration[1] = Pair(max_duty, max_speed);
+  }
+
+  void Calibration::apply_three_pairs(float min_duty, float mid_duty, float max_duty, float min_speed, float mid_speed, float max_speed) {
+    apply_blank_pairs(3);
+    calibration[0] = Pair(min_duty, min_speed);
+    calibration[1] = Pair(mid_duty, mid_speed);
+    calibration[2] = Pair(max_duty, max_speed);
+  }
+
+  void Calibration::apply_uniform_pairs(uint size, float min_duty, float max_duty, float min_speed, float max_speed) {
+    apply_blank_pairs(size);
+    if(size > 0) {
+      float size_minus_one = (float)(size - 1);
+      for(uint i = 0; i < size; i++) {
+        float duty = Calibration::map_float((float)i, 0.0f, size_minus_one, min_duty, max_duty);
+        float speed = Calibration::map_float((float)i, 0.0f, size_minus_one, min_speed, max_speed);
+        calibration[i] = Pair(duty, speed);
+      }
+    }
+  }
+
+  void Calibration::apply_default_pairs(CalibrationType default_type) {
+    switch(default_type) {
+    default:
+    case ANGULAR:
+      apply_three_pairs(DEFAULT_MIN_PULSE, DEFAULT_MID_PULSE, DEFAULT_MAX_PULSE,
+                       -90.0f,            0.0f,              +90.0f);
+      break;
+    case LINEAR:
+      apply_two_pairs(DEFAULT_MIN_PULSE, DEFAULT_MAX_PULSE,
+                     0.0f,              1.0f);
+      break;
+    case CONTINUOUS:
+      apply_three_pairs(DEFAULT_MIN_PULSE, DEFAULT_MID_PULSE, DEFAULT_MAX_PULSE,
+                       -1.0f,            0.0f,              +1.0f);
+      break;
+    }
+  }
+
+  uint Calibration::size() const {
+    return calibration_size;
+  }
+
+  Calibration::Pair &Calibration::pair(uint8_t index) {
+    assert(index < calibration_size);
+    return calibration[index];
+  }
+
+  const Calibration::Pair &Calibration::pair(uint8_t index) const {
+    assert(index < calibration_size);
+    return calibration[index];
+  }
+
+  float Calibration::duty(uint8_t index) const {
+    return pair(index).duty;
+  }
+
+  void Calibration::duty(uint8_t index, float duty) {
+    pair(index).duty = duty;
+  }
+
+  float Calibration::speed(uint8_t index) const {
+    return pair(index).speed;
+  }
+
+  void Calibration::speed(uint8_t index, float speed) {
+    pair(index).speed = speed;
+  }
+
+  Calibration::Pair &Calibration::first() {
+    assert(calibration_size > 0);
+    return calibration[0];
+  }
+
+  const Calibration::Pair &Calibration::first() const {
+    assert(calibration_size > 0);
+    return calibration[0];
+  }
+
+  float Calibration::first_duty() const {
+    return first().duty;
+  }
+
+  void Calibration::first_duty(float duty) {
+    first().duty = duty;
+  }
+
+  float Calibration::first_speed() const {
+    return first().speed;
+  }
+
+  void Calibration::first_speed(float speed) {
+    first().speed = speed;
+  }
+
+  Calibration::Pair &Calibration::last() {
+    assert(calibration_size > 0);
+    return calibration[calibration_size - 1];
+  }
+
+  const Calibration::Pair &Calibration::last() const {
+    assert(calibration_size > 0);
+    return calibration[calibration_size - 1];
+  }
+
+  float Calibration::last_duty() const {
+    return last().duty;
+  }
+
+  void Calibration::last_duty(float duty) {
+    last().duty = duty;
+  }
+
+  float Calibration::last_speed() const {
+    return last().speed;
+  }
+
+  void Calibration::last_speed(float speed) {
+    last().speed = speed;
+  }
+
+  bool Calibration::has_lower_limit() const {
+    return limit_lower;
+  }
+
+  bool Calibration::has_upper_limit() const {
+    return limit_upper;
+  }
+
+  void Calibration::limit_to_calibration(bool lower, bool upper) {
+    limit_lower = lower;
+    limit_upper = upper;
+  }
+
+  bool Calibration::speed_to_duty(float speed, float &duty_out, float &speed_out) const {
+    bool success = false;
+    if(calibration_size >= 2) {
+      uint8_t last = calibration_size - 1;
+
+      speed_out = speed;
+
+      // Is the speed below the bottom most calibration pair?
+      if(speed < calibration[0].speed) {
+        // Should the speed be limited to the calibration or projected below it?
+        if(limit_lower) {
+          duty_out = calibration[0].duty;
+          speed_out = calibration[0].speed;
+        }
+        else {
+          duty_out = map_float(speed, calibration[0].speed, calibration[1].speed,
+                                       calibration[0].duty, calibration[1].duty);
+        }
+      }
+      // Is the speed above the top most calibration pair?
+      else if(speed > calibration[last].speed) {
+        // Should the speed be limited to the calibration or projected above it?
+        if(limit_upper) {
+          duty_out = calibration[last].duty;
+          speed_out = calibration[last].speed;
+        }
+        else {
+          duty_out = map_float(speed, calibration[last - 1].speed, calibration[last].speed,
+                                       calibration[last - 1].duty, calibration[last].duty);
+        }
+      }
+      else {
+        // The speed must between two calibration pairs, so iterate through them to find which ones
+        for(uint8_t i = 0; i < last; i++) {
+          if(speed <= calibration[i + 1].speed) {
+            duty_out = map_float(speed, calibration[i].speed, calibration[i + 1].speed,
+                                         calibration[i].duty, calibration[i + 1].duty);
+            break; // No need to continue checking so break out of the loop
+          }
+        }
+      }
+
+      // Clamp the duty between the hard limits
+      if(duty_out < LOWER_HARD_LIMIT || duty_out > UPPER_HARD_LIMIT) {
+        duty_out = MIN(MAX(duty_out, LOWER_HARD_LIMIT), UPPER_HARD_LIMIT);
+
+        // Is the duty below the bottom most calibration pair?
+        if(duty_out < calibration[0].duty) {
+          speed_out = map_float(duty_out, calibration[0].duty, calibration[1].duty,
+                                           calibration[0].speed, calibration[1].speed);
+        }
+        // Is the duty above the top most calibration pair?
+        else if(duty_out > calibration[last].duty) {
+          speed_out = map_float(duty_out, calibration[last - 1].duty, calibration[last].duty,
+                                           calibration[last - 1].speed, calibration[last].speed);
+        }
+        else {
+          // The duty must between two calibration pairs, so iterate through them to find which ones
+          for(uint8_t i = 0; i < last; i++) {
+            if(duty_out <= calibration[i + 1].duty) {
+              speed_out = map_float(duty_out, calibration[i].duty, calibration[i + 1].duty,
+                                               calibration[i].speed, calibration[i + 1].speed);
+              break; // No need to continue checking so break out of the loop
+            }
+          }
+        }
+      }
+
+      success = true;
+    }
+
+    return success;
+  }
+
+  bool Calibration::duty_to_speed(float duty, float &speed_out, float &duty_out) const {
+    bool success = false;
+    if(calibration_size >= 2) {
+      uint8_t last = calibration_size - 1;
+
+      // Clamp the duty between the hard limits
+      duty_out = MIN(MAX(duty, LOWER_HARD_LIMIT), UPPER_HARD_LIMIT);
+
+      // Is the duty below the bottom most calibration pair?
+      if(duty_out < calibration[0].duty) {
+        // Should the duty be limited to the calibration or projected below it?
+        if(limit_lower) {
+          speed_out = calibration[0].speed;
+          duty_out = calibration[0].duty;
+        }
+        else {
+          speed_out = map_float(duty, calibration[0].duty, calibration[1].duty,
+                                       calibration[0].speed, calibration[1].speed);
+        }
+      }
+      // Is the duty above the top most calibration pair?
+      else if(duty > calibration[last].duty) {
+        // Should the duty be limited to the calibration or projected above it?
+        if(limit_upper) {
+          speed_out = calibration[last].speed;
+          duty_out = calibration[last].duty;
+        }
+        else {
+          speed_out = map_float(duty, calibration[last - 1].duty, calibration[last].duty,
+                                       calibration[last - 1].speed, calibration[last].speed);
+        }
+      }
+      else {
+        // The duty must between two calibration pairs, so iterate through them to find which ones
+        for(uint8_t i = 0; i < last; i++) {
+          if(duty <= calibration[i + 1].duty) {
+            speed_out = map_float(duty, calibration[i].duty, calibration[i + 1].duty,
+                                         calibration[i].speed, calibration[i + 1].speed);
+            break; // No need to continue checking so break out of the loop
+          }
+        }
+      }
+
+      success = true;
+    }
+
+    return success;
+  }
+
+  float Calibration::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/motor/calibration.hpp

@@ -0,0 +1,119 @@
+#pragma once
+
+#include "pico/stdlib.h"
+
+namespace motor {
+
+  enum CalibrationType {
+    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
+
+  private:
+    static constexpr float LOWER_HARD_LIMIT = 400.0f;   // The minimum microsecond duty to send
+    static constexpr float UPPER_HARD_LIMIT = 2600.0f;  // The maximum microsecond duty to send
+
+
+    //--------------------------------------------------
+    // Substructures
+    //--------------------------------------------------
+  public:
+    struct Pair {
+        //--------------------------------------------------
+        // Constructors/Destructor
+        //--------------------------------------------------
+        Pair();
+        Pair(float duty, float speed);
+
+
+        //--------------------------------------------------
+        // Variables
+        //--------------------------------------------------
+        float duty;
+        float speed;
+    };
+
+
+    //--------------------------------------------------
+    // Constructors/Destructor
+    //--------------------------------------------------
+  public:
+    Calibration();
+    Calibration(CalibrationType default_type);
+    Calibration(const Calibration &other);
+    virtual ~Calibration();
+
+
+    //--------------------------------------------------
+    // Operators
+    //--------------------------------------------------
+  public:
+    Calibration &operator=(const Calibration &other);
+    Pair &operator[](uint8_t index);
+    const Pair &operator[](uint8_t index) const;
+
+
+    //--------------------------------------------------
+    // Methods
+    //--------------------------------------------------
+  public:
+    void apply_blank_pairs(uint size);
+    void apply_two_pairs(float min_duty, float max_duty, float min_speed, float max_speed);
+    void apply_three_pairs(float min_duty, float mid_duty, float max_duty, float min_speed, float mid_speed, float max_speed);
+    void apply_uniform_pairs(uint size, float min_duty, float max_duty, float min_speed, float max_speed);
+    void apply_default_pairs(CalibrationType default_type);
+
+    uint size() const;
+
+    Pair &pair(uint8_t index); // Ensure the pairs are assigned in ascending speed order
+    const Pair &pair(uint8_t index) const; // Ensure the pairs are assigned in ascending speed order
+    float duty(uint8_t index) const;
+    void duty(uint8_t index, float duty);
+    float speed(uint8_t index) const;
+    void speed(uint8_t index, float speed);
+
+    Pair &first();
+    const Pair &first() const;
+    float first_duty() const;
+    void first_duty(float duty);
+    float first_speed() const;
+    void first_speed(float speed);
+
+    Pair &last();
+    const Pair &last() const;
+    float last_duty() const;
+    void last_duty(float duty);
+    float last_speed() const;
+    void last_speed(float speed);
+
+    bool has_lower_limit() const;
+    bool has_upper_limit() const;
+    void limit_to_calibration(bool lower, bool upper);
+
+    bool speed_to_duty(float speed, float &duty_out, float &speed_out) const;
+    bool duty_to_speed(float duty, float &speed_out, float &duty_out) const;
+
+    //static float map_float(float in, float in_min, float in_max, float out_min, float out_max);
+
+
+    //--------------------------------------------------
+    // Variables
+    //--------------------------------------------------
+  private:
+    Pair* calibration;
+    uint calibration_size;
+    bool limit_lower;
+    bool limit_upper;
+  };
+
+}

drivers/motor/motor2.cmake

@@ -0,0 +1,15 @@
+set(DRIVER_NAME motor2)
+add_library(${DRIVER_NAME} INTERFACE)
+
+target_sources(${DRIVER_NAME} INTERFACE
+  ${CMAKE_CURRENT_LIST_DIR}/motor2.cpp
+  ${CMAKE_CURRENT_LIST_DIR}/motor_state.cpp
+)
+
+target_include_directories(${DRIVER_NAME} INTERFACE ${CMAKE_CURRENT_LIST_DIR})
+
+target_link_libraries(${DRIVER_NAME} INTERFACE
+    pico_stdlib
+    hardware_pwm
+    pwm
+    )

drivers/motor/motor2.cpp

@@ -0,0 +1,211 @@
+#include "motor2.hpp"
+#include "hardware/clocks.h"
+#include "pwm.hpp"
+
+namespace motor {
+  Motor2::Motor2(uint pin_pos, uint pin_neg, float freq, MotorState::DecayMode mode)
+    : motor_pin_pos(pin_pos), motor_pin_neg(pin_neg), pwm_frequency(freq), motor_decay_mode(mode) {
+  }
+
+  // Motor2::Motor2(uint pin, /*const Calibration& calibration,*/ float freq)
+  //   : motor_pin_pos(pin), /*state(calibration),*/ pwm_frequency(freq) {
+  // }
+
+  Motor2::~Motor2() {
+    gpio_set_function(motor_pin_pos, GPIO_FUNC_NULL);
+    gpio_set_function(motor_pin_neg, GPIO_FUNC_NULL);
+  }
+
+  bool Motor2::init() {
+    bool success = false;
+
+    uint16_t period; uint16_t div16;
+    if(pimoroni::calculate_pwm_factors(pwm_frequency, period, div16)) {
+      pwm_period = period;
+
+      pwm_cfg = pwm_get_default_config();
+
+      // 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);
+
+      // 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(motor_pin_pos), &pwm_cfg, true);
+      gpio_set_function(motor_pin_pos, GPIO_FUNC_PWM);
+
+      pwm_init(pwm_gpio_to_slice_num(motor_pin_neg), &pwm_cfg, true);
+      gpio_set_function(motor_pin_neg, GPIO_FUNC_PWM);
+
+      pwm_set_gpio_level(motor_pin_pos, 0);
+      pwm_set_gpio_level(motor_pin_neg, 0);
+
+      success = true;
+    }
+    return success;
+  }
+
+  uint Motor2::pin() const {
+    return motor_pin_pos;
+  }
+
+  void Motor2::enable() {
+    apply_duty(state.enable());
+  }
+
+  void Motor2::disable() {
+    apply_duty(state.disable());
+  }
+
+  bool Motor2::is_enabled() const {
+    return state.is_enabled();
+  }
+
+  // float Motor2::duty() const {
+  //   return state.get_duty();
+  // }
+
+  // void Motor2::duty(float duty) {
+  //   apply_duty(state.set_duty_with_return(duty));
+  // }
+
+  float Motor2::speed() const {
+    return state.get_speed();
+  }
+
+  void Motor2::speed(float speed) {
+    apply_duty(state.set_speed_with_return(speed));
+  }
+
+  float Motor2::frequency() const {
+    return pwm_frequency;
+  }
+
+  bool Motor2::frequency(float freq) {
+    bool success = false;
+
+    if((freq >= MotorState::MIN_FREQUENCY) && (freq <= MotorState::MAX_FREQUENCY)) {
+      // Calculate a suitable pwm wrap period for this frequency
+      uint16_t period; uint16_t div16;
+      if(pimoroni::calculate_pwm_factors(freq, period, div16)) {
+
+        // Record if the new period will be larger or smaller.
+        // This is used to apply new pwm speeds 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(motor_pin_pos);
+
+        // 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(state.is_enabled() && pre_update_pwm) {
+          apply_duty(state.get_duty());
+        }
+
+        // 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(state.is_enabled() && !pre_update_pwm) {
+          apply_duty(state.get_duty());
+        }
+
+        success = true;
+      }
+    }
+    return success;
+  }
+
+  MotorState::DecayMode Motor2::decay_mode() {
+    return motor_decay_mode;
+  }
+
+  void Motor2::decay_mode(MotorState::DecayMode mode) {
+    motor_decay_mode = mode;
+    apply_duty(state.get_duty());
+  }
+
+  void Motor2::stop() {
+    apply_duty(state.set_speed_with_return(0.0f));
+  }
+
+  void Motor2::coast() {
+    disable();
+  }
+
+  float Motor2::min_speed() const {
+    return state.get_min_speed();
+  }
+
+  // float Motor2::mid_speed() const {
+  //   return state.get_mid_speed();
+  // }
+
+  float Motor2::max_speed() const {
+    return state.get_max_speed();
+  }
+
+  void Motor2::to_min() {
+    apply_duty(state.to_min_with_return());
+  }
+
+  // void Motor2::to_mid() {
+  //   apply_duty(state.to_mid_with_return());
+  // }
+
+  void Motor2::to_max() {
+    apply_duty(state.to_max_with_return());
+  }
+
+  void Motor2::to_percent(float in, float in_min, float in_max) {
+    apply_duty(state.to_percent_with_return(in, in_min, in_max));
+  }
+
+  void Motor2::to_percent(float in, float in_min, float in_max, float speed_min, float speed_max) {
+    apply_duty(state.to_percent_with_return(in, in_min, in_max, speed_min, speed_max));
+  }
+
+  // Calibration& Motor2::calibration() {
+  //   return state.calibration();
+  // }
+
+  // const Calibration& Motor2::calibration() const {
+  //   return state.calibration();
+  // }
+
+  void Motor2::apply_duty(float duty) {
+    int32_t signed_level = MotorState::duty_to_level(duty, pwm_period);
+
+    switch(motor_decay_mode) {
+    case MotorState::SLOW_DECAY: //aka 'Braking'
+      if(signed_level >= 0) {
+        pwm_set_gpio_level(motor_pin_pos, pwm_period);
+        pwm_set_gpio_level(motor_pin_neg, pwm_period - signed_level);
+      }
+      else {
+        pwm_set_gpio_level(motor_pin_pos, pwm_period + signed_level);
+        pwm_set_gpio_level(motor_pin_neg, pwm_period);
+      }
+      break;
+
+    case MotorState::FAST_DECAY: //aka 'Coasting'
+    default:
+      if(signed_level >= 0) {
+        pwm_set_gpio_level(motor_pin_pos, signed_level);
+        pwm_set_gpio_level(motor_pin_neg, 0);
+      }
+      else {
+        pwm_set_gpio_level(motor_pin_pos, 0);
+        pwm_set_gpio_level(motor_pin_neg, 0 - signed_level);
+      }
+      break;
+    }
+  }
+};

drivers/motor/motor2.hpp

@@ -0,0 +1,79 @@
+#pragma once
+
+#include "pico/stdlib.h"
+#include "hardware/pwm.h"
+#include "motor_state.hpp"
+
+namespace motor {
+
+  class Motor2 {
+    //--------------------------------------------------
+    // Variables
+    //--------------------------------------------------
+  private:
+    uint motor_pin_pos;
+    uint motor_pin_neg;
+    MotorState state;
+    pwm_config pwm_cfg;
+    uint16_t pwm_period;
+    float pwm_frequency;
+    MotorState::DecayMode motor_decay_mode;
+
+
+    //--------------------------------------------------
+    // Constructors/Destructor
+    //--------------------------------------------------
+  public:
+    Motor2(uint pin_pos, uint pin_neg, float freq = MotorState::DEFAULT_FREQUENCY, MotorState::DecayMode mode = MotorState::DEFAULT_DECAY_MODE);
+    //Motor2(uint pin, /*const Calibration& calibration,*/ float freq = MotorState::DEFAULT_FREQUENCY);
+    ~Motor2();
+
+
+    //--------------------------------------------------
+    // Methods
+    //--------------------------------------------------
+  public:
+    bool init();
+
+    // For print access in micropython
+    uint pin() const;
+
+    void enable();
+    void disable();
+    bool is_enabled() const;
+
+    //float duty() const;
+    //void duty(float duty);
+
+    float speed() const;
+    void speed(float speed);
+
+    float frequency() const;
+    bool frequency(float freq);
+
+    MotorState::DecayMode decay_mode();
+    void decay_mode(MotorState::DecayMode mode);
+
+    void stop();
+    void coast(); // An alias for disable
+
+    //--------------------------------------------------
+    float min_speed() const;
+    //float mid_speed() const;
+    float max_speed() const;
+
+    void to_min();
+    //void to_mid();
+    void to_max();
+    void to_percent(float in, float in_min = MotorState::ZERO_PERCENT, float in_max = MotorState::ONEHUNDRED_PERCENT);
+    void to_percent(float in, float in_min, float in_max, float speed_min, float speed_max);
+
+    //Calibration& calibration();
+    //const Calibration& calibration() const;
+
+    //--------------------------------------------------
+  private:
+    void apply_duty(float duty);
+  };
+
+}

drivers/motor/motor_cluster.cmake

@@ -0,0 +1,14 @@
+set(DRIVER_NAME motor_cluster)
+add_library(${DRIVER_NAME} INTERFACE)
+
+target_sources(${DRIVER_NAME} INTERFACE
+  ${CMAKE_CURRENT_LIST_DIR}/motor_cluster.cpp
+  ${CMAKE_CURRENT_LIST_DIR}/motor_state.cpp
+)
+
+target_include_directories(${DRIVER_NAME} INTERFACE ${CMAKE_CURRENT_LIST_DIR})
+
+target_link_libraries(${DRIVER_NAME} INTERFACE
+    pico_stdlib
+    pwm_cluster
+    )

drivers/motor/motor_cluster.cpp

@@ -0,0 +1,527 @@
+#include "motor_cluster.hpp"
+#include "pwm.hpp"
+#include <cstdio>
+
+namespace motor {
+  MotorCluster::MotorCluster(PIO pio, uint sm, uint pin_mask, CalibrationType default_type, float freq, bool auto_phase, PWMCluster::Sequence *seq_buffer, PWMCluster::TransitionData *dat_buffer)
+    : pwms(pio, sm, pin_mask, seq_buffer, dat_buffer), pwm_frequency(freq) {
+    create_motor_states(default_type, auto_phase);
+  }
+
+  MotorCluster::MotorCluster(PIO pio, uint sm, uint pin_base, uint pin_count, CalibrationType default_type, float freq, bool auto_phase, PWMCluster::Sequence *seq_buffer, PWMCluster::TransitionData *dat_buffer)
+    : pwms(pio, sm, pin_base, pin_count, seq_buffer, dat_buffer), pwm_frequency(freq) {
+    create_motor_states(default_type, auto_phase);
+  }
+
+  MotorCluster::MotorCluster(PIO pio, uint sm, const uint8_t *pins, uint32_t length, CalibrationType default_type, float freq, bool auto_phase, PWMCluster::Sequence *seq_buffer, PWMCluster::TransitionData *dat_buffer)
+    : pwms(pio, sm, pins, length, seq_buffer, dat_buffer), pwm_frequency(freq) {
+    create_motor_states(default_type, auto_phase);
+  }
+
+  MotorCluster::MotorCluster(PIO pio, uint sm, std::initializer_list<uint8_t> pins, CalibrationType default_type, float freq, bool auto_phase, PWMCluster::Sequence *seq_buffer, PWMCluster::TransitionData *dat_buffer)
+    : pwms(pio, sm, pins, seq_buffer, dat_buffer), pwm_frequency(freq) {
+    create_motor_states(default_type, auto_phase);
+  }
+
+  MotorCluster::MotorCluster(PIO pio, uint sm, uint pin_mask, const Calibration& calibration, float freq, bool auto_phase, PWMCluster::Sequence *seq_buffer, PWMCluster::TransitionData *dat_buffer)
+    : pwms(pio, sm, pin_mask, seq_buffer, dat_buffer), pwm_frequency(freq) {
+    create_motor_states(calibration, auto_phase);
+  }
+
+  MotorCluster::MotorCluster(PIO pio, uint sm, uint pin_base, uint pin_count, const Calibration& calibration, float freq, bool auto_phase, PWMCluster::Sequence *seq_buffer, PWMCluster::TransitionData *dat_buffer)
+    : pwms(pio, sm, pin_base, pin_count, seq_buffer, dat_buffer), pwm_frequency(freq) {
+    create_motor_states(calibration, auto_phase);
+  }
+
+  MotorCluster::MotorCluster(PIO pio, uint sm, const uint8_t *pins, uint32_t length, const Calibration& calibration, float freq, bool auto_phase, PWMCluster::Sequence *seq_buffer, PWMCluster::TransitionData *dat_buffer)
+    : pwms(pio, sm, pins, length, seq_buffer, dat_buffer), pwm_frequency(freq) {
+    create_motor_states(calibration, auto_phase);
+  }
+
+  MotorCluster::MotorCluster(PIO pio, uint sm, std::initializer_list<uint8_t> pins, const Calibration& calibration, float freq, bool auto_phase, PWMCluster::Sequence *seq_buffer, PWMCluster::TransitionData *dat_buffer)
+    : pwms(pio, sm, pins, seq_buffer, dat_buffer), pwm_frequency(freq) {
+    create_motor_states(calibration, auto_phase);
+  }
+
+  MotorCluster::~MotorCluster() {
+    delete[] states;
+    delete[] motor_phases;
+  }
+
+  bool MotorCluster::init() {
+    bool success = false;
+
+    if(pwms.init()) {
+      // Calculate a suitable pwm wrap period for this frequency
+      uint32_t period; uint32_t div256;
+      if(pimoroni::PWMCluster::calculate_pwm_factors(pwm_frequency, period, div256)) {
+        pwm_period = period;
+
+        // Update the pwm before setting the new wrap
+        uint8_t motor_count = pwms.get_chan_count();
+        for(uint8_t motor = 0; motor < motor_count; motor++) {
+          pwms.set_chan_level(motor, 0, false);
+          pwms.set_chan_offset(motor, (uint32_t)(motor_phases[motor] * (float)pwm_period), false);
+        }
+
+        // Set the new wrap (should be 1 less than the period to get full 0 to 100%)
+        pwms.set_wrap(pwm_period, true); // NOTE Minus 1 not needed here. Maybe should change Wrap behaviour so it is needed, for consistency with hardware pwm?
+
+        // Apply the new divider
+        // This is done after loading new PWM speeds to avoid a lockup condition
+        uint8_t div = div256 >> 8;
+        uint8_t mod = div256 % 256;
+        pwms.set_clkdiv_int_frac(div, mod);
+
+        success = true;
+      }
+    }
+
+    return success;
+  }
+
+  uint8_t MotorCluster::count() const {
+    return pwms.get_chan_count();
+  }
+
+  uint8_t MotorCluster::pin(uint8_t motor) const {
+    return pwms.get_chan_pin(motor);
+  }
+
+  void MotorCluster::enable(uint8_t motor, bool load) {
+    assert(motor < pwms.get_chan_count());
+    float new_pulse = states[motor].enable();
+    apply_pulse(motor, new_pulse, load);
+  }
+
+  void MotorCluster::enable(const uint8_t *motors, uint8_t length, bool load) {
+    assert(motors != nullptr);
+    for(uint8_t i = 0; i < length; i++) {
+      enable(motors[i], false);
+    }
+    if(load)
+      pwms.load_pwm();
+  }
+
+  void MotorCluster::enable(std::initializer_list<uint8_t> motors, bool load) {
+    for(auto motor : motors) {
+      enable(motor, false);
+    }
+    if(load)
+      pwms.load_pwm();
+  }
+
+  void MotorCluster::enable_all(bool load) {
+    uint8_t motor_count = pwms.get_chan_count();
+    for(uint8_t motor = 0; motor < motor_count; motor++) {
+      enable(motor, false);
+    }
+    if(load)
+      pwms.load_pwm();
+  }
+
+  void MotorCluster::disable(uint8_t motor, bool load) {
+    assert(motor < pwms.get_chan_count());
+    float new_pulse = states[motor].disable();
+    apply_pulse(motor, new_pulse, load);
+  }
+
+  void MotorCluster::disable(const uint8_t *motors, uint8_t length, bool load) {
+    assert(motors != nullptr);
+    for(uint8_t i = 0; i < length; i++) {
+      disable(motors[i], false);
+    }
+    if(load)
+      pwms.load_pwm();
+  }
+
+  void MotorCluster::disable(std::initializer_list<uint8_t> motors, bool load) {
+    for(auto motor : motors) {
+      disable(motor, false);
+    }
+    if(load)
+      pwms.load_pwm();
+  }
+
+  void MotorCluster::disable_all(bool load) {
+    uint8_t motor_count = pwms.get_chan_count();
+    for(uint8_t motor = 0; motor < motor_count; motor++) {
+      disable(motor, false);
+    }
+    if(load)
+      pwms.load_pwm();
+  }
+
+  bool MotorCluster::is_enabled(uint8_t motor) const {
+    assert(motor < pwms.get_chan_count());
+    return states[motor].is_enabled();
+  }
+
+  float MotorCluster::pulse(uint8_t motor) const {
+    assert(motor < pwms.get_chan_count());
+    return states[motor].get_pulse();
+  }
+
+  void MotorCluster::pulse(uint8_t motor, float pulse, bool load) {
+    assert(motor < pwms.get_chan_count());
+    float new_pulse = states[motor].set_pulse_with_return(pulse);
+    apply_pulse(motor, new_pulse, load);
+  }
+
+  void MotorCluster::pulse(const uint8_t *motors, uint8_t length, float pulse, bool load) {
+    assert(motors != nullptr);
+    for(uint8_t i = 0; i < length; i++) {
+      this->pulse(motors[i], pulse, false);
+    }
+    if(load)
+      pwms.load_pwm();
+  }
+
+  void MotorCluster::pulse(std::initializer_list<uint8_t> motors, float pulse, bool load) {
+    for(auto motor : motors) {
+      this->pulse(motor, pulse, false);
+    }
+    if(load)
+      pwms.load_pwm();
+  }
+
+  void MotorCluster::all_to_pulse(float pulse, bool load) {
+    uint8_t motor_count = pwms.get_chan_count();
+    for(uint8_t motor = 0; motor < motor_count; motor++) {
+      this->pulse(motor, pulse, false);
+    }
+    if(load)
+      pwms.load_pwm();
+  }
+
+  float MotorCluster::speed(uint8_t motor) const {
+    assert(motor < pwms.get_chan_count());
+    return states[motor].get_speed();
+  }
+
+  void MotorCluster::speed(uint8_t motor, float speed, bool load) {
+    assert(motor < pwms.get_chan_count());
+    float new_pulse = states[motor].set_speed_with_return(speed);
+    apply_pulse(motor, new_pulse, load);
+  }
+
+  void MotorCluster::speed(const uint8_t *motors, uint8_t length, float speed, bool load) {
+    assert(motors != nullptr);
+    for(uint8_t i = 0; i < length; i++) {
+      this->speed(motors[i], speed, false);
+    }
+    if(load)
+      pwms.load_pwm();
+  }
+
+  void MotorCluster::speed(std::initializer_list<uint8_t> motors, float speed, bool load) {
+    for(auto motor : motors) {
+      this->speed(motor, speed, false);
+    }
+    if(load)
+      pwms.load_pwm();
+  }
+
+  void MotorCluster::all_to_speed(float speed, bool load) {
+    uint8_t motor_count = pwms.get_chan_count();
+    for(uint8_t motor = 0; motor < motor_count; motor++) {
+      this->speed(motor, speed, false);
+    }
+    if(load)
+      pwms.load_pwm();
+  }
+
+  float MotorCluster::phase(uint8_t motor) const {
+    assert(motor < pwms.get_chan_count());
+    return motor_phases[motor];
+  }
+
+  void MotorCluster::phase(uint8_t motor, float phase, bool load) {
+    assert(motor < pwms.get_chan_count());
+    motor_phases[motor] = MIN(MAX(phase, 0.0f), 1.0f);
+    pwms.set_chan_offset(motor, (uint32_t)(motor_phases[motor] * (float)pwms.get_wrap()), load);
+  }
+
+  void MotorCluster::phase(const uint8_t *motors, uint8_t length, float phase, bool load) {
+    assert(motors != nullptr);
+    for(uint8_t i = 0; i < length; i++) {
+      this->phase(motors[i], phase, false);
+    }
+    if(load)
+      pwms.load_pwm();
+  }
+
+  void MotorCluster::phase(std::initializer_list<uint8_t> motors, float phase, bool load) {
+    for(auto motor : motors) {
+      this->phase(motor, phase, false);
+    }
+    if(load)
+      pwms.load_pwm();
+  }
+
+  void MotorCluster::all_to_phase(float phase, bool load) {
+    uint8_t motor_count = pwms.get_chan_count();
+    for(uint8_t motor = 0; motor < motor_count; motor++) {
+      this->phase(motor, phase, false);
+    }
+    if(load)
+      pwms.load_pwm();
+  }
+
+  float MotorCluster::frequency() const {
+    return pwm_frequency;
+  }
+
+  bool MotorCluster::frequency(float freq) {
+    bool success = false;
+
+    if((freq >= MotorState::MIN_FREQUENCY) && (freq <= MotorState::MAX_FREQUENCY)) {
+      // Calculate a suitable pwm wrap period for this frequency
+      uint32_t period; uint32_t div256;
+      if(pimoroni::PWMCluster::calculate_pwm_factors(freq, period, div256)) {
+
+        pwm_period = period;
+        pwm_frequency = freq;
+
+        // Update the pwm before setting the new wrap
+        uint8_t motor_count = pwms.get_chan_count();
+        for(uint motor = 0; motor < motor_count; motor++) {
+          if(states[motor].is_enabled()) {
+            apply_pulse(motor, states[motor].get_pulse(), false);
+          }
+          pwms.set_chan_offset(motor, (uint32_t)(motor_phases[motor] * (float)pwm_period), false);
+        }
+
+        // Set the new wrap (should be 1 less than the period to get full 0 to 100%)
+        pwms.set_wrap(pwm_period, true);
+
+        // Apply the new divider
+        uint16_t div = div256 >> 8;
+        uint8_t mod = div256 % 256;
+        pwms.set_clkdiv_int_frac(div, mod);
+
+        success = true;
+      }
+    }
+    return success;
+  }
+
+  float MotorCluster::min_speed(uint8_t motor) const {
+    assert(is_assigned(motor));
+    return states[motor].get_min_speed();
+  }
+
+  float MotorCluster::mid_speed(uint8_t motor) const {
+    assert(is_assigned(motor));
+    return states[motor].get_mid_speed();
+  }
+
+  float MotorCluster::max_speed(uint8_t motor) const {
+    assert(is_assigned(motor));
+    return states[motor].get_max_speed();
+  }
+
+  void MotorCluster::to_min(uint8_t motor, bool load) {
+    assert(is_assigned(motor));
+    float new_pulse = states[motor].to_min_with_return();
+    apply_pulse(motor, new_pulse, load);
+  }
+
+  void MotorCluster::to_min(const uint8_t *motors, uint8_t length, bool load) {
+    assert(motors != nullptr);
+    for(uint8_t i = 0; i < length; i++) {
+      to_min(motors[i], false);
+    }
+    if(load)
+      pwms.load_pwm();
+  }
+
+  void MotorCluster::to_min(std::initializer_list<uint8_t> motors, bool load) {
+    for(auto motor : motors) {
+      to_min(motor, false);
+    }
+    if(load)
+      pwms.load_pwm();
+  }
+
+  void MotorCluster::all_to_min(bool load) {
+    uint8_t motor_count = pwms.get_chan_count();
+    for(uint8_t motor = 0; motor < motor_count; motor++) {
+      to_min(motor, false);
+    }
+    if(load)
+      pwms.load_pwm();
+  }
+
+  void MotorCluster::to_mid(uint8_t motor, bool load) {
+    assert(is_assigned(motor));
+    float new_pulse = states[motor].to_mid_with_return();
+    apply_pulse(motor, new_pulse, load);
+  }
+
+  void MotorCluster::to_mid(const uint8_t *motors, uint8_t length, bool load) {
+    assert(motors != nullptr);
+    for(uint8_t i = 0; i < length; i++) {
+      to_mid(motors[i], false);
+    }
+    if(load)
+      pwms.load_pwm();
+  }
+
+  void MotorCluster::to_mid(std::initializer_list<uint8_t> motors, bool load) {
+    for(auto motor : motors) {
+      to_mid(motor, false);
+    }
+    if(load)
+      pwms.load_pwm();
+  }
+
+  void MotorCluster::all_to_mid(bool load) {
+    uint8_t motor_count = pwms.get_chan_count();
+    for(uint8_t motor = 0; motor < motor_count; motor++) {
+      to_mid(motor, false);
+    }
+    if(load)
+      pwms.load_pwm();
+  }
+
+  void MotorCluster::to_max(uint8_t motor, bool load) {
+    assert(is_assigned(motor));
+    float new_pulse = states[motor].to_max_with_return();
+    apply_pulse(motor, new_pulse, load);
+  }
+
+  void MotorCluster::to_max(const uint8_t *motors, uint8_t length, bool load) {
+    assert(motors != nullptr);
+    for(uint8_t i = 0; i < length; i++) {
+      to_max(motors[i], false);
+    }
+    if(load)
+      pwms.load_pwm();
+  }
+
+  void MotorCluster::to_max(std::initializer_list<uint8_t> motors, bool load) {
+    for(auto motor : motors) {
+      to_max(motor, false);
+    }
+    if(load)
+      pwms.load_pwm();
+  }
+
+  void MotorCluster::all_to_max(bool load) {
+    uint8_t motor_count = pwms.get_chan_count();
+    for(uint8_t motor = 0; motor < motor_count; motor++) {
+      to_max(motor, false);
+    }
+    if(load)
+      pwms.load_pwm();
+  }
+
+  void MotorCluster::to_percent(uint8_t motor, float in, float in_min, float in_max, bool load) {
+    assert(is_assigned(motor));
+    float new_pulse = states[motor].to_percent_with_return(in, in_min, in_max);
+    apply_pulse(motor, new_pulse, load);
+  }
+
+  void MotorCluster::to_percent(const uint8_t *motors, uint8_t length, float in, float in_min, float in_max, bool load) {
+    assert(motors != nullptr);
+    for(uint8_t i = 0; i < length; i++) {
+      to_percent(motors[i], in, in_min, in_max, false);
+    }
+    if(load)
+      pwms.load_pwm();
+  }
+
+  void MotorCluster::to_percent(std::initializer_list<uint8_t> motors, float in, float in_min, float in_max, bool load) {
+    for(auto motor : motors) {
+      to_percent(motor, in, in_min, in_max, false);
+    }
+    if(load)
+      pwms.load_pwm();
+  }
+
+  void MotorCluster::all_to_percent(float in, float in_min, float in_max, bool load) {
+    uint8_t motor_count = pwms.get_chan_count();
+    for(uint8_t motor = 0; motor < motor_count; motor++) {
+      to_percent(motor, in, in_min, in_max, false);
+    }
+    if(load)
+      pwms.load_pwm();
+  }
+
+  void MotorCluster::to_percent(uint8_t motor, float in, float in_min, float in_max, float speed_min, float speed_max, bool load) {
+    assert(is_assigned(motor));
+    float new_pulse = states[motor].to_percent_with_return(in, in_min, in_max, speed_min, speed_max);
+    apply_pulse(motor, new_pulse, load);
+  }
+
+  void MotorCluster::to_percent(const uint8_t *motors, uint8_t length, float in, float in_min, float in_max, float speed_min, float speed_max, bool load) {
+    assert(motors != nullptr);
+    for(uint8_t i = 0; i < length; i++) {
+      to_percent(motors[i], in, in_min, in_max, speed_min, speed_max, false);
+    }
+    if(load)
+      pwms.load_pwm();
+  }
+
+  void MotorCluster::to_percent(std::initializer_list<uint8_t> motors, float in, float in_min, float in_max, float speed_min, float speed_max, bool load) {
+    for(auto motor : motors) {
+      to_percent(motor, in, in_min, in_max, speed_min, speed_max, false);
+    }
+    if(load)
+      pwms.load_pwm();
+  }
+
+  void MotorCluster::all_to_percent(float in, float in_min, float in_max, float speed_min, float speed_max, bool load) {
+    uint8_t motor_count = pwms.get_chan_count();
+    for(uint8_t motor = 0; motor < motor_count; motor++) {
+      to_percent(motor, in, in_min, in_max, speed_min, speed_max, false);
+    }
+    if(load)
+      pwms.load_pwm();
+  }
+
+  Calibration& MotorCluster::calibration(uint8_t motor) {
+    assert(is_assigned(motor));
+    return states[motor].calibration();
+  }
+
+  const Calibration& MotorCluster::calibration(uint8_t motor) const {
+    assert(is_assigned(motor));
+    return states[motor].calibration();
+  }
+
+  void MotorCluster::load() {
+    pwms.load_pwm();
+  }
+
+  void MotorCluster::apply_pulse(uint8_t motor, float pulse, bool load) {
+    pwms.set_chan_level(motor, MotorState::pulse_to_level(pulse, pwm_period, pwm_frequency), load);
+  }
+
+  void MotorCluster::create_motor_states(CalibrationType default_type, bool auto_phase) {
+    uint8_t motor_count = pwms.get_chan_count();
+    if(motor_count > 0) {
+      states = new MotorState[motor_count];
+      motor_phases = new float[motor_count];
+
+      for(uint motor = 0; motor < motor_count; motor++) {
+        states[motor] = MotorState(default_type);
+        motor_phases[motor] = (auto_phase) ? (float)motor / (float)motor_count : 0.0f;
+      }
+    }
+  }
+
+  void MotorCluster::create_motor_states(const Calibration& calibration, bool auto_phase) {
+    uint8_t motor_count = pwms.get_chan_count();
+    if(motor_count > 0) {
+      states = new MotorState[motor_count];
+      motor_phases = new float[motor_count];
+
+      for(uint motor = 0; motor < motor_count; motor++) {
+        states[motor] = MotorState(calibration);
+        motor_phases[motor] = (auto_phase) ? (float)motor / (float)motor_count : 0.0f;
+      }
+    }
+  }
+};

drivers/motor/motor_cluster.hpp

@@ -0,0 +1,123 @@
+#pragma once
+
+#include "pico/stdlib.h"
+#include "pwm_cluster.hpp"
+#include "motor_state.hpp"
+
+using namespace pimoroni;
+
+namespace motor {
+
+  class MotorCluster {
+    //--------------------------------------------------
+    // Variables
+    //--------------------------------------------------
+  private:
+    PWMCluster pwms;
+    uint32_t pwm_period;
+    float pwm_frequency;
+    MotorState* states;
+    float* motor_phases;
+
+
+    //--------------------------------------------------
+    // Constructors/Destructor
+    //--------------------------------------------------
+  public:
+    MotorCluster(PIO pio, uint sm, uint pin_mask, CalibrationType default_type = ANGULAR, float freq = MotorState::DEFAULT_FREQUENCY, bool auto_phase = true, PWMCluster::Sequence *seq_buffer = nullptr, PWMCluster::TransitionData *dat_buffer = nullptr);
+    MotorCluster(PIO pio, uint sm, uint pin_base, uint pin_count, CalibrationType default_type = ANGULAR, float freq = MotorState::DEFAULT_FREQUENCY, bool auto_phase = true, PWMCluster::Sequence *seq_buffer = nullptr, PWMCluster::TransitionData *dat_buffer = nullptr);
+    MotorCluster(PIO pio, uint sm, const uint8_t *pins, uint32_t length, CalibrationType default_type = ANGULAR, float freq = MotorState::DEFAULT_FREQUENCY, bool auto_phase = true, PWMCluster::Sequence *seq_buffer = nullptr, PWMCluster::TransitionData *dat_buffer = nullptr);
+    MotorCluster(PIO pio, uint sm, std::initializer_list<uint8_t> pins, CalibrationType default_type = ANGULAR, float freq = MotorState::DEFAULT_FREQUENCY, bool auto_phase = true, PWMCluster::Sequence *seq_buffer = nullptr, PWMCluster::TransitionData *dat_buffer = nullptr);
+
+    MotorCluster(PIO pio, uint sm, uint pin_mask, const Calibration& calibration, float freq = MotorState::DEFAULT_FREQUENCY, bool auto_phase = true, PWMCluster::Sequence *seq_buffer = nullptr, PWMCluster::TransitionData *dat_buffer = nullptr);
+    MotorCluster(PIO pio, uint sm, uint pin_base, uint pin_count, const Calibration& calibration, float freq = MotorState::DEFAULT_FREQUENCY, bool auto_phase = true, PWMCluster::Sequence *seq_buffer = nullptr, PWMCluster::TransitionData *dat_buffer = nullptr);
+    MotorCluster(PIO pio, uint sm, const uint8_t *pins, uint32_t length, const Calibration& calibration, float freq = MotorState::DEFAULT_FREQUENCY, bool auto_phase = true, PWMCluster::Sequence *seq_buffer = nullptr, PWMCluster::TransitionData *dat_buffer = nullptr);
+    MotorCluster(PIO pio, uint sm, std::initializer_list<uint8_t> pins, const Calibration& calibration, float freq = MotorState::DEFAULT_FREQUENCY, bool auto_phase = true, PWMCluster::Sequence *seq_buffer = nullptr, PWMCluster::TransitionData *dat_buffer = nullptr);
+    ~MotorCluster();
+
+
+    //--------------------------------------------------
+    // Methods
+    //--------------------------------------------------
+  public:
+    bool init();
+
+    uint8_t count() const;
+    uint8_t pin(uint8_t motor) const;
+
+    void enable(uint8_t motor, bool load = true);
+    void enable(const uint8_t *motors, uint8_t length, bool load = true);
+    void enable(std::initializer_list<uint8_t> motors, bool load = true);
+    void enable_all(bool load = true);
+
+    void disable(uint8_t motor, bool load = true);
+    void disable(const uint8_t *motors, uint8_t length, bool load = true);
+    void disable(std::initializer_list<uint8_t> motors, bool load = true);
+    void disable_all(bool load = true);
+
+    bool is_enabled(uint8_t motor) const;
+
+    float duty(uint8_t motor) const;
+    void duty(uint8_t motor, float duty, bool load = true);
+    void duty(const uint8_t *motors, uint8_t length, float duty, bool load = true);
+    void duty(std::initializer_list<uint8_t> motors, float duty, bool load = true);
+    void all_to_duty(float duty, bool load = true);
+
+    float speed(uint8_t motor) const;
+    void speed(uint8_t motor, float speed, bool load = true);
+    void speed(const uint8_t *motors, uint8_t length, float speed, bool load = true);
+    void speed(std::initializer_list<uint8_t> motors, float speed, bool load = true);
+    void all_to_speed(float speed, bool load = true);
+
+    float phase(uint8_t motor) const;
+    void phase(uint8_t motor, float phase, bool load = true);
+    void phase(const uint8_t *motors, uint8_t length, float phase, bool load = true);
+    void phase(std::initializer_list<uint8_t> motors, float phase, bool load = true);
+    void all_to_phase(float phase, bool load = true);
+
+    float frequency() const;
+    bool frequency(float freq);
+
+    //--------------------------------------------------
+    float min_speed(uint8_t motor) const;
+    float mid_speed(uint8_t motor) const;
+    float max_speed(uint8_t motor) const;
+
+    void to_min(uint8_t motor, bool load = true);
+    void to_min(const uint8_t *motors, uint8_t length, bool load = true);
+    void to_min(std::initializer_list<uint8_t> motors, bool load = true);
+    void all_to_min(bool load = true);
+
+    void to_mid(uint8_t motor, bool load = true);
+    void to_mid(const uint8_t *motors, uint8_t length, bool load = true);
+    void to_mid(std::initializer_list<uint8_t> motors, bool load = true);
+    void all_to_mid(bool load = true);
+
+    void to_max(uint8_t motor, bool load = true);
+    void to_max(const uint8_t *motors, uint8_t length, bool load = true);
+    void to_max(std::initializer_list<uint8_t> motors, bool load = true);
+    void all_to_max(bool load = true);
+
+    void to_percent(uint8_t motor, float in, float in_min = MotorState::ZERO_PERCENT, float in_max = MotorState::ONEHUNDRED_PERCENT, bool load = true);
+    void to_percent(const uint8_t *motors, uint8_t length, float in, float in_min = MotorState::ZERO_PERCENT, float in_max = MotorState::ONEHUNDRED_PERCENT, bool load = true);
+    void to_percent(std::initializer_list<uint8_t> motors, float in, float in_min = MotorState::ZERO_PERCENT, float in_max = MotorState::ONEHUNDRED_PERCENT, bool load = true);
+    void all_to_percent(float in, float in_min = MotorState::ZERO_PERCENT, float in_max = MotorState::ONEHUNDRED_PERCENT, bool load = true);
+
+    void to_percent(uint8_t motor, float in, float in_min, float in_max, float speed_min, float speed_max, bool load = true);
+    void to_percent(const uint8_t *motors, uint8_t length, float in, float in_min, float in_max, float speed_min, float speed_max, bool load = true);
+    void to_percent(std::initializer_list<uint8_t> motors, float in, float in_min, float in_max, float speed_min, float speed_max, bool load = true);
+    void all_to_percent(float in, float in_min, float in_max, float speed_min, float speed_max, bool load = true);
+
+    Calibration& calibration(uint8_t motor);
+    const Calibration& calibration(uint8_t motor) const;
+
+    void load();
+
+    //--------------------------------------------------
+  private:
+    void apply_duty(uint8_t motor, float duty, bool load);
+    void create_motor_states(CalibrationType default_type, bool auto_phase);
+    void create_motor_states(const Calibration& calibration, bool auto_phase);
+  };
+
+}

drivers/motor/motor_state.cpp

@@ -0,0 +1,129 @@
+#include "motor_state.hpp"
+
+namespace motor {
+  MotorState::MotorState() {
+  }
+
+  //MotorState::MotorState(CalibrationType default_type)
+  //  : calib(default_type) {
+  //}
+
+  //MotorState::MotorState(const Calibration& calibration) 
+  //  : calib(calibration) {
+  //}
+
+  float MotorState::enable() {
+    return _enable();
+  }
+
+  float MotorState::disable() {
+    enabled = false;
+    return 0.0f; // A zero duty
+  }
+
+  float MotorState::_enable() {
+    enabled = true;
+    return last_enabled_duty;
+  }
+
+  bool MotorState::is_enabled() const {
+    return enabled;
+  }
+
+  float MotorState::get_duty() const {
+    return last_enabled_duty;
+  }
+
+  float MotorState::set_duty_with_return(float duty) {
+    //TODO
+    // float speed_out, duty_out;
+    // if(calib.duty_to_speed(duty, speed_out, duty_out)) {
+    //   motor_speed = speed_out;
+    //   last_enabled_duty = duty_out;
+    //   return _enable();
+    // }
+    return disable();
+  }
+
+  float MotorState::get_speed() const {
+    return motor_speed;
+  }
+
+  float MotorState::set_speed_with_return(float speed) {
+    //TODO
+    // float duty_out, speed_out;
+    // if(calib.speed_to_duty(speed, duty_out, speed_out)) {
+    //   last_enabled_duty = duty_out;
+    //   motor_speed = speed_out;
+    //   return _enable();
+    // }
+    return disable();
+  }
+
+  float MotorState::get_min_speed() const {
+    float speed = 0.0f;
+    //TODO
+    //if(calib.size() > 0) {
+    //  speed = calib.first().speed;
+    //}
+    return speed;
+  }
+
+  // float MotorState::get_mid_speed() const {
+  //   float speed = 0.0f;
+  //   if(calib.size() > 0) {
+  //     const Calibration::Pair &first = calib.first();
+  //     const Calibration::Pair &last = calib.last();
+  //     speed = (first.speed + last.speed) / 2.0f;
+  //   }
+  //   return speed;
+  // }
+
+  float MotorState::get_max_speed() const {
+    float speed = 0.0f;
+    //TODO
+    //if(calib.size() > 0) {
+    //  speed = calib.last().speed;
+    //}
+    return speed;
+  }
+
+  float MotorState::to_min_with_return() {
+    return set_speed_with_return(get_min_speed());
+  }
+
+  // float MotorState::to_mid_with_return() {
+  //   return set_speed_with_return(get_mid_speed());
+  // }
+
+  float MotorState::to_max_with_return() {
+    return set_speed_with_return(get_max_speed());
+  }
+
+  float MotorState::to_percent_with_return(float in, float in_min, float in_max) {
+    float speed = MotorState::map_float(in, in_min, in_max, get_min_speed(), get_max_speed());
+    return set_speed_with_return(speed);
+  }
+
+  float MotorState::to_percent_with_return(float in, float in_min, float in_max, float speed_min, float speed_max) {
+    float speed = MotorState::map_float(in, in_min, in_max, speed_min, speed_max);
+    return set_speed_with_return(speed);
+  }
+
+  // Calibration& MotorState::calibration() {
+  //   return calib;
+  // }
+
+  // const Calibration& MotorState::calibration() const {
+  //   return calib;
+  // }
+
+  int32_t MotorState::duty_to_level(float duty, uint32_t resolution) {
+    return (int32_t)(duty * (float)resolution);
+  }
+
+  float MotorState::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/motor/motor_state.hpp

@@ -0,0 +1,90 @@
+#pragma once
+
+#include "pico/stdlib.h"
+#include "calibration.hpp"
+
+namespace motor {
+
+  class MotorState {
+    //--------------------------------------------------
+    // Enums
+    //--------------------------------------------------
+  public:
+    enum DecayMode {
+      FAST_DECAY  = 0, //aka 'Coasting'
+      SLOW_DECAY  = 1, //aka 'Braking'
+    };
+
+
+    //--------------------------------------------------
+    // Constants
+    //--------------------------------------------------
+  public:
+    static constexpr float DEFAULT_FREQUENCY = 25000.0f;      // The standard motor update rate
+    static const DecayMode DEFAULT_DECAY_MODE = SLOW_DECAY;
+    static constexpr float MIN_FREQUENCY = 10.0f;           // Lowest achievable with hardware PWM with good resolution
+    static constexpr float MAX_FREQUENCY = 50000.0f;          // Highest nice speed
+    static constexpr float ZERO_PERCENT = 0.0f;
+    static constexpr float ONEHUNDRED_PERCENT = 1.0f;
+
+  private:
+    static constexpr float MIN_VALID_DUTY = 1.0f;
+
+
+    //--------------------------------------------------
+    // Variables
+    //--------------------------------------------------
+  private:
+    float motor_speed = 0.0f;
+    float last_enabled_duty = 0.0f;
+    bool enabled = false;
+    //Calibration calib;
+
+
+    //--------------------------------------------------
+    // Constructors/Destructor
+    //--------------------------------------------------
+  public:
+    MotorState();
+    //MotorState(CalibrationType default_type);
+    //MotorState(const Calibration& calibration);
+
+
+    //--------------------------------------------------
+    // Methods
+    //--------------------------------------------------
+  public:
+    float enable();
+    float disable();
+    bool is_enabled() const;
+  private:
+    float _enable(); // Internal version of enable without convenient initialisation to the middle
+  public:
+    float get_duty() const;
+    float set_duty_with_return(float duty);
+
+    float get_speed() const;
+    float set_speed_with_return(float speed);
+
+  public:
+    float get_min_speed() const;
+    //float get_mid_speed() const;
+    float get_max_speed() const;
+
+    float to_min_with_return();
+    //float to_mid_with_return();
+    float to_max_with_return();
+    float to_percent_with_return(float in, float in_min = ZERO_PERCENT, float in_max = ONEHUNDRED_PERCENT);
+    float to_percent_with_return(float in, float in_min, float in_max, float speed_min, float speed_max);
+
+    //Calibration& calibration();
+    //const Calibration& calibration() const;
+
+    //--------------------------------------------------
+    static int32_t duty_to_level(float duty, uint32_t resolution);
+
+  private:
+    static float map_float(float in, float in_min, float in_max, float out_min, float out_max);
+  };
+
+}

examples/CMakeLists.txt

@@ -42,3 +42,4 @@ add_subdirectory(plasma2040)
 add_subdirectory(badger2040)
 add_subdirectory(interstate75)
 add_subdirectory(servo2040)
+add_subdirectory(motor2040)

examples/motor2040/CMakeLists.txt

@@ -0,0 +1,10 @@
+#include(servo2040_calibration.cmake)
+#include(servo2040_current_meter.cmake)
+#include(servo2040_led_rainbow.cmake)
+#include(servo2040_multiple_servos.cmake)
+#include(servo2040_read_sensors.cmake)
+#include(servo2040_sensor_feedback.cmake)
+#include(servo2040_servo_cluster.cmake)
+#include(servo2040_servo_wave.cmake)
+#include(servo2040_simple_easing.cmake)
+include(motor2040_single_motor.cmake)

examples/motor2040/motor2040_single_motor.cmake

@@ -0,0 +1,12 @@
+set(OUTPUT_NAME motor2040_single_motor)
+add_executable(${OUTPUT_NAME} motor2040_single_motor.cpp)
+
+target_link_libraries(${OUTPUT_NAME}
+        pico_stdlib
+        motor2040
+        )
+
+# enable usb output
+pico_enable_stdio_usb(${OUTPUT_NAME} 1)
+
+pico_add_extra_outputs(${OUTPUT_NAME})

examples/motor2040/motor2040_single_motor.cpp

@@ -0,0 +1,72 @@
+#include "pico/stdlib.h"
+
+#include "motor2040.hpp"
+
+/*
+Demonstrates how to create a Motor object and control it.
+*/
+
+using namespace motor;
+
+// How many sweeps of the motor to perform
+const uint SWEEPS = 3;
+
+// The number of discrete sweep steps
+const uint STEPS = 10;
+
+// The time in milliseconds between each step of the sequence
+const uint STEPS_INTERVAL_MS = 500;
+
+// How far from zero to move the motor when sweeping
+constexpr float SWEEP_EXTENT = 90.0f;
+
+
+// Create a motor on pin 0 and 1
+Motor2 m = Motor2(motor2040::SERVO_1, motor2040::SERVO_2);
+
+
+int main() {
+  stdio_init_all();
+
+  // Initialise the motor
+  m.init();
+
+  // Enable the motor (this puts it at the middle)
+  m.enable();
+  sleep_ms(2000);
+
+  // Go to min
+  m.to_min();
+  sleep_ms(2000);
+
+  // Go to max
+  m.to_max();
+  sleep_ms(2000);
+
+  // Go back to mid
+  //m.to_mid();
+  //sleep_ms(2000);
+
+  // Do a sine sweep
+  for(auto j = 0u; j < SWEEPS; j++) {
+    for(auto i = 0u; i < 360; i++) {
+      m.speed(sin(((float)i * (float)M_PI) / 180.0f) * SWEEP_EXTENT);
+      sleep_ms(20);
+    }
+  }
+
+  // Do a stepped sweep
+  for(auto j = 0u; j < SWEEPS; j++) {
+    for(auto i = 0u; i < STEPS; i++) {
+      m.to_percent(i, 0, STEPS, 0.0 - SWEEP_EXTENT, SWEEP_EXTENT);
+      sleep_ms(STEPS_INTERVAL_MS);
+    }
+    for(auto i = 0u; i < STEPS; i++) {
+      m.to_percent(i, STEPS, 0, 0.0 - SWEEP_EXTENT, SWEEP_EXTENT);
+      sleep_ms(STEPS_INTERVAL_MS);
+    }
+  }
+
+  // Disable the motor
+  m.disable();
+}

libraries/CMakeLists.txt

@@ -29,3 +29,4 @@ add_subdirectory(pico_wireless)
 add_subdirectory(plasma2040)
 add_subdirectory(badger2040)
 add_subdirectory(servo2040)
+add_subdirectory(motor2040)

libraries/motor2040/CMakeLists.txt

@@ -0,0 +1 @@
+include(motor2040.cmake)

libraries/motor2040/motor2040.cmake

@@ -0,0 +1,6 @@
+add_library(motor2040 INTERFACE)
+
+target_include_directories(motor2040 INTERFACE ${CMAKE_CURRENT_LIST_DIR})
+
+# Pull in pico libraries that we need
+target_link_libraries(motor2040 INTERFACE pico_stdlib plasma motor2) # motor_cluster)

libraries/motor2040/motor2040.hpp

@@ -0,0 +1,60 @@
+#pragma once
+#include "pico/stdlib.h"
+
+#include "ws2812.hpp"
+#include "motor2.hpp"
+#include "motor_cluster.hpp"
+
+namespace motor {
+  namespace motor2040 {
+    const uint SERVO_1 = 0;
+    const uint SERVO_2 = 1;
+    const uint SERVO_3 = 2;
+    const uint SERVO_4 = 3;
+    const uint SERVO_5 = 4;
+    const uint SERVO_6 = 5;
+    const uint SERVO_7 = 6;
+    const uint SERVO_8 = 7;
+    const uint SERVO_9 = 8;
+    const uint SERVO_10 = 9;
+    const uint SERVO_11 = 10;
+    const uint SERVO_12 = 11;
+    const uint SERVO_13 = 12;
+    const uint SERVO_14 = 13;
+    const uint SERVO_15 = 14;
+    const uint SERVO_16 = 15;
+    const uint SERVO_17 = 16;
+    const uint SERVO_18 = 17;
+    const uint NUM_SERVOS = 18;
+
+    const uint LED_DATA = 18;
+    const uint NUM_LEDS = 1;
+
+    const uint USER_SW = 23;
+
+    const uint ADC_ADDR_0 = 22;
+    const uint ADC_ADDR_1 = 24;
+    const uint ADC_ADDR_2 = 25;
+
+    const uint ADC0 = 26;
+    const uint ADC1 = 27;
+    const uint ADC2 = 28;
+    const uint SHARED_ADC = 29; // The pin used for the board's sensing features
+
+    const uint SENSOR_1_ADDR = 0b000;
+    const uint SENSOR_2_ADDR = 0b001;
+    const uint SENSOR_3_ADDR = 0b010;
+    const uint SENSOR_4_ADDR = 0b011;
+    const uint SENSOR_5_ADDR = 0b100;
+    const uint SENSOR_6_ADDR = 0b101;
+    const uint NUM_SENSORS = 6;
+
+    const uint VOLTAGE_SENSE_ADDR = 0b110;
+    const uint CURRENT_SENSE_ADDR = 0b111;
+
+    constexpr float SHUNT_RESISTOR = 0.003f;
+    constexpr float CURRENT_GAIN = 69;
+    constexpr float VOLTAGE_GAIN = 3.9f / 13.9f;
+    constexpr float CURRENT_OFFSET = -0.02f;
+  }
+}