Ported the majority of Servo2040 MP examples to C++

examples/servo2040/CMakeLists.txt

@@ -1,2 +1,7 @@
-include(servo2040_functions.cmake)
-include(servo2040_rainbow.cmake)
+include(servo2040_calibration.cmake)
+include(servo2040_led_rainbow.cmake)
+include(servo2040_multiple_servos.cmake)
+include(servo2040_servo_cluster.cmake)
+include(servo2040_servo_wave.cmake)
+include(servo2040_simple_easing.cmake)
+include(servo2040_single_servo.cmake)

examples/servo2040/servo2040_calibration.cmake

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

examples/servo2040/servo2040_calibration.cpp

@@ -0,0 +1,122 @@
+#include <cstdio>
+#include "pico/stdlib.h"
+
+#include "servo2040.hpp"
+
+/*
+Shows how to create servos with different common
+calibrations, modify a servo's existing calibration,
+and create a servo with a custom calibration.
+*/
+
+using namespace servo;
+
+// Function for printing out the details of a Calibration object
+void print_calibration(const Calibration &calib) {
+  printf("Calibration(");
+
+  uint size = calib.size();
+  printf("size = %d", size);
+
+  printf(", pairs = {");
+  for(uint i = 0; i < size; i++) {
+      const Calibration::Pair &pair = calib.pair(i);
+      printf("{%f, %f}",pair.pulse, pair.value);
+      if(i < size - 1)
+          printf(", ");
+  }
+  printf("}, limit_lower = %s", calib.has_lower_limit() ? "true" : "false");
+  printf(", limit_upper = %s", calib.has_upper_limit() ? "true" : "false");
+  printf(")\n\n");
+}
+
+int main() {
+  stdio_init_all();
+
+  // Sleep 8 seconds to give enough time to connect up a terminal
+  sleep_ms(8000);
+
+  // -----------------------------------------------------
+  // Create and modify the calibration of an angular servo
+  // -----------------------------------------------------
+
+  // Create an angular servo on pin 0. By default its value ranges from -90 to +90
+  Servo angular_servo = Servo(servo2040::SERVO_1, ANGULAR);
+
+  // Get a reference to the calibration and print it out
+  Calibration &acal = angular_servo.calibration();
+  printf("Angular Servo: ");
+  print_calibration(acal);
+
+  // The range we want the anglular servo to cover
+  constexpr float WIDE_ANGLE_RANGE = 270.0f;
+
+  // Lets modify the calibration to increase its range
+  acal.first().value = -WIDE_ANGLE_RANGE / 2;
+  acal.last().value = WIDE_ANGLE_RANGE / 2;
+
+  // As the calibration was a reference, the servo has already
+  // been updated, but lets access it again to confirm it worked
+  printf("Wide Angle Servo: ");
+  print_calibration(angular_servo.calibration());
+
+
+  // ---------------------------------------------------
+  // Create and modify the calibration of a linear servo
+  // ---------------------------------------------------
+
+  // The range we want the linear servo to cover
+  constexpr float LINEAR_RANGE = 50.0f;
+
+  // Create a linear servo on pin 1. By default its value ranges from 0.0 to 1.0
+  Servo linear_servo = Servo(servo2040::SERVO_2, LINEAR);
+
+  // Update the linear servo so its max value matches the real distance travelled
+  Calibration &lcal = linear_servo.calibration();
+  lcal.last().value = LINEAR_RANGE;
+
+  // As the calibration was a reference, the servo has already
+  // been updated, but lets access it again to confirm it worked
+  printf("Linear Servo: ");
+  print_calibration(linear_servo.calibration());
+
+
+  // ----------------------------------------------------------------
+  // Create and modify the calibration of a continuous rotation servo
+  // ----------------------------------------------------------------
+
+  // The speed we want the continuous servo to cover
+  constexpr float CONTINUOUS_SPEED = 10.0f;
+
+  // Create a continous rotation servo on pin 2. By default its value ranges from -1.0 to +1.0
+  Servo continuous_servo = Servo(servo2040::SERVO_3, CONTINUOUS);
+
+  // Update the continuous rotation servo so its value matches its real speed
+  Calibration &ccal = continuous_servo.calibration();
+  ccal.first().value = -CONTINUOUS_SPEED;
+  ccal.last().value = CONTINUOUS_SPEED;
+
+  // As the calibration was a reference, the servo has already
+  // been updated, but lets access it again to confirm it worked
+  printf("Continuous Servo: ");
+  print_calibration(continuous_servo.calibration());
+
+
+  // ------------------------------------------------------
+  // Create a custom calibration and build a servo using it
+  // ------------------------------------------------------
+
+  // Create an empty calibration
+  Calibration ecal = Calibration();
+
+  // Give it a range of -45 to 45 degrees, corresponding to pulses of 1000 and 2000 microseconds
+  ecal.apply_two_pairs(1000, 2000, -45, 45);
+
+  // Turn off the lower and upper limits, so the servo can go beyond 45 degrees
+  ecal.limit_to_calibration(false, false);
+
+  // Create a servo on pin 3 using the custom calibration and confirmed it worked
+  Servo custom_servo = Servo(servo2040::SERVO_4, ecal);
+  printf("Custom Servo: ");
+  print_calibration(custom_servo.calibration());
+}

