Setup and partial implementations of C++ examples

drivers/CMakeLists.txt

@@ -21,6 +21,7 @@ add_subdirectory(bme68x)
 add_subdirectory(bmp280)
 add_subdirectory(bme280)
 add_subdirectory(button)
+add_subdirectory(pid)
 add_subdirectory(plasma)
 add_subdirectory(rgbled)
 add_subdirectory(icp10125)

drivers/analogmux/analogmux.cpp

@@ -77,4 +77,11 @@ namespace pimoroni {
         pull_downs &= ~(1u << address);
     }
   }
+
+  bool AnalogMux::read() {
+    if(muxed_pin != PIN_UNUSED) {
+      return gpio_get(muxed_pin);
+    }
+    return false;
+  }
 }

drivers/analogmux/analogmux.hpp

@@ -14,6 +14,7 @@ namespace pimoroni {
     void select(uint8_t address);
     void disable();
     void configure_pulls(uint8_t address, bool pullup, bool pulldown);
+    bool read();
 
   private:
     uint addr0_pin;

drivers/encoder/encoder.hpp

@@ -6,6 +6,8 @@
 using namespace pimoroni;
 
 namespace encoder {
+  static constexpr float MMME_CPR = 12.0f;
+  static constexpr float ROTARY_CPR = 24.0f;
 
   class Encoder {
     //--------------------------------------------------

drivers/pid/CMakeLists.txt

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

drivers/pid/pid.cmake

@@ -0,0 +1,11 @@
+set(DRIVER_NAME pid)
+add_library(${DRIVER_NAME} INTERFACE)
+
+target_sources(${DRIVER_NAME} INTERFACE
+  ${CMAKE_CURRENT_LIST_DIR}/${DRIVER_NAME}.cpp
+)
+
+target_include_directories(${DRIVER_NAME} INTERFACE ${CMAKE_CURRENT_LIST_DIR})
+
+# Pull in pico libraries that we need
+target_link_libraries(${DRIVER_NAME} INTERFACE pico_stdlib)

drivers/pid/pid.cpp

@@ -0,0 +1,25 @@
+#include "pid.hpp"
+
+namespace pimoroni {
+  PID::PID(float kp, float ki, float kd, float sample_rate)
+  : kp(kp), ki(ki), kd(kd), setpoint(0.0f)
+  , error_sum(0.0f), last_value(0.0f), sample_rate(sample_rate) {
+  }
+
+  float PID::calculate(float value) {
+    float error = setpoint - value;
+    error_sum += error * sample_rate;
+    float rate_error = (value - last_value) / sample_rate;
+    last_value = value;
+
+    return (error * kp) + (error_sum * ki) - (rate_error * kd);
+  }
+
+  float PID::calculate(float value, float value_change) {
+    float error = setpoint - value;
+    error_sum += error * sample_rate;
+    last_value = value;
+
+    return (error * kp) + (error_sum * ki) - (value_change * kd);
+  }
+}

drivers/pid/pid.hpp

@@ -0,0 +1,27 @@
+#pragma once
+
+//#include <stdint.h>
+#include "pico/stdlib.h"
+//include "common/pimoroni_common.hpp"
+
+namespace pimoroni {
+
+  class PID {
+  public:
+    PID(float kp, float ki, float kd, float sample_rate);
+
+    float calculate(float value);
+    float calculate(float value, float value_change);
+
+  public:
+    float kp;
+    float ki;
+    float kd;
+    float setpoint;
+  private:
+    float error_sum;
+    float last_value;
+    float sample_rate;
+  };
+
+}

examples/motor2040/CMakeLists.txt

@@ -1,10 +1,17 @@
-#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)
+include(motor2040_led_rainbow.cmake)
+include(motor2040_motor_cluster.cmake)
+include(motor2040_motor_profiler.cmake)
+include(motor2040_motor_wave.cmake)
+include(motor2040_multiple_motors.cmake)
+include(motor2040_position_control.cmake)
+include(motor2040_position_tuning.cmake)
+include(motor2040_position_on_velocity_control.cmake)
+include(motor2040_position_on_velocity_tuning.cmake)
+include(motor2040_quad_position_wave.cmake)
+include(motor2040_quad_velocity_sequence.cmake)
+include(motor2040_reactive_encoder.cmake)
+include(motor2040_read_encoders.cmake)
+include(motor2040_read_sensors.cmake)
+include(motor2040_single_motor.cmake)
+include(motor2040_velocity_control.cmake)
+include(motor2040_velocity_tuning.cmake)

examples/motor2040/README.md

@@ -0,0 +1,135 @@
+# Motor 2040 C++ Examples <!-- omit in toc -->
+
+- [Motor Examples](#motor-examples)
+  - [Single Motor](#single-motor)
+  - [Multiple Motors](#multiple-motors)
+  - [Motor Cluster](#motor-cluster)
+  - [Motor Wave](#motor-wave)
+  - [Stop Motors](#stop-motors)
+- [Function Examples](#function-examples)
+  - [Read Sensors](#read-sensors)
+  - [Read Encoders](#read-encoders)
+  - [Motor Profiler](#motor-profiler)
+  - [LED Rainbow](#led-rainbow)
+  - [Turn Off LEDs](#turn-off-leds)
+- [Control Examples](#control-examples)
+  - [Position Control](#position-control)
+  - [Velocity Control](#velocity-control)
+  - [Position on Velocity Control](#position-on-velocity-control)
+  - [Reactive Encoder](#reactive-encoder)
+  - [Quad Position Wave](#quad-position-wave)
+  - [Quad Velocity Sequence](#quad-velocity-sequence)
+- [Tuning Examples](#tuning-examples)
+  - [Position Tuning](#position-tuning)
+  - [Velocity Tuning](#velocity-tuning)
+  - [Position on Velocity Tuning](#position-on-velocity-tuning)
+
+
+## Motor Examples
+
+### Single Motor
+[motor2040_single_motor.py](motor2040_single_motor.py)
+
+Demonstrates how to create a Motor object and control it.
+
+
+### Multiple Motors
+[motor2040_multiple_motors.py](motor2040_multiple_motors.py)
+
+Demonstrates how to create multiple Motor objects and control them together.
+
+
+### Motor Cluster
+[motor2040_motor_cluster.py](motor2040_motor_cluster.py)
+
+Demonstrates how to create a MotorCluster object to control multiple motors at once.
+
+
+### Motor Wave
+[motor2040_motor_wave.py](motor2040_motor_wave.py)
+
+An example of applying a wave pattern to a group of motors and the LEDs.
+
+
+## Function Examples
+
+### Read Sensors
+[motor2040_read_sensors.py](motor2040_read_sensors.py)
+
+Shows how to initialise and read the 2 external and 6 internal sensors of Motor 2040.
+
+
+### Read Encoders
+[motor2040_read_encoders.py](motor2040_read_encoders.py)
+
+Demonstrates how to read the angles of Motor 2040's four encoders.
+
+
+### Motor Profiler
+[motor2040_motor_profiler.py](motor2040_motor_profiler.py)
+
+A program that profiles the speed of a motor across its PWM
+duty cycle range using the attached encoder for feedback.
+
+
+### LED Rainbow
+[motor2040_led_rainbow.py](motor2040_led_rainbow.py)
+
+Displays a rotating rainbow pattern on the Motor 2040's onboard LED.
+
+
+## Control Examples
+
+### Position Control
+[motor2040_position_control.py](motor2040_position_control.py)
+
+An example of how to move a motor smoothly between random positions, with the help of it's attached encoder and PID control.
+
+
+### Velocity Control
+[motor2040_velocity_control.py](motor2040_velocity_control.py)
+
+An example of how to drive a motor smoothly between random speeds, with the help of it's attached encoder and PID control.
+
+
+### Position on Velocity Control
+[motor2040_position_on_velocity_control.py](motor2040_position_on_velocity_control.py)
+
+An example of how to move a motor smoothly between random positions, with velocity limits, with the help of it's attached encoder and PID control.
+
+
+### Reactive Encoder
+[motor2040_reactive_encoder.py](motor2040_reactive_encoder.py)
+
+A demonstration of how a motor with an encoder can be used as a programmable rotary encoder for user input, with force-feedback for arbitrary detents and end stops.
+
+
+### Quad Position Wave
+[motor2040_quad_position_wave.py](motor2040_quad_position_wave.py)
+
+A demonstration of driving all four of Motor 2040's motor outputs between positions, with the help of their attached encoders and PID control.
+
+### Quad Velocity Sequence
+[motor2040_quad_velocity_sequence.py](motor2040_quad_velocity_sequence.py)
+
+A demonstration of driving all four of Motor 2040's motor outputs through a sequence of velocities, with the help of their attached encoders and PID control.
+
+
+## Tuning Examples
+
+### Position Tuning
+[motor2040_position_tuning.py](motor2040_position_tuning.py)
+
+A program to aid in the discovery and tuning of motor PID values for position control. It does this by commanding the motor to move repeatedly between two setpoint angles and plots the measured response.
+
+
+### Velocity Tuning
+[motor2040_velocity_tuning.py](motor2040_velocity_tuning.py)
+
+A program to aid in the discovery and tuning of motor PID values for velocity control. It does this by commanding the motor to drive repeatedly between two setpoint speeds and plots the measured response.
+
+
+### Position on Velocity Tuning
+[motor2040_position_on_velocity_tuning.py](motor2040_position_on_velocity_tuning.py)
+
+A program to aid in the discovery and tuning of motor PID values for position on velocity control. It does this by commanding the motor to move repeatedly between two setpoint angles and plots the measured response.

examples/motor2040/motor2040_led_rainbow.cmake

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

examples/motor2040/motor2040_led_rainbow.cpp

@@ -0,0 +1,56 @@
+#include "pico/stdlib.h"
+
+#include "motor2040.hpp"
+#include "button.hpp"
+
+/*
+Displays a rotating rainbow pattern on the Motor 2040's onboard LED.
+
+Press "Boot" to exit the program.
+*/
+
+using namespace plasma;
+using namespace motor;
+
+// The speed that the LED will cycle at
+const uint SPEED = 5;
+
+// The brightness of the LED
+constexpr float BRIGHTNESS = 0.4f;
+
+// How many times the LED will be updated per second
+const uint UPDATES = 50;
+
+
+// Create the LED, using PIO 1 and State Machine 0
+WS2812 led(motor2040::NUM_LEDS, pio1, 0, motor2040::LED_DATA);
+
+// Create the user button
+Button user_sw(motor2040::USER_SW);
+
+
+int main() {
+  stdio_init_all();
+
+  // Start updating the LED
+  led.start();
+
+  float hue = 0.0f;
+
+  // Make rainbows until the user button is pressed
+  while(!user_sw.raw()) {
+
+    hue += (float)SPEED / 1000.0f;
+
+    // Update the LED
+    led.set_hsv(0, hue, 1.0f, BRIGHTNESS);
+
+    sleep_ms(1000 / UPDATES);
+  }
+
+  // Turn off the LED
+  led.clear();
+
+  // Sleep a short time so the clear takes effect
+  sleep_ms(100);
+}

examples/motor2040/motor2040_motor_cluster.cmake

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

examples/motor2040/motor2040_motor_cluster.cpp

@@ -0,0 +1,79 @@
+#include "pico/stdlib.h"
+
+#include "motor2040.hpp"
+
+/*
+Demonstrates how to create multiple Motor objects and control them together.
+*/
+
+using namespace motor;
+
+// How many sweeps of the motors to perform
+const uint SWEEPS = 2;
+
+// 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 drive the motors when sweeping
+constexpr float SPEED_EXTENT = 1.0f;
+
+// Create a motor cluster, using PIO 0 and State Machine 0
+const pin_pair motor_pins[] = {motor2040::MOTOR_A, motor2040::MOTOR_B,
+                               motor2040::MOTOR_C, motor2040::MOTOR_D};
+const uint NUM_MOTORS = count_of(motor_pins);
+MotorCluster motors = MotorCluster(pio0, 0, motor_pins, NUM_MOTORS);
+
+
+int main() {
+  stdio_init_all();
+
+  // Initialise the motor cluster
+  motors.init();
+
+  // Enable all motors
+  motors.enable_all();
+  sleep_ms(2000);
+
+  // Drive at full positive
+  motors.all_full_positive();
+  sleep_ms(2000);
+
+  // Stop all moving
+  motors.stop_all();
+  sleep_ms(2000);
+
+  // Drive at full negative
+  motors.all_full_negative();
+  sleep_ms(2000);
+
+  // Coast all to a gradual stop
+  motors.coast_all();
+  sleep_ms(2000);
+
+  // Do a sine speed sweep
+  for(auto j = 0u; j < SWEEPS; j++) {
+    for(auto i = 0u; i < 360; i++) {
+      float speed = sin(((float)i * (float)M_PI) / 180.0f) * SPEED_EXTENT;
+      motors.all_to_speed(speed);
+      sleep_ms(20);
+    }
+  }
+
+  // Do a stepped speed sweep
+  for(auto j = 0u; j < SWEEPS; j++) {
+    for(auto i = 0u; i < STEPS; i++) {
+      motors.all_to_percent(i, 0, STEPS, 0.0 - SPEED_EXTENT, SPEED_EXTENT);
+      sleep_ms(STEPS_INTERVAL_MS);
+    }
+    for(auto i = 0u; i < STEPS; i++) {
+      motors.all_to_percent(i, STEPS, 0, 0.0 - SPEED_EXTENT, SPEED_EXTENT);
+      sleep_ms(STEPS_INTERVAL_MS);
+    }
+  }
+
+  // Disable the motors
+  motors.disable_all();
+}

examples/motor2040/motor2040_motor_profiler.cmake

@@ -0,0 +1,12 @@
+set(OUTPUT_NAME motor2040_motor_profiler)
+add_executable(${OUTPUT_NAME} motor2040_motor_profiler.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_motor_profiler.cpp

@@ -0,0 +1,73 @@
+#include "pico/stdlib.h"
+
+#include "motor2040.hpp"
+
+/*
+A program that profiles the speed of a motor across its PWM
+duty cycle range using the attached encoder for feedback.
+*/
+
+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
+Motor m = Motor(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/motor2040/motor2040_motor_wave.cmake

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

examples/motor2040/motor2040_motor_wave.cpp

@@ -0,0 +1,82 @@
+#include "pico/stdlib.h"
+
+#include "motor2040.hpp"
+#include "button.hpp"
+
+/*
+An example of applying a wave pattern to a group of motors and the LED.
+
+Press "Boot" to exit the program.
+*/
+
+using namespace plasma;
+using namespace motor;
+
+// The speed that the LEDs and motors will cycle at
+const uint SPEED = 5;
+
+// The brightness of the LEDs
+constexpr float BRIGHTNESS = 0.4;
+
+// How many times to update LEDs and motors per second
+const uint UPDATES = 50;
+
+// How far from zero to drive the motors
+constexpr float SPEED_EXTENT = 1.0f;
+
+
+// Create an array of motor pointers
+const pin_pair motor_pins[] = {motor2040::MOTOR_A, motor2040::MOTOR_B,
+                               motor2040::MOTOR_C, motor2040::MOTOR_D};
+const uint NUM_MOTORS = count_of(motor_pins);
+Motor *motors[NUM_MOTORS];
+
+// Create the LED, using PIO 1 and State Machine 0
+WS2812 led(motor2040::NUM_LEDS, pio1, 0, motor2040::LED_DATA);
+
+// Create the user button
+Button user_sw(motor2040::USER_SW);
+
+
+int main() {
+  stdio_init_all();
+
+  // Fill the array of motors, and initialise them. Up to 8 motors can be created
+  for(auto m = 0u; m < NUM_MOTORS; m++) {
+    motors[m] = new Motor(motor_pins[m]);
+    motors[m]->init();
+  }
+
+  // Start updating the LED
+  led.start();
+
+  float offset = 0.0f;
+
+  // Make rainbows until the user button is pressed
+  while(!user_sw.raw()) {
+
+    offset += (float)SPEED / 1000.0f;
+
+    // Update the LED
+    led.set_hsv(0, offset / 2.0f, 1.0f, BRIGHTNESS);
+
+    // Update all the motors
+      for(auto m = 0u; m < NUM_MOTORS; m++) {
+        float angle = (((float)m / (float)NUM_MOTORS) + offset) * (float)M_TWOPI;
+        motors[m]->speed(sin(angle) * SPEED_EXTENT);
+    }
+
+    sleep_ms(1000 / UPDATES);
+  }
+
+  // Stop all the motors
+  for(auto m = 0u; m < NUM_MOTORS; m++) {
+    motors[m]->disable();
+  }
+
+  // Turn off the LED
+  led.clear();
+
+  // Sleep a short time so the clear takes effect
+  sleep_ms(100);
+}

examples/motor2040/motor2040_multiple_motors.cmake

@@ -0,0 +1,12 @@
+set(OUTPUT_NAME motor2040_multiple_motors)
+add_executable(${OUTPUT_NAME} motor2040_multiple_motors.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_multiple_motors.cpp

@@ -0,0 +1,100 @@
+#include "pico/stdlib.h"
+
+#include "motor2040.hpp"
+
+/*
+Demonstrates how to create multiple Motor objects and control them together.
+*/
+
+using namespace motor;
+
+// How many sweeps of the motors to perform
+const uint SWEEPS = 2;
+
+// 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 drive the motors when sweeping
+constexpr float SPEED_EXTENT = 1.0f;
+
+// Create an array of motor pointers
+const pin_pair motor_pins[] = {motor2040::MOTOR_A, motor2040::MOTOR_B,
+                               motor2040::MOTOR_C, motor2040::MOTOR_D};
+const uint NUM_MOTORS = count_of(motor_pins);
+Motor *motors[NUM_MOTORS];
+
+
+int main() {
+  stdio_init_all();
+
+  // Fill the array of motors, and initialise them. Up to 8 motors can be created
+  for(auto m = 0u; m < NUM_MOTORS; m++) {
+    motors[m] = new Motor(motor_pins[m]);
+    motors[m]->init();
+  }
+
+  // Enable all motors
+  for(auto m = 0u; m < NUM_MOTORS; m++) {
+    motors[m]->enable();
+  }
+  sleep_ms(2000);
+
+  // Drive at full positive
+  for(auto m = 0u; m < NUM_MOTORS; m++) {
+    motors[m]->full_positive();
+  }
+  sleep_ms(2000);
+
+  // Stop all moving
+  for(auto m = 0u; m < NUM_MOTORS; m++) {
+    motors[m]->stop();
+  }
+  sleep_ms(2000);
+
+  // Drive at full negative
+  for(auto m = 0u; m < NUM_MOTORS; m++) {
+    motors[m]->full_negative();
+  }
+  sleep_ms(2000);
+
+  // Coast all to a gradual stop
+  for(auto m = 0u; m < NUM_MOTORS; m++) {
+    motors[m]->coast();
+  }
+  sleep_ms(2000);
+
+  // Do a sine speed sweep
+  for(auto j = 0u; j < SWEEPS; j++) {
+    for(auto i = 0u; i < 360; i++) {
+      float speed = sin(((float)i * (float)M_PI) / 180.0f) * SPEED_EXTENT;
+      for(auto m = 0u; m < NUM_MOTORS; m++) {
+        motors[m]->speed(speed);
+      }
+      sleep_ms(20);
+    }
+  }
+
+  // Do a stepped speed sweep
+  for(auto j = 0u; j < SWEEPS; j++) {
+    for(auto i = 0u; i < STEPS; i++) {
+      for(auto m = 0u; m < NUM_MOTORS; m++) {
+        motors[m]->to_percent(i, 0, STEPS, 0.0 - SPEED_EXTENT, SPEED_EXTENT);
+      }
+      sleep_ms(STEPS_INTERVAL_MS);
+    }
+    for(auto i = 0u; i < STEPS; i++) {
+      for(auto m = 0u; m < NUM_MOTORS; m++) {
+        motors[m]->to_percent(i, STEPS, 0, 0.0 - SPEED_EXTENT, SPEED_EXTENT);
+      }
+      sleep_ms(STEPS_INTERVAL_MS);
+    }
+  }
+
+  // Disable the motors
+  for(auto m = 0u; m < NUM_MOTORS; m++) {
+    motors[m]->disable();
+  }
+}

examples/motor2040/motor2040_position_control.cmake

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

examples/motor2040/motor2040_position_control.cpp

@@ -0,0 +1,75 @@
+#include "pico/stdlib.h"
+
+#include "motor2040.hpp"
+
+/*
+An example of how to move a motor smoothly between random positions,
+with the help of it's attached encoder and PID control.
+
+Press "Boot" to exit the program.
+*/
+
+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
+Motor m = Motor(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/motor2040/motor2040_position_on_velocity_control.cmake

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

examples/motor2040/motor2040_position_on_velocity_control.cpp

@@ -0,0 +1,75 @@
+#include "pico/stdlib.h"
+
+#include "motor2040.hpp"
+
+/*
+An example of how to move a motor smoothly between random positions,
+with velocity limits, with the help of it's attached encoder and PID control.
+
+Press "Boot" to exit the program.
+*/
+
+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
+Motor m = Motor(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/motor2040/motor2040_position_on_velocity_tuning.cmake

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

examples/motor2040/motor2040_position_on_velocity_tuning.cpp

@@ -0,0 +1,77 @@
+#include "pico/stdlib.h"
+
+#include "motor2040.hpp"
+
+/*
+A program to aid in the discovery and tuning of motor PID
+values for position on velocity control. It does this by
+commanding the motor to move repeatedly between two setpoint
+angles and plots the measured response.
+
+Press "Boot" to exit the program.
+*/
+
+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
+Motor m = Motor(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/motor2040/motor2040_position_tuning.cmake

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

examples/motor2040/motor2040_position_tuning.cpp

@@ -0,0 +1,131 @@
+#include <cstdio>
+#include "pico/stdlib.h"
+
+#include "motor2040.hpp"
+#include "button.hpp"
+#include "pid.hpp"
+
+/*
+A program to aid in the discovery and tuning of motor PID
+values for position control. It does this by commanding the
+motor to move repeatedly between two setpoint angles and
+plots the measured response.
+
+Press "Boot" to exit the program.
+*/
+
+using namespace motor;
+using namespace encoder;
+
+// The pins of the motor being profiled
+const pin_pair MOTOR_PINS = motor2040::MOTOR_A;
+
+// The pins of the encoder attached to the profiled motor
+const pin_pair ENCODER_PINS = motor2040::ENCODER_A;
+
+// The gear ratio of the motor
+constexpr float GEAR_RATIO = 50.0f;
+
+// The counts per revolution of the motor's output shaft
+constexpr float COUNTS_PER_REV = MMME_CPR * GEAR_RATIO;
+
+// The direction to spin the motor in. NORMAL_DIR (0), REVERSED_DIR (1)
+const Direction DIRECTION = NORMAL_DIR;
+
+// The scaling to apply to the motor's speed to match its real-world speed
+float SPEED_SCALE = 5.4f;
+
+// How many times to update the motor per second
+const uint UPDATES = 100;
+constexpr float UPDATE_RATE = 1.0f / (float)UPDATES;
+
+// The time (in seconds) after a new setpoint, to display print out motor values
+constexpr float PRINT_WINDOW = 0.25f;
+
+// The time (in seconds) between each new setpoint being set
+constexpr float MOVEMENT_WINDOW = 2.0f;
+
+// How many of the updates should be printed (i.e. 2 would be every other update)
+const uint PRINT_DIVIDER = 1;
+
+// Multipliers for the different printed values, so they appear nicely on the Thonny plotter
+constexpr float SPD_PRINT_SCALE = 10.0f;    // Driving Speed multipler
+
+// How far from zero to move the motor, in degrees
+constexpr float POSITION_EXTENT = 90.0f;
+
+// PID values
+constexpr float POS_KP = 0.14;              // Position proportional (P) gain
+constexpr float POS_KI = 0.0;               // Position integral (I) gain
+constexpr float POS_KD = 0.0022;            // Position derivative (D) gain
+
+
+// Create a motor and set its direction and speed scale
+Motor m = Motor(MOTOR_PINS, DIRECTION, SPEED_SCALE);
+
+// Create an encoder and set its direction and counts per rev, using PIO 0 and State Machine 0
+Encoder enc = Encoder(pio0, 0, ENCODER_PINS, PIN_UNUSED, DIRECTION, COUNTS_PER_REV, true);
+
+// Create the user button
+Button user_sw(motor2040::USER_SW);
+
+// Create PID object for position control
+PID pos_pid = PID(POS_KP, POS_KI, POS_KD, UPDATE_RATE);
+
+
+int main() {
+  stdio_init_all();
+
+  // Initialise the motor and encoder
+  m.init();
+  enc.init();
+
+  // Enable the motor
+  m.enable();
+
+  // Set the initial setpoint position
+  pos_pid.setpoint = POSITION_EXTENT;
+
+
+  uint update = 0;
+  uint print_count = 0;
+
+  // Continually move the motor until the user button is pressed
+  while(!user_sw.raw()) {
+
+    // Capture the state of the encoder
+    Encoder::Capture capture = enc.capture();
+
+    // Calculate the velocity to move the motor closer to the position setpoint
+    float vel = pos_pid.calculate(capture.degrees(), capture.degrees_per_second());
+
+    // Set the new motor driving speed
+    m.speed(vel);
+
+    // Print out the current motor values and their setpoints,
+    // but only for the first few updates and only every multiple
+    if(update < (PRINT_WINDOW * UPDATES) && print_count == 0) {
+        printf("Pos = %f, ", capture.degrees());
+        printf("Pos SP = %f, ", pos_pid.setpoint);
+        printf("Speed = %f\n", m.speed() * SPD_PRINT_SCALE);
+    }
+
+    // Increment the print count, and wrap it
+    print_count = (print_count + 1) % PRINT_DIVIDER;
+
+    update++;   // Move along in time
+
+    // Have we reached the end of this time window?
+    if(update >= (MOVEMENT_WINDOW * UPDATES)){
+        update = 0;  // Reset the counter
+
+        // Set the new position setpoint to be the inverse of the current setpoint
+        pos_pid.setpoint = 0.0 - pos_pid.setpoint;
+    }
+
+    sleep_ms(UPDATE_RATE * 1000.0f);
+  }
+
+  // Disable the motor
+  m.disable();
+}

examples/motor2040/motor2040_quad_position_wave.cmake

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

examples/motor2040/motor2040_quad_position_wave.cpp

@@ -0,0 +1,75 @@
+#include "pico/stdlib.h"
+
+#include "motor2040.hpp"
+
+/*
+A demonstration of driving all four of Motor 2040's motor outputs between
+positions, with the help of their attached encoders and PID control.
+
+Press "Boot" to exit the program.
+*/
+
+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
+Motor m = Motor(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/motor2040/motor2040_quad_velocity_sequence.cmake

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

examples/motor2040/motor2040_quad_velocity_sequence.cpp

@@ -0,0 +1,75 @@
+#include "pico/stdlib.h"
+
+#include "motor2040.hpp"
+
+/*
+A demonstration of driving all four of Motor 2040's motor outputs through a
+sequence of velocities, with the help of their attached encoders and PID control.
+
+Press "Boot" to exit the program.
+*/
+
+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
+Motor m = Motor(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/motor2040/motor2040_reactive_encoder.cmake

@@ -0,0 +1,12 @@
+set(OUTPUT_NAME motor2040_reactive_encoder)
+add_executable(${OUTPUT_NAME} motor2040_reactive_encoder.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_reactive_encoder.cpp

@@ -0,0 +1,76 @@
+#include "pico/stdlib.h"
+
+#include "motor2040.hpp"
+
+/*
+A demonstration of how a motor with an encoder can be used
+as a programmable rotary encoder for user input, with
+force-feedback for arbitrary detents and end stops.
+
+Press "Boot" to exit the program.
+*/
+
+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
+Motor m = Motor(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/motor2040/motor2040_read_encoders.cmake

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

examples/motor2040/motor2040_read_encoders.cpp

@@ -0,0 +1,60 @@
+#include <cstdio>
+#include "pico/stdlib.h"
+
+#include "motor2040.hpp"
+#include "button.hpp"
+
+/*
+Demonstrates how to read the angles of Motor 2040's four encoders.
+
+Press "Boot" to exit the program.
+*/
+
+using namespace motor;
+using namespace encoder;
+
+// The gear ratio of the motor
+constexpr float GEAR_RATIO = 50.0f;
+
+// The counts per revolution of the motor's output shaft
+constexpr float COUNTS_PER_REV = MMME_CPR * GEAR_RATIO;
+
+
+// Create an array of encoder pointers
+const pin_pair encoder_pins[] = {motor2040::ENCODER_A, motor2040::ENCODER_B,
+                                 motor2040::ENCODER_C, motor2040::ENCODER_D};
+const char* ENCODER_NAMES[] = {"A", "B", "C", "D"};
+const uint NUM_ENCODERS = count_of(encoder_pins);
+Encoder *encoders[NUM_ENCODERS];
+
+// Create the user button
+Button user_sw(motor2040::USER_SW);
+
+
+int main() {
+  stdio_init_all();
+
+  // Fill the array of motors, and initialise them. Up to 8 motors can be created
+  for(auto e = 0u; e < NUM_ENCODERS; e++) {
+    encoders[e] = new Encoder(pio0, e, encoder_pins[e], PIN_UNUSED, NORMAL_DIR, COUNTS_PER_REV, true);
+    encoders[e]->init();
+  }
+
+  // Uncomment the below lines to reverse
+  // the counting direction of an encoder
+  // encoders[0].direction(REVERSED_DIR);
+  // encoders[1].direction(REVERSED_DIR);
+  // encoders[2].direction(REVERSED_DIR);
+  // encoders[3].direction(REVERSED_DIR);
+
+  // Read the encoders until the user button is pressed
+  while(!user_sw.raw()) {
+    // Print out the angle of each encoder
+    for(auto e = 0u; e < NUM_ENCODERS; e++) {
+      printf("%s = %f, ", ENCODER_NAMES[e], encoders[e]->degrees());
+    }
+    printf("\n");
+
+    sleep_ms(100);
+  }
+}

examples/motor2040/motor2040_read_sensors.cmake

@@ -0,0 +1,15 @@
+set(OUTPUT_NAME motor2040_read_sensors)
+add_executable(${OUTPUT_NAME} motor2040_read_sensors.cpp)
+
+target_link_libraries(${OUTPUT_NAME}
+        pico_stdlib
+        motor2040
+        analogmux
+        analog
+        button
+        )
+
+# enable usb output
+pico_enable_stdio_usb(${OUTPUT_NAME} 1)
+
+pico_add_extra_outputs(${OUTPUT_NAME})

examples/motor2040/motor2040_read_sensors.cpp

@@ -0,0 +1,68 @@
+#include <cstdio>
+#include "pico/stdlib.h"
+
+#include "motor2040.hpp"
+#include "analogmux.hpp"
+#include "analog.hpp"
+#include "button.hpp"
+
+/*
+Shows how to initialise and read the 2 external
+and 6 internal sensors of Motor 2040.
+
+Press "Boot" to exit the program.
+*/
+
+using namespace motor;
+
+// Set up the shared analog inputs
+Analog sen_adc = Analog(motor2040::SHARED_ADC);
+Analog vol_adc = Analog(motor2040::SHARED_ADC, motor2040::VOLTAGE_GAIN);
+Analog cur_adc = Analog(motor2040::SHARED_ADC, motor2040::CURRENT_GAIN,
+                        motor2040::SHUNT_RESISTOR, motor2040::CURRENT_OFFSET);
+
+// Set up the analog multiplexer, including the pin for controlling pull-up/pull-down
+AnalogMux mux = AnalogMux(motor2040::ADC_ADDR_0, motor2040::ADC_ADDR_1, motor2040::ADC_ADDR_2,
+                          PIN_UNUSED, motor2040::SHARED_ADC);
+
+// Create the user button
+Button user_sw(motor2040::USER_SW);
+
+
+int main() {
+  stdio_init_all();
+
+  // Set up the sensor addresses with pull downs
+  for(auto i = 0u; i < motor2040::NUM_SENSORS; i++) {
+    mux.configure_pulls(motor2040::SENSOR_1_ADDR + i, false, true);
+  }
+
+  // Set up the pull-up for the fault sense
+  mux.configure_pulls(motor2040::FAULT_SENSE_ADDR, true, false);
+
+  // Read sensors until the user button is pressed
+  while(!user_sw.raw()) {
+
+    // Read each sensor in turn and print its voltage
+    for(auto i = 0u; i < motor2040::NUM_SENSORS; i++) {
+      mux.select(motor2040::SENSOR_1_ADDR + i);
+      printf("S%d = %f, ", i + 1, sen_adc.read_voltage());
+    }
+
+    // Read the voltage sense and print the value
+    mux.select(motor2040::VOLTAGE_SENSE_ADDR);
+    printf("Voltage = %f, ", vol_adc.read_voltage());
+
+    // Read the current sense's of each motor and print the value
+    for(auto i = 0u; i < motor2040::NUM_MOTORS; i++) {
+      mux.select(motor2040::CURRENT_SENSE_A_ADDR + i);
+      printf("C%d = %f, ", i + 1, cur_adc.read_current());
+    }
+
+    // Read the fault sense and print the value
+    mux.select(motor2040::FAULT_SENSE_ADDR);
+    printf("Fault = %s\n", mux.read() ? "false" : "true");
+
+    sleep_ms(500);
+  }
+}

examples/motor2040/motor2040_single_motor.cpp

@@ -9,7 +9,7 @@ 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;
+const uint SWEEPS = 2;
 
 // The number of discrete sweep steps
 const uint STEPS = 10;
@@ -18,10 +18,10 @@ const uint STEPS = 10;
 const uint STEPS_INTERVAL_MS = 500;
 
 // How far from zero to move the motor when sweeping
-constexpr float SWEEP_EXTENT = 90.0f;
+constexpr float SPEED_EXTENT = 10.0f;
 
 
-// Create a motor on pin 0 and 1
+// Create a motor
 Motor m = Motor(motor2040::MOTOR_A);
 
 
@@ -35,34 +35,39 @@ int main() {
   m.enable();
   sleep_ms(2000);
 
-  // Go at full neative
-  m.full_negative();
-  sleep_ms(2000);
-
-  // Go at full positive
+  // Drive at full positive
   m.full_positive();
   sleep_ms(2000);
 
-  // Stop the motor
+  // Stop moving
   m.stop();
   sleep_ms(2000);
 
-  // Do a sine sweep
+  // Drive at full negative
+  m.full_negative();
+  sleep_ms(2000);
+
+  // Coast to a gradual stop
+  m.coast();
+  sleep_ms(2000);
+
+
+  // Do a sine speed 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);
+      m.speed(sin(((float)i * (float)M_PI) / 180.0f) * SPEED_EXTENT);
       sleep_ms(20);
     }
   }
 
-  // Do a stepped sweep
+  // Do a stepped speed 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);
+      m.to_percent(i, 0, STEPS, 0.0 - SPEED_EXTENT, SPEED_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);
+      m.to_percent(i, STEPS, 0, 0.0 - SPEED_EXTENT, SPEED_EXTENT);
       sleep_ms(STEPS_INTERVAL_MS);
     }
   }

examples/motor2040/motor2040_velocity_control.cmake

@@ -0,0 +1,12 @@
+set(OUTPUT_NAME motor2040_velocity_control)
+add_executable(${OUTPUT_NAME} motor2040_velocity_control.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_velocity_control.cpp

@@ -0,0 +1,75 @@
+#include "pico/stdlib.h"
+
+#include "motor2040.hpp"
+
+/*
+An example of how to drive a motor smoothly between random speeds,
+with the help of it's attached encoder and PID control.
+
+Press "Boot" to exit the program.
+*/
+
+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
+Motor m = Motor(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/motor2040/motor2040_velocity_tuning.cmake

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

examples/motor2040/motor2040_velocity_tuning.cpp

@@ -0,0 +1,77 @@
+#include "pico/stdlib.h"
+
+#include "motor2040.hpp"
+
+/*
+A program to aid in the discovery and tuning of motor PID
+values for velocity control. It does this by commanding the
+motor to drive repeatedly between two setpoint speeds and
+plots the measured response.
+
+Press "Boot" to exit the program.
+*/
+
+using namespace motor;
+
+// How many sweeps of the motor to perform
+const uint SWEEPS = 2;
+
+// 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 drive the motor when sweeping
+constexpr float SWEEP_EXTENT = 90.0f;
+
+
+// Create a motor on pin 0 and 1
+Motor m = Motor(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();
+}

libraries/motor2040/motor2040.cmake

@@ -3,4 +3,4 @@ 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 motor motor_cluster)
+target_link_libraries(motor2040 INTERFACE pico_stdlib plasma motor motor_cluster encoder)

libraries/motor2040/motor2040.hpp

@@ -4,6 +4,7 @@
 #include "ws2812.hpp"
 #include "motor.hpp"
 #include "motor_cluster.hpp"
+#include "encoder.hpp"
 
 namespace motor {
   namespace motor2040 {

micropython/examples/motor2040/led_rainbow.py

@@ -13,9 +13,9 @@ 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
+SPEED = 5           # The speed that the LED will cycle at
+BRIGHTNESS = 0.4    # The brightness of the LED
+UPDATES = 50        # How many times the LED 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)

micropython/examples/motor2040/motor_wave.py

@@ -47,7 +47,7 @@ while user_sw.raw() is not True:
     # Update the LED
     led.set_hsv(0, offset / 2, 1.0, BRIGHTNESS)
 
-    # Update all the MOTORs
+    # Update all the motors
     for i in range(len(motors)):
         angle = ((i / len(motors)) + offset) * math.pi
         motors[i].speed(math.sin(angle) * SPEED_EXTENT)
@@ -58,5 +58,5 @@ while user_sw.raw() is not True:
 for m in motors:
     m.disable()
 
-# Turn off the LED bar
+# Turn off the LED
 led.clear()

micropython/examples/motor2040/multiple_motors.py

@@ -36,7 +36,7 @@ for m in motors:
 time.sleep(2)
 
 
-SWEEPS = 2              # How many speed sweeps of the motor to perform
+SWEEPS = 2              # How many speed sweeps of the motors 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 drive the motors when sweeping

micropython/examples/motor2040/position_tuning.py

@@ -41,10 +41,10 @@ POS_KD = 0.0022                         # Position derivative (D) gain
 # Free up hardware resources ahead of creating a new Encoder
 gc.collect()
 
-# Create a motor and set its speed scale
-m = Motor(MOTOR_PINS, direction=DIRECTION, speed_scale=SPEED_SCALE, deadzone=0.0)
+# Create a motor and set its direction and speed scale
+m = Motor(MOTOR_PINS, direction=DIRECTION, speed_scale=SPEED_SCALE)
 
-# Create an encoder, using PIO 0 and State Machine 0
+# Create an encoder and set its direction and counts per rev, using PIO 0 and State Machine 0
 enc = Encoder(0, 0, ENCODER_PINS, direction=DIRECTION, counts_per_rev=COUNTS_PER_REV, count_microsteps=True)
 
 # Create the user button
@@ -96,5 +96,5 @@ while user_sw.raw() is not True:
 
     time.sleep(UPDATE_RATE)
 
-# Disable the servo
+# Disable the motor
 m.disable()