Merge branch 'motor-pio' into motor-and-encoder

common/pimoroni_common.hpp

@@ -6,6 +6,12 @@
 #define PIMORONI_I2C_DEFAULT_INSTANCE i2c0
 #define PIMORONI_SPI_DEFAULT_INSTANCE spi0
 
+// Macro to return a value clamped between a minimum and maximum
+#ifndef CLAMP
+#define CLAMP(a, mn, mx) ((a)<(mx)?((a)>(mn)?(a):(mn)):(mx))
+#endif
+
+
 namespace pimoroni {
     static const unsigned int PIN_UNUSED = INT_MAX; // Intentionally INT_MAX to avoid overflowing MicroPython's int type
 

drivers/CMakeLists.txt

@@ -30,3 +30,4 @@ add_subdirectory(uc8151)
 add_subdirectory(pwm)
 add_subdirectory(servo)
 add_subdirectory(encoder-pio)
+add_subdirectory(motor)

drivers/motor/CMakeLists.txt

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

drivers/motor/motor.cmake

@@ -0,0 +1,15 @@
+set(DRIVER_NAME motor)
+add_library(${DRIVER_NAME} INTERFACE)
+
+target_sources(${DRIVER_NAME} INTERFACE
+  ${CMAKE_CURRENT_LIST_DIR}/motor.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/motor.cpp

@@ -0,0 +1,147 @@
+#include "motor.hpp"
+#include "pwm.hpp"
+
+namespace motor {
+  Motor::Motor(const pin_pair &pins, float freq, DecayMode mode)
+    : pins(pins), pwm_frequency(freq), motor_decay_mode(mode) {
+  }
+
+  Motor::~Motor() {
+    gpio_set_function(pins.positive, GPIO_FUNC_NULL);
+    gpio_set_function(pins.negative, GPIO_FUNC_NULL);
+  }
+
+  bool Motor::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, period - 1);
+
+      //Apply the divider
+      pwm_config_set_clkdiv(&pwm_cfg, (float)div16 / 16.0f);
+
+      pwm_init(pwm_gpio_to_slice_num(pins.positive), &pwm_cfg, true);
+      gpio_set_function(pins.positive, GPIO_FUNC_PWM);
+
+      pwm_init(pwm_gpio_to_slice_num(pins.negative), &pwm_cfg, true);
+      gpio_set_function(pins.negative, GPIO_FUNC_PWM);
+      update_pwm();
+
+      success = true;
+    }
+    return success;
+  }
+
+  float Motor::get_speed() {
+    return motor_speed;
+  }
+
+  void Motor::set_speed(float speed) {
+    motor_speed = MIN(MAX(speed, -1.0f), 1.0f);
+    update_pwm();
+  }
+
+  float Motor::get_frequency() {
+    return pwm_frequency;
+  }
+
+  bool Motor::set_frequency(float freq) {
+    bool success = false;
+
+    //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 values either before or after the wrap is applied,
+      //to avoid momentary blips in PWM output on SLOW_DECAY
+      bool pre_update_pwm = (period > pwm_period);
+
+      pwm_period = period;
+      pwm_frequency = freq;
+
+      uint pos_num = pwm_gpio_to_slice_num(pins.positive);
+      uint neg_num = pwm_gpio_to_slice_num(pins.negative);
+
+      //Apply the new divider
+      uint8_t div = div16 >> 4;
+      uint8_t mod = div16 % 16;
+      pwm_set_clkdiv_int_frac(pos_num, div, mod);
+      if(neg_num != pos_num) {
+        pwm_set_clkdiv_int_frac(neg_num, div, mod);
+      }
+
+      //If the the period is larger, update the pwm before setting the new wraps
+      if(pre_update_pwm)
+        update_pwm();
+
+      //Set the new wrap (should be 1 less than the period to get full 0 to 100%)
+      pwm_set_wrap(pos_num, pwm_period - 1);
+      if(neg_num != pos_num) {
+        pwm_set_wrap(neg_num, pwm_period - 1);
+      }
+
+      //If the the period is smaller, update the pwm after setting the new wraps
+      if(!pre_update_pwm)
+        update_pwm();
+
+      success = true;
+    }
+    return success;
+  }
+
+  Motor::DecayMode Motor::get_decay_mode() {
+    return motor_decay_mode;
+  }
+
+  void Motor::set_decay_mode(Motor::DecayMode mode) {
+    motor_decay_mode = mode;
+    update_pwm();
+  }
+
+  void Motor::stop() {
+    motor_speed = 0.0f;
+    update_pwm();
+  }
+
+  void Motor::disable() {
+    motor_speed = 0.0f;
+    pwm_set_gpio_level(pins.positive, 0);
+    pwm_set_gpio_level(pins.negative, 0);
+  }
+
+  void Motor::update_pwm() {
+    int32_t signed_duty_cycle = (int32_t)(motor_speed * (float)pwm_period);
+
+    switch(motor_decay_mode) {
+    case SLOW_DECAY: //aka 'Braking'
+      if(signed_duty_cycle >= 0) {
+        pwm_set_gpio_level(pins.positive, pwm_period);
+        pwm_set_gpio_level(pins.negative, pwm_period - signed_duty_cycle);
+      }
+      else {
+        pwm_set_gpio_level(pins.positive, pwm_period + signed_duty_cycle);
+        pwm_set_gpio_level(pins.negative, pwm_period);
+      }
+      break;
+
+    case FAST_DECAY: //aka 'Coasting'
+    default:
+      if(signed_duty_cycle >= 0) {
+        pwm_set_gpio_level(pins.positive, signed_duty_cycle);
+        pwm_set_gpio_level(pins.negative, 0);
+      }
+      else {
+        pwm_set_gpio_level(pins.positive, 0);
+        pwm_set_gpio_level(pins.negative, 0 - signed_duty_cycle);
+      }
+      break;
+    }
+  }
+};

drivers/motor/motor.hpp

@@ -0,0 +1,75 @@
+#pragma once
+
+#include "pico/stdlib.h"
+#include "hardware/pwm.h"
+#include "common/pimoroni_common.hpp"
+#include "motor_state.hpp"
+
+using namespace pimoroni;
+
+namespace motor {
+
+  class Motor {
+    //--------------------------------------------------
+    // Enums
+    //--------------------------------------------------
+  public:
+    enum DecayMode {
+      FAST_DECAY  = 0, //aka 'Coasting'
+      SLOW_DECAY  = 1, //aka 'Braking'
+    };
+
+    //--------------------------------------------------
+    // Constants
+    //--------------------------------------------------
+  public:
+    static const uint16_t DEFAULT_PWM_FREQUENCY = 25000;      // Chose 25KHz because it is outside of hearing
+                                                              // and divides nicely into the RP2040's 125MHz PWM frequency
+    static const DecayMode DEFAULT_DECAY_MODE = SLOW_DECAY;
+
+
+    //--------------------------------------------------
+    // Variables
+    //--------------------------------------------------
+  private:
+    pin_pair pins;
+    pwm_config pwm_cfg;
+    uint16_t pwm_period;
+    float pwm_frequency = DEFAULT_PWM_FREQUENCY;
+
+    DecayMode motor_decay_mode = DEFAULT_DECAY_MODE;
+    float motor_speed = 0.0f;
+
+
+    //--------------------------------------------------
+    // Constructors/Destructor
+    //--------------------------------------------------
+  public:
+    Motor(const pin_pair &pins, float freq = DEFAULT_PWM_FREQUENCY, DecayMode mode = DEFAULT_DECAY_MODE);
+    ~Motor();
+
+
+    //--------------------------------------------------
+    // Methods
+    //--------------------------------------------------
+  public:
+    bool init();
+
+    float get_speed();
+    void set_speed(float speed);
+
+    float get_frequency();
+    bool set_frequency(float freq);
+
+    DecayMode get_decay_mode();
+    void set_decay_mode(DecayMode mode);
+
+    void stop();
+    void disable();
+
+    //--------------------------------------------------
+  private:
+    void update_pwm();
+  };
+
+}

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,223 @@
+#include "motor2.hpp"
+#include "hardware/clocks.h"
+#include "pwm.hpp"
+#include "math.h"
+
+namespace motor {
+  Motor2::Motor2(const pin_pair &pins, Direction direction, float speed_scale, float deadzone, float freq, DecayMode mode)
+    : motor_pins(pins), state(direction, speed_scale, deadzone), pwm_frequency(freq), motor_mode(mode) {
+  }
+
+  Motor2::~Motor2() {
+    gpio_set_function(motor_pins.positive, GPIO_FUNC_NULL);
+    gpio_set_function(motor_pins.negative, 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_pins.positive), &pwm_cfg, true);
+      pwm_init(pwm_gpio_to_slice_num(motor_pins.negative), &pwm_cfg, true);
+
+      gpio_set_function(motor_pins.positive, GPIO_FUNC_PWM);
+      gpio_set_function(motor_pins.negative, GPIO_FUNC_PWM);
+
+      pwm_set_gpio_level(motor_pins.positive, 0);
+      pwm_set_gpio_level(motor_pins.negative, 0);
+
+      success = true;
+    }
+    return success;
+  }
+
+  pin_pair Motor2::pins() const {
+    return motor_pins;
+  }
+
+  void Motor2::enable() {
+    apply_duty(state.enable_with_return(), motor_mode);
+  }
+
+  void Motor2::disable() {
+    apply_duty(state.disable_with_return(), motor_mode);
+  }
+
+  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), motor_mode);
+  }
+
+  float Motor2::speed() const {
+    return state.get_speed();
+  }
+
+  void Motor2::speed(float speed) {
+    apply_duty(state.set_speed_with_return(speed), motor_mode);
+  }
+
+  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 pos_pin_num = pwm_gpio_to_slice_num(motor_pins.positive);
+        uint neg_pin_num = pwm_gpio_to_slice_num(motor_pins.negative);
+
+        // Apply the new divider
+        uint8_t div = div16 >> 4;
+        uint8_t mod = div16 % 16;
+        pwm_set_clkdiv_int_frac(pos_pin_num, div, mod);
+        if(neg_pin_num != pos_pin_num)
+          pwm_set_clkdiv_int_frac(neg_pin_num, div, mod);
+
+        // If the period is larger, update the pwm before setting the new wraps
+        if(pre_update_pwm) {
+          apply_duty(state.get_deadzoned_duty(), motor_mode);
+        }
+
+        // Set the new wrap (should be 1 less than the period to get full 0 to 100%)
+        pwm_set_wrap(pos_pin_num, pwm_period - 1);
+        if(neg_pin_num != pos_pin_num)
+          pwm_set_wrap(neg_pin_num, pwm_period - 1);
+
+        // If the period is smaller, update the pwm after setting the new wraps
+        if(!pre_update_pwm) {
+          apply_duty(state.get_deadzoned_duty(), motor_mode);
+        }
+
+        success = true;
+      }
+    }
+    return success;
+  }
+
+  void Motor2::stop() {
+    apply_duty(state.stop_with_return(), motor_mode);
+  }
+
+  void Motor2::coast() {
+    apply_duty(state.stop_with_return(), FAST_DECAY);
+  }
+
+  void Motor2::brake() {
+    apply_duty(state.stop_with_return(), SLOW_DECAY);
+  }
+
+  void Motor2::full_negative() {
+    apply_duty(state.full_negative_with_return(), motor_mode);
+  }
+
+  void Motor2::full_positive() {
+    apply_duty(state.full_positive_with_return(), motor_mode);
+  }
+
+  void Motor2::to_percent(float in, float in_min, float in_max) {
+    apply_duty(state.to_percent_with_return(in, in_min, in_max), motor_mode);
+  }
+
+  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), motor_mode);
+  }
+
+  Direction Motor2::direction() const {
+    return state.get_direction();
+  }
+
+  void Motor2::direction(Direction direction) {
+    state.set_direction(direction);
+  }
+
+  float Motor2::speed_scale() const {
+    return state.get_speed_scale();
+  }
+
+  void Motor2::speed_scale(float speed_scale) {
+    state.set_speed_scale(speed_scale);
+  }
+
+  float Motor2::deadzone() const {
+    return state.get_deadzone();
+  }
+
+  void Motor2::deadzone(float deadzone) {
+    apply_duty(state.set_deadzone_with_return(deadzone), motor_mode);
+  }
+
+  DecayMode Motor2::decay_mode() {
+    return motor_mode;
+  }
+
+  void Motor2::decay_mode(DecayMode mode) {
+    motor_mode = mode;
+    apply_duty(state.get_deadzoned_duty(), motor_mode);
+  }
+
+  void Motor2::apply_duty(float duty, DecayMode mode) {
+    if(isfinite(duty)) {
+      int32_t signed_level = MotorState::duty_to_level(duty, pwm_period);
+
+      switch(mode) {
+      case SLOW_DECAY: //aka 'Braking'
+        if(signed_level >= 0) {
+          pwm_set_gpio_level(motor_pins.positive, pwm_period);
+          pwm_set_gpio_level(motor_pins.negative, pwm_period - signed_level);
+        }
+        else {
+          pwm_set_gpio_level(motor_pins.positive, pwm_period + signed_level);
+          pwm_set_gpio_level(motor_pins.negative, pwm_period);
+        }
+        break;
+
+      case FAST_DECAY: //aka 'Coasting'
+      default:
+        if(signed_level >= 0) {
+          pwm_set_gpio_level(motor_pins.positive, signed_level);
+          pwm_set_gpio_level(motor_pins.negative, 0);
+        }
+        else {
+          pwm_set_gpio_level(motor_pins.positive, 0);
+          pwm_set_gpio_level(motor_pins.negative, 0 - signed_level);
+        }
+        break;
+      }
+    }
+    else {
+      pwm_set_gpio_level(motor_pins.positive, 0);
+      pwm_set_gpio_level(motor_pins.negative, 0);
+    }
+  }
+};

drivers/motor/motor2.hpp