examples/servo2040/servo2040_led_rainbow.cmake

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

examples/servo2040/servo2040_led_rainbow.cpp

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

examples/servo2040/servo2040_multiple_servos.cmake

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

examples/servo2040/servo2040_multiple_servos.cpp

@@ -0,0 +1,95 @@
+#include "pico/stdlib.h"
+
+#include "servo2040.hpp"
+
+/*
+Demonstrates how to create multiple Servo objects and control them together.
+*/
+
+using namespace servo;
+
+// How many sweeps of the servo 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 servo when sweeping
+constexpr float SWEEP_EXTENT = 90.0f;
+
+// Create an array of servo pointers
+const uint START_PIN = servo2040::SERVO_1;
+const uint END_PIN = servo2040::SERVO_4;
+const uint NUM_SERVOS = (END_PIN - START_PIN) + 1;
+Servo *servos[NUM_SERVOS];
+
+
+int main() {
+  stdio_init_all();
+
+  // Fill the array of servos for pins 0 to 3, and initialise them. Up to 16 servos can be created
+  for(auto s = 0u; s < NUM_SERVOS; s++) {
+    servos[s] = new Servo(s + START_PIN);
+    servos[s]->init();
+  }
+
+  // Enable all servos (this puts them at the middle)
+  for(auto s = 0u; s < NUM_SERVOS; s++) {
+    servos[s]->enable();
+  }
+  sleep_ms(2000);
+
+  // Go to min
+  for(auto s = 0u; s < NUM_SERVOS; s++) {
+    servos[s]->to_min();
+  }
+  sleep_ms(2000);
+
+  // Go to max
+  for(auto s = 0u; s < NUM_SERVOS; s++) {
+    servos[s]->to_max();
+  }
+  sleep_ms(2000);
+
+  // Go back to mid
+  for(auto s = 0u; s < NUM_SERVOS; s++) {
+    servos[s]->to_mid();
+  }
+  sleep_ms(2000);
+
+  // Do a sine sweep
+  for(auto j = 0u; j < SWEEPS; j++) {
+    for(auto i = 0u; i < 360; i++) {
+      float value = sin(((float)i * (float)M_PI) / 180.0f) * SWEEP_EXTENT;
+      for(auto s = 0u; s < NUM_SERVOS; s++) {
+        servos[s]->set_value(value);
+      }
+      sleep_ms(20);
+    }
+  }
+
+  // Do a stepped sweep
+  for(auto j = 0u; j < SWEEPS; j++) {
+    for(auto i = 0u; i < STEPS; i++) {
+      for(auto s = 0u; s < NUM_SERVOS; s++) {
+        servos[s]->to_percent(i, 0, STEPS, 0.0 - SWEEP_EXTENT, SWEEP_EXTENT);
+      }
+      sleep_ms(STEPS_INTERVAL_MS);
+    }
+    for(auto i = 0u; i < STEPS; i++) {
+      for(auto s = 0u; s < NUM_SERVOS; s++) {
+        servos[s]->to_percent(i, STEPS, 0, 0.0 - SWEEP_EXTENT, SWEEP_EXTENT);
+      }
+      sleep_ms(STEPS_INTERVAL_MS);
+    }
+  }
+
+  // Disable the servos
+  for(auto s = 0u; s < NUM_SERVOS; s++) {
+    servos[s]->disable();
+    delete servos[s];
+  }
+}

