From 415a1e559abedbdb153f59606cba8b76cd70e127 Mon Sep 17 00:00:00 2001 From: ZodiusInfuser Date: Tue, 26 Apr 2022 23:15:13 +0100 Subject: [PATCH] Setup and partial implementations of C++ examples --- drivers/CMakeLists.txt | 1 + drivers/analogmux/analogmux.cpp | 7 + drivers/analogmux/analogmux.hpp | 1 + drivers/encoder/encoder.hpp | 2 + drivers/pid/CMakeLists.txt | 1 + drivers/pid/pid.cmake | 11 ++ drivers/pid/pid.cpp | 25 ++++ drivers/pid/pid.hpp | 27 ++++ examples/motor2040/CMakeLists.txt | 27 ++-- examples/motor2040/README.md | 135 ++++++++++++++++++ .../motor2040/motor2040_led_rainbow.cmake | 13 ++ examples/motor2040/motor2040_led_rainbow.cpp | 56 ++++++++ .../motor2040/motor2040_motor_cluster.cmake | 13 ++ .../motor2040/motor2040_motor_cluster.cpp | 79 ++++++++++ .../motor2040/motor2040_motor_profiler.cmake | 12 ++ .../motor2040/motor2040_motor_profiler.cpp | 73 ++++++++++ examples/motor2040/motor2040_motor_wave.cmake | 13 ++ examples/motor2040/motor2040_motor_wave.cpp | 82 +++++++++++ .../motor2040/motor2040_multiple_motors.cmake | 12 ++ .../motor2040/motor2040_multiple_motors.cpp | 100 +++++++++++++ .../motor2040_position_control.cmake | 14 ++ .../motor2040/motor2040_position_control.cpp | 75 ++++++++++ ...tor2040_position_on_velocity_control.cmake | 14 ++ ...motor2040_position_on_velocity_control.cpp | 75 ++++++++++ ...otor2040_position_on_velocity_tuning.cmake | 14 ++ .../motor2040_position_on_velocity_tuning.cpp | 77 ++++++++++ .../motor2040/motor2040_position_tuning.cmake | 14 ++ .../motor2040/motor2040_position_tuning.cpp | 131 +++++++++++++++++ .../motor2040_quad_position_wave.cmake | 14 ++ .../motor2040_quad_position_wave.cpp | 75 ++++++++++ .../motor2040_quad_velocity_sequence.cmake | 14 ++ .../motor2040_quad_velocity_sequence.cpp | 75 ++++++++++ .../motor2040_reactive_encoder.cmake | 12 ++ .../motor2040/motor2040_reactive_encoder.cpp | 76 ++++++++++ .../motor2040/motor2040_read_encoders.cmake | 13 ++ .../motor2040/motor2040_read_encoders.cpp | 60 ++++++++ .../motor2040/motor2040_read_sensors.cmake | 15 ++ examples/motor2040/motor2040_read_sensors.cpp | 68 +++++++++ examples/motor2040/motor2040_single_motor.cpp | 33 +++-- .../motor2040_velocity_control.cmake | 12 ++ .../motor2040/motor2040_velocity_control.cpp | 75 ++++++++++ .../motor2040/motor2040_velocity_tuning.cmake | 13 ++ .../motor2040/motor2040_velocity_tuning.cpp | 77 ++++++++++ libraries/motor2040/motor2040.cmake | 2 +- libraries/motor2040/motor2040.hpp | 1 + micropython/examples/motor2040/led_rainbow.py | 6 +- micropython/examples/motor2040/motor_wave.py | 4 +- .../examples/motor2040/multiple_motors.py | 2 +- .../examples/motor2040/position_tuning.py | 8 +- 49 files changed, 1724 insertions(+), 35 deletions(-) create mode 100644 drivers/pid/CMakeLists.txt create mode 100644 drivers/pid/pid.cmake create mode 100644 drivers/pid/pid.cpp create mode 100644 drivers/pid/pid.hpp create mode 100644 examples/motor2040/README.md create mode 100644 examples/motor2040/motor2040_led_rainbow.cmake create mode 100644 examples/motor2040/motor2040_led_rainbow.cpp create mode 100644 examples/motor2040/motor2040_motor_cluster.cmake create mode 100644 examples/motor2040/motor2040_motor_cluster.cpp create mode 100644 examples/motor2040/motor2040_motor_profiler.cmake create mode 100644 examples/motor2040/motor2040_motor_profiler.cpp create mode 100644 examples/motor2040/motor2040_motor_wave.cmake create mode 100644 examples/motor2040/motor2040_motor_wave.cpp create mode 100644 examples/motor2040/motor2040_multiple_motors.cmake create mode 100644 examples/motor2040/motor2040_multiple_motors.cpp create mode 100644 examples/motor2040/motor2040_position_control.cmake create mode 100644 examples/motor2040/motor2040_position_control.cpp create mode 100644 examples/motor2040/motor2040_position_on_velocity_control.cmake create mode 100644 examples/motor2040/motor2040_position_on_velocity_control.cpp create mode 100644 examples/motor2040/motor2040_position_on_velocity_tuning.cmake create mode 100644 examples/motor2040/motor2040_position_on_velocity_tuning.cpp create mode 100644 examples/motor2040/motor2040_position_tuning.cmake create mode 100644 examples/motor2040/motor2040_position_tuning.cpp create mode 100644 examples/motor2040/motor2040_quad_position_wave.cmake create mode 100644 examples/motor2040/motor2040_quad_position_wave.cpp create mode 100644 examples/motor2040/motor2040_quad_velocity_sequence.cmake create mode 100644 examples/motor2040/motor2040_quad_velocity_sequence.cpp create mode 100644 examples/motor2040/motor2040_reactive_encoder.cmake create mode 100644 examples/motor2040/motor2040_reactive_encoder.cpp create mode 100644 examples/motor2040/motor2040_read_encoders.cmake create mode 100644 examples/motor2040/motor2040_read_encoders.cpp create mode 100644 examples/motor2040/motor2040_read_sensors.cmake create mode 100644 examples/motor2040/motor2040_read_sensors.cpp create mode 100644 examples/motor2040/motor2040_velocity_control.cmake create mode 100644 examples/motor2040/motor2040_velocity_control.cpp create mode 100644 examples/motor2040/motor2040_velocity_tuning.cmake create mode 100644 examples/motor2040/motor2040_velocity_tuning.cpp diff --git a/drivers/CMakeLists.txt b/drivers/CMakeLists.txt index 9fc45193..c76e13fa 100644 --- a/drivers/CMakeLists.txt +++ b/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) diff --git a/drivers/analogmux/analogmux.cpp b/drivers/analogmux/analogmux.cpp index c1e36094..3898df0a 100644 --- a/drivers/analogmux/analogmux.cpp +++ b/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; + } } \ No newline at end of file diff --git a/drivers/analogmux/analogmux.hpp b/drivers/analogmux/analogmux.hpp index 63236848..66011e37 100644 --- a/drivers/analogmux/analogmux.hpp +++ b/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; diff --git a/drivers/encoder/encoder.hpp b/drivers/encoder/encoder.hpp index 45441ef3..e28e73b5 100644 --- a/drivers/encoder/encoder.hpp +++ b/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 { //-------------------------------------------------- diff --git a/drivers/pid/CMakeLists.txt b/drivers/pid/CMakeLists.txt new file mode 100644 index 00000000..b8b8971d --- /dev/null +++ b/drivers/pid/CMakeLists.txt @@ -0,0 +1 @@ +include(pid.cmake) \ No newline at end of file diff --git a/drivers/pid/pid.cmake b/drivers/pid/pid.cmake new file mode 100644 index 00000000..3b9f6720 --- /dev/null +++ b/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) \ No newline at end of file diff --git a/drivers/pid/pid.cpp b/drivers/pid/pid.cpp new file mode 100644 index 00000000..9134e5d7 --- /dev/null +++ b/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); + } +} \ No newline at end of file diff --git a/drivers/pid/pid.hpp b/drivers/pid/pid.hpp new file mode 100644 index 00000000..7dd17c8a --- /dev/null +++ b/drivers/pid/pid.hpp @@ -0,0 +1,27 @@ +#pragma once + +//#include +#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; + }; + +} \ No newline at end of file diff --git a/examples/motor2040/CMakeLists.txt b/examples/motor2040/CMakeLists.txt index d2009f34..30694b51 100644 --- a/examples/motor2040/CMakeLists.txt +++ b/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) \ No newline at end of file +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) \ No newline at end of file diff --git a/examples/motor2040/README.md b/examples/motor2040/README.md new file mode 100644 index 00000000..ad152b7b --- /dev/null +++ b/examples/motor2040/README.md @@ -0,0 +1,135 @@ +# Motor 2040 C++ Examples + +- [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. diff --git a/examples/motor2040/motor2040_led_rainbow.cmake b/examples/motor2040/motor2040_led_rainbow.cmake new file mode 100644 index 00000000..938d0216 --- /dev/null +++ b/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}) diff --git a/examples/motor2040/motor2040_led_rainbow.cpp b/examples/motor2040/motor2040_led_rainbow.cpp new file mode 100644 index 00000000..b46e2344 --- /dev/null +++ b/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); +} diff --git a/examples/motor2040/motor2040_motor_cluster.cmake b/examples/motor2040/motor2040_motor_cluster.cmake new file mode 100644 index 00000000..efe9b5f6 --- /dev/null +++ b/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}) diff --git a/examples/motor2040/motor2040_motor_cluster.cpp b/examples/motor2040/motor2040_motor_cluster.cpp new file mode 100644 index 00000000..804b77ca --- /dev/null +++ b/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(); +} diff --git a/examples/motor2040/motor2040_motor_profiler.cmake b/examples/motor2040/motor2040_motor_profiler.cmake new file mode 100644 index 00000000..39c19738 --- /dev/null +++ b/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}) diff --git a/examples/motor2040/motor2040_motor_profiler.cpp b/examples/motor2040/motor2040_motor_profiler.cpp new file mode 100644 index 00000000..a257c6f4 --- /dev/null +++ b/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(); +} diff --git a/examples/motor2040/motor2040_motor_wave.cmake b/examples/motor2040/motor2040_motor_wave.cmake new file mode 100644 index 00000000..68719ef2 --- /dev/null +++ b/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}) diff --git a/examples/motor2040/motor2040_motor_wave.cpp b/examples/motor2040/motor2040_motor_wave.cpp new file mode 100644 index 00000000..a66a5713 --- /dev/null +++ b/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); +} diff --git a/examples/motor2040/motor2040_multiple_motors.cmake b/examples/motor2040/motor2040_multiple_motors.cmake new file mode 100644 index 00000000..43efe984 --- /dev/null +++ b/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}) diff --git a/examples/motor2040/motor2040_multiple_motors.cpp b/examples/motor2040/motor2040_multiple_motors.cpp new file mode 100644 index 00000000..ec68548f --- /dev/null +++ b/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(); + } +} diff --git a/examples/motor2040/motor2040_position_control.cmake b/examples/motor2040/motor2040_position_control.cmake new file mode 100644 index 00000000..48a2d567 --- /dev/null +++ b/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}) diff --git a/examples/motor2040/motor2040_position_control.cpp b/examples/motor2040/motor2040_position_control.cpp new file mode 100644 index 00000000..ee6f4133 --- /dev/null +++ b/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(); +} diff --git a/examples/motor2040/motor2040_position_on_velocity_control.cmake b/examples/motor2040/motor2040_position_on_velocity_control.cmake new file mode 100644 index 00000000..f3a54587 --- /dev/null +++ b/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}) diff --git a/examples/motor2040/motor2040_position_on_velocity_control.cpp b/examples/motor2040/motor2040_position_on_velocity_control.cpp new file mode 100644 index 00000000..3463e37c --- /dev/null +++ b/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(); +} diff --git a/examples/motor2040/motor2040_position_on_velocity_tuning.cmake b/examples/motor2040/motor2040_position_on_velocity_tuning.cmake new file mode 100644 index 00000000..43f948e2 --- /dev/null +++ b/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}) diff --git a/examples/motor2040/motor2040_position_on_velocity_tuning.cpp b/examples/motor2040/motor2040_position_on_velocity_tuning.cpp new file mode 100644 index 00000000..51847ea4 --- /dev/null +++ b/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(); +} diff --git a/examples/motor2040/motor2040_position_tuning.cmake b/examples/motor2040/motor2040_position_tuning.cmake new file mode 100644 index 00000000..9b8feaea --- /dev/null +++ b/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}) diff --git a/examples/motor2040/motor2040_position_tuning.cpp b/examples/motor2040/motor2040_position_tuning.cpp new file mode 100644 index 00000000..2c20beea --- /dev/null +++ b/examples/motor2040/motor2040_position_tuning.cpp @@ -0,0 +1,131 @@ +#include +#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(); +} diff --git a/examples/motor2040/motor2040_quad_position_wave.cmake b/examples/motor2040/motor2040_quad_position_wave.cmake new file mode 100644 index 00000000..84f7a15b --- /dev/null +++ b/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}) diff --git a/examples/motor2040/motor2040_quad_position_wave.cpp b/examples/motor2040/motor2040_quad_position_wave.cpp new file mode 100644 index 00000000..240cc936 --- /dev/null +++ b/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(); +} diff --git a/examples/motor2040/motor2040_quad_velocity_sequence.cmake b/examples/motor2040/motor2040_quad_velocity_sequence.cmake new file mode 100644 index 00000000..f9e32c49 --- /dev/null +++ b/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}) diff --git a/examples/motor2040/motor2040_quad_velocity_sequence.cpp b/examples/motor2040/motor2040_quad_velocity_sequence.cpp new file mode 100644 index 00000000..e612f655 --- /dev/null +++ b/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(); +} diff --git a/examples/motor2040/motor2040_reactive_encoder.cmake b/examples/motor2040/motor2040_reactive_encoder.cmake new file mode 100644 index 00000000..c5173529 --- /dev/null +++ b/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}) diff --git a/examples/motor2040/motor2040_reactive_encoder.cpp b/examples/motor2040/motor2040_reactive_encoder.cpp new file mode 100644 index 00000000..bde6948b --- /dev/null +++ b/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(); +} diff --git a/examples/motor2040/motor2040_read_encoders.cmake b/examples/motor2040/motor2040_read_encoders.cmake new file mode 100644 index 00000000..7d3e586b --- /dev/null +++ b/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}) diff --git a/examples/motor2040/motor2040_read_encoders.cpp b/examples/motor2040/motor2040_read_encoders.cpp new file mode 100644 index 00000000..01c61305 --- /dev/null +++ b/examples/motor2040/motor2040_read_encoders.cpp @@ -0,0 +1,60 @@ +#include +#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); + } +} diff --git a/examples/motor2040/motor2040_read_sensors.cmake b/examples/motor2040/motor2040_read_sensors.cmake new file mode 100644 index 00000000..837451a6 --- /dev/null +++ b/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}) diff --git a/examples/motor2040/motor2040_read_sensors.cpp b/examples/motor2040/motor2040_read_sensors.cpp new file mode 100644 index 00000000..7cf775b8 --- /dev/null +++ b/examples/motor2040/motor2040_read_sensors.cpp @@ -0,0 +1,68 @@ +#include +#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); + } +} diff --git a/examples/motor2040/motor2040_single_motor.cpp b/examples/motor2040/motor2040_single_motor.cpp index d08767a1..bafe03a8 100644 --- a/examples/motor2040/motor2040_single_motor.cpp +++ b/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); } } diff --git a/examples/motor2040/motor2040_velocity_control.cmake b/examples/motor2040/motor2040_velocity_control.cmake new file mode 100644 index 00000000..be841677 --- /dev/null +++ b/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}) diff --git a/examples/motor2040/motor2040_velocity_control.cpp b/examples/motor2040/motor2040_velocity_control.cpp new file mode 100644 index 00000000..722c7639 --- /dev/null +++ b/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(); +} diff --git a/examples/motor2040/motor2040_velocity_tuning.cmake b/examples/motor2040/motor2040_velocity_tuning.cmake new file mode 100644 index 00000000..076cfc92 --- /dev/null +++ b/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}) diff --git a/examples/motor2040/motor2040_velocity_tuning.cpp b/examples/motor2040/motor2040_velocity_tuning.cpp new file mode 100644 index 00000000..7ee2fa93 --- /dev/null +++ b/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(); +} diff --git a/libraries/motor2040/motor2040.cmake b/libraries/motor2040/motor2040.cmake index 73ae2487..647c53c7 100644 --- a/libraries/motor2040/motor2040.cmake +++ b/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) \ No newline at end of file +target_link_libraries(motor2040 INTERFACE pico_stdlib plasma motor motor_cluster encoder) \ No newline at end of file diff --git a/libraries/motor2040/motor2040.hpp b/libraries/motor2040/motor2040.hpp index a89fd549..7ee5a688 100644 --- a/libraries/motor2040/motor2040.hpp +++ b/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 { diff --git a/micropython/examples/motor2040/led_rainbow.py b/micropython/examples/motor2040/led_rainbow.py index bb3db6b6..e20f1dd6 100644 --- a/micropython/examples/motor2040/led_rainbow.py +++ b/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) diff --git a/micropython/examples/motor2040/motor_wave.py b/micropython/examples/motor2040/motor_wave.py index 369f326c..7aab108b 100644 --- a/micropython/examples/motor2040/motor_wave.py +++ b/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() diff --git a/micropython/examples/motor2040/multiple_motors.py b/micropython/examples/motor2040/multiple_motors.py index 19ebacd5..008e963b 100644 --- a/micropython/examples/motor2040/multiple_motors.py +++ b/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 diff --git a/micropython/examples/motor2040/position_tuning.py b/micropython/examples/motor2040/position_tuning.py index 9ecfd13e..69e0df42 100644 --- a/micropython/examples/motor2040/position_tuning.py +++ b/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()