@@ -0,0 +1,84 @@
+#pragma once
+
+#include "pico/stdlib.h"
+#include "hardware/pwm.h"
+#include "common/pimoroni_common.hpp"
+#include "motor_state.hpp"
+
+using namespace pimoroni;
+
+namespace motor {
+
+  class Motor2 {
+    //--------------------------------------------------
+    // Variables
+    //--------------------------------------------------
+  private:
+    pin_pair motor_pins;
+    MotorState state;
+    pwm_config pwm_cfg;
+    uint16_t pwm_period;
+    float pwm_frequency;
+    DecayMode motor_mode;
+
+    //--------------------------------------------------
+    // Constructors/Destructor
+    //--------------------------------------------------
+  public:
+    Motor2(const pin_pair &pins, Direction direction = NORMAL, float speed_scale = MotorState::DEFAULT_SPEED_SCALE,
+           float deadzone = MotorState::DEFAULT_DEADZONE, float freq = MotorState::DEFAULT_FREQUENCY, DecayMode mode = MotorState::DEFAULT_DECAY_MODE);
+    ~Motor2();
+
+
+    //--------------------------------------------------
+    // Methods
+    //--------------------------------------------------
+  public:
+    bool init();
+
+    // For print access in micropython
+    pin_pair pins() 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);
+
+    //--------------------------------------------------
+
+    void stop();
+    void coast();
+    void brake();
+    void full_negative();
+    void full_positive();
+    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);
+
+    //--------------------------------------------------
+
+    Direction direction() const;
+    void direction(Direction direction);
+
+    float speed_scale() const;
+    void speed_scale(float speed_scale);
+
+    float deadzone() const;
+    void deadzone(float deadzone);
+
+    DecayMode decay_mode();
+    void decay_mode(DecayMode mode);
+
+    //--------------------------------------------------
+  private:
+    void apply_duty(float duty, DecayMode mode);
+  };
+
+}

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,696 @@
+#include "motor_cluster.hpp"
+#include "pwm.hpp"
+#include <cstdio>
+#include "math.h"
+
+#define POS_MOTOR(motor)  (PWMCluster::channel_from_pair(motor))
+#define NEG_MOTOR(motor)  (PWMCluster::channel_from_pair(motor) + 1)
+
+namespace motor {
+  MotorCluster::MotorCluster(PIO pio, uint sm, uint pin_base, uint pin_pair_count, Direction direction,
+                             float speed_scale, float deadzone, float freq, DecayMode mode,
+                             bool auto_phase, PWMCluster::Sequence *seq_buffer, PWMCluster::TransitionData *dat_buffer)
+    : pwms(pio, sm, pin_base, (pin_pair_count * 2), seq_buffer, dat_buffer), pwm_frequency(freq) {
+    create_motor_states(direction, speed_scale, deadzone, mode, auto_phase);
+  }
+
+  MotorCluster::MotorCluster(PIO pio, uint sm, const pin_pair *pin_pairs, uint32_t length, Direction direction,
+                             float speed_scale, float deadzone, float freq, DecayMode mode,
+                             bool auto_phase, PWMCluster::Sequence *seq_buffer, PWMCluster::TransitionData *dat_buffer)
+    : pwms(pio, sm, pin_pairs, length, seq_buffer, dat_buffer), pwm_frequency(freq) {
+    create_motor_states(direction, speed_scale, deadzone, mode, auto_phase);
+  }
+
+  MotorCluster::MotorCluster(PIO pio, uint sm, std::initializer_list<pin_pair> pin_pairs, Direction direction,
+                             float speed_scale, float deadzone, float freq, DecayMode mode,
+                             bool auto_phase, PWMCluster::Sequence *seq_buffer, PWMCluster::TransitionData *dat_buffer)
+    : pwms(pio, sm, pin_pairs, seq_buffer, dat_buffer), pwm_frequency(freq) {
+    create_motor_states(direction, speed_scale, deadzone, mode, auto_phase);
+  }
+
+  MotorCluster::~MotorCluster() {
+    delete[] states;
+    delete[] configs;
+  }
+
+  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_pair_count();
+        for(uint8_t motor = 0; motor < motor_count; motor++) {
+          pwms.set_chan_level(POS_MOTOR(motor), 0, false);
+          pwms.set_chan_level(NEG_MOTOR(motor), 0, false);
+          pwms.set_chan_offset(POS_MOTOR(motor), (uint32_t)(configs[motor].phase * (float)pwm_period), false);
+          pwms.set_chan_offset(NEG_MOTOR(motor), (uint32_t)(configs[motor].phase * (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_pair_count();
+  }
+
+  pin_pair MotorCluster::pins(uint8_t motor) const {
+    assert(motor < pwms.get_chan_pair_count());
+    return pwms.get_chan_pin_pair(motor);
+  }
+
+  void MotorCluster::enable(uint8_t motor, bool load) {
+    assert(motor < pwms.get_chan_pair_count());
+    float new_duty = states[motor].enable_with_return();
+    apply_duty(motor, new_duty, configs[motor].mode, 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_pair_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_pair_count());
+    float new_duty = states[motor].disable_with_return();
+    apply_duty(motor, new_duty, configs[motor].mode, 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_pair_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_pair_count());
+    return states[motor].is_enabled();
+  }
+
+  float MotorCluster::duty(uint8_t motor) const {
+    assert(motor < pwms.get_chan_pair_count());
+    return states[motor].get_duty();
+  }
+
+  void MotorCluster::duty(uint8_t motor, float duty, bool load) {
+    assert(motor < pwms.get_chan_pair_count());
+    float new_duty = states[motor].set_duty_with_return(duty);
+    apply_duty(motor, new_duty, configs[motor].mode, load);
+  }
+
+  void MotorCluster::duty(const uint8_t *motors, uint8_t length, float duty, bool load) {
+    assert(motors != nullptr);
+    for(uint8_t i = 0; i < length; i++) {
+      this->duty(motors[i], duty, false);
+    }
+    if(load)
+      pwms.load_pwm();
+  }
+
+  void MotorCluster::duty(std::initializer_list<uint8_t> motors, float duty, bool load) {
+    for(auto motor : motors) {
+      this->duty(motor, duty, false);
+    }
+    if(load)
+      pwms.load_pwm();
+  }
+
+  void MotorCluster::all_to_duty(float duty, bool load) {
+    uint8_t motor_count = pwms.get_chan_pair_count();
+    for(uint8_t motor = 0; motor < motor_count; motor++) {
+      this->duty(motor, duty, false);
+    }
+    if(load)
+      pwms.load_pwm();
+  }
+
+  float MotorCluster::speed(uint8_t motor) const {
+    assert(motor < pwms.get_chan_pair_count());
+    return states[motor].get_speed();
+  }
+
+  void MotorCluster::speed(uint8_t motor, float speed, bool load) {
+    assert(motor < pwms.get_chan_pair_count());
+    float new_duty = states[motor].set_speed_with_return(speed);
+    apply_duty(motor, new_duty, configs[motor].mode, 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_pair_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_pair_count());
+    return configs[motor].phase;
+  }
+
+  void MotorCluster::phase(uint8_t motor, float phase, bool load) {
+    assert(motor < pwms.get_chan_pair_count());
+    configs[motor].phase = MIN(MAX(phase, 0.0f), 1.0f);
+    pwms.set_chan_offset(motor, (uint32_t)(configs[motor].phase * (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_pair_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_pair_count();
+        for(uint motor = 0; motor < motor_count; motor++) {
+          apply_duty(motor, states[motor].get_deadzoned_duty(), configs[motor].mode, false);
+          pwms.set_chan_offset(POS_MOTOR(motor), (uint32_t)(configs[motor].phase * (float)pwm_period), false);
+          pwms.set_chan_offset(NEG_MOTOR(motor), (uint32_t)(configs[motor].phase * (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;
+  }
+
+  void MotorCluster::stop(uint8_t motor, bool load) {
+    assert(motor < pwms.get_chan_pair_count());
+    float new_duty = states[motor].stop_with_return();
+    apply_duty(motor, new_duty, configs[motor].mode, load);
+  }
+
+  void MotorCluster::stop(const uint8_t *motors, uint8_t length, bool load) {
+    assert(motors != nullptr);
+    for(uint8_t i = 0; i < length; i++) {
+      this->stop(motors[i], false);
+    }
+    if(load)
+      pwms.load_pwm();
+  }
+
+  void MotorCluster::stop(std::initializer_list<uint8_t> motors, bool load) {
+    for(auto motor : motors) {
+      this->stop(motor, false);
+    }
+    if(load)
+      pwms.load_pwm();
+  }
+
+  void MotorCluster::stop_all(bool load) {
+    uint8_t motor_count = pwms.get_chan_pair_count();
+    for(uint8_t motor = 0; motor < motor_count; motor++) {
+      this->stop(motor, false);
+    }
+    if(load)
+      pwms.load_pwm();
+  }
+
+  void MotorCluster::coast(uint8_t motor, bool load) {
+    assert(motor < pwms.get_chan_pair_count());
+    float new_duty = states[motor].stop_with_return();
+    apply_duty(motor, new_duty, FAST_DECAY, load);
+  }
+
+  void MotorCluster::coast(const uint8_t *motors, uint8_t length, bool load) {
+    assert(motors != nullptr);
+    for(uint8_t i = 0; i < length; i++) {
+      this->coast(motors[i], false);
+    }
+    if(load)
+      pwms.load_pwm();
+  }
+
+  void MotorCluster::coast(std::initializer_list<uint8_t> motors, bool load) {
+    for(auto motor : motors) {
+      this->coast(motor, false);
+    }
+    if(load)
+      pwms.load_pwm();
+  }
+
+  void MotorCluster::coast_all(bool load) {
+    uint8_t motor_count = pwms.get_chan_pair_count();
+    for(uint8_t motor = 0; motor < motor_count; motor++) {
+      this->coast(motor, false);
+    }
+    if(load)
+      pwms.load_pwm();
+  }
+
+  void MotorCluster::brake(uint8_t motor, bool load) {
+    assert(motor < pwms.get_chan_pair_count());
+    float new_duty = states[motor].stop_with_return();
+    apply_duty(motor, new_duty, SLOW_DECAY, load);
+  }
+
+  void MotorCluster::brake(const uint8_t *motors, uint8_t length, bool load) {
+    assert(motors != nullptr);
+    for(uint8_t i = 0; i < length; i++) {
+      this->brake(motors[i], false);
+    }
+    if(load)
+      pwms.load_pwm();
+  }
+
+  void MotorCluster::brake(std::initializer_list<uint8_t> motors, bool load) {
+    for(auto motor : motors) {
+      this->brake(motor, false);
+    }
+    if(load)
+      pwms.load_pwm();
+  }
+
+  void MotorCluster::brake_all(bool load) {
+    uint8_t motor_count = pwms.get_chan_pair_count();
+    for(uint8_t motor = 0; motor < motor_count; motor++) {
+      this->brake(motor, false);
+    }
+    if(load)
+      pwms.load_pwm();
+  }
+
+  void MotorCluster::full_negative(uint8_t motor, bool load) {
+    assert(motor < pwms.get_chan_pair_count());
+    float new_duty = states[motor].full_negative_with_return();
+    apply_duty(motor, new_duty, configs[motor].mode, load);
+  }
+
+  void MotorCluster::full_negative(const uint8_t *motors, uint8_t length, bool load) {
+    assert(motors != nullptr);
+    for(uint8_t i = 0; i < length; i++) {
+      this->full_negative(motors[i], false);
+    }
+    if(load)
+      pwms.load_pwm();
+  }
+
+  void MotorCluster::full_negative(std::initializer_list<uint8_t> motors, bool load) {
+    for(auto motor : motors) {
+      this->full_negative(motor, false);
+    }
+    if(load)
+      pwms.load_pwm();
+  }
+
+  void MotorCluster::all_full_negative(bool load) {
+    uint8_t motor_count = pwms.get_chan_pair_count();
+    for(uint8_t motor = 0; motor < motor_count; motor++) {
+      this->full_negative(motor, false);
+    }
+    if(load)
+      pwms.load_pwm();
+  }
+
+  void MotorCluster::full_positive(uint8_t motor, bool load) {
+    assert(motor < pwms.get_chan_pair_count());
+    float new_duty = states[motor].full_positive_with_return();
+    apply_duty(motor, new_duty, configs[motor].mode, load);
+  }
+
+  void MotorCluster::full_positive(const uint8_t *motors, uint8_t length, bool load) {
+    assert(motors != nullptr);
+    for(uint8_t i = 0; i < length; i++) {
+      this->full_positive(motors[i], false);
+    }
+    if(load)
+      pwms.load_pwm();
+  }
+
+  void MotorCluster::full_positive(std::initializer_list<uint8_t> motors, bool load) {
+    for(auto motor : motors) {
+      this->full_positive(motor, false);
+    }
+    if(load)
+      pwms.load_pwm();
+  }
+
+  void MotorCluster::all_full_positive(bool load) {
+    uint8_t motor_count = pwms.get_chan_pair_count();
+    for(uint8_t motor = 0; motor < motor_count; motor++) {
+      this->full_positive(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(motor < pwms.get_chan_pair_count());
+    float new_duty = states[motor].to_percent_with_return(in, in_min, in_max);
+    apply_duty(motor, new_duty, configs[motor].mode, 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_pair_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(motor < pwms.get_chan_pair_count());
+    float new_duty = states[motor].to_percent_with_return(in, in_min, in_max, speed_min, speed_max);
+    apply_duty(motor, new_duty, configs[motor].mode, 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_pair_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();
+  }
+
+  void MotorCluster::load() {
+    pwms.load_pwm();
+  }
+
+  Direction MotorCluster::direction(uint8_t motor) const {
+    assert(motor < pwms.get_chan_pair_count());
+    return states[motor].get_direction();
+  }
+
+  void MotorCluster::direction(uint8_t motor, Direction direction) {
+    assert(motor < pwms.get_chan_pair_count());
+    states[motor].set_direction(direction);
+  }
+
+  void MotorCluster::direction(const uint8_t *motors, uint8_t length, Direction direction) {
+    assert(motors != nullptr);
+    for(uint8_t i = 0; i < length; i++) {
+      this->direction(motors[i], direction);
+    }
+  }
+
+  void MotorCluster::direction(std::initializer_list<uint8_t> motors, Direction direction) {
+    for(auto motor : motors) {
+      this->direction(motor, direction);
+    }
+  }
+
+  void MotorCluster::all_to_direction(Direction direction) {
+    uint8_t motor_count = pwms.get_chan_pair_count();
+    for(uint8_t motor = 0; motor < motor_count; motor++) {
+      this->direction(motor, direction);
+    }
+  }
+
+  float MotorCluster::speed_scale(uint8_t motor) const {
+    assert(motor < pwms.get_chan_pair_count());
+    return states[motor].get_speed_scale();
+  }
+
+  void MotorCluster::speed_scale(uint8_t motor, float speed_scale) {
+    assert(motor < pwms.get_chan_pair_count());
+    states[motor].set_speed_scale(speed_scale);
+  }
+
+  void MotorCluster::speed_scale(const uint8_t *motors, uint8_t length, float speed_scale) {
+    assert(motors != nullptr);
+    for(uint8_t i = 0; i < length; i++) {
+      this->speed_scale(motors[i], speed_scale);
+    }
+  }
+
+  void MotorCluster::speed_scale(std::initializer_list<uint8_t> motors, float speed_scale) {
+    for(auto motor : motors) {
+      this->speed_scale(motor, speed_scale);
+    }
+  }
+
+  void MotorCluster::all_to_speed_scale(float speed_scale) {
+    uint8_t motor_count = pwms.get_chan_pair_count();
+    for(uint8_t motor = 0; motor < motor_count; motor++) {
+      this->speed_scale(motor, speed_scale);
+    }
+  }
+
+  float MotorCluster::deadzone(uint8_t motor) const {
+    assert(motor < pwms.get_chan_pair_count());
+    return states[motor].get_deadzone();
+  }
+
+  void MotorCluster::deadzone(uint8_t motor, float deadzone, bool load) {
+    assert(motor < pwms.get_chan_pair_count());
+    float new_duty = states[motor].set_deadzone_with_return(deadzone);
+    apply_duty(motor, new_duty, configs[motor].mode, load);
+  }
+
+  void MotorCluster::deadzone(const uint8_t *motors, uint8_t length, float deadzone, bool load) {
+    assert(motors != nullptr);
+    for(uint8_t i = 0; i < length; i++) {
+      this->deadzone(motors[i], deadzone);
+    }
+  }
+
+  void MotorCluster::deadzone(std::initializer_list<uint8_t> motors, float deadzone, bool load) {
+    for(auto motor : motors) {
+      this->deadzone(motor, deadzone);
+    }
+  }
+
+  void MotorCluster::all_to_deadzone(float deadzone, bool load) {
+    uint8_t motor_count = pwms.get_chan_pair_count();
+    for(uint8_t motor = 0; motor < motor_count; motor++) {
+      this->deadzone(motor, deadzone);
+    }
+  }
+
+  DecayMode MotorCluster::decay_mode(uint8_t motor) const {
+    assert(motor < pwms.get_chan_pair_count());
+    return configs[motor].mode;
+  }
+
+  void MotorCluster::decay_mode(uint8_t motor, DecayMode mode, bool load) {
+    assert(motor < pwms.get_chan_pair_count());
+    configs[motor].mode = mode;
+    apply_duty(motor, states[motor].get_deadzoned_duty(), configs[motor].mode, false);
+  }
+
+  void MotorCluster::decay_mode(const uint8_t *motors, uint8_t length, DecayMode mode, bool load) {
+    assert(motors != nullptr);
+    for(uint8_t i = 0; i < length; i++) {
+      this->decay_mode(motors[i], mode);
+    }
+  }
+
+  void MotorCluster::decay_mode(std::initializer_list<uint8_t> motors, DecayMode mode, bool load) {
+    for(auto motor : motors) {
+      this->decay_mode(motor, mode);
+    }
+  }
+
+  void MotorCluster::all_to_decay_mode(DecayMode mode, bool load) {
+    uint8_t motor_count = pwms.get_chan_pair_count();
+    for(uint8_t motor = 0; motor < motor_count; motor++) {
+      this->decay_mode(motor, mode);
+    }
+  }
+
+
+  void MotorCluster::apply_duty(uint8_t motor, float duty, DecayMode mode, bool load) {
+    if(isfinite(duty)) {
+      int32_t signed_level = MotorState::duty_to_level(duty, pwm_period);
+
+      switch(mode) {
+      case SLOW_DECAY: //aka 'Braking'
+        if(signed_level >= 0) {
+          pwms.set_chan_level(POS_MOTOR(motor), pwm_period, false);
+          pwms.set_chan_level(NEG_MOTOR(motor), pwm_period - signed_level, load);
+        }
+        else {
+          pwms.set_chan_level(POS_MOTOR(motor), pwm_period + signed_level, false);
+          pwms.set_chan_level(NEG_MOTOR(motor), pwm_period, load);
+        }
+        break;
+
+      case FAST_DECAY: //aka 'Coasting'
+      default:
+        if(signed_level >= 0) {
+          pwms.set_chan_level(POS_MOTOR(motor), signed_level, false);
+          pwms.set_chan_level(NEG_MOTOR(motor), 0, load);
+        }
+        else {
+          pwms.set_chan_level(POS_MOTOR(motor), 0, false);
+          pwms.set_chan_level(NEG_MOTOR(motor), 0 - signed_level, load);
+        }
+        break;
+      }
+    }
+    else {
+      pwms.set_chan_level(POS_MOTOR(motor), 0, false);
+      pwms.set_chan_level(NEG_MOTOR(motor), 0, load);
+    }
+  }
+
+  void MotorCluster::create_motor_states(Direction direction, float speed_scale,
+                                         float deadzone, DecayMode mode, bool auto_phase) {
+    uint8_t motor_count = pwms.get_chan_pair_count();
+    if(motor_count > 0) {
+      states = new MotorState[motor_count];
+      configs = new motor_config[motor_count];
+
+      for(uint motor = 0; motor < motor_count; motor++) {
+        states[motor] = MotorState(direction, speed_scale, deadzone);
+        configs[motor].phase = (auto_phase) ? (float)motor / (float)motor_count : 0.0f;
+        configs[motor].mode = mode;
+      }
+    }
+  }
+};

drivers/motor/motor_cluster.hpp

@@ -0,0 +1,161 @@
+#pragma once
+
+#include "pico/stdlib.h"
+#include "pwm_cluster.hpp"
+#include "motor_state.hpp"
+
+using namespace pimoroni;
+
+namespace motor {
+
+  class MotorCluster {
+    //--------------------------------------------------
+    // Substructures
+    //--------------------------------------------------
+  private:
+    struct motor_config {
+      float phase;
+      DecayMode mode;
+    };
+
+
+    //--------------------------------------------------
+    // Variables
+    //--------------------------------------------------
+  private:
+    PWMCluster pwms;
+    uint32_t pwm_period;
+    float pwm_frequency;
+    MotorState* states;
+    motor_config* configs;
+
+
+    //--------------------------------------------------
+    // Constructors/Destructor
+    //--------------------------------------------------
+  public:
+    MotorCluster(PIO pio, uint sm, uint pin_base, uint pin_pair_count, Direction direction = NORMAL, float speed_scale = MotorState::DEFAULT_SPEED_SCALE,
+                 float deadzone = MotorState::DEFAULT_DEADZONE, float freq = MotorState::DEFAULT_FREQUENCY, DecayMode mode = MotorState::DEFAULT_DECAY_MODE,
+                 bool auto_phase = true, PWMCluster::Sequence *seq_buffer = nullptr, PWMCluster::TransitionData *dat_buffer = nullptr);
+    MotorCluster(PIO pio, uint sm, const pin_pair *pin_pairs, uint32_t length, Direction direction = NORMAL, float speed_scale = MotorState::DEFAULT_SPEED_SCALE,
+                 float deadzone = MotorState::DEFAULT_DEADZONE, float freq = MotorState::DEFAULT_FREQUENCY, DecayMode mode = MotorState::DEFAULT_DECAY_MODE,
+                 bool auto_phase = true, PWMCluster::Sequence *seq_buffer = nullptr, PWMCluster::TransitionData *dat_buffer = nullptr);
+    MotorCluster(PIO pio, uint sm, std::initializer_list<pin_pair> pin_pairs, Direction direction = NORMAL, float speed_scale = MotorState::DEFAULT_SPEED_SCALE,
+                 float deadzone = MotorState::DEFAULT_DEADZONE, float freq = MotorState::DEFAULT_FREQUENCY, DecayMode mode = MotorState::DEFAULT_DECAY_MODE,
+                 bool auto_phase = true, PWMCluster::Sequence *seq_buffer = nullptr, PWMCluster::TransitionData *dat_buffer = nullptr);
+    ~MotorCluster();
+
+
+    //--------------------------------------------------
+    // Methods
+    //--------------------------------------------------
+  public:
+    bool init();
+
+    uint8_t count() const;
+    pin_pair pins(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);
+
+    //--------------------------------------------------
+    void stop(uint8_t motor, bool load = true);
+    void stop(const uint8_t *motors, uint8_t length, bool load = true);
+    void stop(std::initializer_list<uint8_t> motors, bool load = true);
+    void stop_all(bool load = true);
+
+    void coast(uint8_t motor, bool load = true);
+    void coast(const uint8_t *motors, uint8_t length, bool load = true);
+    void coast(std::initializer_list<uint8_t> motors, bool load = true);
+    void coast_all(bool load = true);
+
+    void brake(uint8_t motor, bool load = true);
+    void brake(const uint8_t *motors, uint8_t length, bool load = true);
+    void brake(std::initializer_list<uint8_t> motors, bool load = true);
+    void brake_all(bool load = true);
+
+    void full_negative(uint8_t motor, bool load = true);
+    void full_negative(const uint8_t *motors, uint8_t length, bool load = true);
+    void full_negative(std::initializer_list<uint8_t> motors, bool load = true);
+    void all_full_negative(bool load = true);
+
+    void full_positive(uint8_t motor, bool load = true);
+    void full_positive(const uint8_t *motors, uint8_t length, bool load = true);
+    void full_positive(std::initializer_list<uint8_t> motors, bool load = true);
+    void all_full_positive(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);
+
+    void load();
+
+    //--------------------------------------------------
+
+    Direction direction(uint8_t motor) const;
+    void direction(uint8_t motor, Direction direction);
+    void direction(const uint8_t *motors, uint8_t length, Direction direction);
+    void direction(std::initializer_list<uint8_t> motors, Direction direction);
+    void all_to_direction(Direction direction);
+
+    float speed_scale(uint8_t motor) const;
+    void speed_scale(uint8_t motor, float speed_scale);
+    void speed_scale(const uint8_t *motors, uint8_t length, float speed_scale);
+    void speed_scale(std::initializer_list<uint8_t> motors, float speed_scale);
+    void all_to_speed_scale(float speed_scale);
+
+    float deadzone(uint8_t motor) const;
+    void deadzone(uint8_t motor, float deadzone, bool load = true);
+    void deadzone(const uint8_t *motors, uint8_t length, float deadzone, bool load = true);
+    void deadzone(std::initializer_list<uint8_t> motors, float deadzone, bool load = true);
+    void all_to_deadzone(float deadzone, bool load = true);
+
+    DecayMode decay_mode(uint8_t motor) const;
+    void decay_mode(uint8_t motor, DecayMode mode, bool load = true);
+    void decay_mode(const uint8_t *motors, uint8_t length, DecayMode mode, bool load = true);
+    void decay_mode(std::initializer_list<uint8_t> motors, DecayMode mode, bool load = true);
+    void all_to_decay_mode(DecayMode mode, bool load = true);
+
+    //--------------------------------------------------
+  private:
+    void apply_duty(uint8_t motor, float duty, DecayMode mode, bool load);
+    void create_motor_states(Direction direction, float speed_scale, float deadzone, DecayMode mode, bool auto_phase);
+  };
+
+}

drivers/motor/motor_state.cpp

@@ -0,0 +1,128 @@
+#include "motor_state.hpp"
+#include "common/pimoroni_common.hpp"
+#include "float.h"
+#include "math.h"
+
+namespace motor {
+  MotorState::MotorState()
+    : motor_speed(0.0f), last_enabled_duty(0.0f), enabled(false),
+    motor_direction(NORMAL), motor_scale(DEFAULT_SPEED_SCALE), motor_deadzone(DEFAULT_DEADZONE){
+  }
+
+  MotorState::MotorState(Direction direction, float speed_scale, float deadzone)
+    : motor_speed(0.0f), last_enabled_duty(0.0f), enabled(false),
+    motor_direction(direction), motor_scale(MAX(speed_scale, FLT_EPSILON)), motor_deadzone(CLAMP(deadzone, 0.0f, 1.0f)) {
+  }
+
+  float MotorState::enable_with_return() {
+    enabled = true;
+    return get_deadzoned_duty();
+  }
+
+  float MotorState::disable_with_return() {
+    enabled = false;
+    return NAN;
+  }
+
+  bool MotorState::is_enabled() const {
+    return enabled;
+  }
+
+  float MotorState::get_duty() const {
+    return last_enabled_duty;
+  }
+
+  float MotorState::get_deadzoned_duty() const {
+    float duty = 0.0f;
+    if((last_enabled_duty <= 0.0f - motor_deadzone) || (last_enabled_duty >= motor_deadzone)) {
+      duty = last_enabled_duty;
+    }
+
+    if(enabled)
+      return duty;
+    else
+      return NAN;
+  }
+
+  float MotorState::set_duty_with_return(float duty) {
+    // Clamp the duty between the hard limits
+    last_enabled_duty = CLAMP(duty, -1.0f, 1.0f);
+    motor_speed = last_enabled_duty * motor_scale;
+
+    return enable_with_return();
+  }
+
+  float MotorState::get_speed() const {
+    return (motor_direction == NORMAL) ? motor_speed : 0.0f - motor_speed;
+  }
+
+  float MotorState::set_speed_with_return(float speed) {
+    // Invert provided speed if the motor direction is reversed
+    if(motor_direction == REVERSED)
+      speed = 0.0f - speed;
+
+    // Clamp the speed between the hard limits
+    motor_speed = CLAMP(speed, -motor_scale, motor_scale);
+    last_enabled_duty = motor_speed / motor_scale;
+
+    return enable_with_return();
+  }
+
+  float MotorState::stop_with_return() {
+    return set_duty_with_return(0.0f);
+  }
+
+  float MotorState::full_negative_with_return() {
+    return set_duty_with_return(-1.0f);
+  }
+
+  float MotorState::full_positive_with_return() {
+    return set_duty_with_return(1.0f);
+  }
+
+  float MotorState::to_percent_with_return(float in, float in_min, float in_max) {
+    float speed = MotorState::map_float(in, in_min, in_max, 0.0f - motor_speed, motor_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);
+  }
+
+  Direction MotorState::get_direction() const {
+    return motor_direction;
+  }
+
+  void MotorState::set_direction(Direction direction) {
+    motor_direction = direction;
+  }
+
+  float MotorState::get_speed_scale() const {
+    return motor_scale;
+  }
+
+  void MotorState::set_speed_scale(float speed_scale) {
+    motor_scale = MAX(speed_scale, FLT_EPSILON);
+    motor_speed = last_enabled_duty * motor_scale;
+  }
+
+
+  float MotorState::get_deadzone() const {
+    return motor_deadzone;
+  }
+
+  float MotorState::set_deadzone_with_return(float deadzone) {
+    motor_deadzone = CLAMP(deadzone, 0.0f, 1.0f);
+    return get_deadzoned_duty();
+  }
+
+  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,96 @@
+#pragma once
+
+#include "pico/stdlib.h"
+
+namespace motor {
+
+  enum Direction {
+    NORMAL    = 0,
+    REVERSED  = 1,
+  };
+
+  enum DecayMode {
+    FAST_DECAY  = 0, //aka 'Coasting'
+    SLOW_DECAY  = 1, //aka 'Braking'
+  };
+
+  class MotorState {
+    //--------------------------------------------------
+    // Constants
+    //--------------------------------------------------
+  public:
+    static constexpr float DEFAULT_SPEED_SCALE = 1.0f;        // The standard motor speed scale
+    static constexpr float DEFAULT_DEADZONE = 0.1f;           // The standard motor deadzone
+
+    static const DecayMode DEFAULT_DECAY_MODE = SLOW_DECAY;   // The standard motor decay behaviour
+    static constexpr float DEFAULT_FREQUENCY = 25000.0f;      // The standard motor update rate
+    static constexpr float MIN_FREQUENCY = 10.0f;
+    static constexpr float MAX_FREQUENCY = 50000.0f;
+
+    static constexpr float ZERO_PERCENT = 0.0f;
+    static constexpr float ONEHUNDRED_PERCENT = 1.0f;
+
+
+    //--------------------------------------------------
+    // Variables
+    //--------------------------------------------------
+  private:
+    float motor_speed;
+    float last_enabled_duty;
+    bool enabled;
+
+    // Customisation variables
+    Direction motor_direction;
+    float motor_scale;
+    float motor_deadzone;
+
+
+    //--------------------------------------------------
+    // Constructors/Destructor
+    //--------------------------------------------------
+  public:
+    MotorState();
+    MotorState(Direction direction, float speed_scale, float deadzone);
+
+
+    //--------------------------------------------------
+    // Methods
+    //--------------------------------------------------
+  public:
+    float enable_with_return();
+    float disable_with_return();
+    bool is_enabled() const;
+
+    float get_duty() const;
+    float get_deadzoned_duty() const;
+    float set_duty_with_return(float duty);
+
+    float get_speed() const;
+    float set_speed_with_return(float speed);
+
+    //--------------------------------------------------
+
+    float stop_with_return();
+    float full_negative_with_return();
+    float full_positive_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);
+
+    //--------------------------------------------------
+
+    Direction get_direction() const;
+    void set_direction(Direction direction);
+
+    float get_speed_scale() const;
+    void set_speed_scale(float speed_scale);
+
+    float get_deadzone() const;
+    float set_deadzone_with_return(float deadzone);
+
+    //--------------------------------------------------
+    static int32_t duty_to_level(float duty, uint32_t resolution);
+
+    static float map_float(float in, float in_min, float in_max, float out_min, float out_max);
+  };
+
+}

drivers/pwm/pwm_cluster.cpp

@@ -102,6 +102,55 @@ PWMCluster::PWMCluster(PIO pio, uint sm, std::initializer_list<uint8_t> pins, Se
   constructor_common(seq_buffer, dat_buffer);
 }
 
+PWMCluster::PWMCluster(PIO pio, uint sm, const pin_pair *pin_pairs, uint32_t length, Sequence *seq_buffer, TransitionData *dat_buffer)
+: pio(pio)
+, sm(sm)
+, pin_mask(0x00000000)
+, channel_count(0)
+, channels(nullptr)
+, wrap_level(0) {
+
+  // Create the pin mask and channel mapping
+  for(uint i = 0; i < length; i++) {
+    pin_pair pair = pin_pairs[i];
+    if((pair.first < NUM_BANK0_GPIOS) && (pair.second < NUM_BANK0_GPIOS)) {
+      pin_mask |= (1u << pair.first);
+      channel_to_pin_map[channel_count] = pair.first;
+      channel_count++;
+
+      pin_mask |= (1u << pair.second);
+      channel_to_pin_map[channel_count] = pair.second;
+      channel_count++;
+    }
+  }
+
+  constructor_common(seq_buffer, dat_buffer);
+}
+
+PWMCluster::PWMCluster(PIO pio, uint sm, std::initializer_list<pin_pair> pin_pairs, Sequence *seq_buffer, TransitionData *dat_buffer)
+: pio(pio)
+, sm(sm)
+, pin_mask(0x00000000)
+, channel_count(0)
+, channels(nullptr)
+, wrap_level(0) {
+
+  // Create the pin mask and channel mapping
+  for(auto pair : pin_pairs) {
+    if((pair.first < NUM_BANK0_GPIOS) && (pair.second < NUM_BANK0_GPIOS)) {
+      pin_mask |= (1u << pair.first);
+      channel_to_pin_map[channel_count] = pair.first;
+      channel_count++;
+
+      pin_mask |= (1u << pair.second);
+      channel_to_pin_map[channel_count] = pair.second;
+      channel_count++;
+    }
+  }
+
+  constructor_common(seq_buffer, dat_buffer);
+}
+
 void PWMCluster::constructor_common(Sequence *seq_buffer, TransitionData *dat_buffer) {
   // Initialise all the channels this PWM will control
   if(channel_count > 0) {
@@ -332,11 +381,25 @@ uint8_t PWMCluster::get_chan_count() const {
   return channel_count;
 }
 
+uint8_t PWMCluster::get_chan_pair_count() const {
+  return (channel_count / 2);
+}
+
 uint8_t PWMCluster::get_chan_pin(uint8_t channel) const {
   assert(channel < channel_count);
   return channel_to_pin_map[channel];
 }
 
+pin_pair PWMCluster::get_chan_pin_pair(uint8_t channel_pair) const {
+  assert(channel_pair < get_chan_pair_count());
+  uint8_t channel_base = channel_from_pair(channel_pair);
+  return pin_pair(channel_to_pin_map[channel_base], channel_to_pin_map[channel_base + 1]);
+}
+
+uint8_t PWMCluster::channel_from_pair(uint8_t channel_pair) {
+  return (channel_pair * 2);
+}
+
 uint32_t PWMCluster::get_chan_level(uint8_t channel) const {
   assert(channel < channel_count);
   return channels[channel].level;

drivers/pwm/pwm_cluster.hpp

@@ -4,6 +4,7 @@
 #include "hardware/pio.h"
 #include "hardware/dma.h"
 #include "hardware/irq.h"
+#include "common/pimoroni_common.hpp"
 #include <initializer_list>
 
 namespace pimoroni {
@@ -137,6 +138,9 @@ namespace pimoroni {
     PWMCluster(PIO pio, uint sm, uint pin_base, uint pin_count, Sequence *seq_buffer = nullptr, TransitionData *dat_buffer = nullptr);
     PWMCluster(PIO pio, uint sm, const uint8_t *pins, uint32_t length, Sequence *seq_buffer = nullptr, TransitionData *dat_buffer = nullptr);
     PWMCluster(PIO pio, uint sm, std::initializer_list<uint8_t> pins, Sequence *seq_buffer = nullptr, TransitionData *dat_buffer = nullptr);
+
+    PWMCluster(PIO pio, uint sm, const pin_pair *pin_pairs, uint32_t length, Sequence *seq_buffer = nullptr, TransitionData *dat_buffer = nullptr);
+    PWMCluster(PIO pio, uint sm, std::initializer_list<pin_pair> pin_pairs, Sequence *seq_buffer = nullptr, TransitionData *dat_buffer = nullptr);
     ~PWMCluster();
 
   private:
@@ -150,7 +154,10 @@ namespace pimoroni {
     bool init();
 
     uint8_t get_chan_count() const;
+    uint8_t get_chan_pair_count() const;
     uint8_t get_chan_pin(uint8_t channel) const;
+    pin_pair get_chan_pin_pair(uint8_t channel_pair) const;
+    static uint8_t channel_from_pair(uint8_t channel_pair);
 
     uint32_t get_chan_level(uint8_t channel) const;
     void set_chan_level(uint8_t channel, uint32_t level, bool load = true);

examples/CMakeLists.txt

@@ -31,6 +31,7 @@ add_subdirectory(pico_enc_explorer)
 add_subdirectory(pico_explorer)
 add_subdirectory(pico_pot_explorer)
 add_subdirectory(pico_explorer_encoder)
+add_subdirectory(pico_motor_shim)
 add_subdirectory(pico_rgb_keypad)
 add_subdirectory(pico_rtc_display)
 add_subdirectory(pico_tof_display)
@@ -42,3 +43,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::MOTOR_A);
+
+
+int main() {
+  stdio_init_all();
+
+  // Initialise the motor
+  m.init();
+
+  // Enable the motor
+  m.enable();
+  sleep_ms(2000);
+
+  // Go at full neative
+  m.full_negative();
+  sleep_ms(2000);
+
+  // Go at full positive
+  m.full_positive();
+  sleep_ms(2000);
+
+  // Stop the motor
+  m.stop();
+  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();
+}

examples/pico_motor_shim/CMakeLists.txt

@@ -0,0 +1,3 @@
+add_subdirectory(balance)
+add_subdirectory(sequence)
+add_subdirectory(song)

examples/pico_motor_shim/balance/CMakeLists.txt

@@ -0,0 +1,16 @@
+set(OUTPUT_NAME motor_shim_balance)
+
+add_executable(
+  ${OUTPUT_NAME}
+  demo.cpp
+)
+
+# enable usb output, disable uart output
+pico_enable_stdio_usb(${OUTPUT_NAME} 1)
+pico_enable_stdio_uart(${OUTPUT_NAME} 1)
+
+# Pull in pico libraries that we need
+target_link_libraries(${OUTPUT_NAME} pico_stdlib pico_motor_shim motor button breakout_msa301)
+
+# create map/bin/hex file etc.
+pico_add_extra_outputs(${OUTPUT_NAME})

examples/pico_motor_shim/balance/demo.cpp

@@ -0,0 +1,119 @@
+#include <stdio.h>
+#include "pico_motor_shim.hpp"
+
+#include "common/pimoroni_common.hpp"
+#include "motor.hpp"
+#include "button.hpp"
+#include "breakout_msa301.hpp"
+#include <math.h>
+
+/*
+A very basic balancing robot implementation, using an MSA301 to give accelerating values that are passed to the motors using proportional control.
+Press "A" to start and stop the balancer
+*/
+
+using namespace pimoroni;
+using namespace motor;
+
+static constexpr float TOP_SPEED = 1.0f; //A value between 0 and 1
+static constexpr float Z_BIAS_CORRECTION = 0.5f; //A magic number that seems to correct the MSA301's Z bias
+static constexpr float PROPORTIONAL = 0.03f;
+
+
+
+Button button_a(pico_motor_shim::BUTTON_A, Polarity::ACTIVE_LOW, 0);
+
+Motor motor_1(pico_motor_shim::MOTOR_1);//, Motor::DEFAULT_PWM_FREQUENCY, Motor::DEFAULT_DECAY_MODE);
+Motor motor_2(pico_motor_shim::MOTOR_2);//, Motor::DEFAULT_PWM_FREQUENCY, Motor::DEFAULT_DECAY_MODE);
+
+I2C i2c(BOARD::BREAKOUT_GARDEN);
+BreakoutMSA301 msa301(&i2c);
+
+static bool button_toggle = false;
+static float target_angle = 0.0f;
+
+/**
+ * Checks if the button has been pressed, toggling a value that is also returned.
+ */
+bool check_button_toggle() {
+  bool button_pressed = button_a.read();
+  if(button_pressed) {
+    button_toggle = !button_toggle;
+  }
+  return button_toggle;
+}
+
+/**
+ * Takes and angle and wraps it around so that it stays within a -180 to +180 degree range.
+ *
+ * Note, it will only work for values between -540 and +540 degrees.
+ * This can be resolved by changing the 'if's into 'while's, but for most uses it is unnecessary
+ */
+float wrap_angle(float angle) {
+  if(angle <= -180.0f)
+    angle += 360.0f;
+
+  if(angle > 180.0f)
+    angle -= 360.0f;
+
+  return angle;
+}
+
+/**
+ * The entry point of the program.
+ */
+int main() {
+  stdio_init_all();
+
+  //Initialise the LED. We use this to indicate that the sequence is running.
+  gpio_init(PICO_DEFAULT_LED_PIN);
+  gpio_set_dir(PICO_DEFAULT_LED_PIN, GPIO_OUT);
+  for(int i = 0; i < 20; i++) {
+    gpio_put(PICO_DEFAULT_LED_PIN, true);
+    sleep_ms(250);
+    gpio_put(PICO_DEFAULT_LED_PIN, false);
+    sleep_ms(250);
+  }
+
+  //Initialise the two motors
+  if(!motor_1.init() || !motor_2.init()) {
+    printf("Cannot initialise motors. Check the provided parameters\n");
+    return 0;
+  }
+
+  if(!msa301.init()) {
+    printf("Cannot initialise msa301. Check that it is connected\n");
+    return 0;
+  }
+
+  printf("Ready\n");
+
+  while(true) {
+    //Turn the Pico's LED on to show that the sequence has started
+    gpio_put(PICO_DEFAULT_LED_PIN, true);
+    sleep_ms(50);
+
+    //Has the user has pressed the button to start the sequence
+    while(check_button_toggle()) {
+      float y = msa301.get_y_axis();
+      float z = msa301.get_z_axis() + Z_BIAS_CORRECTION;
+
+      float current_angle = (atan2(z, -y) * 180.0f) / M_PI;
+      float angle_error = wrap_angle(target_angle - current_angle);
+      printf("Y: %f, Z: %f, AngErr: %f\n", y, z, angle_error);
+
+      float output = angle_error * PROPORTIONAL; //No need to clamp this value as set_speed does this internally
+      motor_1.set_speed(output);
+      motor_2.set_speed(-output);
+
+      sleep_ms(1);
+    }
+
+    //The sequence loop has ended, so turn off the Pico's LED and disable the motors
+    gpio_put(PICO_DEFAULT_LED_PIN, false);
+    motor_1.disable();
+    motor_2.disable();
+    sleep_ms(50);
+  }
+  return 0;
+}

examples/pico_motor_shim/sequence/CMakeLists.txt

@@ -0,0 +1,16 @@
+set(OUTPUT_NAME motor_shim_sequence)
+
+add_executable(
+  ${OUTPUT_NAME}
+  demo.cpp
+)
+
+# enable usb output, disable uart output
+pico_enable_stdio_usb(${OUTPUT_NAME} 1)
+pico_enable_stdio_uart(${OUTPUT_NAME} 1)
+
+# Pull in pico libraries that we need
+target_link_libraries(${OUTPUT_NAME} pico_stdlib pico_motor_shim motor button)
+
+# create map/bin/hex file etc.
+pico_add_extra_outputs(${OUTPUT_NAME})

examples/pico_motor_shim/sequence/demo.cpp

@@ -0,0 +1,170 @@
+#include <stdio.h>
+#include "pico_motor_shim.hpp"
+
+#include "common/pimoroni_common.hpp"
+#include "motor.hpp"
+#include "button.hpp"
+
+/*
+Program showing how the two motors of the Pico Motor Shim can be perform a sequence of movements.
+Press "A" to start and stop the movement sequence
+*/
+
+using namespace pimoroni;
+using namespace motor;
+
+static constexpr float TOP_SPEED = 1.0f; //A value between 0 and 1
+static const uint32_t ACCELERATE_TIME_MS = 2000;
+static const uint32_t WAIT_TIME_MS = 1000;
+static const uint32_t STOP_TIME_MS = 1000;
+
+
+Button button_a(pico_motor_shim::BUTTON_A, Polarity::ACTIVE_LOW, 0);
+
+Motor motor_1(pico_motor_shim::MOTOR_1);//, Motor::DEFAULT_PWM_FREQUENCY, Motor::DEFAULT_DECAY_MODE);
+Motor motor_2(pico_motor_shim::MOTOR_2);//, Motor::DEFAULT_PWM_FREQUENCY, Motor::DEFAULT_DECAY_MODE);
+
+static bool button_toggle = false;
+
+/**
+ * Checks if the button has been pressed, toggling a value that is also returned.
+ */
+bool check_button_toggle() {
+  bool button_pressed = button_a.read();
+  if(button_pressed) {
+    button_toggle = !button_toggle;
+  }
+  return button_toggle;
+}
+
+/**
+ * Waits for a given amount of time (in milliseconds).
+ * Exits early if the user presses the button to stop the sequence, returning false.
+ */
+bool wait_for(uint32_t duration_ms) {
+  uint32_t start_time = millis();
+  uint32_t ellapsed = 0;
+
+  //Loops until the duration has elapsed, checking the button state every millisecond
+  while(ellapsed < duration_ms) {
+    if(!check_button_toggle())
+      return false;
+
+    sleep_ms(1);
+    ellapsed = millis() - start_time;
+  }
+  return true;
+}
+
+/**
+ * Accelerate/Decelerate the motors from their current speed to the target speed over the given amount of time (in milliseconds).
+ * Exits early if the user presses the button to stop the sequence, returning false.
+ */
+bool accelerate_over(float left_speed, float right_speed, uint32_t duration_ms) {
+  uint32_t start_time = millis();
+  uint32_t ellapsed = 0;
+
+  //Get the current motor speeds
+  float last_left = motor_1.get_speed();
+  float last_right = motor_2.get_speed();
+
+  //Loops until the duration has elapsed, checking the button state every millisecond, and updating motor speeds
+  while(ellapsed <= duration_ms) {
+    if(!check_button_toggle())
+      return false;
+
+    //Calculate and set the new motor speeds
+    float percentage = (float)ellapsed / (float)duration_ms;
+    motor_1.set_speed(((left_speed - last_left) * percentage) + last_left);
+    motor_2.set_speed(((right_speed - last_right) * percentage) + last_right);
+
+    sleep_ms(1);
+    ellapsed = millis() - start_time;
+  }
+
+  //Set the final motor speeds as loop may not reach 100%
+  motor_1.set_speed(left_speed);
+  motor_2.set_speed(right_speed);
+
+  return true;
+}
+
+/**
+ * The function that performs the driving sequence.
+ * Exits early if the user presses the button to stop the sequence, returning false.
+ */
+bool sequence() {
+  printf("accelerate forward\n");
+  if(!accelerate_over(-TOP_SPEED, TOP_SPEED, ACCELERATE_TIME_MS))
+    return false; //Early exit if the button was toggled
+
+  printf("driving forward\n");
+  if(!wait_for(WAIT_TIME_MS))
+    return false; //Early exit if the button was toggled
+
+  printf("deccelerate forward\n");
+  if(!accelerate_over(0.0f, 0.0f, ACCELERATE_TIME_MS))
+    return false; //Early exit if the button was toggled
+
+  printf("stop\n");
+  motor_1.stop();
+  motor_2.stop();
+  if(!wait_for(STOP_TIME_MS))
+    return false; //Early exit if the button was toggled
+
+  printf("accelerate turn left\n");
+  if(!accelerate_over(TOP_SPEED * 0.5f, TOP_SPEED * 0.5f, ACCELERATE_TIME_MS * 0.5f))
+    return false; //Early exit if the button was toggled
+
+  printf("turning left\n");
+  if(!wait_for(WAIT_TIME_MS))
+    return false; //Early exit if the button was toggled
+
+  printf("deccelerate turn left\n");
+  if(!accelerate_over(0.0f, 0.0f, ACCELERATE_TIME_MS * 0.5f))
+    return false; //Early exit if the button was toggled
+
+  printf("stop\n");
+  motor_1.stop();
+  motor_2.stop();
+  if(!wait_for(STOP_TIME_MS))
+    return false; //Early exit if the button was toggled
+
+  //Signal that the sequence completed successfully
+  return true;
+}
+
+/**
+ * The entry point of the program.
+ */
+int main() {
+  stdio_init_all();
+
+  //Initialise the LED. We use this to indicate that the sequence is running.
+  gpio_init(PICO_DEFAULT_LED_PIN);
+  gpio_set_dir(PICO_DEFAULT_LED_PIN, GPIO_OUT);
+
+  //Initialise the two motors
+  if(!motor_1.init() || !motor_2.init()) {
+    printf("Cannot initialise motors. Check the provided parameters\n");
+    return 0;
+  }
+
+  while(true) {
+    //Has the user has pressed the button to start the sequence
+    if(check_button_toggle()) {
+
+      //Turn the Pico's LED on to show that the sequence has started
+      gpio_put(PICO_DEFAULT_LED_PIN, true);
+
+      //Run the sequence in a perpetual loop, exiting early if the button is pressed again
+      while(sequence());
+    }
+
+    //The sequence loop has ended, so turn off the Pico's LED and disable the motors
+    gpio_put(PICO_DEFAULT_LED_PIN, false);
+    motor_1.disable();
+    motor_2.disable();
+  }
+  return 0;
+}

examples/pico_motor_shim/song/CMakeLists.txt

@@ -0,0 +1,16 @@
+set(OUTPUT_NAME motor_shim_song)
+
+add_executable(
+  ${OUTPUT_NAME}
+  demo.cpp
+)
+
+# enable usb output, disable uart output
+pico_enable_stdio_usb(${OUTPUT_NAME} 1)
+pico_enable_stdio_uart(${OUTPUT_NAME} 1)
+
+# Pull in pico libraries that we need
+target_link_libraries(${OUTPUT_NAME} pico_stdlib pico_motor_shim motor button breakout_msa301)
+
+# create map/bin/hex file etc.
+pico_add_extra_outputs(${OUTPUT_NAME})

examples/pico_motor_shim/song/demo.cpp

@@ -0,0 +1,127 @@
+#include <stdio.h>
+#include "pico_motor_shim.hpp"
+
+#include "common/pimoroni_common.hpp"
+#include "motor.hpp"
+#include "button.hpp"
+
+/*
+Play a song using a motor! Works by setting the PWM duty cycle to 50% and changing the frequency on the fly.
+Plug a motor into connector 1, and press "A" to start the song playing (does not loop). Press the button again will stop the song early.
+*/
+
+using namespace pimoroni;
+using namespace motor;
+
+// List frequencies (in hz) to play in sequence here. Use zero for when silence or a pause is wanted
+// Song from PicoExplorer noise.py
+constexpr float SONG[] = {1397, 1397, 1319, 1397, 698,  0,    698,  0,    1047, 932,
+                          880,  1047, 1397, 0,    1397, 0,    1568, 1480, 1568, 784,
+                          0,    784,  0,    1568, 1397, 1319, 1175, 1047, 0,    1047,
+                          0,    1175, 1319, 1397, 1319, 1175, 1047, 1175, 1047, 932,
+                          880,  932,  880,  784,  698,  784,  698,  659,  587,  523,
+                          587,  659,  698,  784,  932,  880,  784,  880,  698,  0,    698};
+constexpr uint SONG_LENGTH = sizeof(SONG) / sizeof(float);
+
+//The time (in milliseconds) to play each note for. Change this to make the song play faster or slower
+static const uint NOTE_DURATION_MS = 150;
+
+//Uncomment this lineto have the song be played without the motor turning
+//Note, this will affect the audio quality of the sound produced
+//#define STATIONARY_PLAYBACK
+
+//The time (in microseconds) between each direction switch of the motor when using STATIONARY_PLAYBACK
+static const uint STATIONARY_TOGGLE_US = 2000;
+
+//Uncomment this line to use the fast decay (coasting) motor mode.
+//This seems to produce a louder sound with STATIONARY_PLAYBACK enabled, but will make movement poorer when STATIONARY_PLAYBACK is disabled
+//#define USE_FAST_DECAY
+
+
+Button button_a(pico_motor_shim::BUTTON_A, Polarity::ACTIVE_LOW, 0);
+#ifdef USE_FAST_DECAY
+  Motor motor_1(pico_motor_shim::MOTOR_1, Motor::DEFAULT_PWM_FREQUENCY, Motor::FAST_DECAY);
+  Motor motor_2(pico_motor_shim::MOTOR_2, Motor::DEFAULT_PWM_FREQUENCY, Motor::FAST_DECAY);
+#else
+  Motor motor_1(pico_motor_shim::MOTOR_1, Motor::DEFAULT_PWM_FREQUENCY, Motor::SLOW_DECAY);
+  Motor motor_2(pico_motor_shim::MOTOR_2, Motor::DEFAULT_PWM_FREQUENCY, Motor::SLOW_DECAY);
+#endif
+
+static bool button_toggle = false;
+
+/**
+ * Checks if the button has been pressed, toggling a value that is also returned.
+ */
+bool check_button_toggle() {
+  bool button_pressed = button_a.read();
+  if(button_pressed) {
+    button_toggle = !button_toggle;
+  }
+  return button_toggle;
+}
+
+/**
+ * The entry point of the program.
+ */
+int main() {
+  stdio_init_all();
+
+  //Initialise the LED. We use this to indicate that the sequence is running.
+  gpio_init(PICO_DEFAULT_LED_PIN);
+  gpio_set_dir(PICO_DEFAULT_LED_PIN, GPIO_OUT);
+
+  //Initialise the motor
+  if(!motor_1.init() || !motor_2.init()) {
+    printf("Cannot initialise the motors. Check the provided parameters\n");
+    return 0;
+  }
+
+  while(true) {
+    if(check_button_toggle()) {
+      //Turn the Pico's LED on to show that the song has started
+      gpio_put(PICO_DEFAULT_LED_PIN, true);
+
+      //Play the song
+      for(uint i = 0; i < SONG_LENGTH && check_button_toggle(); i++) {
+        if(motor_1.set_frequency(SONG[i]) && motor_2.set_frequency(SONG[i])) {
+          #ifdef STATIONARY_PLAYBACK
+            //Set the motors to 50% duty cycle to play the note, but alternate
+            //the direction so that the motor does not actually spin
+            uint t = 0;
+            while(t < NOTE_DURATION_MS * 1000) {
+              motor_1.set_speed(0.5f);
+              motor_2.set_speed(0.5f);
+              sleep_us(STATIONARY_TOGGLE_US);
+              t += STATIONARY_TOGGLE_US;
+
+              motor_1.set_speed(-0.5f);
+              motor_2.set_speed(-0.5f);
+              sleep_us(STATIONARY_TOGGLE_US);
+              t += STATIONARY_TOGGLE_US;
+            }
+          #else
+            //Set the motors to 50% duty cycle to play the note
+            motor_1.set_speed(0.5f);
+            motor_2.set_speed(0.5f);
+            sleep_ms(NOTE_DURATION_MS);
+          #endif
+        }
+        else {
+          //The frequency was invalid, so we are treating that to mean this is a pause note
+          motor_1.stop();
+          motor_2.stop();
+          sleep_ms(NOTE_DURATION_MS);
+        }
+      }
+      button_toggle = false;
+
+      //The song has finished, so turn off the Pico's LED and disable the motors
+      gpio_put(PICO_DEFAULT_LED_PIN, false);
+      motor_1.disable();
+      motor_2.disable();
+    }
+
+    sleep_ms(10);
+  }
+  return 0;
+}

libraries/CMakeLists.txt

@@ -25,8 +25,10 @@ add_subdirectory(pico_display_2)
 add_subdirectory(pico_unicorn)
 add_subdirectory(pico_scroll)
 add_subdirectory(pico_explorer)
+add_subdirectory(pico_motor_shim)
 add_subdirectory(pico_rgb_keypad)
 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,73 @@
+#pragma once
+#include "pico/stdlib.h"
+
+#include "ws2812.hpp"
+#include "motor2.hpp"
+#include "motor_cluster.hpp"
+
+namespace motor {
+  namespace motor2040 {
+    const uint MOTOR_A_P = 4;
+    const uint MOTOR_A_N = 5;
+    const uint MOTOR_B_P = 6;
+    const uint MOTOR_B_N = 7;
+    const uint MOTOR_C_P = 8;
+    const uint MOTOR_C_N = 9;
+    const uint MOTOR_D_P = 10;
+    const uint MOTOR_D_N = 11;
+
+    const pin_pair MOTOR_A(MOTOR_A_P, MOTOR_A_N);
+    const pin_pair MOTOR_B(MOTOR_B_P, MOTOR_B_N);
+    const pin_pair MOTOR_C(MOTOR_C_P, MOTOR_C_N);
+    const pin_pair MOTOR_D(MOTOR_D_P, MOTOR_D_N);
+    const uint NUM_MOTORS = 4;
+
+    const uint ENCODER_A_A = 0;
+    const uint ENCODER_A_B = 1;
+    const uint ENCODER_B_A = 2;
+    const uint ENCODER_B_B = 3;
+    const uint ENCODER_C_A = 12;
+    const uint ENCODER_C_B = 13;
+    const uint ENCODER_D_A = 14;
+    const uint ENCODER_D_B = 15;
+
+    const pin_pair ENCODER_A(ENCODER_A_A, ENCODER_A_B);
+    const pin_pair ENCODER_B(ENCODER_B_A, ENCODER_B_B);
+    const pin_pair ENCODER_C(ENCODER_C_A, ENCODER_C_B);
+    const pin_pair ENCODER_D(ENCODER_D_A, ENCODER_D_B);
+    const uint NUM_ENCODERS = 4;
+
+    const uint TX_TRIG = 16;
+    const uint RX_ECHO = 17;
+
+    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 CURRENT_SENSE_A_ADDR = 0b000;
+    const uint CURRENT_SENSE_B_ADDR = 0b001;
+    const uint CURRENT_SENSE_C_ADDR = 0b010;
+    const uint CURRENT_SENSE_D_ADDR = 0b011;
+    const uint VOLTAGE_SENSE_ADDR = 0b100;
+    const uint FAULT_SENSE_ADDR = 0b101;
+
+    const uint SENSOR_1_ADDR = 0b110;
+    const uint SENSOR_2_ADDR = 0b111;
+    const uint NUM_SENSORS = 2;
+
+    constexpr float SHUNT_RESISTOR = 0.47f;
+    constexpr float CURRENT_GAIN = 5;
+    constexpr float VOLTAGE_GAIN = 3.9f / 13.9f;
+    constexpr float CURRENT_OFFSET = -0.02f;
+  }
+}

libraries/pico_motor_shim/CMakeLists.txt

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

libraries/pico_motor_shim/pico_motor_shim.cmake

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

libraries/pico_motor_shim/pico_motor_shim.hpp

@@ -0,0 +1,19 @@
+#pragma once
+#include "pico/stdlib.h"
+
+#include "motor2.hpp"
+
+namespace motor {
+  namespace pico_motor_shim {
+    const uint BUTTON_A = 2;
+
+    const uint MOTOR_1_P = 6;
+    const uint MOTOR_1_N = 7;
+    const uint MOTOR_2_P = 27;
+    const uint MOTOR_2_N = 26;
+
+    const pin_pair MOTOR_1(MOTOR_1_P, MOTOR_1_N);
+    const pin_pair MOTOR_2(MOTOR_2_P, MOTOR_2_N);
+    const uint NUM_MOTORS = 2;
+  }
+}

micropython/examples/motor2040/README.md

@@ -0,0 +1,85 @@
+# Motor 2040 Micropython Examples <!-- omit in toc -->
+
+- [Motor Examples](#motor-examples)
+  - [Single Motor](#single-motor)
+  - [Multiple Motors](#multiple-motors)
+  - [Motor Cluster](#motor-cluster)
+  - [Simple Easing](#simple-easing)
+  - [Motor Wave](#motor-wave)
+  - [Calibration](#calibration)
+- [Function Examples](#function-examples)
+  - [Read Sensors](#read-sensors)
+  - [Sensor Feedback](#sensor-feedback)
+  - [Current Meter](#current-meter)
+  - [LED Rainbow](#led-rainbow)
+  - [Turn Off LEDs](#turn-off-leds)
+
+
+## Motor Examples
+
+### Single Motor
+[single_motor.py](single_motor.py)
+
+Demonstrates how to create a Motor object and control it.
+
+
+### Multiple Motors
+[multiple_motors.py](multiple_motors.py)
+
+Demonstrates how to create multiple Motor objects and control them together.
+
+
+### Motor Cluster
+[motor_cluster.py](motor_cluster.py)
+
+Demonstrates how to create a MotorCluster object to control multiple motors at once.
+
+
+### Simple Easing
+[simple_easing.py](simple_easing.py)
+
+An example of how to move a motor smoothly between random positions.
+
+
+### Motor Wave
+[motor_wave.py](motor_wave.py)
+
+An example of applying a wave pattern to a group of motors and the LEDs.
+
+
+### Calibration
+[calibration.py](calibration.py)
+
+Shows how to create motors with different common calibrations, modify a motor's existing calibration, and create a motor with a custom calibration.
+
+
+## Function Examples
+
+### Read Sensors
+[read_sensors.py](read_sensors.py)
+
+Shows how to initialise and read the 6 external and 2 internal sensors of Motor 2040.
+
+
+### Sensor Feedback
+[sensor_feedback.py](sensor_feedback.py)
+
+Show how to read the 6 external sensors and display their values on the neighbouring LEDs.
+
+
+### Current Meter
+[current_meter.py](current_meter.py)
+
+An example of how to use Motor 2040's current measuring ability and display the value on the onboard LED bar.
+
+
+### LED Rainbow
+[led_rainbow.py](led_rainbow.py)
+
+Displays a rotating rainbow pattern on the Motor 2040's onboard LED.
+
+
+### Turn Off LED
+[turn_off_led.py](turn_off_led.py)
+
+A simple program that turns off the onboard LED.

micropython/examples/motor2040/current_meter.py

@@ -0,0 +1,90 @@
+import gc
+import time
+from machine import Pin
+from pimoroni import Analog, AnalogMux, Button
+from plasma import WS2812
+from servo import ServoCluster, servo2040
+
+"""
+An example of how to use Servo 2040's current measuring
+ability and display the value on the onboard LED bar.
+
+Press "Boot" to exit the program.
+
+NOTE: ServoCluster and Plasma WS2812 use the RP2040's PIO system,
+and as such may have problems when running code multiple times.
+If you encounter issues, try resetting your board.
+"""
+
+BRIGHTNESS = 0.4      # The brightness of the LEDs
+UPDATES = 50          # How many times to update LEDs and Servos per second
+MAX_CURRENT = 3.0     # The maximum current, in amps, to show on the meter
+SAMPLES = 50          # The number of current measurements to take per reading
+TIME_BETWEEN = 0.001  # The time between each current measurement
+
+# Free up hardware resources ahead of creating a new ServoCluster
+gc.collect()
+
+# Create a servo cluster for pins 0 to 7, using PIO 0 and State Machine 0
+START_PIN = servo2040.SERVO_1
+END_PIN = servo2040.SERVO_8
+servos = ServoCluster(pio=0, sm=0, pins=list(range(START_PIN, END_PIN + 1)))
+
+# Set up the shared analog inputs
+cur_adc = Analog(servo2040.SHARED_ADC, servo2040.CURRENT_GAIN,
+                 servo2040.SHUNT_RESISTOR, servo2040.CURRENT_OFFSET)
+
+# Set up the analog multiplexer, including the pin for controlling pull-up/pull-down
+mux = AnalogMux(servo2040.ADC_ADDR_0, servo2040.ADC_ADDR_1, servo2040.ADC_ADDR_2,
+                muxed_pin=Pin(servo2040.SHARED_ADC))
+
+# Create the LED bar, using PIO 1 and State Machine 0
+led_bar = WS2812(servo2040.NUM_LEDS, 1, 0, servo2040.LED_DATA)
+
+# Create the user button
+user_sw = Button(servo2040.USER_SW)
+
+# Start updating the LED bar
+led_bar.start()
+
+# Enable all servos (this puts them at the middle).
+# The servos are not going to be moved, but are activated to give a current draw
+servos.enable_all()
+
+# Read sensors until the user button is pressed
+while user_sw.raw() is not True:
+
+    # Select the current sense
+    mux.select(servo2040.CURRENT_SENSE_ADDR)
+
+    # Read the current sense several times and average the result
+    current = 0
+    for i in range(SAMPLES):
+        current += cur_adc.read_current()
+        time.sleep(TIME_BETWEEN)
+    current /= SAMPLES
+
+    # Print out the current sense value
+    print("Current =", round(current, 4))
+
+    # Convert the current to a percentage of the maximum we want to show
+    percent = (current / MAX_CURRENT)
+
+    # Update all the LEDs
+    for i in range(servo2040.NUM_LEDS):
+        # Calculate the LED's hue, with Red for high currents and Green for low
+        hue = (1.0 - i / (servo2040.NUM_LEDS - 1)) * 0.333
+
+        # Calculate the current level the LED represents
+        level = (i + 0.5) / servo2040.NUM_LEDS
+        # If the percent is above the level, light the LED, otherwise turn it off
+        if percent >= level:
+            led_bar.set_hsv(i, hue, 1.0, BRIGHTNESS)
+        else:
+            led_bar.set_hsv(i, hue, 1.0, 0.0)
+
+# Disable the servos
+servos.disable_all()
+
+# Turn off the LED bar
+led_bar.clear()

micropython/examples/motor2040/led_rainbow.py

@@ -0,0 +1,43 @@
+import time
+from pimoroni import Button
+from plasma import WS2812
+from motor import motor2040
+
+"""
+Displays a rotating rainbow pattern on the Motor 2040's onboard LED.
+
+Press "Boot" to exit the program.
+
+NOTE: Plasma WS2812 uses the RP2040's PIO system, and as
+such may have problems when running code multiple times.
+If you encounter issues, try resetting your board.
+"""
+
+SPEED = 5           # The speed that the LEDs will cycle at
+BRIGHTNESS = 0.4    # The brightness of the LEDs
+UPDATES = 50        # How many times the LEDs will be updated per second
+
+# Create the LED, using PIO 1 and State Machine 0
+led = WS2812(motor2040.NUM_LEDS, 1, 0, motor2040.LED_DATA)
+
+# Create the user button
+user_sw = Button(motor2040.USER_SW)
+
+# Start updating the LED
+led.start()
+
+
+hue = 0.0
+
+# Make rainbows until the user button is pressed
+while user_sw.raw() is not True:
+
+    hue += SPEED / 1000.0
+
+    # Update the LED
+    led.set_hsv(0, hue, 1.0, BRIGHTNESS)
+
+    time.sleep(1.0 / UPDATES)
+
+# Turn off the LED
+led.clear()

micropython/examples/motor2040/motor_cluster.py

@@ -0,0 +1,60 @@
+import gc
+import time
+import math
+from servo import ServoCluster, servo2040
+
+"""
+Demonstrates how to create a ServoCluster object to control multiple servos at once.
+
+NOTE: ServoCluster uses the RP2040's PIO system, and as
+such may have problems when running code multiple times.
+If you encounter issues, try resetting your board.
+"""
+
+# Free up hardware resources ahead of creating a new ServoCluster
+gc.collect()
+
+# Create a servo cluster for pins 0 to 3, using PIO 0 and State Machine 0
+START_PIN = servo2040.SERVO_1
+END_PIN = servo2040.SERVO_4
+servos = ServoCluster(pio=0, sm=0, pins=list(range(START_PIN, END_PIN + 1)))
+
+# Enable all servos (this puts them at the middle)
+servos.enable_all()
+time.sleep(2)
+
+# Go to min
+servos.all_to_min()
+time.sleep(2)
+
+# Go to max
+servos.all_to_max()
+time.sleep(2)
+
+# Go back to mid
+servos.all_to_mid()
+time.sleep(2)
+
+SWEEPS = 3              # How many sweeps of the servo to perform
+STEPS = 10              # The number of discrete sweep steps
+STEPS_INTERVAL = 0.5    # The time in seconds between each step of the sequence
+SWEEP_EXTENT = 90.0     # How far from zero to move the servos when sweeping
+
+# Do a sine sweep
+for j in range(SWEEPS):
+    for i in range(360):
+        value = math.sin(math.radians(i)) * SWEEP_EXTENT
+        servos.all_to_value(value)
+        time.sleep(0.02)
+
+# Do a stepped sweep
+for j in range(SWEEPS):
+    for i in range(0, STEPS):
+        servos.all_to_percent(i, 0, STEPS, 0.0 - SWEEP_EXTENT, SWEEP_EXTENT)
+        time.sleep(STEPS_INTERVAL)
+    for i in range(0, STEPS):
+        servos.all_to_percent(i, STEPS, 0, 0.0 - SWEEP_EXTENT, SWEEP_EXTENT)
+        time.sleep(STEPS_INTERVAL)
+
+# Disable the servos
+servos.disable_all()

micropython/examples/motor2040/motor_wave.py

@@ -0,0 +1,64 @@
+import gc
+import time
+import math
+from pimoroni import Button
+from plasma import WS2812
+from motor import Motor, MotorCluster, motor2040
+
+"""
+An example of applying a wave pattern to a group of motors and the LED.
+
+Press "Boot" to exit the program.
+
+NOTE: MotorCluster and Plasma WS2812 use the RP2040's PIO system,
+and as such may have problems when running code multiple times.
+If you encounter issues, try resetting your board.
+"""
+
+SPEED = 5             # The speed that the LEDs will cycle at
+BRIGHTNESS = 0.4      # The brightness of the LEDs
+UPDATES = 50          # How many times to update LEDs and Motors per second
+MOTOR_EXTENT = 1.0    # How far from zero to move the motors
+
+# Free up hardware resources ahead of creating a new MotorCluster
+gc.collect()
+
+# Create a motor cluster for pins 0 to 7, using PIO 0 and State Machine 0
+# motors = MotorCluster(pio=0, sm=0, pins=list(range(START_PIN, END_PIN + 1)))
+MOTOR_PINS = [ motor2040.MOTOR_1, motor2040.MOTOR_2, motor2040.MOTOR_3, motor2040.MOTOR_4]
+motors = [Motor(pins) for pins in MOTOR_PINS]
+
+# Create the LED, using PIO 1 and State Machine 0
+led = WS2812(motor2040.NUM_LEDS, 1, 0, motor2040.LED_DATA)
+
+# Create the user button
+user_sw = Button(motor2040.USER_SW)
+
+# Start updating the LED
+led.start()
+
+
+offset = 0.0
+
+# Make waves until the user button is pressed
+while user_sw.raw() is not True:
+
+    offset += SPEED / 1000.0
+
+    # Update the LED
+    led.set_hsv(0, offset / 2, 1.0, BRIGHTNESS)
+
+    # Update all the MOTORs
+    #for i in range(motors.count()):
+    for i in range(len(motors)):
+        angle = ((i / len(motors)) + offset) * math.pi
+        motors[i].speed(math.sin(angle) * MOTOR_EXTENT)
+        
+    time.sleep(1.0 / UPDATES)
+
+# Stop all the motors
+for m in motors:
+    m.disable()
+
+# Turn off the LED bar
+led.clear()

micropython/examples/motor2040/multiple_motors.py

@@ -0,0 +1,59 @@
+import time
+import math
+from motor import Motor, motor2040
+
+"""
+Demonstrates how to create multiple Motor objects and control them together.
+"""
+
+# Create a list of motors
+MOTOR_PINS = [ motor2040.MOTOR_1, motor2040.MOTOR_2, motor2040.MOTOR_3, motor2040.MOTOR_4]
+motors = [Motor(pins) for pins in MOTOR_PINS]
+
+# Enable all motors (this puts them at the middle)
+for m in motors:
+    m.enable()
+time.sleep(2)
+
+# Go to min
+for m in motors:
+    m.full_positive()
+time.sleep(2)
+
+# Go to max
+for m in motors:
+    m.full_negative()
+time.sleep(2)
+
+# Go back to mid
+for m in motors:
+    m.stop()
+time.sleep(2)
+
+SWEEPS = 3              # How many sweeps of the motor to perform
+STEPS = 10              # The number of discrete sweep steps
+STEPS_INTERVAL = 0.5    # The time in seconds between each step of the sequence
+SPEED_EXTENT = 1.0      # How far from zero to move the motor when sweeping
+
+# Do a sine sweep
+for j in range(SWEEPS):
+    for i in range(360):
+        speed = math.sin(math.radians(i)) * SPEED_EXTENT
+        for s in motors:
+            s.speed(speed)
+        time.sleep(0.02)
+
+# Do a stepped sweep
+for j in range(SWEEPS):
+    for i in range(0, STEPS):
+        for m in motors:
+            m.to_percent(i, 0, STEPS, 0.0 - SPEED_EXTENT, SPEED_EXTENT)
+        time.sleep(STEPS_INTERVAL)
+    for i in range(0, STEPS):
+        for m in motors:
+            m.to_percent(i, STEPS, 0, 0.0 - SPEED_EXTENT, SPEED_EXTENT)
+        time.sleep(STEPS_INTERVAL)
+
+# Disable the motors
+for m in motors:
+    m.disable()

micropython/examples/motor2040/read_sensors.py

@@ -0,0 +1,48 @@
+import time
+from machine import Pin
+from pimoroni import Analog, AnalogMux, Button
+from servo import servo2040
+
+"""
+Shows how to initialise and read the 6 external
+and 2 internal sensors of Servo 2040.
+
+Press "Boot" to exit the program.
+"""
+
+# Set up the shared analog inputs
+sen_adc = Analog(servo2040.SHARED_ADC)
+vol_adc = Analog(servo2040.SHARED_ADC, servo2040.VOLTAGE_GAIN)
+cur_adc = Analog(servo2040.SHARED_ADC, servo2040.CURRENT_GAIN,
+                 servo2040.SHUNT_RESISTOR, servo2040.CURRENT_OFFSET)
+
+# Set up the analog multiplexer, including the pin for controlling pull-up/pull-down
+mux = AnalogMux(servo2040.ADC_ADDR_0, servo2040.ADC_ADDR_1, servo2040.ADC_ADDR_2,
+                muxed_pin=Pin(servo2040.SHARED_ADC))
+
+# Set up the sensor addresses and have them pulled down by default
+sensor_addrs = list(range(servo2040.SENSOR_1_ADDR, servo2040.SENSOR_6_ADDR + 1))
+for addr in sensor_addrs:
+    mux.configure_pull(addr, Pin.PULL_DOWN)
+
+# Create the user button
+user_sw = Button(servo2040.USER_SW)
+
+
+# Read sensors until the user button is pressed
+while user_sw.raw() is not True:
+
+    # Read each sensor in turn and print its voltage
+    for i in range(len(sensor_addrs)):
+        mux.select(sensor_addrs[i])
+        print("S", i + 1, " = ", round(sen_adc.read_voltage(), 3), sep="", end=", ")
+
+    # Read the voltage sense and print the value
+    mux.select(servo2040.VOLTAGE_SENSE_ADDR)
+    print("Voltage =", round(vol_adc.read_voltage(), 4), end=", ")
+
+    # Read the current sense and print the value
+    mux.select(servo2040.CURRENT_SENSE_ADDR)
+    print("Current =", round(cur_adc.read_current(), 4))
+
+    time.sleep(0.5)

micropython/examples/motor2040/sensor_feedback.py

@@ -0,0 +1,58 @@
+import time
+from machine import Pin
+from pimoroni import Analog, AnalogMux, Button
+from plasma import WS2812
+from servo import servo2040
+
+"""
+Show how to read the 6 external sensors and
+display their values on the neighbouring LEDs.
+
+Press "Boot" to exit the program.
+
+NOTE: Plasma WS2812 uses the RP2040's PIO system, and as
+such may have problems when running code multiple times.
+If you encounter issues, try resetting your board.
+"""
+
+BRIGHTNESS = 0.4      # The brightness of the LEDs
+UPDATES = 50          # How many times to update LEDs and Servos per second
+
+# Set up the shared analog inputs
+sen_adc = Analog(servo2040.SHARED_ADC)
+
+# Set up the analog multiplexer, including the pin for controlling pull-up/pull-down
+mux = AnalogMux(servo2040.ADC_ADDR_0, servo2040.ADC_ADDR_1, servo2040.ADC_ADDR_2,
+                muxed_pin=Pin(servo2040.SHARED_ADC))
+
+# Set up the sensor addresses and have them pulled down by default
+sensor_addrs = list(range(servo2040.SENSOR_1_ADDR, servo2040.SENSOR_6_ADDR + 1))
+for addr in sensor_addrs:
+    mux.configure_pull(addr, Pin.PULL_DOWN)
+
+# Create the LED bar, using PIO 1 and State Machine 0
+led_bar = WS2812(servo2040.NUM_LEDS, 1, 0, servo2040.LED_DATA)
+
+# Create the user button
+user_sw = Button(servo2040.USER_SW)
+
+# Start updating the LED bar
+led_bar.start()
+
+
+# Read sensors until the user button is pressed
+while user_sw.raw() is not True:
+
+    # Read each sensor in turn and print its voltage
+    for i in range(len(sensor_addrs)):
+        mux.select(sensor_addrs[i])
+        sensor_voltage = sen_adc.read_voltage()
+
+        # Calculate the LED's hue, with Green for high voltages and Blue for low
+        hue = (2.0 - (sensor_voltage / 3.3)) * 0.333
+        led_bar.set_hsv(i, hue, 1.0, BRIGHTNESS)
+
+        print("S", i + 1, " = ", round(sensor_voltage, 3), sep="", end=", ")
+    print()
+
+    time.sleep(1.0 / UPDATES)

micropython/examples/motor2040/simple_easing.py

@@ -0,0 +1,64 @@
+import time
+import math
+import random
+from pimoroni import Button
+from servo import Servo, servo2040
+
+"""
+An example of how to move a servo smoothly between random positions.
+
+Press "Boot" to exit the program.
+"""
+
+UPDATES = 50            # How many times to update Servos per second
+TIME_FOR_EACH_MOVE = 2  # The time to travel between each random value
+UPDATES_PER_MOVE = TIME_FOR_EACH_MOVE * UPDATES
+
+SERVO_EXTENT = 80       # How far from zero to move the servo
+USE_COSINE = True       # Whether or not to use a cosine path between values
+
+# Create a servo on pin 0
+s = Servo(servo2040.SERVO_1)
+
+# Get the initial value and create a random end value between the extents
+start_value = s.mid_value()
+end_value = random.uniform(-SERVO_EXTENT, SERVO_EXTENT)
+
+# Create the user button
+user_sw = Button(servo2040.USER_SW)
+
+
+update = 0
+
+# Continually move the servo until the user button is pressed
+while user_sw.raw() is not True:
+
+    # Calculate how far along this movement to be
+    percent_along = update / UPDATES_PER_MOVE
+
+    if USE_COSINE:
+        # Move the servo between values using cosine
+        s.to_percent(math.cos(percent_along * math.pi), 1.0, -1.0, start_value, end_value)
+    else:
+        # Move the servo linearly between values
+        s.to_percent(percent_along, 0.0, 1.0, start_value, end_value)
+
+    # Print out the value the servo is now at
+    print("Value = ", round(s.value(), 3), sep="")
+
+    # Move along in time
+    update += 1
+
+    # Have we reached the end of this movement?
+    if update >= UPDATES_PER_MOVE:
+        # Reset the counter
+        update = 0
+
+        # Set the start as the last end and create a new random end value
+        start_value = end_value
+        end_value = random.uniform(-SERVO_EXTENT, SERVO_EXTENT)
+
+    time.sleep(1.0 / UPDATES)
+
+# Disable the servo
+s.disable()

micropython/examples/motor2040/single_motor.py

@@ -0,0 +1,49 @@
+import time
+import math
+from motor import Motor, motor2040
+
+"""
+Demonstrates how to create a Motor object and control it.
+"""
+
+# Create a motor on pins 4 and 5
+m = Motor(motor2040.MOTOR_1)
+
+# Enable the motor (this puts it at the middle)
+m.enable()
+time.sleep(2)
+
+# Go to full positive
+m.full_positive()
+time.sleep(2)
+
+# Go to full negative
+m.full_negative()
+time.sleep(2)
+
+# Stop moving
+m.stop()
+time.sleep(2)
+
+
+SWEEPS = 3              # How many sweeps of the motor to perform
+STEPS = 10              # The number of discrete sweep steps
+STEPS_INTERVAL = 0.5    # The time in seconds between each step of the sequence
+
+# Do a sine sweep
+for j in range(SWEEPS):
+    for i in range(360):
+        m.speed(math.sin(math.radians(i)))
+        time.sleep(0.02)
+
+# Do a stepped sweep
+for j in range(SWEEPS):
+    for i in range(0, STEPS):
+        m.to_percent(i, 0, STEPS)
+        time.sleep(STEPS_INTERVAL)
+    for i in range(0, STEPS):
+        m.to_percent(i, STEPS, 0)
+        time.sleep(STEPS_INTERVAL)
+
+# Disable the motor
+m.disable()

micropython/examples/motor2040/turn_off_led.py

@@ -0,0 +1,16 @@
+from plasma import WS2812
+from motor import motor2040
+
+"""
+A simple program that turns off the onboard LED.
+
+NOTE: Plasma WS2812 uses the RP2040's PIO system, and as
+such may have problems when running code multiple times.
+If you encounter issues, try resetting your board.
+"""
+
+# Create the LED, using PIO 1 and State Machine 0
+led = WS2812(motor2040.NUM_LEDS, 1, 0, motor2040.LED_DATA)
+
+# Start updating the LED
+led.start()

micropython/examples/motor2040/turn_off_motors.py

@@ -0,0 +1,13 @@
+from motor import Motor, motor2040
+
+"""
+A simple program that turns off the motors.
+"""
+
+# Create four motor objects.
+# This will initialise the pins, stopping any
+# previous movement that may be stuck on
+m1 = Motor(motor2040.MOTOR_1)
+m2 = Motor(motor2040.MOTOR_2)
+m3 = Motor(motor2040.MOTOR_3)
+m4 = Motor(motor2040.MOTOR_4)

micropython/modules/micropython.cmake

@@ -36,14 +36,17 @@ include(pico_unicorn/micropython)
 include(pico_display/micropython)
 include(pico_display_2/micropython)
 include(pico_explorer/micropython)
+include(pico_motor_shim/micropython)
 include(pico_wireless/micropython)
 
 include(bitmap_fonts/micropython)
 
 include(plasma/micropython)
 include(hub75/micropython)
+include(pwm/micropython)
 include(servo/micropython)
 include(encoder/micropython)
+include(motor/micropython)
 include(ulab/code/micropython)
 include(qrcode/micropython/micropython)
 

micropython/modules/motor/README.md

@@ -0,0 +1,727 @@
+# Motors and Motor 2040 <!-- omit in toc -->
+
+The Motor library lets you drive DC motors from a Raspberry Pi Pico or any other RP2040-based board, such as the [Pimoroni Motor 2040](https://pimoroni.com/servo2040).
+
+This library offers two motor implementations:
+* a `Motor` class that uses hardware PWM to drive a single motor, with support for up to 8 motors.
+* a `MotorCluster` class that uses Programmable IO (PIO) hardware to drive up to 15 servos.
+
+
+## Table of Content
+- [Motor 2040](#motor-2040)
+  - [Reading the User Button](#reading-the-user-button)
+  - [Reading the Sensors](#reading-the-sensors)
+    - [Fault Sensing and Configuring Pulls](#fault-sensing-and-configuring-pulls)
+  - [Controlling the LED](#controlling-the-led)
+  - [Pin Constants](#pin-constants)
+    - [Motor Pin Tuples](#motor-pin-tuples)
+    - [Encoder Pin Tuples](#encoder-pin-tuples)
+    - [LED Pin](#led-pin)
+    - [I2C Pins](#i2c-pins)
+    - [Button Pin](#button-pin)
+    - [Address Pins](#address-pins)
+    - [ADC Pins](#adc-pins)
+  - [Other Constants](#other-constants)
+    - [Counts](#counts)
+    - [Addresses](#addresses)
+    - [Sensing](#sensing)
+- [Motor](#motor)
+  - [Getting Started](#getting-started)
+  - [Control by Speed](#control-by-speed)
+  - [Control by Percent](#control-by-percent)
+  - [Control by Duty Cycle](#control-by-duty-cycle)
+  - [Frequency Control](#frequency-control)
+  - [Configuration](#configuration)
+  - [Function Reference](#function-reference)
+  - [PWM Limitations](#pwm-limitations)
+- [MotorCluster](#motorcluster)
+  - [Getting Started](#getting-started-1)
+  - [Control by Speed](#control-by-speed-1)
+  - [Control by Percent](#control-by-percent-1)
+  - [Control by Duty Cycle](#control-by-duty-cycle-1)
+  - [Frequency Control](#frequency-control-1)
+  - [Phase Control](#phase-control)
+  - [Configuration](#configuration-1)
+  - [Delayed Loading](#delayed-loading)
+  - [Function Reference](#function-reference-1)
+  - [PIO Limitations](#pio-limitations)
+
+
+## Motor 2040
+
+Motor 2040 is a **standalone motor controller** for driving DC motors with encoder feedback. It has JST-SH connectors for plugging in up to 4 motors, with additional bells and whistles like sensor headers, current monitoring, an RGB LED, and a user button that make it ideal for many robotics projects!
+
+For more information on this board, check out the store page: [pimoroni.com/motor2040](https://pimoroni.com/motor2040).
+
+
+### Reading the User Button
+
+As is the case for many of Pimoroni's RP2040-based boards, the boot button on Motor 2040 that is used for programming also acts as a user button once a program is running. To simplify the use of this and other buttons, the `pimoroni` module contains a `Button` class that handles button debounce and auto-repeat.
+
+```python
+Button(button, invert=True, repeat_time=200, hold_time=1000)
+```
+
+To set up the user button, first import the `Button` class from the `pimoroni` module and the pin constant for the button from `motor`:
+```python
+from pimoroni import Button
+from motor import motor2040
+```
+
+Then create an instance of `Button` for the user button:
+```python
+user_sw = Button(motor2040.USER_SW)
+```
+
+To get the button state, call `.read()`. If the button is held down, then this will return `True` at the interval specified by `repeat_time` until `hold_time` is reached, at which point it will return `True` every `repeat_time / 3` milliseconds. This is useful for rapidly increasing/decreasing values:
+
+```python
+state = user_sw.read()
+```
+
+It is also possible to read the raw button state without the repeat feature, if you prefer:
+```python
+state = user_sw.raw()
+```
+
+
+### Reading the Sensors
+
+On the right-hand edge of Motor 2040 are two analog inputs, with neighbouring 3.3V and GND. These let you connect up sensors to enable your mechanical creations to sense how they are interacting with the world. For example, a pair of analog proximity sensors could be hooked up for wall avoidance or line following, or they could have microswitches wired to report when a motor driven mechanism has reached an end-stop.
+
+Motor 2040 also has six internal sensors:
+* A voltage sensor, letting you measure the supply voltage to the motors.
+* Four current sensors, letting you measure how much current each motor is drawing.
+* A fault sensor, letting you know if there is an issue with one of more of your motors
+These could be used just for monitoring, or as the trigger to turn off motors safely when voltage gets too low or current draw gets too high.
+
+To allow for all of these inputs, Motor 2040 has an onboard analog multiplexer that expands a single analog pin into eight, by use of three digital address pins that select a single input at a time. As such, the setup for these sensors is more involved than it would be to just read eight analog pins directly.
+
+To begin reading these inputs, first import the `Analog` and `AnalogMux` classes from `pimoroni` and the pin, address, and gain constants from `servo`:
+
+```python
+from pimoroni import Analog
+from motor import motor2040
+```
+
+Then set up three instances of `Analog` for the sensor and fault, voltage, and current sensing:
+
+```python
+sen_fault_adc = Analog(motor2040.SHARED_ADC)
+vol_adc = Analog(motor2040.SHARED_ADC, motor2040.VOLTAGE_GAIN)
+cur_adc = Analog(motor2040.SHARED_ADC, motor2040.CURRENT_GAIN,
+                 motor2040.SHUNT_RESISTOR, motor2040.CURRENT_OFFSET)
+```
+
+You may notice, all three of these use the same `SHARED_ADC` pin. This is intentional as it is just a single pin that is being used for all three different functions, only the gains differ.
+
+The next step is to set up the analog multiplexer, by providing it with the three address pins:
+```python
+mux = AnalogMux(motor2040.ADC_ADDR_0, motor2040.ADC_ADDR_1, motor2040.ADC_ADDR_2)
+```
+Note that the `AnalogMux` does not know about any of the `Analog` classes that were created before.
+
+With the multiplexer now configured, reading each sensor is simply a case of 'selecting' the sensor on the multiplexer then reading the value from one of the three `Analog` classes created at the start.
+
+To read the two sensor headers:
+```python
+for addr in range(motor2040.NUM_SENSORS):
+    mux.select(addr + motor2040.SENSOR_1_ADDR)
+    print("Sensor", addr + 1, "=", sen_fault_adc.read_voltage())
+```
+
+To read the voltage sense:
+```python
+mux.select(motor2040.VOLTAGE_SENSE_ADDR)
+print("Voltage =", vol_adc.read_voltage(), "V")
+```
+
+To read the current draw in amps (A):
+```python
+for addr in range(motor2040.NUM_MOTORS):
+    mux.select(addr + motor2040.CURRENT_SENSE_A_ADDR)
+    print("Current", addr + 1, "=", cur_adc.read_current(), "A")
+```
+
+
+#### Configuring Pulls
+
+For some sensors, as well as the internal fault sensor, you may need to have the input be pulled high or low before taking a reading. To support this there is an optional `muxed_pin` parameter that can be passed into the `AnalogMux` when creating it, which gives the multiplexer access to the pin to control the pulls.
+
+```python
+mux = AnalogMux(motor2040.ADC_ADDR_0, motor2040.ADC_ADDR_1, motor2040.ADC_ADDR_2,
+                muxed_pin=Pin(servo2040.SHARED_ADC))
+```
+
+From there the pull state of each of the multiplexer's addresses can be configured independently by calling `.configure_pull()`, with the address and the pull state (either `Pin.PULL_UP`, `Pin.PULL_DOWN`, or `None`).
+
+The below example shows how to set both sensor addresses to have pull-downs:
+```python
+sensor_addrs = list(range(motor2040.SENSOR_1_ADDR, motor2040.SENSOR_2_ADDR + 1))
+for addr in sensor_addrs:
+    mux.configure_pull(addr, Pin.PULL_DOWN)
+```
+
+#### Fault Sensing
+
+The drivers on Motor 2040 can detect when there is a fault with their connected motors, such as if a short occurs, and shut themselves off for safety. As part of this they also signal that a fault has occurred, which can be read. The way they signal this is by pulling the line to ground. This means that the line needs to be high, and so the 'AnalogMux' needs to be configured accordingly.
+
+```python
+adc_as_io = Pin(motor2040.SHARED_ADC)
+mux = AnalogMux(motor2040.ADC_ADDR_0, motor2040.ADC_ADDR_1, motor2040.ADC_ADDR_2,
+                muxed_pin=adc_as_io)
+mux.configure_pull(motor2040.FAULT_SENSE_ADDR, Pin.PULL_UP)
+```
+
+Then in your main code:
+```python
+mux.select(motor2040.FAULT_SENSE_ADDR)
+if not adc_as_io.value():
+    print("Fault!")
+else:
+    print("No Fault")
+```
+//TODO make this neater
+
+
+#### Controlling the LED
+
+Between Motor 2040's for motor connectors is a single addressable RGB LEDs. This works using the same chainable 1-wire signalling as WS2812 LED's, commonly known as Neopixels. As such, it can be controlled using the same Plasma Library used by the [Pimoroni Plasma 2040](https://shop.pimoroni.com/products/plasma-2040).
+
+To set up the LED, first import the `WS2812` class from the `plasma` module and the pin constants for the LED from `motor`:
+```python
+from plasma import WS2812
+from motor import motor2040
+```
+
+Then construct a new `WS2812` instance, specifying the number of LEDs, PIO, PIO state-machine and GPIO pin.
+```python
+led = WS2812(motor2040.NUM_LEDS, 1, 0, motor2040.LED_DATA)
+```
+
+Finally, start the LED by calling `start`:
+```python
+led.start()
+```
+
+For more information on how to control the LED, please refer to the [Plasma Library](https://github.com/pimoroni/pimoroni-pico/tree/main/micropython/modules/plasma).
+
+
+### Pin Constants
+
+The `motor` module contains a `motor2040` sub module with constants for the motor, encoder, LED, sensor and button pins.
+
+
+#### Motor Pin Tuples
+
+* `MOTOR_A` = `(4, 5)`
+* `MOTOR_B` = `(6, 7)`
+* `MOTOR_C` = `(8, 9)`
+* `MOTOR_D` = `(10, 11)`
+
+
+#### Encoder Pin Tuples
+
+* `ENCODER_A` = `(0, 1)`
+* `ENCODER_B` = `(2, 3)`
+* `ENCODER_C` = `(12, 13)`
+* `ENCODER_D` = `(14, 15)`
+
+
+#### UART/ULTRASOUND Pins
+
+* `TX_TRIG` = `16`
+* `RX_ECHO` = `17`
+
+
+#### LED Pin
+
+* `LED_DATA` = `18`
+
+
+#### I2C Pins
+
+* `INT` = `19`
+* `SDA` = `20`
+* `SCL` = `21`
+
+
+#### Button Pin
+
+* `USER_SW` = `23`
+
+
+#### Address Pins
+
+* `ADC_ADDR_0` = `22`
+* `ADC_ADDR_1` = `24`
+* `ADC_ADDR_2` = `25`
+
+
+#### ADC Pins
+
+* `ADC0` = `26`
+* `ADC1` = `27`
+* `ADC2` = `28`
+* `SHARED_ADC` = `29`
+
+
+### Other Constants
+
+The `motor2040` sub module also contains other constants to help with the control of Motor 2040's various features:
+
+
+#### Counts
+
+* `NUM_MOTORS` = `4`
+* `NUM_ENCODERS` = `4`
+* `NUM_SENSORS` = `2`
+* `NUM_LEDS` = `1`
+
+
+#### Addresses
+
+* `CURRENT_SENSE_A_ADDR` = `0b000`
+* `CURRENT_SENSE_B_ADDR` = `0b001`
+* `CURRENT_SENSE_C_ADDR` = `0b010`
+* `CURRENT_SENSE_D_ADDR` = `0b011`
+* `VOLTAGE_SENSE_ADDR` = `0b100`
+* `FAULT_SENSE_ADDR` = `0b101`
+* `SENSOR_1_ADDR` = `0b110`
+* `SENSOR_2_ADDR` = `0b111`
+
+
+#### Sensing
+
+* `VOLTAGE_GAIN` = `0.28058`
+* `SHUNT_RESISTOR` = `0.47`
+* `CURRENT_GAIN` = `5`
+* `CURRENT_OFFSET` = `-0.02`
+
+
+## Motor
+
+### Getting Started
+
+To start using motors with your Motor 2040, you will need to first import the `Motor` class.
+```python
+from motor import Motor, motor2040
+```
+If you are using another RP2040 based board, then `motor2040` can be omitted from the above line
+
+To create your motor, choose which GPIO pins it will be connected to, and pass that into `Motor`. For this example we will use one of the handy constants of the `motor2040`.
+```python
+m = Motor(motor2040.MOTOR_A)
+```
+
+You now have a `Motor` class called `m` that will control the physical motor connected to `MOTOR_A`. To start using this motor, simply enable it using:
+```python
+m.enable()
+```
+
+This activates the motor and sets it to its last known speed. Since this is the first time enabling the motor, there is no last known speed, so instead it will be zero.
+
+Once you have finished with the motor, it can be disabled by calling:
+```python
+m.disable()
+```
+
+From here the motor can be controlled in several ways. These are covered in more detail in the following sections.
+
+
+### Control by Speed
+
+The most intuitive way of controlling a motor is by speed. Speed can be any number that has a real-world meaning for that type of motor, for example revolutions per minute, or the linear or angular speed of the mechanism it is driving. By default the speed is a value ranging from `-1.0` to `1.0` but this can be changed by setting a new `speed_scale`. See [Configuration](#configuration) for more details.
+
+The speed of a motor can be set by calling `.speed(speed)`, which takes a float as its `speed` input. If the motor is disabled, this will enable it. The resulting duty cycle will also be stored.
+
+To read back the current speed of the motor, call `.speed()` without any input. If the motor is disabled, this will be the last speed that was provided when enabled.
+
+At this time the speed of a motor is the same as `m.duty() * m.speed_scale()`, though there could be the option in the future to add a curve to a motor's speed that make the relationship between speed and duty cycle become non-linear.
+
+
+#### Common Speeds
+
+To simplify certain motion patterns, a motor can be commanded to three common speeds. These are, full negative, full positive, and stopped. These are performed by calling `.full_negative()`, `.full_positive()`, and `.stop()`, respectively. If the motor is disabled, these will enable it.
+
+The full negative and full positive speed can be read back using `.speed_scale()`. This can be useful as an input to equations that provide numbers directly to `.speed(speed)`, for example.
+
+
+### Control by Percent
+
+Sometimes there are projects where a motor needs to drive based on the reading from a sensor or another device, but the numbers given out are not easy to convert to speeds the motor accepts. To overcome this the library lets you drive the motor at a percent between its negative and positive speeds, or two speeds provided, based on that input.
+
+With an input between `-1.0` and `1.0`, a motor can be driven at a percent between its negative and positive speeds using `.at_percent(in)`.
+
+With an input between a provided min and max, a motor can be driven at a percent between its negative and positive speeds using `.at_percent(in, in_min, in_max)`.
+
+With an input between a provided min and max, a motor can be driven at a percent between two provided speeds using `.at_percent(in, in_min, value_min, value_max)`.
+
+If the motor is disabled, these will enable it.
+
+
+### Control by Duty Cycle
+
+At a hardware level DC motors operate by receiving a voltage across their two terminals, with positive causing a motion in one direction and negative causing a motion in the other. To avoid needing a negative voltage supply, motor drivers employ a H-Bridge arrangement of transistors or mosfets to flip which side of the motor is connected to ground and which is connected to power. By rapidly turing these transistors or mosfets on and off both the speed and direction of the motor can be varied. The common way this is achieved is by using a pair of pulse width modulated signals, where the duty cycle of the active signal controls the speed, and which signal is active controls the direction. Braking can also be controlled (see //TODO)
+
+The duty cycle of a motor can be set by calling `.duty(duty)`, which takes a float from `-1.0` to `1.0` as its `duty` input. If the motor is disabled this will enable it. This function will also recalculate the related speed.
+
+To read back the current duty cycle of the motor, call `.duty()` without any input. If the motor is disabled, this will be the last duty that was provided when enabled.
+
+At this time the duty cycle of a motor is the same as `m.speed() / m.speed_scale()`, though there could be the option in the future to add a curve to a motor's speed that make the relationship between speed and duty cycle become non-linear.
+
+
+### Frequency Control
+
+Motors can be driven at a variety of frequencies, with a common values being above the range of human hearing. As such this library uses 25KHz as its default, but this can easily be changed.
+
+The frequency (in Hz) of a motor can be set by calling `.frequency(freq)`, which takes a float as its `freq` input. //TODO The supported range of this input is `10` to `350` Hz.
+
+To read back the current frequency (in Hz) of the motor, call `.frequency()` without any input.
+
+Note that changing the frequency does not change the duty cycle sent to the motors, only how frequently pulses are sent.
+
+
+### Configuration
+
+There are several options for configuring a motor. As mentioned in earlier sections, the first is to adjust their speed scale. There is also the option to change their direction, change their decay mode, and add in a dead-zone. This is a percentage of the duty cycle below which the motor will be stopped. This is included to avoid annoying situations where the duty cycle is too low for a motor to move, but enough for it to make an audable sound.
+
+Decay mode is //TODO `.brake()` `.coast()`
+
+
+### Function Reference
+
+Here is the complete list of functions available on the `Motor` class:
+```python
+//TODO
+Motor(pins, calibration=ANGULAR, freq=50)        # calibration can either be an integer or a Calibration class
+pins()
+enable()
+disable()
+is_enabled()
+duty()
+duty(duty)
+speed()
+speed(speed)
+frequency()
+frequency(freq)
+stop()
+coast()
+brake()
+full_negative()
+full_positive()
+at_percent(in)
+at_percent(in, in_min, in_max)
+at_percent(in, in_min, in_max, speed_min, speed_max)
+direction()
+direction(direction)
+speed_scale()
+speed_scale(speed_scale)
+deadzone()
+deadzone(deadzone)
+decay_mode()
+decay_mode(mode)
+```
+
+
+### PWM Limitations
+
+Although the RP2040 is capable of outputting up to 16 PWM signals, there are limitations of which pins can be used together:
+* The PWMs output from pins 0 to 15 are repeated for pins 16 to 29, meaning that those pins share the same signals if PWM is enabled on both. For example if you used pin 3 for PWM and then tried to use pin 19, they would both output the same signal and it would not be possible to control them independently.
+* The 16 PWM channels are grouped into 8 PWM slices, each containing A and B sub channels that are synchronised with each other. This means that parameters such as frequency are shared, which can cause issues if you want one motor to operate at a different frequency to it's pin neighbour or to drive an LED with PWM at a high frequency.
+
+The official [RP2040 datasheet](https://datasheets.raspberrypi.com/rp2040/rp2040-datasheet.pdf), includes the handy _Table 525_ that details the pwm channel for each GPIO pin. This is shown below for convenience:
+
+| GPIO        | 0  | 1  | 2  | 3  | 4  | 5  | 6  | 7  | 8  | 9  | 10 | 11 | 12 | 13 | 14 | 15 |
+|-------------|----|----|----|----|----|----|----|----|----|----|----|----|----|----|----|----|
+| PWM Channel | 0A | 0B | 1A | 1B | 2A | 2B | 3A | 3B | 4A | 4B | 5A | 5B | 6A | 6B | 7A | 7B |
+
+| GPIO        | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 |
+|-------------|----|----|----|----|----|----|----|----|----|----|----|----|----|----|
+| PWM Channel | 0A | 0B | 1A | 1B | 2A | 2B | 3A | 3B | 4A | 4B | 5A | 5B | 6A | 6B |
+
+
+## MotorCluster
+
+### Getting Started
+
+An alternative way of controlling motors with your Motor 2040 is to use the `MotorCluster` class. This can be imported as follows:
+
+```python
+from motor import MotorCluster, motor2040
+```
+(If you are using another RP2040 based board, then `motor2040` can be omitted from the above line)
+
+The next step is to choose which GPIO pins the cluster will be connected to and store them in a `list`. For example, using the handy constants of the `motor2040`, the below line creates the list `[ (4, 5), (6, 7), (8, 9), (10, 11) ]`
+```python
+pins = [ motor2040.MOTOR_A, motor2040.MOTOR_B, motor2040.MOTOR_C, motor2040.MOTOR_D ]
+```
+
+To create your motor cluster, specify the PIO, PIO state-machine and GPIO pins you chose a moment ago, and pass those into `MotorCluster`.
+
+```python
+cluster = MotorCluster(0, 0, pins)
+```
+
+You now have a `MotorCluster` class called `cluster` that will control the physical motors connected to `MOTOR_A`, `MOTOR_B`, `MOTOR_C`, and `MOTOR_D`. To start using these motors, simply enable them using:
+```python
+cluster.enable_all()
+```
+or
+```python
+cluster.enable(motor)
+```
+where `motor` is the motor's number within the cluster from `0` to `cluster.count() - 1`.
+
+These functions activate the motors and drives them at their last known speed. Since this is the first time enabling the motors, there are no last known speeds, so instead they will be set to zero instead.
+
+Once you have finished with the motors, they can be disabled by calling:
+```python
+cluster.disable_all()
+```
+or
+```python
+cluster.disable(motor)
+```
+where `motor` is the motor's number within the cluster from `0` to `cluster.count() - 1`.
+
+From here the motors can be controlled in several ways. These are covered in more detail in the following sections.
+
+
+### Control by Speed
+
+The most intuitive way of controlling a motor is by speed. Speed can be any number that has a real-world meaning for that type of motor, for example revolutions per minute, or the linear or angular speed of the mechanism it is driving. By default the speed is a value ranging from `-1.0` to `1.0` but this can be changed by setting a new `speed_scale`. See [Configuration](#configuration-1) for more details.
+
+The speed of a motor on a cluster can be set calling `.speed(motor, speed)` or `.all_at_speed(speed)`, which take a float as their `speed` input. If a motor is disabled, these will enable it. The resulting duty cycle will also be stored.
+
+To read back the current speed of a motor on the cluster, call `.speed(motor)`. If the motor is disabled, this will be the last speed that was provided when enabled.
+
+
+#### Common Speeds
+
+To simplify certain motion patterns, motors on a cluster can be commanded to three common speeds. These are, full negative, full positive, and stopped. These are performed by calling `.full_negative(motor)`, `.full_positive(motor)`, and `.stop(servo)`, respectively. If the motor is disabled, these will enable it. There are also `.all_full_negative()`, `.all_full_positive()`, and `.stop_all()` for having all the motors on the cluster drive at once.
+
+The full negative and full positive speed of each motor on a cluster can be read back using `.speed_scale(motor)`. This can be useful as an input to equations that provide numbers directly to `.speed(motor, speed)`, for example.
+
+
+### Control by Percent
+
+Sometimes there are projects where motors need to move based on the readings from sensors or another devices, but the numbers given out are not easy to convert to speeds the motors accept. To overcome this the library lets you drive the motors on a cluster at a percent between their negative and positive speeds, or two speeds provided, based on that input.
+
+With an input between `-1.0` and `1.0`, a motor on a cluster can be driven at a percent between its negative and positive speeds using `.at_percent(motor, in)`, or all motors using `.all_at_percent(in)`.
+
+With an input between a provided min and max, a motor on a cluster can be driven at a percent between its negative and postive speeds using `.at_percent(motor, in, in_min, in_max)`, or all motors using `.all_at_percent(in, in_min, in_max)`.
+
+With an input between a provided min and max, a motor on a cluster can be driven at a percent between two provided speeds using `.at_percent(motor, in, in_min, speed_min, speed_max)`, or all motors using `.all_at_percent(in, in_min, speed_min, speed_max)`.
+
+If the motor is disabled, these will enable it.
+
+
+### Control by Duty Cycle
+
+At a hardware level DC motors operate by receiving a voltage across their two terminals, with positive causing a motion in one direction and negative causing a motion in the other. To avoid needing a negative voltage supply, motor drivers employ a H-Bridge arrangement of transistors or mosfets to flip which side of the motor is connected to ground and which is connected to power. By rapidly turing these transistors or mosfets on and off both the speed and direction of the motor can be varied. The common way this is achieved is by using a pair of pulse width modulated signals, where the duty cycle of the active signal controls the speed, and which signal is active controls the direction. Braking can also be controlled (see //TODO)
+
+The duty cycle of a motor on a cluster can be set by calling `.duty(motor, duty)` or `.all_at_duty(duty)`, which take a float as their `duty` input. If a motor is disabled, these will enable it. These functions will also recalculate the related speed.
+
+To read back the current duty cycle of a motor on a cluster, call `.duty(motor)`. If the motor is disabled, this will be the last duty that was provided when enabled.
+
+
+### Frequency Control
+
+Motors can be driven at a variety of frequencies, with a common values being above the range of human hearing. As such this library uses 25KHz as its default, but this can easily be changed.
+
+The frequency (in Hz) of all the motors on a cluster can be set by calling `.frequency(freq)`, which takes a float as its `freq` input. //TODO The supported range of this input is `10` to `350` Hz. Due to how `MotorCluster` works, there is no way to set independent frequencies for each motor.
+
+To read back the current frequency (in Hz) of all the motors on a cluster, call `.frequency()` without any input.
+
+Note that changing the frequency does not change the duty cycle sent to the motors, only how frequently pulses are sent.
+
+Also, be aware that currently the frequency changes immediately, even if part-way through outputting a pulse. It is therefore recommended to disable all motors first before changing the frequency.
+
+
+### Phase Control
+
+When dealing with many servos, there can often be large current draw spikes caused by them all responding to pulses at the same time. To minimise this, the ServoCluster class allows for the start time of each servo's pulses to be delayed by a percentage of the available time period. This is called their phase.
+
+The phase of a servo on a cluster can be set by calling `.phase(servo, phase)` or `.all_to_phase(phase)`, which take a float between `0.0` and `1.0` as their `phase` input.
+
+To read back the current phase of a servo on a cluster, call `.phase(servo)`.
+
+By default all servos on a cluster will start with different phases unless `auto_phase=False` is provided when creating the `ServoCluster`.
+
+
+### Calibration
+
+There are different types of servos, with `ANGULAR`, `LINEAR`, and `CONTINUOUS` being common. To support these different types, each `ServoCluster` class contains calibration objects for each of its servos that store the specific value to pulse mappings needed for their types. A copy of a servo's calibration on a cluster can be accessed using `.calibration(servo)`. It is also possible to provide a servo on a cluster with a new calibration using `.calibration(servo, calibration)`.
+
+
+### Delayed Loading
+
+To match behaviour with the regular `Motor` class, `MotorCluster` automatically applies each change to its motor's states immediately. However, sometimes this may not be wanted, and instead you want all motors to receive updated duty cycles at the same time, regardless of how long the code ran that calculated the update.
+
+For this purpose, all the functions that modify a motor state on a cluster include an optional parameter `load`, which by default is `True`. To avoid this "loading" include `load=False` in the relevant function calls. Then either the last call can include `load=True`, or a specific call to `.load()` can be made.
+
+
+### Function Reference
+
+Here is the complete list of functions available on the `MotorCluster` class:
+
+```python
+//TODO
+MotorCluster(pio, sm, pins, calibration=ANGULAR, freq=50, auto_phase=True)    # calibration can either be an integer or a Calibration class
+count()
+pins(motor)
+enable(motor, load=True)
+enable_all(load=True)
+disable(motor, load=True)
+disable_all(load=True)
+is_enabled(motor)
+duty(motor)
+duty(motor, duty, load=True)
+all_at_duty(motor, load=True)
+speed(motor)
+speed(motor, speed, load=True)
+all_at_speed(speed, load=True)
+phase(motor)
+phase(motor, phase, load=True)
+all_to_phase(phase, load=True)
+frequency()
+frequency(freq)
+full_negative(motor, load=True)
+all_full_negative(load=True)
+full_positive(motor, load=True)
+all_full_positive(load=True)
+stop(motor, load=True)
+stop_all(load=True)
+coast(motor, load=True)
+coast_all(load=True)
+brake(motor, load=True)
+brake_all(load=True)
+at_percent(motor, in, load=True)
+at_percent(motor, in, in_min, in_max, load=True)
+at_percent(motor, in, in_min, in_max, value_min, value_max, load=True)
+all_at_percent(in, load=True)
+all_at_percent(in, in_min, in_max, load=True)
+all_at_percent(in, in_min, in_max, value_min, value_max, load=True)
+load()
+```
+
+
+### PIO Limitations
+
+The RP2040 features two PIOs with four state machines each. This places a hard limit on how many MotorClusters can be created. As this class is capable of driving all 30 GPIO pins, the only time this limit will be of concern is when motors with different frequencies are wanted, as all the outputs a MotorCluster controls share the same frequency. Relating this to the hardware PWM, think of it as a single PWM slice with up to 30 sub channels, A, B, C, D etc.
+
+When creating a MotorCluster, in most cases you'll use `0` for PIO and `0` for PIO state-machine. You should change these though if you plan on running multiple clusters, or using a cluster alongside something else that uses PIO, such as our [Plasma library](https://github.com/pimoroni/pimoroni-pico/tree/main/micropython/modules/plasma).
+
+
+## Configuration
+
+After using servos for a while, you may notice that some don't quite go to the positions you would expect. Or perhaps you are giving values to a continuous rotation servo in degrees when it would make more sense to use a speed or rpm. To compensate for these cases, each `Servo` class or servo within a `ServoCluster` has an individual `Calibration` class. This class contains a set of pulse-value pairs that are used by the `.value(value)` functions (and those similar) to convert real-world numbers into pulses each servo understand.
+
+### Common Types
+
+There are three common `type`s of servo's supported:
+* `ANGULAR` = `0` - A servo with a value that ranges from -90 to +90 degrees.
+* `LINEAR` = `1` - A servo with a value that ranges from 0 to +1.0.
+* `CONTINUOUS` = `2` - A servo with a value that ranges from -1.0 to +1.0.
+
+By default all `Servo` classes or servo within a `ServoCluster` are `ANGULAR`. This can be changed by providing one of the other types as a parameter during their creation, as shown below:
+```python
+angular = Servo(servo2040.SERVO_1)  # ANGULAR is the default so does not need to be specified here
+linear = Servo(servo2040.SERVO_2, LINEAR)
+continuous = Servo(servo2040.SERVO_3, CONTINUOUS)
+```
+
+
+### Custom Calibration
+
+As well as the common types, a custom calibration can also be provided to one or more servos during creation. Below is an example that creates an angular servo that can only travel from -45 degrees to 45 degrees.
+
+```python
+cal = Calibration()
+cal.apply_two_pairs(1000, 2000, -45, 45)
+s = Servo(servo2040.SERVO_1, cal)
+```
+
+This could be useful for example if the servo turning beyond those values would cause damage to whatever mechanism it is driving, since it would not be possible to go to angles beyond these unless limits were disabled (see [Limits](#limits)). Also it lets the exact pulse widths matching the angles be set (the `1000` and `2000` in the example). Discovering these values can take some trial and error, and will offen be different for each servo.
+
+
+
+# Modifying a Calibration
+
+It is also possible to access and modify the calibration of a `Servo` or a servo on a `ServoCluster` after their creation. This is done by first getting a copy of the servo's calibration using `.calibration()` or `.calibration(servo)`, modifying its pulses or values, then applying the modified calibration back onto to the servo.
+
+Below, an angular servo is modified to increase its reported rotation range from 180 degrees to 270 degrees.
+```python
+wide_angle = Servo(servo2040.SERVO_1)
+cal = wide_angle.calibration()
+cal.first_value(-135)
+cal.last_value(+135)
+wide_angle.calibration(cal)
+```
+
+
+### Movement Limits
+
+As well as providing a mapping between pulses and values, the calibration class also limits a servo from going beyond its minimum and maximum values. In some cases this may not be wanted, so the state of the limits can be modified by calling `.limit_to_calibration(lower, upper)` where `lower` and `upper` are booleans. Additionally, the current state of these limits can be queried by calling `.has_lower_limit()` and `.has_upper_limit()`, respectively.
+
+A case where you may want to disable limits is if you want a servo to go to a value (e.g. 90 degrees), but are not physically able to get a pulse measurement for that but can do another value instead (e.g. 60 degrees).
+
+Note, even with limits disables, servos still have hard limits of `400` and `2600` microsecond pulse widths. These are intended to protect servos from receiving pulses that are too far beyond their expected range. These can vary from servo to servo though, with some hitting a physical end-stop before getting to the typical `500` and `2500` associated with -90 and +90 degrees.
+
+
+### Populating a Calibration
+
+To aid in populating a `Calibration` class, there are five helper functions that fill the class with pulse-value pairs:
+* `apply_blank_pairs(size)` - Fills the calibration with the specified number of zero pairs
+* `apply_two_pairs(min_pulse, max_pulse, min_value, max_value)` - Fills the calibration with two pairs using the min and max numbers provided
+* `apply_three_pairs(min_pulse, mid_pulse, max_pulse, min_value, mid_value, max_value)` - Fills the calibration with three pairs using the min, mid and max numbers provided
+* `apply_uniform_pairs(size, min_pulse, max_pulse, min_value, max_value)` - Fills the calibration with the specified number of pairs, interpolated from the min to max numbers provided
+* `apply_default_pairs(type)` - Fills the calibration with the pairs of one of the common types
+
+Once a `Calibration` class contains pairs (as checked `.size() > 0`), these can then be accessed by calling `.pair(index)` and can then be modified by calling `.pair(index, pair)`. The former function returns a list containing the pulse and value of the pair, and the latter accepts a list or tuple containing the pulse and value. For situations when only a single element of each pair is needed, `.pulse(index)` and `.value(index)` will return the current numbers, and `.pulse(index, pulse)` and `.value(index, value)` will override them.
+
+For further convenience there are functions for accessing and modifying the `.first()` and `.last()` pair/pulse/value of the calibration.
+
+
+### Viewing the Mapping
+
+To aid in visualising a calibration's pulse-value mapping, the pulse for any given value can be queried by calling `.value_to_pulse(value)`. Similarly, the value for any given pulse can be queried by calling `.pulse_to_value(pulse)`. These are the same functions used by `Servo` and `ServoCluster` when controlling their servos.
+
+
+### Function Reference
+
+Here is the complete list of functions available on the `Calibration` class:
+```python
+Calibration()
+Calibration(type)
+apply_blank_pairs(size)
+apply_two_pairs(min_pulse, max_pulse, min_value, max_value)
+apply_three_pairs(min_pulse, mid_pulse, max_pulse, min_value, mid_value, max_value)
+apply_uniform_pairs(size, min_pulse, max_pulse, min_value, max_value)
+apply_default_pairs(type)
+size()
+pair(index)
+pair(index, pair)
+pulse(index)
+pulse(index, pulse)
+value(index)
+value(index, value)
+first()
+first(pair)
+first_pulse()
+first_pulse(pulse)
+first_value()
+first_value(value)
+last()
+last(pair)
+last_pulse()
+last_pulse(pulse)
+last_value()
+last_value(value)
+has_lower_limit()
+has_upper_limit()
+limit_to_calibration(lower, upper)
+value_to_pulse(value)
+pulse_to_value(pulse)
+```

micropython/modules/motor/micropython.cmake

@@ -0,0 +1,27 @@
+set(MOD_NAME motor)
+string(TOUPPER ${MOD_NAME} MOD_NAME_UPPER)
+add_library(usermod_${MOD_NAME} INTERFACE)
+
+target_sources(usermod_${MOD_NAME} INTERFACE
+    ${CMAKE_CURRENT_LIST_DIR}/${MOD_NAME}.c
+    ${CMAKE_CURRENT_LIST_DIR}/${MOD_NAME}.cpp
+    ${CMAKE_CURRENT_LIST_DIR}/../../../drivers/pwm/pwm.cpp
+    ${CMAKE_CURRENT_LIST_DIR}/../../../drivers/pwm/pwm_cluster.cpp
+    ${CMAKE_CURRENT_LIST_DIR}/../../../drivers/motor/motor2.cpp
+    ${CMAKE_CURRENT_LIST_DIR}/../../../drivers/motor/motor_cluster.cpp
+    ${CMAKE_CURRENT_LIST_DIR}/../../../drivers/motor/motor_state.cpp
+)
+# pico_generate_pio_header(usermod_${MOD_NAME} ${CMAKE_CURRENT_LIST_DIR}/../../../drivers/pwm/pwm_cluster.pio)
+
+target_include_directories(usermod_${MOD_NAME} INTERFACE
+    ${CMAKE_CURRENT_LIST_DIR}
+    ${CMAKE_CURRENT_LIST_DIR}/../../../drivers/pwm/
+    ${CMAKE_CURRENT_LIST_DIR}/../../../drivers/motor/
+)
+
+target_compile_definitions(usermod_${MOD_NAME} INTERFACE
+    MODULE_PWM_ENABLED=1
+    MODULE_MOTOR_ENABLED=1
+)
+
+target_link_libraries(usermod INTERFACE usermod_${MOD_NAME})

micropython/modules/motor/motor.c

@@ -0,0 +1,270 @@
+#include "motor.h"
+
+/***** Methods *****/
+MP_DEFINE_CONST_FUN_OBJ_1(Motor___del___obj, Motor___del__);
+MP_DEFINE_CONST_FUN_OBJ_1(Motor_pins_obj, Motor_pins);
+MP_DEFINE_CONST_FUN_OBJ_1(Motor_enable_obj, Motor_enable);
+MP_DEFINE_CONST_FUN_OBJ_1(Motor_disable_obj, Motor_disable);
+MP_DEFINE_CONST_FUN_OBJ_1(Motor_is_enabled_obj, Motor_is_enabled);
+MP_DEFINE_CONST_FUN_OBJ_KW(Motor_duty_obj, 1, Motor_duty);
+MP_DEFINE_CONST_FUN_OBJ_KW(Motor_speed_obj, 1, Motor_speed);
+MP_DEFINE_CONST_FUN_OBJ_KW(Motor_frequency_obj, 1, Motor_frequency);
+MP_DEFINE_CONST_FUN_OBJ_1(Motor_stop_obj, Motor_stop);
+MP_DEFINE_CONST_FUN_OBJ_1(Motor_coast_obj, Motor_coast);
+MP_DEFINE_CONST_FUN_OBJ_1(Motor_brake_obj, Motor_brake);
+MP_DEFINE_CONST_FUN_OBJ_1(Motor_full_negative_obj, Motor_full_negative);
+MP_DEFINE_CONST_FUN_OBJ_1(Motor_full_positive_obj, Motor_full_positive);
+MP_DEFINE_CONST_FUN_OBJ_KW(Motor_to_percent_obj, 2, Motor_to_percent);
+MP_DEFINE_CONST_FUN_OBJ_KW(Motor_direction_obj, 1, Motor_direction);
+MP_DEFINE_CONST_FUN_OBJ_KW(Motor_speed_scale_obj, 1, Motor_speed_scale);
+MP_DEFINE_CONST_FUN_OBJ_KW(Motor_deadzone_obj, 1, Motor_deadzone);
+MP_DEFINE_CONST_FUN_OBJ_KW(Motor_decay_mode_obj, 1, Motor_decay_mode);
+
+MP_DEFINE_CONST_FUN_OBJ_1(MotorCluster___del___obj, MotorCluster___del__);
+MP_DEFINE_CONST_FUN_OBJ_1(MotorCluster_count_obj, MotorCluster_count);
+MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_pins_obj, 2, MotorCluster_pins);
+MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_enable_obj, 2, MotorCluster_enable);
+MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_enable_all_obj, 1, MotorCluster_enable_all);
+MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_disable_obj, 2, MotorCluster_disable);
+MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_disable_all_obj, 1, MotorCluster_disable_all);
+MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_is_enabled_obj, 2, MotorCluster_is_enabled);
+MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_duty_obj, 2, MotorCluster_duty);
+MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_all_to_duty_obj, 1, MotorCluster_all_to_duty);
+MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_speed_obj, 2, MotorCluster_speed);
+MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_all_to_speed_obj, 1, MotorCluster_all_to_speed);
+MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_phase_obj, 2, MotorCluster_phase);
+MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_all_to_phase_obj, 1, MotorCluster_all_to_phase);
+MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_frequency_obj, 1, MotorCluster_frequency);
+MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_stop_obj, 2, MotorCluster_stop);
+MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_stop_all_obj, 1, MotorCluster_stop_all);
+MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_coast_obj, 2, MotorCluster_coast);
+MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_coast_all_obj, 1, MotorCluster_coast_all);
+MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_brake_obj, 2, MotorCluster_brake);
+MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_brake_all_obj, 1, MotorCluster_brake_all);
+MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_full_negative_obj, 2, MotorCluster_full_negative);
+MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_all_full_negative_obj, 1, MotorCluster_all_full_negative);
+MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_full_positive_obj, 2, MotorCluster_full_positive);
+MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_all_full_positive_obj, 1, MotorCluster_all_full_positive);
+MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_to_percent_obj, 3, MotorCluster_to_percent);
+MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_all_to_percent_obj, 2, MotorCluster_all_to_percent);
+MP_DEFINE_CONST_FUN_OBJ_1(MotorCluster_load_obj, MotorCluster_load);
+MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_direction_obj, 2, MotorCluster_direction);
+MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_all_to_direction_obj, 1, MotorCluster_all_to_direction);
+MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_speed_scale_obj, 2, MotorCluster_speed_scale);
+MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_all_to_speed_scale_obj, 1, MotorCluster_all_to_speed_scale);
+MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_deadzone_obj, 2, MotorCluster_deadzone);
+MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_all_to_deadzone_obj, 1, MotorCluster_all_to_deadzone);
+MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_decay_mode_obj, 2, MotorCluster_decay_mode);
+MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_all_to_decay_mode_obj, 1, MotorCluster_all_to_decay_mode);
+
+/***** Binding of Methods *****/
+STATIC const mp_rom_map_elem_t Motor_locals_dict_table[] = {
+    { MP_ROM_QSTR(MP_QSTR___del__), MP_ROM_PTR(&Motor___del___obj) },
+    { MP_ROM_QSTR(MP_QSTR_pins), MP_ROM_PTR(&Motor_pins_obj) },
+    { MP_ROM_QSTR(MP_QSTR_enable), MP_ROM_PTR(&Motor_enable_obj) },
+    { MP_ROM_QSTR(MP_QSTR_disable), MP_ROM_PTR(&Motor_disable_obj) },
+    { MP_ROM_QSTR(MP_QSTR_is_enabled), MP_ROM_PTR(&Motor_is_enabled_obj) },
+    { MP_ROM_QSTR(MP_QSTR_duty), MP_ROM_PTR(&Motor_duty_obj) },
+    { MP_ROM_QSTR(MP_QSTR_speed), MP_ROM_PTR(&Motor_speed_obj) },
+    { MP_ROM_QSTR(MP_QSTR_frequency), MP_ROM_PTR(&Motor_frequency_obj) },
+    { MP_ROM_QSTR(MP_QSTR_stop), MP_ROM_PTR(&Motor_stop_obj) },
+    { MP_ROM_QSTR(MP_QSTR_coast), MP_ROM_PTR(&Motor_coast_obj) },
+    { MP_ROM_QSTR(MP_QSTR_brake), MP_ROM_PTR(&Motor_brake_obj) },
+    { MP_ROM_QSTR(MP_QSTR_full_negative), MP_ROM_PTR(&Motor_full_negative_obj) },
+    { MP_ROM_QSTR(MP_QSTR_full_positive), MP_ROM_PTR(&Motor_full_positive_obj) },
+    { MP_ROM_QSTR(MP_QSTR_to_percent), MP_ROM_PTR(&Motor_to_percent_obj) },
+    { MP_ROM_QSTR(MP_QSTR_direction), MP_ROM_PTR(&Motor_direction_obj) },
+    { MP_ROM_QSTR(MP_QSTR_speed_scale), MP_ROM_PTR(&Motor_speed_scale_obj) },
+    { MP_ROM_QSTR(MP_QSTR_deadzone), MP_ROM_PTR(&Motor_deadzone_obj) },
+    { MP_ROM_QSTR(MP_QSTR_decay_mode), MP_ROM_PTR(&Motor_decay_mode_obj) },
+};
+
+STATIC const mp_rom_map_elem_t MotorCluster_locals_dict_table[] = {
+    { MP_ROM_QSTR(MP_QSTR___del__), MP_ROM_PTR(&MotorCluster___del___obj) },
+    { MP_ROM_QSTR(MP_QSTR_count), MP_ROM_PTR(&MotorCluster_count_obj) },
+    { MP_ROM_QSTR(MP_QSTR_pins), MP_ROM_PTR(&MotorCluster_pins_obj) },
+    { MP_ROM_QSTR(MP_QSTR_enable), MP_ROM_PTR(&MotorCluster_enable_obj) },
+    { MP_ROM_QSTR(MP_QSTR_enable_all), MP_ROM_PTR(&MotorCluster_enable_all_obj) },
+    { MP_ROM_QSTR(MP_QSTR_disable), MP_ROM_PTR(&MotorCluster_disable_obj) },
+    { MP_ROM_QSTR(MP_QSTR_disable_all), MP_ROM_PTR(&MotorCluster_disable_all_obj) },
+    { MP_ROM_QSTR(MP_QSTR_is_enabled), MP_ROM_PTR(&MotorCluster_is_enabled_obj) },
+    { MP_ROM_QSTR(MP_QSTR_duty), MP_ROM_PTR(&MotorCluster_duty_obj) },
+    { MP_ROM_QSTR(MP_QSTR_all_to_duty), MP_ROM_PTR(&MotorCluster_all_to_duty_obj) },
+    { MP_ROM_QSTR(MP_QSTR_speed), MP_ROM_PTR(&MotorCluster_speed_obj) },
+    { MP_ROM_QSTR(MP_QSTR_all_to_speed), MP_ROM_PTR(&MotorCluster_all_to_speed_obj) },
+    { MP_ROM_QSTR(MP_QSTR_phase), MP_ROM_PTR(&MotorCluster_phase_obj) },
+    { MP_ROM_QSTR(MP_QSTR_all_to_phase), MP_ROM_PTR(&MotorCluster_all_to_phase_obj) },
+    { MP_ROM_QSTR(MP_QSTR_frequency), MP_ROM_PTR(&MotorCluster_frequency_obj) },
+    { MP_ROM_QSTR(MP_QSTR_stop), MP_ROM_PTR(&MotorCluster_stop_obj) },
+    { MP_ROM_QSTR(MP_QSTR_stop_all), MP_ROM_PTR(&MotorCluster_stop_all_obj) },
+    { MP_ROM_QSTR(MP_QSTR_coast), MP_ROM_PTR(&MotorCluster_coast_obj) },
+    { MP_ROM_QSTR(MP_QSTR_coast_all), MP_ROM_PTR(&MotorCluster_coast_all_obj) },
+    { MP_ROM_QSTR(MP_QSTR_brake), MP_ROM_PTR(&MotorCluster_brake_obj) },
+    { MP_ROM_QSTR(MP_QSTR_brake_all), MP_ROM_PTR(&MotorCluster_brake_all_obj) },
+    { MP_ROM_QSTR(MP_QSTR_full_negative), MP_ROM_PTR(&MotorCluster_full_negative_obj) },
+    { MP_ROM_QSTR(MP_QSTR_all_full_negative), MP_ROM_PTR(&MotorCluster_all_full_negative_obj) },
+    { MP_ROM_QSTR(MP_QSTR_full_positive), MP_ROM_PTR(&MotorCluster_full_positive_obj) },
+    { MP_ROM_QSTR(MP_QSTR_all_full_positive), MP_ROM_PTR(&MotorCluster_all_full_positive_obj) },
+    { MP_ROM_QSTR(MP_QSTR_to_percent), MP_ROM_PTR(&MotorCluster_to_percent_obj) },
+    { MP_ROM_QSTR(MP_QSTR_all_to_percent), MP_ROM_PTR(&MotorCluster_all_to_percent_obj) },
+    { MP_ROM_QSTR(MP_QSTR_load), MP_ROM_PTR(&MotorCluster_load_obj) },
+    { MP_ROM_QSTR(MP_QSTR_direction), MP_ROM_PTR(&MotorCluster_direction_obj) },
+    { MP_ROM_QSTR(MP_QSTR_all_to_direction), MP_ROM_PTR(&MotorCluster_direction_obj) },
+    { MP_ROM_QSTR(MP_QSTR_speed_scale), MP_ROM_PTR(&MotorCluster_speed_scale_obj) },
+    { MP_ROM_QSTR(MP_QSTR_all_to_speed_scale), MP_ROM_PTR(&MotorCluster_all_to_speed_scale_obj) },
+    { MP_ROM_QSTR(MP_QSTR_deadzone), MP_ROM_PTR(&MotorCluster_deadzone_obj) },
+    { MP_ROM_QSTR(MP_QSTR_all_to_deadzone), MP_ROM_PTR(&MotorCluster_all_to_deadzone_obj) },
+    { MP_ROM_QSTR(MP_QSTR_decay_mode), MP_ROM_PTR(&MotorCluster_decay_mode_obj) },
+    { MP_ROM_QSTR(MP_QSTR_all_to_decay_mode), MP_ROM_PTR(&MotorCluster_all_to_decay_mode_obj) },
+};
+
+STATIC MP_DEFINE_CONST_DICT(Motor_locals_dict, Motor_locals_dict_table);
+STATIC MP_DEFINE_CONST_DICT(MotorCluster_locals_dict, MotorCluster_locals_dict_table);
+
+/***** Class Definition *****/
+const mp_obj_type_t Motor_type = {
+    { &mp_type_type },
+    .name = MP_QSTR_motor,
+    .print = Motor_print,
+    .make_new = Motor_make_new,
+    .locals_dict = (mp_obj_dict_t*)&Motor_locals_dict,
+};
+
+const mp_obj_type_t MotorCluster_type = {
+    { &mp_type_type },
+    .name = MP_QSTR_motor_cluster,
+    .print = MotorCluster_print,
+    .make_new = MotorCluster_make_new,
+    .locals_dict = (mp_obj_dict_t*)&MotorCluster_locals_dict,
+};
+
+
+/***** Module Constants *****/
+const mp_rom_obj_tuple_t pico_motor_shim_motor1_pins = {
+    {&mp_type_tuple}, 2, { MP_ROM_INT(6), MP_ROM_INT(7), },
+};
+const mp_rom_obj_tuple_t pico_motor_shim_motor2_pins = {
+    {&mp_type_tuple}, 2, { MP_ROM_INT(27), MP_ROM_INT(26), },
+};
+
+const mp_rom_obj_tuple_t motor2040_motorA_pins = {
+    {&mp_type_tuple}, 2, { MP_ROM_INT(4), MP_ROM_INT(5) },
+};
+const mp_rom_obj_tuple_t motor2040_motorB_pins = {
+    {&mp_type_tuple}, 2, { MP_ROM_INT(6), MP_ROM_INT(7) },
+};
+const mp_rom_obj_tuple_t motor2040_motorC_pins = {
+    {&mp_type_tuple}, 2, { MP_ROM_INT(8), MP_ROM_INT(9) },
+};
+const mp_rom_obj_tuple_t motor2040_motorD_pins = {
+    {&mp_type_tuple}, 2, { MP_ROM_INT(10), MP_ROM_INT(11) },
+};
+const mp_rom_obj_tuple_t motor2040_encoderA_pins = {
+    {&mp_type_tuple}, 2, { MP_ROM_INT(0), MP_ROM_INT(1) },
+};
+const mp_rom_obj_tuple_t motor2040_encoderB_pins = {
+    {&mp_type_tuple}, 2, { MP_ROM_INT(2), MP_ROM_INT(3) },
+};
+const mp_rom_obj_tuple_t motor2040_encoderC_pins = {
+    {&mp_type_tuple}, 2, { MP_ROM_INT(12), MP_ROM_INT(13) },
+};
+const mp_rom_obj_tuple_t motor2040_encoderD_pins = {
+    {&mp_type_tuple}, 2, { MP_ROM_INT(14), MP_ROM_INT(15) },
+};
+
+typedef struct _mp_obj_float_t {
+    mp_obj_base_t base;
+    mp_float_t value;
+} mp_obj_float_t;
+//TODO confirm below numbers are correct
+mp_obj_float_t motor2040_shunt_resistor = {{&mp_type_float}, 0.47f};
+mp_obj_float_t motor2040_voltage_gain = {{&mp_type_float}, 3.9f / 13.9f};
+mp_obj_float_t motor2040_current_offset = {{&mp_type_float}, -0.02f};
+
+
+/***** Globals Table *****/
+STATIC const mp_rom_map_elem_t pico_motor_shim_globals_table[] = {
+    { MP_OBJ_NEW_QSTR(MP_QSTR___name__), MP_OBJ_NEW_QSTR(MP_QSTR_pico_motor_shim) },
+    { MP_ROM_QSTR(MP_QSTR_BUTTON_A), MP_ROM_INT(2) },
+    { MP_ROM_QSTR(MP_QSTR_MOTOR_1), MP_ROM_PTR(&pico_motor_shim_motor1_pins) },
+    { MP_ROM_QSTR(MP_QSTR_MOTOR_2), MP_ROM_PTR(&pico_motor_shim_motor2_pins) },
+    { MP_ROM_QSTR(MP_QSTR_NUM_MOTORS), MP_ROM_INT(2) },
+    { MP_ROM_QSTR(MP_QSTR_SDA), MP_ROM_INT(4) },
+    { MP_ROM_QSTR(MP_QSTR_SCL), MP_ROM_INT(5) },
+};
+
+STATIC const mp_rom_map_elem_t motor2040_globals_table[] = {
+    { MP_OBJ_NEW_QSTR(MP_QSTR___name__), MP_OBJ_NEW_QSTR(MP_QSTR_motor2040) },
+    { MP_ROM_QSTR(MP_QSTR_MOTOR_A), MP_ROM_PTR(&motor2040_motorA_pins) },
+    { MP_ROM_QSTR(MP_QSTR_MOTOR_B), MP_ROM_PTR(&motor2040_motorB_pins) },
+    { MP_ROM_QSTR(MP_QSTR_MOTOR_C), MP_ROM_PTR(&motor2040_motorC_pins) },
+    { MP_ROM_QSTR(MP_QSTR_MOTOR_D), MP_ROM_PTR(&motor2040_motorD_pins) },
+    { MP_ROM_QSTR(MP_QSTR_NUM_MOTORS), MP_ROM_INT(4) },
+    { MP_ROM_QSTR(MP_QSTR_ENCODER_A), MP_ROM_PTR(&motor2040_encoderA_pins) },
+    { MP_ROM_QSTR(MP_QSTR_ENCODER_B), MP_ROM_PTR(&motor2040_encoderB_pins) },
+    { MP_ROM_QSTR(MP_QSTR_ENCODER_C), MP_ROM_PTR(&motor2040_encoderC_pins) },
+    { MP_ROM_QSTR(MP_QSTR_ENCODER_D), MP_ROM_PTR(&motor2040_encoderD_pins) },
+    { MP_ROM_QSTR(MP_QSTR_NUM_ENCODERS), MP_ROM_INT(4) },
+    { MP_ROM_QSTR(MP_QSTR_TX_TRIG), MP_ROM_INT(16) },
+    { MP_ROM_QSTR(MP_QSTR_RX_ECHO), MP_ROM_INT(17) },
+    { MP_ROM_QSTR(MP_QSTR_LED_DATA), MP_ROM_INT(18) },
+    { MP_ROM_QSTR(MP_QSTR_NUM_LEDS), MP_ROM_INT(1) },
+    { MP_ROM_QSTR(MP_QSTR_INT), MP_ROM_INT(19) },
+    { MP_ROM_QSTR(MP_QSTR_SDA), MP_ROM_INT(20) },
+    { MP_ROM_QSTR(MP_QSTR_SCL), MP_ROM_INT(21) },
+    { MP_ROM_QSTR(MP_QSTR_USER_SW), MP_ROM_INT(23) },
+    { MP_ROM_QSTR(MP_QSTR_ADC_ADDR_0), MP_ROM_INT(22) },
+    { MP_ROM_QSTR(MP_QSTR_ADC_ADDR_1), MP_ROM_INT(24) },
+    { MP_ROM_QSTR(MP_QSTR_ADC_ADDR_2), MP_ROM_INT(25) },
+    { MP_ROM_QSTR(MP_QSTR_ADC0), MP_ROM_INT(26) },
+    { MP_ROM_QSTR(MP_QSTR_ADC1), MP_ROM_INT(27) },
+    { MP_ROM_QSTR(MP_QSTR_ADC2), MP_ROM_INT(28) },
+    { MP_ROM_QSTR(MP_QSTR_SHARED_ADC), MP_ROM_INT(29) },
+    { MP_ROM_QSTR(MP_QSTR_CURRENT_SENSE_A_ADDR), MP_ROM_INT(0b000) },
+    { MP_ROM_QSTR(MP_QSTR_CURRENT_SENSE_B_ADDR), MP_ROM_INT(0b001) },
+    { MP_ROM_QSTR(MP_QSTR_CURRENT_SENSE_C_ADDR), MP_ROM_INT(0b010) },
+    { MP_ROM_QSTR(MP_QSTR_CURRENT_SENSE_D_ADDR), MP_ROM_INT(0b011) },
+    { MP_ROM_QSTR(MP_QSTR_VOLTAGE_SENSE_ADDR), MP_ROM_INT(0b100) },
+    { MP_ROM_QSTR(MP_QSTR_FAULT_SENSE_ADDR), MP_ROM_INT(0b101) },
+    { MP_ROM_QSTR(MP_QSTR_SENSOR_1_ADDR), MP_ROM_INT(0b110) },
+    { MP_ROM_QSTR(MP_QSTR_SENSOR_2_ADDR), MP_ROM_INT(0b111) },
+    { MP_ROM_QSTR(MP_QSTR_NUM_SENSORS), MP_ROM_INT(2) },
+    { MP_ROM_QSTR(MP_QSTR_SHUNT_RESISTOR), MP_ROM_PTR(&motor2040_shunt_resistor) },
+    { MP_ROM_QSTR(MP_QSTR_CURRENT_GAIN), MP_ROM_INT(5) },
+    { MP_ROM_QSTR(MP_QSTR_VOLTAGE_GAIN), MP_ROM_PTR(&motor2040_voltage_gain) },
+    { MP_ROM_QSTR(MP_QSTR_CURRENT_OFFSET), MP_ROM_PTR(&motor2040_current_offset) },
+};
+
+STATIC MP_DEFINE_CONST_DICT(mp_module_pico_motor_shim_globals, pico_motor_shim_globals_table);
+STATIC MP_DEFINE_CONST_DICT(mp_module_motor2040_globals, motor2040_globals_table);
+
+const mp_obj_module_t pico_motor_shim_user_cmodule = {
+    .base = { &mp_type_module },
+    .globals = (mp_obj_dict_t*)&mp_module_pico_motor_shim_globals,
+};
+
+const mp_obj_module_t motor2040_user_cmodule = {
+    .base = { &mp_type_module },
+    .globals = (mp_obj_dict_t*)&mp_module_motor2040_globals,
+};
+
+STATIC const mp_map_elem_t motor_globals_table[] = {
+    { MP_OBJ_NEW_QSTR(MP_QSTR___name__), MP_OBJ_NEW_QSTR(MP_QSTR_motor) },
+    { MP_OBJ_NEW_QSTR(MP_QSTR_Motor), (mp_obj_t)&Motor_type },
+    { MP_OBJ_NEW_QSTR(MP_QSTR_MotorCluster), (mp_obj_t)&MotorCluster_type },
+    { MP_OBJ_NEW_QSTR(MP_QSTR_pico_motor_shim), (mp_obj_t)&pico_motor_shim_user_cmodule },
+    { MP_OBJ_NEW_QSTR(MP_QSTR_motor2040), (mp_obj_t)&motor2040_user_cmodule },
+
+    { MP_ROM_QSTR(MP_QSTR_NORMAL), MP_ROM_INT(0x00) },
+    { MP_ROM_QSTR(MP_QSTR_REVERSED), MP_ROM_INT(0x01) },
+    { MP_ROM_QSTR(MP_QSTR_FAST_DECAY), MP_ROM_INT(0x00) },
+    { MP_ROM_QSTR(MP_QSTR_SLOW_DECAY), MP_ROM_INT(0x01) },
+};
+STATIC MP_DEFINE_CONST_DICT(mp_module_motor_globals, motor_globals_table);
+
+/***** Module Definition *****/
+const mp_obj_module_t motor_user_cmodule = {
+    .base = { &mp_type_module },
+    .globals = (mp_obj_dict_t*)&mp_module_motor_globals,
+};
+MP_REGISTER_MODULE(MP_QSTR_motor, motor_user_cmodule, MODULE_MOTOR_ENABLED);

micropython/modules/motor/motor.h

@@ -0,0 +1,67 @@
+// Include MicroPython API.
+#include "py/runtime.h"
+
+/***** Extern of Class Definition *****/
+extern const mp_obj_type_t Motor_type;
+extern const mp_obj_type_t MotorCluster_type;
+
+/***** Extern of Class Methods *****/
+extern void Motor_print(const mp_print_t *print, mp_obj_t self_in, mp_print_kind_t kind);
+extern mp_obj_t Motor_make_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw, const mp_obj_t *all_args);
+extern mp_obj_t Motor___del__(mp_obj_t self_in);
+extern mp_obj_t Motor_pins(mp_obj_t self_in);
+extern mp_obj_t Motor_enable(mp_obj_t self_in);
+extern mp_obj_t Motor_disable(mp_obj_t self_in);
+extern mp_obj_t Motor_is_enabled(mp_obj_t self_in);
+extern mp_obj_t Motor_duty(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
+extern mp_obj_t Motor_speed(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
+extern mp_obj_t Motor_frequency(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
+extern mp_obj_t Motor_stop(mp_obj_t self_in);
+extern mp_obj_t Motor_coast(mp_obj_t self_in);
+extern mp_obj_t Motor_brake(mp_obj_t self_in);
+extern mp_obj_t Motor_full_negative(mp_obj_t self_in);
+extern mp_obj_t Motor_full_positive(mp_obj_t self_in);
+extern mp_obj_t Motor_to_percent(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
+extern mp_obj_t Motor_direction(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
+extern mp_obj_t Motor_speed_scale(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
+extern mp_obj_t Motor_deadzone(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
+extern mp_obj_t Motor_decay_mode(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
+
+extern void MotorCluster_print(const mp_print_t *print, mp_obj_t self_in, mp_print_kind_t kind);
+extern mp_obj_t MotorCluster_make_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw, const mp_obj_t *all_args);
+extern mp_obj_t MotorCluster___del__(mp_obj_t self_in);
+extern mp_obj_t MotorCluster_count(mp_obj_t self_in);
+extern mp_obj_t MotorCluster_pins(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
+extern mp_obj_t MotorCluster_enable(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
+extern mp_obj_t MotorCluster_enable_all(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
+extern mp_obj_t MotorCluster_disable(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
+extern mp_obj_t MotorCluster_disable_all(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
+extern mp_obj_t MotorCluster_is_enabled(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
+extern mp_obj_t MotorCluster_duty(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
+extern mp_obj_t MotorCluster_all_to_duty(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
+extern mp_obj_t MotorCluster_speed(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
+extern mp_obj_t MotorCluster_all_to_speed(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
+extern mp_obj_t MotorCluster_phase(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
+extern mp_obj_t MotorCluster_all_to_phase(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
+extern mp_obj_t MotorCluster_frequency(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
+extern mp_obj_t MotorCluster_stop(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
+extern mp_obj_t MotorCluster_stop_all(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
+extern mp_obj_t MotorCluster_coast(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
+extern mp_obj_t MotorCluster_coast_all(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
+extern mp_obj_t MotorCluster_brake(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
+extern mp_obj_t MotorCluster_brake_all(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
+extern mp_obj_t MotorCluster_full_negative(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
+extern mp_obj_t MotorCluster_all_full_negative(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
+extern mp_obj_t MotorCluster_full_positive(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
+extern mp_obj_t MotorCluster_all_full_positive(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
+extern mp_obj_t MotorCluster_to_percent(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
+extern mp_obj_t MotorCluster_all_to_percent(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
+extern mp_obj_t MotorCluster_load(mp_obj_t self_in);
+extern mp_obj_t MotorCluster_direction(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
+extern mp_obj_t MotorCluster_all_to_direction(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
+extern mp_obj_t MotorCluster_speed_scale(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
+extern mp_obj_t MotorCluster_all_to_speed_scale(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
+extern mp_obj_t MotorCluster_deadzone(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
+extern mp_obj_t MotorCluster_all_to_deadzone(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
+extern mp_obj_t MotorCluster_decay_mode(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
+extern mp_obj_t MotorCluster_all_to_decay_mode(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);

micropython/modules/pico_motor_shim/micropython.cmake

@@ -0,0 +1,18 @@
+set(MOD_NAME picomotorshim)
+string(TOUPPER ${MOD_NAME} MOD_NAME_UPPER)
+add_library(usermod_${MOD_NAME} INTERFACE)
+
+target_sources(usermod_${MOD_NAME} INTERFACE
+    ${CMAKE_CURRENT_LIST_DIR}/pico_motor_shim.c
+)
+
+target_include_directories(usermod_${MOD_NAME} INTERFACE
+    ${CMAKE_CURRENT_LIST_DIR}
+    ${CMAKE_CURRENT_LIST_DIR}/../../../libraries/pico_motor_shim/
+)
+
+target_compile_definitions(usermod_${MOD_NAME} INTERFACE
+    -DMODULE_${MOD_NAME_UPPER}_ENABLED=1
+)
+
+target_link_libraries(usermod INTERFACE usermod_${MOD_NAME})

micropython/modules/pico_motor_shim/pico_motor_shim.c

@@ -0,0 +1,40 @@
+#include "pico_motor_shim.h"
+
+/***** Constants *****/
+enum pins
+{
+    BUTTON_A = 2,
+
+    MOTOR_1_POS = 6,
+    MOTOR_1_NEG = 7,
+
+    MOTOR_2_POS = 27,
+    MOTOR_2_NEG = 26,
+};
+
+
+////////////////////////////////////////////////////////////////////////////////////////////////////
+// picomotorshim Module
+////////////////////////////////////////////////////////////////////////////////////////////////////
+
+/***** Globals Table *****/
+STATIC const mp_map_elem_t picomotorshim_globals_table[] = {
+    { MP_ROM_QSTR(MP_QSTR___name__), MP_ROM_QSTR(MP_QSTR_picomotorshim) },
+    { MP_ROM_QSTR(MP_QSTR_PIN_BUTTON_A), MP_ROM_INT(BUTTON_A) },
+    { MP_ROM_QSTR(MP_QSTR_PIN_MOTOR_1_POS), MP_ROM_INT(MOTOR_1_POS) },
+    { MP_ROM_QSTR(MP_QSTR_PIN_MOTOR_1_NEG), MP_ROM_INT(MOTOR_1_NEG) },
+    { MP_ROM_QSTR(MP_QSTR_PIN_MOTOR_2_POS), MP_ROM_INT(MOTOR_2_POS) },
+    { MP_ROM_QSTR(MP_QSTR_PIN_MOTOR_2_NEG), MP_ROM_INT(MOTOR_2_NEG) },
+};
+STATIC MP_DEFINE_CONST_DICT(mp_module_picomotorshim_globals, picomotorshim_globals_table);
+
+/***** Module Definition *****/
+const mp_obj_module_t picomotorshim_user_cmodule = {
+    .base = { &mp_type_module },
+    .globals = (mp_obj_dict_t*)&mp_module_picomotorshim_globals,
+};
+
+////////////////////////////////////////////////////////////////////////////////////////////////////
+MP_REGISTER_MODULE(MP_QSTR_picomotorshim, picomotorshim_user_cmodule, MODULE_PICOMOTORSHIM_ENABLED);
+////////////////////////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////////////////////////

micropython/modules/pico_motor_shim/pico_motor_shim.h

@@ -0,0 +1,3 @@
+// Include MicroPython API.
+#include "py/runtime.h"
+#include "py/objstr.h"

micropython/modules/pwm/micropython.cmake

@@ -0,0 +1,20 @@
+set(MOD_NAME pwm)
+string(TOUPPER ${MOD_NAME} MOD_NAME_UPPER)
+add_library(usermod_${MOD_NAME} INTERFACE)
+
+target_sources(usermod_${MOD_NAME} INTERFACE
+    ${CMAKE_CURRENT_LIST_DIR}/../../../drivers/pwm/pwm.cpp
+    ${CMAKE_CURRENT_LIST_DIR}/../../../drivers/pwm/pwm_cluster.cpp
+)
+pico_generate_pio_header(usermod_${MOD_NAME} ${CMAKE_CURRENT_LIST_DIR}/../../../drivers/pwm/pwm_cluster.pio)
+
+target_include_directories(usermod_${MOD_NAME} INTERFACE
+    ${CMAKE_CURRENT_LIST_DIR}
+    ${CMAKE_CURRENT_LIST_DIR}/../../../drivers/pwm/
+)
+
+target_compile_definitions(usermod_${MOD_NAME} INTERFACE
+    MODULE_PWM_ENABLED=1
+)
+
+target_link_libraries(usermod INTERFACE usermod_${MOD_NAME})

micropython/modules/servo/micropython.cmake

@@ -12,7 +12,7 @@ target_sources(usermod_${MOD_NAME} INTERFACE
     ${CMAKE_CURRENT_LIST_DIR}/../../../drivers/servo/servo_state.cpp
     ${CMAKE_CURRENT_LIST_DIR}/../../../drivers/servo/calibration.cpp
 )
-pico_generate_pio_header(usermod_${MOD_NAME} ${CMAKE_CURRENT_LIST_DIR}/../../../drivers/pwm/pwm_cluster.pio)
+# pico_generate_pio_header(usermod_${MOD_NAME} ${CMAKE_CURRENT_LIST_DIR}/../../../drivers/pwm/pwm_cluster.pio)
 
 target_include_directories(usermod_${MOD_NAME} INTERFACE
     ${CMAKE_CURRENT_LIST_DIR}
@@ -21,6 +21,7 @@ target_include_directories(usermod_${MOD_NAME} INTERFACE
 )
 
 target_compile_definitions(usermod_${MOD_NAME} INTERFACE
+    MODULE_PWM_ENABLED=1
     MODULE_SERVO_ENABLED=1
 )
 

micropython/modules_py/pimoroni.py

@@ -141,3 +141,76 @@ class RGBLED:
         self.led_r.duty_u16(int((r * 65535) / 255))
         self.led_g.duty_u16(int((g * 65535) / 255))
         self.led_b.duty_u16(int((b * 65535) / 255))
+
+
+class Motor:
+    FAST_DECAY = 0  # Recirculation current fast decay mode (coasting)
+    SLOW_DECAY = 1  # Recirculation current slow decay mode (braking)
+
+    def __init__(self, pos, neg, freq=25000, decay_mode=SLOW_DECAY):
+        self.speed = 0.0
+        self.freq = freq
+        if decay_mode in (self.FAST_DECAY, self.SLOW_DECAY):
+            self.decay_mode = decay_mode
+        else:
+            raise ValueError("Decay mode value must be either Motor.FAST_DECAY or Motor.SLOW_DECAY")
+
+        self.pos_pwm = PWM(Pin(pos))
+        self.pos_pwm.freq(freq)
+        self.neg_pwm = PWM(Pin(neg))
+        self.neg_pwm.freq(freq)
+
+    def get_speed(self):
+        return self.speed
+
+    def set_speed(self, speed):
+        if speed > 1.0 or speed < -1.0:
+            raise ValueError("Speed must be between -1.0 and +1.0")
+        self.speed = speed
+        self._update_pwm()
+
+    def get_frequency(self):
+        return self.freq
+
+    def set_frequency(self, freq):
+        self.pos_pwm.freq(freq)
+        self.neg_pwm.freq(freq)
+        self._update_pwm()
+
+    def get_decay_mode(self):
+        return self.decay_mode
+
+    def set_decay_mode(self, mode):
+        if mode in (self.FAST_DECAY, self.SLOW_DECAY):
+            self.decay_mode = mode
+            self._update_pwm()
+        else:
+            raise ValueError("Decay mode value must be either Motor.FAST_DECAY or Motor.SLOW_DECAY")
+
+    def stop(self):
+        self.speed = 0.0
+        self._update_pwm()
+
+    def disable(self):
+        self.speed = 0.0
+        self.pos_pwm.duty_u16(0)
+        self.neg_pwm.duty_u16(0)
+
+    def _update_pwm(self):
+        signed_duty_cycle = int(self.speed * 0xFFFF)
+
+        if self.decay_mode is self.SLOW_DECAY:  # aka 'Braking'
+            if signed_duty_cycle >= 0:
+                self.pos_pwm.duty_u16(0xFFFF)
+                self.neg_pwm.duty_u16(0xFFFF - signed_duty_cycle)
+            else:
+                self.pos_pwm.duty_u16(0xFFFF + signed_duty_cycle)
+                self.neg_pwm.duty_u16(0xFFFF)
+
+        else:  # aka 'Coasting'
+            if signed_duty_cycle >= 0:
+                self.pos_pwm.duty_u16(signed_duty_cycle)
+                self.neg_pwm.duty_u16(0)
+            else:
+                self.pos_pwm.duty_u16(0)
+                self.neg_pwm.duty_u16(0 - signed_duty_cycle)