examples/servo2040/servo2040_rainbow.cpp

@@ -1,69 +0,0 @@
-#include <stdio.h>
-#include <math.h>
-#include <cstdint>
-
-#include "pico/stdlib.h"
-
-#include "servo2040.hpp"
-
-#include "common/pimoroni_common.hpp"
-#include "button.hpp"
-
-/*
-A simple balancing game, where you use the MSA301 accelerometer to line up a band with a goal on the strip.
-This can either be done using:
-- Angle mode: Where position on the strip directly matches the accelerometer's angle
-- Velocity mode: Where tilting the accelerometer changes the speed the band moves at
-When the goal position is reached, a new position is randomly selected
-
-Press "A" to change the game mode.
-Press "B" to start or stop the game mode.
-Press "Boot" to invert the direction of the accelerometer tilt
-*/
-
-using namespace pimoroni;
-using namespace plasma;
-
-// Set how many LEDs you have
-const uint N_LEDS = 6;
-
-// The speed that the LEDs will start cycling at
-const uint DEFAULT_SPEED = 10;
-
-// How many times the LEDs will be updated per second
-const uint UPDATES = 60;
-
-// WS28X-style LEDs with a single signal line. AKA NeoPixel
-WS2812 led_bar(N_LEDS, pio0, 0, servo2040::LED_DAT);
-
-
-Button user_sw(servo2040::USER_SW, Polarity::ACTIVE_LOW, 0);
-
-int main() {
-  stdio_init_all();
-
-  led_bar.start(UPDATES);
-
-  int speed = DEFAULT_SPEED;
-  float offset = 0.0f;
-
-  while(true) {
-    bool sw_pressed = user_sw.read();
-
-    if(sw_pressed) {
-      speed = DEFAULT_SPEED;
-    }
-    speed = std::min((int)255, std::max((int)1, speed));
-
-    offset += float(speed) / 2000.0f;
-
-    for(auto i = 0u; i < led_bar.num_leds; ++i) {
-      float hue = float(i) / led_bar.num_leds;
-      led_bar.set_hsv(i, hue + offset, 1.0f, 0.5f);
-    }
-
-    // Sleep time controls the rate at which the LED buffer is updated
-    // but *not* the actual framerate at which the buffer is sent to the LEDs
-    sleep_ms(1000 / UPDATES);
-  }
-}

examples/servo2040/servo2040_servo_cluster.cmake

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

examples/servo2040/servo2040_servo_cluster.cpp

