From f46deb1c5bf4830c95ec858941d5baa8267d487f Mon Sep 17 00:00:00 2001 From: ZodiusInfuser Date: Fri, 25 Mar 2022 18:26:57 +0000 Subject: [PATCH] Ported the majority of Servo2040 MP examples to C++ --- examples/servo2040/CMakeLists.txt | 9 +- .../servo2040/servo2040_calibration.cmake | 12 ++ examples/servo2040/servo2040_calibration.cpp | 122 ++++++++++++++++++ .../servo2040/servo2040_led_rainbow.cmake | 13 ++ examples/servo2040/servo2040_led_rainbow.cpp | 59 +++++++++ .../servo2040/servo2040_multiple_servos.cmake | 12 ++ .../servo2040/servo2040_multiple_servos.cpp | 95 ++++++++++++++ examples/servo2040/servo2040_rainbow.cpp | 69 ---------- .../servo2040/servo2040_servo_cluster.cmake | 12 ++ .../servo2040/servo2040_servo_cluster.cpp | 74 +++++++++++ ...inbow.cmake => servo2040_servo_wave.cmake} | 4 +- examples/servo2040/servo2040_servo_wave.cpp | 81 ++++++++++++ .../servo2040/servo2040_simple_easing.cmake | 13 ++ .../servo2040/servo2040_simple_easing.cpp | 84 ++++++++++++ .../servo2040/servo2040_single_servo.cmake | 12 ++ examples/servo2040/servo2040_single_servo.cpp | 72 +++++++++++ libraries/servo2040/servo2040.hpp | 75 +++++++---- 17 files changed, 717 insertions(+), 101 deletions(-) create mode 100644 examples/servo2040/servo2040_calibration.cmake create mode 100644 examples/servo2040/servo2040_calibration.cpp create mode 100644 examples/servo2040/servo2040_led_rainbow.cmake create mode 100644 examples/servo2040/servo2040_led_rainbow.cpp create mode 100644 examples/servo2040/servo2040_multiple_servos.cmake create mode 100644 examples/servo2040/servo2040_multiple_servos.cpp delete mode 100644 examples/servo2040/servo2040_rainbow.cpp create mode 100644 examples/servo2040/servo2040_servo_cluster.cmake create mode 100644 examples/servo2040/servo2040_servo_cluster.cpp rename examples/servo2040/{servo2040_rainbow.cmake => servo2040_servo_wave.cmake} (68%) create mode 100644 examples/servo2040/servo2040_servo_wave.cpp create mode 100644 examples/servo2040/servo2040_simple_easing.cmake create mode 100644 examples/servo2040/servo2040_simple_easing.cpp create mode 100644 examples/servo2040/servo2040_single_servo.cmake create mode 100644 examples/servo2040/servo2040_single_servo.cpp diff --git a/examples/servo2040/CMakeLists.txt b/examples/servo2040/CMakeLists.txt index 79d553dd..e0fcd985 100644 --- a/examples/servo2040/CMakeLists.txt +++ b/examples/servo2040/CMakeLists.txt @@ -1,2 +1,7 @@ -include(servo2040_functions.cmake) -include(servo2040_rainbow.cmake) \ No newline at end of file +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) \ No newline at end of file diff --git a/examples/servo2040/servo2040_calibration.cmake b/examples/servo2040/servo2040_calibration.cmake new file mode 100644 index 00000000..cb04855b --- /dev/null +++ b/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}) diff --git a/examples/servo2040/servo2040_calibration.cpp b/examples/servo2040/servo2040_calibration.cpp new file mode 100644 index 00000000..3df151c2 --- /dev/null +++ b/examples/servo2040/servo2040_calibration.cpp @@ -0,0 +1,122 @@ +#include +#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()); +} diff --git a/examples/servo2040/servo2040_led_rainbow.cmake b/examples/servo2040/servo2040_led_rainbow.cmake new file mode 100644 index 00000000..523ceb01 --- /dev/null +++ b/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}) diff --git a/examples/servo2040/servo2040_led_rainbow.cpp b/examples/servo2040/servo2040_led_rainbow.cpp new file mode 100644 index 00000000..b848e682 --- /dev/null +++ b/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); +} diff --git a/examples/servo2040/servo2040_multiple_servos.cmake b/examples/servo2040/servo2040_multiple_servos.cmake new file mode 100644 index 00000000..85d5e115 --- /dev/null +++ b/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}) diff --git a/examples/servo2040/servo2040_multiple_servos.cpp b/examples/servo2040/servo2040_multiple_servos.cpp new file mode 100644 index 00000000..ee07cd75 --- /dev/null +++ b/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]; + } +} diff --git a/examples/servo2040/servo2040_rainbow.cpp b/examples/servo2040/servo2040_rainbow.cpp deleted file mode 100644 index f88108a0..00000000 --- a/examples/servo2040/servo2040_rainbow.cpp +++ /dev/null @@ -1,69 +0,0 @@ -#include -#include -#include - -#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); - } -} diff --git a/examples/servo2040/servo2040_servo_cluster.cmake b/examples/servo2040/servo2040_servo_cluster.cmake new file mode 100644 index 00000000..4c6f8dd5 --- /dev/null +++ b/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}) diff --git a/examples/servo2040/servo2040_servo_cluster.cpp b/examples/servo2040/servo2040_servo_cluster.cpp new file mode 100644 index 00000000..ae4a6542 --- /dev/null +++ b/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(); +} diff --git a/examples/servo2040/servo2040_rainbow.cmake b/examples/servo2040/servo2040_servo_wave.cmake similarity index 68% rename from examples/servo2040/servo2040_rainbow.cmake rename to examples/servo2040/servo2040_servo_wave.cmake index 873b5ce0..f0309032 100644 --- a/examples/servo2040/servo2040_rainbow.cmake +++ b/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 diff --git a/examples/servo2040/servo2040_servo_wave.cpp b/examples/servo2040/servo2040_servo_wave.cpp new file mode 100644 index 00000000..d15c8e7e --- /dev/null +++ b/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); +} diff --git a/examples/servo2040/servo2040_simple_easing.cmake b/examples/servo2040/servo2040_simple_easing.cmake new file mode 100644 index 00000000..a2eaedc8 --- /dev/null +++ b/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}) diff --git a/examples/servo2040/servo2040_simple_easing.cpp b/examples/servo2040/servo2040_simple_easing.cpp new file mode 100644 index 00000000..9119102e --- /dev/null +++ b/examples/servo2040/servo2040_simple_easing.cpp @@ -0,0 +1,84 @@ +#include +#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(); +} diff --git a/examples/servo2040/servo2040_single_servo.cmake b/examples/servo2040/servo2040_single_servo.cmake new file mode 100644 index 00000000..aa3f5822 --- /dev/null +++ b/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}) diff --git a/examples/servo2040/servo2040_single_servo.cpp b/examples/servo2040/servo2040_single_servo.cpp new file mode 100644 index 00000000..04f5ee1b --- /dev/null +++ b/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(); +} diff --git a/libraries/servo2040/servo2040.hpp b/libraries/servo2040/servo2040.hpp index d74f44a0..2b4d6b59 100644 --- a/libraries/servo2040/servo2040.hpp +++ b/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; + } } \ No newline at end of file