@@ -0,0 +1,74 @@
+#include "pico/stdlib.h"
+
+#include "servo2040.hpp"
+
+/*
+Demonstrates how to create a ServoCluster object to control multiple servos at once.
+*/
+
+using namespace servo;
+
+// How many sweeps of the servo 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 servo when sweeping
+constexpr float SWEEP_EXTENT = 90.0f;
+
+// Create a servo cluster for pins 0 to 3, using PIO 0 and State Machine 0
+const uint START_PIN = servo2040::SERVO_1;
+const uint END_PIN = servo2040::SERVO_4;
+const uint NUM_SERVOS = (END_PIN - START_PIN) + 1;
+ServoCluster servos = ServoCluster(pio0, 0, START_PIN, NUM_SERVOS);
+
+
+int main() {
+  stdio_init_all();
+
+  // Initialise the servo cluster
+  servos.init();
+
+  // Enable all servos (this puts them at the middle)
+  servos.enable_all();
+  sleep_ms(2000);
+
+  // Go to min
+  servos.all_to_min();
+  sleep_ms(2000);
+
+  // Go to max
+  servos.all_to_max();
+  sleep_ms(2000);
+
+  // Go back to mid
+  servos.all_to_mid();
+  sleep_ms(2000);
+
+  // Do a sine sweep
+  for(auto j = 0u; j < SWEEPS; j++) {
+    for(auto i = 0u; i < 360; i++) {
+      servos.set_all_values(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++) {
+      servos.all_to_percent(i, 0, STEPS, 0.0 - SWEEP_EXTENT, SWEEP_EXTENT);
+      sleep_ms(STEPS_INTERVAL_MS);
+    }
+    for(auto i = 0u; i < STEPS; i++) {
+      servos.all_to_percent(i, STEPS, 0, 0.0 - SWEEP_EXTENT, SWEEP_EXTENT);
+      sleep_ms(STEPS_INTERVAL_MS);
+    }
+  }
+
+  // Disable the servos
+  servos.disable_all();
+}

examples/servo2040/servo2040_servo_wave.cmake

@@ -1,5 +1,5 @@
-set(OUTPUT_NAME servo2040_rainbow)
-add_executable(${OUTPUT_NAME} servo2040_rainbow.cpp)
+set(OUTPUT_NAME servo2040_servo_wave)
+add_executable(${OUTPUT_NAME} servo2040_servo_wave.cpp)
 
 target_link_libraries(${OUTPUT_NAME}
         pico_stdlib

examples/servo2040/servo2040_servo_wave.cpp

@@ -0,0 +1,81 @@
+#include "pico/stdlib.h"
+
+#include "servo2040.hpp"
+#include "button.hpp"
+
+/*
+An example of applying a wave pattern to a group of servos and the LEDs.
+
+Press "Boot" to exit the program.
+*/
+
+using namespace plasma;
+using namespace servo;
+
+// The speed that the LEDs will cycle at
+const uint SPEED = 5;
+
+// The brightness of the LEDs
+constexpr float BRIGHTNESS = 0.4;
+
+// How many times to update LEDs and Servos per second
+const uint UPDATES = 50;
+
+// How far from zero to move the servos
+constexpr float SERVO_EXTENT = 80.0f;
+
+// Create a servo cluster for pins 0 to 7, using PIO 0 and State Machine 0
+const uint START_PIN = servo2040::SERVO_1;
+const uint END_PIN = servo2040::SERVO_8;
+const uint NUM_SERVOS = (END_PIN - START_PIN) + 1;
+ServoCluster servos = ServoCluster(pio0, 0, START_PIN, NUM_SERVOS);
+
+// Create the LED bar, using PIO 1 and State Machine 0
+WS2812 led_bar(servo2040::NUM_LEDS, pio1, 0, servo2040::LED_DATA);
+
+// Create the user button
+Button user_sw(servo2040::USER_SW);
+
+
+int main() {
+  stdio_init_all();
+
+  // Initialise the servo cluster
+  servos.init();
+
+  // Start updating the LED bar
+  led_bar.start();
+
+  float offset = 0.0f;
+
+  // Make rainbows until the user button is pressed
+  while(!user_sw.raw()) {
+
+    offset += (float)SPEED / 1000.0f;
+
+    // Update all the LEDs
+    for(auto i = 0u; i < servo2040::NUM_LEDS; i++) {
+        float hue = (float)i / (float)(servo2040::NUM_LEDS * 4);
+        led_bar.set_hsv(i, hue + offset, 1.0f, BRIGHTNESS);
+    }
+
+    // Update all the Servos
+    for(auto i = 0u; i < servos.get_count(); i++) {
+        float angle = (((float)i / (float)servos.get_count()) + offset) * (float)M_TWOPI;
+        servos.set_value(i, sin(angle) * SERVO_EXTENT, false);
+    }
+    // We have now set all the servo values, so load them
+    servos.load();
+
+    sleep_ms(1000 / UPDATES);
+  }
+
+  // Stop all the servos
+  servos.disable_all();
+
+  // Turn off the LED bar
+  led_bar.clear();
+
+  // Sleep a short time so the clear takes effect
+  sleep_ms(100);
+}

examples/servo2040/servo2040_simple_easing.cmake

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

examples/servo2040/servo2040_simple_easing.cpp

@@ -0,0 +1,84 @@
+#include <cstdio>
+#include "pico/stdlib.h"
+
+#include "servo2040.hpp"
+#include "button.hpp"
+
+/*
+An example of how to move a servo smoothly between random positions.
+
+Press "Boot" to exit the program.
+*/
+
+using namespace servo;
+
+// How many times to update Servos per second
+const uint UPDATES = 50;
+
+// The time to travel between each random value
+const uint TIME_FOR_EACH_MOVE = 2;
+const uint UPDATES_PER_MOVE = TIME_FOR_EACH_MOVE * UPDATES;
+
+// How far from zero to move the servo
+constexpr float SERVO_EXTENT = 80.0f;
+
+// Whether or not to use a cosine path between values
+const bool USE_COSINE = true;
+
+
+// Create the user button
+Button user_sw = Button(servo2040::USER_SW);
+
+// Create a servo on pin 0
+Servo s = Servo(servo2040::SERVO_1);
+
+
+int main() {
+  stdio_init_all();
+
+  // Initialise the servo
+  s.init();
+
+  // Get the initial value and create a random end value between the extents
+  float start_value = s.get_mid_value();
+  float end_value = (((float)rand() / (float)RAND_MAX) * (SERVO_EXTENT * 2.0f)) - SERVO_EXTENT;
+
+  uint update = 0;
+
+  // Continually move the servo until the user button is pressed
+  while(!user_sw.raw()) {
+
+    // Calculate how far along this movement to be
+    float percent_along = (float)update / (float)UPDATES_PER_MOVE;
+
+    if(USE_COSINE) {
+      // Move the servo between values using cosine
+      s.to_percent(cos(percent_along * (float)M_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
+    printf("Value = %f\n", s.get_value());
+
+    // Move along in time
+    update++;
+
+    // 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 = (((float)rand() / (float)RAND_MAX) * (SERVO_EXTENT * 2.0f)) - SERVO_EXTENT;
+    }
+
+    sleep_ms(1000 / UPDATES);
+  }
+
+  // Disable the servo
+  s.disable();
+}

examples/servo2040/servo2040_single_servo.cmake

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

examples/servo2040/servo2040_single_servo.cpp

@@ -0,0 +1,72 @@
+#include "pico/stdlib.h"
+
+#include "servo2040.hpp"
+
+/*
+Demonstrates how to create a Servo object and control it.
+*/
+
+using namespace servo;
+
+// How many sweeps of the servo 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 servo when sweeping
+constexpr float SWEEP_EXTENT = 90.0f;
+
+
+// Create a servo on pin 0
+Servo s = Servo(servo2040::SERVO_1);
+
+
+int main() {
+  stdio_init_all();
+
+  // Initialise the servo
+  s.init();
+
+  // Enable the servo (this puts it at the middle)
+  s.enable();
+  sleep_ms(2000);
+
+  // Go to min
+  s.to_min();
+  sleep_ms(2000);
+
+  // Go to max
+  s.to_max();
+  sleep_ms(2000);
+
+  // Go back to mid
+  s.to_mid();
+  sleep_ms(2000);
+
+  // Do a sine sweep
+  for(auto j = 0u; j < SWEEPS; j++) {
+    for(auto i = 0u; i < 360; i++) {
+      s.set_value(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++) {
+      s.to_percent(i, 0, STEPS, 0.0 - SWEEP_EXTENT, SWEEP_EXTENT);
+      sleep_ms(STEPS_INTERVAL_MS);
+    }
+    for(auto i = 0u; i < STEPS; i++) {
+      s.to_percent(i, STEPS, 0, 0.0 - SWEEP_EXTENT, SWEEP_EXTENT);
+      sleep_ms(STEPS_INTERVAL_MS);
+    }
+  }
+
+  // Disable the servo
+  s.disable();
+}

libraries/servo2040/servo2040.hpp

@@ -5,37 +5,56 @@
 #include "servo.hpp"
 #include "servo_cluster.hpp"
 
-namespace servo2040 {
-  const uint SERVO_1 = 0;
-  const uint SERVO_2 = 1;
-  const uint SERVO_3 = 2;
-  const uint SERVO_4 = 3;
-  const uint SERVO_5 = 4;
-  const uint SERVO_6 = 5;
-  const uint SERVO_7 = 6;
-  const uint SERVO_8 = 7;
-  const uint SERVO_9 = 8;
-  const uint SERVO_10 = 9;
-  const uint SERVO_11 = 10;
-  const uint SERVO_12 = 11;
-  const uint SERVO_13 = 12;
-  const uint SERVO_14 = 13;
-  const uint SERVO_15 = 14;
-  const uint SERVO_16 = 15;
-  const uint SERVO_17 = 16;
-  const uint SERVO_18 = 17;
+namespace servo {
+  namespace servo2040 {
+    const uint SERVO_1 = 0;
+    const uint SERVO_2 = 1;
+    const uint SERVO_3 = 2;
+    const uint SERVO_4 = 3;
+    const uint SERVO_5 = 4;
+    const uint SERVO_6 = 5;
+    const uint SERVO_7 = 6;
+    const uint SERVO_8 = 7;
+    const uint SERVO_9 = 8;
+    const uint SERVO_10 = 9;
+    const uint SERVO_11 = 10;
+    const uint SERVO_12 = 11;
+    const uint SERVO_13 = 12;
+    const uint SERVO_14 = 13;
+    const uint SERVO_15 = 14;
+    const uint SERVO_16 = 15;
+    const uint SERVO_17 = 16;
+    const uint SERVO_18 = 17;
+    const uint NUM_SERVOS = 18;
 
-  const uint LED_DATA = 18;
+    const uint LED_DATA = 18;
+    const uint NUM_LEDS = 6;
 
-  const uint ADC_ADDR_0 = 22;
-  const uint ADC_ADDR_1 = 24;
-  const uint ADC_ADDR_2 = 25;
+    const uint USER_SW = 23;
 
-  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 SENSE = 29; // The pin used for the board's sensing features
+    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
 
-  constexpr float ADC_GAIN = 69;
-  constexpr float ADC_OFFSET = 0.0145f;
-  constexpr float SHUNT_RESISTOR = 0.003f;
+    const uint SENSOR_1_ADDR = 0b000;
+    const uint SENSOR_2_ADDR = 0b001;
+    const uint SENSOR_3_ADDR = 0b010;
+    const uint SENSOR_4_ADDR = 0b011;
+    const uint SENSOR_5_ADDR = 0b100;
+    const uint SENSOR_6_ADDR = 0b101;
+    const uint NUM_SENSORS = 6;
+
+    const uint VOLTAGE_SENSE_ADDR = 0b110;
+    const uint VOLTAGE_SENSE_ADDR = 0b111;
+
+    constexpr float SHUNT_RESISTOR = 0.003f;
+    constexpr float CURRENT_GAIN = 69;
+    constexpr float VOLTAGE_GAIN = 3.9f / 13.9f;
+    constexpr float CURRENT_OFFSET = -0.02f;
+  }
 }