From 15e5eaa89043d3a3a433ebeee4bf8286aab5c7e1 Mon Sep 17 00:00:00 2001 From: ZodiusInfuser Date: Thu, 28 Apr 2022 21:23:18 +0100 Subject: [PATCH] Finished C++ examples and tweaked MP examples --- drivers/analogmux/analogmux.cpp | 4 + drivers/pid/pid.cpp | 5 + drivers/pid/pid.hpp | 1 + .../motor2040/motor2040_motor_cluster.cpp | 3 + .../motor2040/motor2040_motor_profiler.cpp | 136 +++++++---- examples/motor2040/motor2040_motor_wave.cpp | 2 +- .../motor2040/motor2040_position_control.cpp | 147 +++++++++--- ...motor2040_position_on_velocity_control.cpp | 164 ++++++++++--- .../motor2040_position_on_velocity_tuning.cpp | 144 ++++++++--- .../motor2040/motor2040_position_tuning.cpp | 12 +- .../motor2040_quad_position_wave.cpp | 184 ++++++++++---- .../motor2040_quad_velocity_sequence.cpp | 224 ++++++++++++++---- .../motor2040_reactive_encoder.cmake | 2 + .../motor2040/motor2040_reactive_encoder.cpp | 161 ++++++++++--- examples/motor2040/motor2040_single_motor.cpp | 2 +- .../motor2040_velocity_control.cmake | 2 + .../motor2040/motor2040_velocity_control.cpp | 147 +++++++++--- .../motor2040/motor2040_velocity_tuning.cmake | 1 + .../motor2040/motor2040_velocity_tuning.cpp | 127 +++++++--- libraries/motor2040/motor2040.hpp | 4 +- .../examples/motor2040/motor_profiler.py | 4 +- .../examples/motor2040/position_control.py | 2 +- .../motor2040/position_on_velocity_control.py | 3 - .../motor2040/position_on_velocity_tuning.py | 2 +- .../examples/motor2040/quad_position_wave.py | 8 +- .../motor2040/quad_velocity_sequence.py | 46 ++-- .../examples/motor2040/reactive_encoder.py | 4 +- .../examples/motor2040/read_sensors.py | 2 +- .../examples/motor2040/velocity_control.py | 5 +- .../examples/motor2040/velocity_tuning.py | 6 +- 30 files changed, 1156 insertions(+), 398 deletions(-) diff --git a/drivers/analogmux/analogmux.cpp b/drivers/analogmux/analogmux.cpp index 3898df0a..916d3d77 100644 --- a/drivers/analogmux/analogmux.cpp +++ b/drivers/analogmux/analogmux.cpp @@ -27,6 +27,10 @@ namespace pimoroni { gpio_init(en_pin); gpio_set_dir(en_pin, GPIO_OUT); } + + if(muxed_pin != PIN_UNUSED) { + gpio_set_input_enabled(muxed_pin, true); + } } void AnalogMux::select(uint8_t address) { diff --git a/drivers/pid/pid.cpp b/drivers/pid/pid.cpp index 9134e5d7..0d8cdb4f 100644 --- a/drivers/pid/pid.cpp +++ b/drivers/pid/pid.cpp @@ -1,6 +1,11 @@ #include "pid.hpp" namespace pimoroni { + PID::PID() + : kp(0.0f), ki(0.0f), kd(0.0f), setpoint(0.0f) + , error_sum(0.0f), last_value(0.0f), sample_rate(1.0f) { + } + 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) { diff --git a/drivers/pid/pid.hpp b/drivers/pid/pid.hpp index 7dd17c8a..ef603941 100644 --- a/drivers/pid/pid.hpp +++ b/drivers/pid/pid.hpp @@ -8,6 +8,7 @@ namespace pimoroni { class PID { public: + PID(); PID(float kp, float ki, float kd, float sample_rate); float calculate(float value); diff --git a/examples/motor2040/motor2040_motor_cluster.cpp b/examples/motor2040/motor2040_motor_cluster.cpp index 804b77ca..27fc16e5 100644 --- a/examples/motor2040/motor2040_motor_cluster.cpp +++ b/examples/motor2040/motor2040_motor_cluster.cpp @@ -76,4 +76,7 @@ int main() { // Disable the motors motors.disable_all(); + + // Sleep a short time so the disable takes effect + sleep_ms(100); } diff --git a/examples/motor2040/motor2040_motor_profiler.cpp b/examples/motor2040/motor2040_motor_profiler.cpp index a257c6f4..55fa3ce1 100644 --- a/examples/motor2040/motor2040_motor_profiler.cpp +++ b/examples/motor2040/motor2040_motor_profiler.cpp @@ -1,3 +1,4 @@ +#include #include "pico/stdlib.h" #include "motor2040.hpp" @@ -8,66 +9,119 @@ duty cycle range using the attached encoder for feedback. */ using namespace motor; +using namespace encoder; -// How many sweeps of the motor to perform -const uint SWEEPS = 3; +// The pins of the motor being profiled +const pin_pair MOTOR_PINS = motor2040::MOTOR_A; -// The number of discrete sweep steps -const uint STEPS = 10; +// The pins of the encoder attached to the profiled motor +const pin_pair ENCODER_PINS = motor2040::ENCODER_A; -// The time in milliseconds between each step of the sequence -const uint STEPS_INTERVAL_MS = 500; +// The gear ratio of the motor +constexpr float GEAR_RATIO = 50; -// How far from zero to move the motor when sweeping -constexpr float SWEEP_EXTENT = 90.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. Set this to the maximum measured speed +constexpr float SPEED_SCALE = 5.4f; + +// The duty cycle that corresponds with zero speed when plotting the motor's speed as a straight line +constexpr float ZERO_POINT = 0.0f; + +// The duty cycle below which the motor's friction prevents it from moving +constexpr float DEAD_ZONE = 0.0f; + +// How many duty cycle steps to sample the speed of +const uint DUTY_STEPS = 100; + +// How long to wait after changing motor duty cycle +const uint SETTLE_TIME = 100; + +// How long to capture the motor's speed at each step +const uint CAPTURE_TIME = 200; -// Create a motor on pin 0 and 1 -Motor m = Motor(motor2040::MOTOR_A); +// Create a motor and set its direction, speed scale, zero point, and dead zone +Motor m = Motor(MOTOR_PINS, DIRECTION, SPEED_SCALE, ZERO_POINT, DEAD_ZONE); + +// 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); + + +// Function that performs a single profiling step +void profile_at_duty(float duty) { + // Set the motor to a new duty cycle and wait for it to settle + if(DIRECTION == REVERSED_DIR) + m.duty(0.0 - duty); + else + m.duty(duty); + sleep_ms(SETTLE_TIME); + + // Perform a dummy capture to clear the encoder + enc.capture(); + + // Wait for the capture time to pass + sleep_ms(CAPTURE_TIME); + + // Perform a capture and read the measured speed + Encoder::Capture capture = enc.capture(); + float measured_speed = capture.revolutions_per_second(); + + // These are some alternate speed measurements from the encoder + // float measured_speed = capture.revolutions_per_minute(); + // float measured_speed = capture.degrees_per_second(); + // float measured_speed = capture.radians_per_second(); + + // Print out the expected and measured speeds, as well as their difference + printf("Duty = %f, Expected = %f, Measured = %f, Diff = %f\n", + m.duty(), m.speed(), measured_speed, m.speed() - measured_speed); +} int main() { stdio_init_all(); - // Initialise the motor + // Give some time to connect up a serial terminal + sleep_ms(10000); + + // Initialise the motor and enable it m.init(); - - // Enable the motor m.enable(); - sleep_ms(2000); - // Go at full neative - m.full_negative(); - sleep_ms(2000); + // Initialise the encoder + enc.init(); - // Go at full positive - m.full_positive(); - sleep_ms(2000); + printf("Profiler Starting...\n"); - // 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); - } + // Profile from 0% up to one step below 100% + for(uint i = 0; i < DUTY_STEPS; i++) { + profile_at_duty((float)i / (float)DUTY_STEPS); } - // 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); - } + // Profile from 100% down to one step above 0% + for(uint i = 0; i < DUTY_STEPS; i++) { + profile_at_duty((float)(DUTY_STEPS - i) / (float)DUTY_STEPS); } - // Disable the motor + // Profile from 0% down to one step above -100% + for(uint i = 0; i < DUTY_STEPS; i++) { + profile_at_duty(-(float)i / (float)DUTY_STEPS); + } + + // Profile from -100% up to one step below 0% + for(uint i = 0; i < DUTY_STEPS; i++) { + profile_at_duty(-(float)(DUTY_STEPS - i) / (float)DUTY_STEPS); + } + + // Profile 0% again + profile_at_duty(0.0f); + + printf("Profiler Finished...\n"); + + // Disable the motor now the profiler has finished m.disable(); } diff --git a/examples/motor2040/motor2040_motor_wave.cpp b/examples/motor2040/motor2040_motor_wave.cpp index a66a5713..14ab9cbd 100644 --- a/examples/motor2040/motor2040_motor_wave.cpp +++ b/examples/motor2040/motor2040_motor_wave.cpp @@ -62,7 +62,7 @@ int main() { // Update all the motors for(auto m = 0u; m < NUM_MOTORS; m++) { - float angle = (((float)m / (float)NUM_MOTORS) + offset) * (float)M_TWOPI; + float angle = (((float)m / (float)NUM_MOTORS) + offset) * (float)M_PI; motors[m]->speed(sin(angle) * SPEED_EXTENT); } diff --git a/examples/motor2040/motor2040_position_control.cpp b/examples/motor2040/motor2040_position_control.cpp index ee6f4133..3eac307d 100644 --- a/examples/motor2040/motor2040_position_control.cpp +++ b/examples/motor2040/motor2040_position_control.cpp @@ -1,6 +1,9 @@ +#include #include "pico/stdlib.h" #include "motor2040.hpp" +#include "button.hpp" +#include "pid.hpp" /* An example of how to move a motor smoothly between random positions, @@ -10,64 +13,138 @@ Press "Boot" to exit the program. */ using namespace motor; +using namespace encoder; -// How many sweeps of the motor to perform -const uint SWEEPS = 3; +// The pins of the motor being profiled +const pin_pair MOTOR_PINS = motor2040::MOTOR_A; -// The number of discrete sweep steps -const uint STEPS = 10; +// The pins of the encoder attached to the profiled motor +const pin_pair ENCODER_PINS = motor2040::ENCODER_A; -// The time in milliseconds between each step of the sequence -const uint STEPS_INTERVAL_MS = 500; +// The gear ratio of the motor +constexpr float GEAR_RATIO = 50.0f; -// How far from zero to move the motor when sweeping -constexpr float SWEEP_EXTENT = 90.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 to travel between each random value +constexpr float TIME_FOR_EACH_MOVE = 1.0f; +const uint UPDATES_PER_MOVE = TIME_FOR_EACH_MOVE * UPDATES; + +// How many of the updates should be printed (i.e. 2 would be every other update) +const uint PRINT_DIVIDER = 4; + +// Multipliers for the different printed values, so they appear nicely on the Thonny plotter +constexpr float SPD_PRINT_SCALE = 20.0f; // Driving Speed multipler + +// How far from zero to move the motor, in degrees +constexpr float POSITION_EXTENT = 180.0f; + +// The interpolating mode between setpoints. STEP (0), LINEAR (1), COSINE (2) +const uint INTERP_MODE = 2; -// Create a motor on pin 0 and 1 -Motor m = Motor(motor2040::MOTOR_A); +// PID values +constexpr float POS_KP = 0.14f; // Position proportional (P) gain +constexpr float POS_KI = 0.0f; // Position integral (I) gain +constexpr float POS_KD = 0.002f; // 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 + // Initialise the motor and encoder m.init(); + enc.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); + uint update = 0; + uint print_count = 0; - // Stop the motor - m.stop(); - sleep_ms(2000); + // Set the initial value and create a random end value between the extents + float start_value = 0.0f; + float end_value = (((float)rand() / (float)RAND_MAX) * (POSITION_EXTENT * 2.0f)) - POSITION_EXTENT; - // 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); + // 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 how far along this movement to be + float percent_along = (float)update / (float)UPDATES_PER_MOVE; + + switch(INTERP_MODE) { + case 0: + // Move the motor instantly to the end value + pos_pid.setpoint = end_value; + break; + + case 2: + // Move the motor between values using cosine + pos_pid.setpoint = (((-cosf(percent_along * (float)M_PI) + 1.0) / 2.0) * (end_value - start_value)) + start_value; + break; + + case 1: + default: + // Move the motor linearly between values + pos_pid.setpoint = (percent_along * (end_value - start_value)) + start_value; } - } - // 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); + // 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 on every multiple + if(print_count == 0) { + printf("Pos = %f, ", capture.degrees()); + printf("Pos SP = %f, ", pos_pid.setpoint); + printf("Speed = %f\n", m.speed() * SPD_PRINT_SCALE); } - for(auto i = 0u; i < STEPS; i++) { - m.to_percent(i, STEPS, 0, 0.0 - SWEEP_EXTENT, SWEEP_EXTENT); - sleep_ms(STEPS_INTERVAL_MS); + + // 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 movement? + if(update >= UPDATES_PER_MOVE) { + update = 0; // Reset the counter + + // 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) * (POSITION_EXTENT * 2.0f)) - POSITION_EXTENT; } + + sleep_ms(UPDATE_RATE * 1000.0f); } // Disable the motor diff --git a/examples/motor2040/motor2040_position_on_velocity_control.cpp b/examples/motor2040/motor2040_position_on_velocity_control.cpp index 3463e37c..f53c91e9 100644 --- a/examples/motor2040/motor2040_position_on_velocity_control.cpp +++ b/examples/motor2040/motor2040_position_on_velocity_control.cpp @@ -1,6 +1,9 @@ +#include #include "pico/stdlib.h" #include "motor2040.hpp" +#include "button.hpp" +#include "pid.hpp" /* An example of how to move a motor smoothly between random positions, @@ -10,64 +13,155 @@ Press "Boot" to exit the program. */ using namespace motor; +using namespace encoder; -// How many sweeps of the motor to perform -const uint SWEEPS = 3; +// The pins of the motor being profiled +const pin_pair MOTOR_PINS = motor2040::MOTOR_A; -// The number of discrete sweep steps -const uint STEPS = 10; +// The pins of the encoder attached to the profiled motor +const pin_pair ENCODER_PINS = motor2040::ENCODER_A; -// The time in milliseconds between each step of the sequence -const uint STEPS_INTERVAL_MS = 500; +// The gear ratio of the motor +constexpr float GEAR_RATIO = 50.0f; -// How far from zero to move the motor when sweeping -constexpr float SWEEP_EXTENT = 90.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 to travel between each random value, in seconds +constexpr float TIME_FOR_EACH_MOVE = 1.0f; +const uint UPDATES_PER_MOVE = TIME_FOR_EACH_MOVE * UPDATES; + +// How many of the updates should be printed (i.e. 2 would be every other update) +const uint PRINT_DIVIDER = 4; + +// Multipliers for the different printed values, so they appear nicely on the Thonny plotter +constexpr float ACC_PRINT_SCALE = 2.0f; // Acceleration multiplier +constexpr float SPD_PRINT_SCALE = 40.0f; // Driving Speed multipler + +// How far from zero to move the motor, in degrees +constexpr float POSITION_EXTENT = 180.0f; + +// The maximum speed to move the motor at, in revolutions per second +constexpr float MAX_SPEED = 1.0f; + +// The interpolating mode between setpoints. STEP (0), LINEAR (1), COSINE (2) +const uint INTERP_MODE = 0; + +// PID values +constexpr float POS_KP = 0.025f; // Position proportional (P) gain +constexpr float POS_KI = 0.0f; // Position integral (I) gain +constexpr float POS_KD = 0.0f; // Position derivative (D) gain + +constexpr float VEL_KP = 30.0f; // Velocity proportional (P) gain +constexpr float VEL_KI = 0.0f; // Velocity integral (I) gain +constexpr float VEL_KD = 0.4f; // Velocity derivative (D) gain -// Create a motor on pin 0 and 1 -Motor m = Motor(motor2040::MOTOR_A); +// 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 both position and velocity control +PID pos_pid = PID(POS_KP, POS_KI, POS_KD, UPDATE_RATE); +PID vel_pid = PID(VEL_KP, VEL_KI, VEL_KD, UPDATE_RATE); int main() { stdio_init_all(); - // Initialise the motor + // Initialise the motor and encoder m.init(); + enc.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); + uint update = 0; + uint print_count = 0; - // Stop the motor - m.stop(); - sleep_ms(2000); + // Set the initial value and create a random end value between the extents + float start_value = 0.0f; + float end_value = (((float)rand() / (float)RAND_MAX) * (POSITION_EXTENT * 2.0f)) - POSITION_EXTENT; - // 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); + // 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 how far along this movement to be + float percent_along = (float)update / (float)UPDATES_PER_MOVE; + + switch(INTERP_MODE) { + case 0: + // Move the motor instantly to the end value + pos_pid.setpoint = end_value; + break; + + case 2: + // Move the motor between values using cosine + pos_pid.setpoint = (((-cosf(percent_along * (float)M_PI) + 1.0) / 2.0) * (end_value - start_value)) + start_value; + break; + + case 1: + default: + // Move the motor linearly between values + pos_pid.setpoint = (percent_along * (end_value - start_value)) + start_value; } - } - // 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); + // Calculate the velocity to move the motor closer to the position setpoint + float vel = pos_pid.calculate(capture.degrees(), capture.degrees_per_second()); + + // Limit the velocity between user defined limits, and set it as the new setpoint of the velocity PID + vel_pid.setpoint = CLAMP(vel, -MAX_SPEED, MAX_SPEED); + + // Calculate the acceleration to apply to the motor to move it closer to the velocity setpoint + float accel = vel_pid.calculate(capture.revolutions_per_second()); + + // Accelerate or decelerate the motor + m.speed(m.speed() + (accel * UPDATE_RATE)); + + // Print out the current motor values and their setpoints, but only on every multiple + if(print_count == 0) { + printf("Pos = %f, ", capture.degrees()); + printf("Pos SP = %f, ", pos_pid.setpoint); + printf("Vel = %f, ", capture.revolutions_per_second() * SPD_PRINT_SCALE); + printf("Vel SP = %f, ", vel_pid.setpoint * SPD_PRINT_SCALE); + printf("Accel = %f, ", accel * ACC_PRINT_SCALE); + printf("Speed = %f\n", m.speed()); } - for(auto i = 0u; i < STEPS; i++) { - m.to_percent(i, STEPS, 0, 0.0 - SWEEP_EXTENT, SWEEP_EXTENT); - sleep_ms(STEPS_INTERVAL_MS); + + // 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 movement? + if(update >= UPDATES_PER_MOVE) { + update = 0; // Reset the counter + + // 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) * (POSITION_EXTENT * 2.0f)) - POSITION_EXTENT; } + + sleep_ms(UPDATE_RATE * 1000.0f); } // Disable the motor diff --git a/examples/motor2040/motor2040_position_on_velocity_tuning.cpp b/examples/motor2040/motor2040_position_on_velocity_tuning.cpp index 51847ea4..c97c4bf6 100644 --- a/examples/motor2040/motor2040_position_on_velocity_tuning.cpp +++ b/examples/motor2040/motor2040_position_on_velocity_tuning.cpp @@ -1,6 +1,9 @@ +#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 @@ -12,64 +15,133 @@ Press "Boot" to exit the program. */ using namespace motor; +using namespace encoder; -// How many sweeps of the motor to perform -const uint SWEEPS = 3; +// The pins of the motor being profiled +const pin_pair MOTOR_PINS = motor2040::MOTOR_A; -// The number of discrete sweep steps -const uint STEPS = 10; +// The pins of the encoder attached to the profiled motor +const pin_pair ENCODER_PINS = motor2040::ENCODER_A; -// The time in milliseconds between each step of the sequence -const uint STEPS_INTERVAL_MS = 500; +// The gear ratio of the motor +constexpr float GEAR_RATIO = 50.0f; -// How far from zero to move the motor when sweeping -constexpr float SWEEP_EXTENT = 90.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 = 1.0f; + +// 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 = 4; + +// Multipliers for the different printed values, so they appear nicely on the Thonny plotter +constexpr float ACC_PRINT_SCALE = 1.0f; // Acceleration multiplier +constexpr float SPD_PRINT_SCALE = 20.0f; // Driving Speed multipler + +// How far from zero to move the motor, in degrees +constexpr float POSITION_EXTENT = 180.0f; + +// The maximum speed to move the motor at, in revolutions per second +constexpr float MAX_SPEED = 2.0f; + +// PID values +constexpr float POS_KP = 0.025f; // Position proportional (P) gain +constexpr float POS_KI = 0.0f; // Position integral (I) gain +constexpr float POS_KD = 0.0f; // Position derivative (D) gain + +constexpr float VEL_KP = 30.0f; // Velocity proportional (P) gain +constexpr float VEL_KI = 0.0f; // Velocity integral (I) gain +constexpr float VEL_KD = 0.4f; // Velocity derivative (D) gain -// Create a motor on pin 0 and 1 -Motor m = Motor(motor2040::MOTOR_A); +// 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 both position and velocity control +PID pos_pid = PID(POS_KP, POS_KI, POS_KD, UPDATE_RATE); +PID vel_pid = PID(VEL_KP, VEL_KI, VEL_KD, UPDATE_RATE); int main() { stdio_init_all(); - // Initialise the motor + // Initialise the motor and encoder m.init(); + enc.init(); // Enable the motor m.enable(); - sleep_ms(2000); - // Go at full neative - m.full_negative(); - sleep_ms(2000); + // Set the initial setpoint position + pos_pid.setpoint = POSITION_EXTENT; - // Go at full positive - m.full_positive(); - sleep_ms(2000); - // Stop the motor - m.stop(); - sleep_ms(2000); + uint update = 0; + uint print_count = 0; - // 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); + // 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()); + + // Limit the velocity between user defined limits, and set it as the new setpoint of the velocity PID + vel_pid.setpoint = CLAMP(vel, -MAX_SPEED, MAX_SPEED); + + // Calculate the acceleration to apply to the motor to move it closer to the velocity setpoint + float accel = vel_pid.calculate(capture.revolutions_per_second()); + + // Accelerate or decelerate the motor + m.speed(m.speed() + (accel * UPDATE_RATE)); + + // Print out the current motor values and their setpoints, + // but only for the first few updates and only every multiple + if(update < (uint)(PRINT_WINDOW * UPDATES) && print_count == 0) { + printf("Pos = %f, ", capture.degrees()); + printf("Pos SP = %f, ", pos_pid.setpoint); + printf("Vel = %f, ", capture.revolutions_per_second() * SPD_PRINT_SCALE); + printf("Vel SP = %f, ", vel_pid.setpoint * SPD_PRINT_SCALE); + printf("Accel = %f, ", accel * ACC_PRINT_SCALE); + printf("Speed = %f\n", m.speed()); } - } - // 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); + // 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 >= (uint)(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 diff --git a/examples/motor2040/motor2040_position_tuning.cpp b/examples/motor2040/motor2040_position_tuning.cpp index 2c20beea..4710a359 100644 --- a/examples/motor2040/motor2040_position_tuning.cpp +++ b/examples/motor2040/motor2040_position_tuning.cpp @@ -55,9 +55,9 @@ constexpr float SPD_PRINT_SCALE = 10.0f; // Driving Speed multipler 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 +constexpr float POS_KP = 0.14f; // Position proportional (P) gain +constexpr float POS_KI = 0.0f; // Position integral (I) gain +constexpr float POS_KD = 0.002f; // Position derivative (D) gain // Create a motor and set its direction and speed scale @@ -104,7 +104,7 @@ int main() { // 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) { + if(update < (uint)(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); @@ -116,11 +116,11 @@ int main() { update++; // Move along in time // Have we reached the end of this time window? - if(update >= (MOVEMENT_WINDOW * UPDATES)){ + if(update >= (uint)(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; + pos_pid.setpoint = 0.0f - pos_pid.setpoint; } sleep_ms(UPDATE_RATE * 1000.0f); diff --git a/examples/motor2040/motor2040_quad_position_wave.cpp b/examples/motor2040/motor2040_quad_position_wave.cpp index 240cc936..67895d2c 100644 --- a/examples/motor2040/motor2040_quad_position_wave.cpp +++ b/examples/motor2040/motor2040_quad_position_wave.cpp @@ -1,6 +1,9 @@ +#include #include "pico/stdlib.h" #include "motor2040.hpp" +#include "button.hpp" +#include "pid.hpp" /* A demonstration of driving all four of Motor 2040's motor outputs between @@ -9,67 +12,158 @@ positions, with the help of their attached encoders and PID control. Press "Boot" to exit the program. */ +using namespace plasma; using namespace motor; +using namespace encoder; -// How many sweeps of the motor to perform -const uint SWEEPS = 3; +// The gear ratio of the motor +constexpr float GEAR_RATIO = 50.0f; -// The number of discrete sweep steps -const uint STEPS = 10; +// The counts per revolution of the motor's output shaft +constexpr float COUNTS_PER_REV = MMME_CPR * GEAR_RATIO; -// The time in milliseconds between each step of the sequence -const uint STEPS_INTERVAL_MS = 500; +// The scaling to apply to the motor's speed to match its real-world speed +float SPEED_SCALE = 5.4f; -// How far from zero to move the motor when sweeping -constexpr float SWEEP_EXTENT = 90.0f; +// How many times to update the motor per second +const uint UPDATES = 100; +constexpr float UPDATE_RATE = 1.0f / (float)UPDATES; + +// The time to travel between each random value +constexpr float TIME_FOR_EACH_MOVE = 2.0f; +const uint UPDATES_PER_MOVE = TIME_FOR_EACH_MOVE * UPDATES; + +// How many of the updates should be printed (i.e. 2 would be every other update) +const uint PRINT_DIVIDER = 4; + +// The brightness of the RGB LED +constexpr float BRIGHTNESS = 0.4f; + +// PID values +constexpr float POS_KP = 0.14f; // Position proportional (P) gain +constexpr float POS_KI = 0.0f; // Position integral (I) gain +constexpr float POS_KD = 0.002f; // Position derivative (D) gain -// Create a motor on pin 0 and 1 -Motor m = Motor(motor2040::MOTOR_A); +// 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 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 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); + +// Create an array of PID pointers +PID pos_pids[NUM_MOTORS]; int main() { stdio_init_all(); - // Initialise the motor - m.init(); + // Fill the arrays of motors, encoders, and pids, and initialise them + for(auto i = 0u; i < NUM_MOTORS; i++) { + motors[i] = new Motor(motor_pins[i], NORMAL_DIR, SPEED_SCALE); + motors[i]->init(); - // Enable the motor - m.enable(); - sleep_ms(2000); + encoders[i] = new Encoder(pio0, i, encoder_pins[i], PIN_UNUSED, NORMAL_DIR, COUNTS_PER_REV, true); + encoders[i]->init(); - // 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); - } + pos_pids[i] = PID(POS_KP, POS_KI, POS_KD, UPDATE_RATE); } - // 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); - } + // Reverse the direction of the B and D motors and encoders + motors[1]->direction(REVERSED_DIR); + motors[3]->direction(REVERSED_DIR); + encoders[1]->direction(REVERSED_DIR); + encoders[3]->direction(REVERSED_DIR); + + // Start updating the LED + led.start(); + + // Enable all motors + for(auto i = 0u; i < NUM_MOTORS; i++) { + motors[i]->enable(); } - // Disable the motor - m.disable(); + uint update = 0; + uint print_count = 0; + + // Set the initial and end values + float start_value = 0.0f; + float end_value = 270.0f; + + Encoder::Capture captures[NUM_MOTORS]; + + // Continually move the motor until the user button is pressed + while(!user_sw.raw()) { + + // Capture the state of all the encoders + for(auto i = 0u; i < NUM_MOTORS; i++) { + captures[i] = encoders[i]->capture(); + } + + // Calculate how far along this movement to be + float percent_along = (float)update / (float)UPDATES_PER_MOVE; + + for(auto i = 0u; i < NUM_MOTORS; i++) { + // Move the motor between values using cosine + pos_pids[i].setpoint = (((-cosf(percent_along * (float)M_PI) + 1.0) / 2.0) * (end_value - start_value)) + start_value; + + // Calculate the velocity to move the motor closer to the position setpoint + float vel = pos_pids[i].calculate(captures[i].degrees(), captures[i].degrees_per_second()); + + // Set the new motor driving speed + motors[i]->speed(vel); + } + + // Update the LED + led.set_hsv(0, percent_along, 1.0f, BRIGHTNESS); + + // Print out the current motor values and their setpoints, but only on every multiple + if(print_count == 0) { + for(auto i = 0u; i < NUM_ENCODERS; i++) { + printf("%s = %f, ", ENCODER_NAMES[i], captures[i].degrees()); + } + printf("\n"); + } + + // 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 movement? + if(update >= UPDATES_PER_MOVE) { + update = 0; // Reset the counter + + // Swap the start and end values + float temp = start_value; + start_value = end_value; + end_value = temp; + } + + sleep_ms(UPDATE_RATE * 1000.0f); + } + + // 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_quad_velocity_sequence.cpp b/examples/motor2040/motor2040_quad_velocity_sequence.cpp index e612f655..5b3825f0 100644 --- a/examples/motor2040/motor2040_quad_velocity_sequence.cpp +++ b/examples/motor2040/motor2040_quad_velocity_sequence.cpp @@ -1,6 +1,9 @@ +#include #include "pico/stdlib.h" #include "motor2040.hpp" +#include "button.hpp" +#include "pid.hpp" /* A demonstration of driving all four of Motor 2040's motor outputs through a @@ -10,66 +13,197 @@ Press "Boot" to exit the program. */ using namespace motor; +using namespace encoder; -// How many sweeps of the motor to perform -const uint SWEEPS = 3; +enum Wheels { + FL = 2, + FR = 3, + RL = 1, + RR = 0, +}; -// The number of discrete sweep steps -const uint STEPS = 10; +// The gear ratio of the motor +constexpr float GEAR_RATIO = 50.0f; -// The time in milliseconds between each step of the sequence -const uint STEPS_INTERVAL_MS = 500; +// The counts per revolution of the motor's output shaft +constexpr float COUNTS_PER_REV = MMME_CPR * GEAR_RATIO; -// How far from zero to move the motor when sweeping -constexpr float SWEEP_EXTENT = 90.0f; +// 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 to travel between each random value +constexpr float TIME_FOR_EACH_MOVE = 2.0f; +const uint UPDATES_PER_MOVE = TIME_FOR_EACH_MOVE * UPDATES; + +// How many of the updates should be printed (i.e. 2 would be every other update) +const uint PRINT_DIVIDER = 4; + +// The speed to drive the wheels at +constexpr float DRIVING_SPEED = 1.0f; + +// PID values +constexpr float VEL_KP = 30.0f; // Velocity proportional (P) gain +constexpr float VEL_KI = 0.0f; // Velocity integral (I) gain +constexpr float VEL_KD = 0.4f; // Velocity derivative (D) gain -// Create a motor on pin 0 and 1 -Motor m = Motor(motor2040::MOTOR_A); +// 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 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[] = {"RR", "RL", "FL", "FR"}; +const uint NUM_ENCODERS = count_of(encoder_pins); +Encoder *encoders[NUM_ENCODERS]; + +// Create the user button +Button user_sw(motor2040::USER_SW); + +// Create an array of PID pointers +PID vel_pids[NUM_MOTORS]; + + +// Helper functions for driving in common directions +void drive_forward(float speed) { + vel_pids[FL].setpoint = speed; + vel_pids[FR].setpoint = speed; + vel_pids[RL].setpoint = speed; + vel_pids[RR].setpoint = speed; +} + +void turn_right(float speed) { + vel_pids[FL].setpoint = speed; + vel_pids[FR].setpoint = -speed; + vel_pids[RL].setpoint = speed; + vel_pids[RR].setpoint = -speed; +} + +void strafe_right(float speed) { + vel_pids[FL].setpoint = speed; + vel_pids[FR].setpoint = -speed; + vel_pids[RL].setpoint = -speed; + vel_pids[RR].setpoint = speed; +} + +void stop() { + vel_pids[FL].setpoint = 0.0f; + vel_pids[FR].setpoint = 0.0f; + vel_pids[RL].setpoint = 0.0f; + vel_pids[RR].setpoint = 0.0f; +} int main() { stdio_init_all(); - // Initialise the motor - m.init(); + // Fill the arrays of motors, encoders, and pids, and initialise them + for(auto i = 0u; i < NUM_MOTORS; i++) { + motors[i] = new Motor(motor_pins[i], NORMAL_DIR, SPEED_SCALE); + motors[i]->init(); - // Enable the motor - m.enable(); - sleep_ms(2000); + encoders[i] = new Encoder(pio0, i, encoder_pins[i], PIN_UNUSED, NORMAL_DIR, COUNTS_PER_REV, true); + encoders[i]->init(); - // 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); - } + vel_pids[i] = PID(VEL_KP, VEL_KI, VEL_KD, UPDATE_RATE); } - // 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); - } + // Reverse the direction of the B and D motors and encoders + motors[FL]->direction(REVERSED_DIR); + motors[RL]->direction(REVERSED_DIR); + encoders[FL]->direction(REVERSED_DIR); + encoders[RL]->direction(REVERSED_DIR); + + // Enable all motors + for(auto i = 0u; i < NUM_MOTORS; i++) { + motors[i]->enable(); } - // Disable the motor - m.disable(); + uint update = 0; + uint print_count = 0; + uint sequence = 0; + + Encoder::Capture captures[NUM_MOTORS]; + + // Continually move the motor until the user button is pressed + while(!user_sw.raw()) { + + // Capture the state of all the encoders + for(auto i = 0u; i < NUM_MOTORS; i++) { + captures[i] = encoders[i]->capture(); + } + + for(auto i = 0u; i < NUM_MOTORS; i++) { + // Calculate the acceleration to apply to the motor to move it closer to the velocity setpoint + float accel = vel_pids[i].calculate(captures[i].revolutions_per_second()); + + // Accelerate or decelerate the motor + motors[i]->speed(motors[i]->speed() + (accel * UPDATE_RATE)); + } + + // Print out the current motor values and their setpoints, but only on every multiple + if(print_count == 0) { + for(auto i = 0u; i < NUM_ENCODERS; i++) { + printf("%s = %f, ", ENCODER_NAMES[i], captures[i].revolutions_per_second()); + } + printf("\n"); + } + + // 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 movement? + if(update >= UPDATES_PER_MOVE) { + update = 0; // Reset the counter + + // Move on to the next part of the sequence + sequence += 1; + + // Loop the sequence back around + if(sequence >= 7) { + sequence = 0; + } + } + + // Set the motor speeds, based on the sequence + switch(sequence) { + case 0: + drive_forward(DRIVING_SPEED); + break; + case 1: + drive_forward(-DRIVING_SPEED); + break; + case 2: + turn_right(DRIVING_SPEED); + break; + case 3: + turn_right(-DRIVING_SPEED); + break; + case 4: + strafe_right(DRIVING_SPEED); + break; + case 5: + strafe_right(-DRIVING_SPEED); + break; + default: + stop(); + break; + } + + sleep_ms(UPDATE_RATE * 1000.0f); + } + + // Stop all the motors + for(auto m = 0u; m < NUM_MOTORS; m++) { + motors[m]->disable(); + } } diff --git a/examples/motor2040/motor2040_reactive_encoder.cmake b/examples/motor2040/motor2040_reactive_encoder.cmake index c5173529..1c943253 100644 --- a/examples/motor2040/motor2040_reactive_encoder.cmake +++ b/examples/motor2040/motor2040_reactive_encoder.cmake @@ -4,6 +4,8 @@ add_executable(${OUTPUT_NAME} motor2040_reactive_encoder.cpp) target_link_libraries(${OUTPUT_NAME} pico_stdlib motor2040 + button + pid ) # enable usb output diff --git a/examples/motor2040/motor2040_reactive_encoder.cpp b/examples/motor2040/motor2040_reactive_encoder.cpp index bde6948b..8a2317f2 100644 --- a/examples/motor2040/motor2040_reactive_encoder.cpp +++ b/examples/motor2040/motor2040_reactive_encoder.cpp @@ -1,6 +1,9 @@ +#include #include "pico/stdlib.h" #include "motor2040.hpp" +#include "button.hpp" +#include "pid.hpp" /* A demonstration of how a motor with an encoder can be used @@ -10,67 +13,153 @@ force-feedback for arbitrary detents and end stops. Press "Boot" to exit the program. */ +using namespace plasma; using namespace motor; +using namespace encoder; -// How many sweeps of the motor to perform -const uint SWEEPS = 3; +// The pins of the motor being profiled +const pin_pair MOTOR_PINS = motor2040::MOTOR_A; -// The number of discrete sweep steps -const uint STEPS = 10; +// The pins of the encoder attached to the profiled motor +const pin_pair ENCODER_PINS = motor2040::ENCODER_A; -// The time in milliseconds between each step of the sequence -const uint STEPS_INTERVAL_MS = 500; +// The gear ratio of the motor +constexpr float GEAR_RATIO = 50.0f; -// How far from zero to move the motor when sweeping -constexpr float SWEEP_EXTENT = 90.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; + +// How many of the updates should be printed (i.e. 2 would be every other update) +const uint PRINT_DIVIDER = 4; + +// Multipliers for the different printed values, so they appear nicely on the Thonny plotter +constexpr float SPD_PRINT_SCALE = 20.0f; // Driving Speed multipler + +// The size (in degrees) of each detent region +constexpr float DETENT_SIZE = 20.0f; + +// The minimum detent that can be counted to +const int MIN_DETENT = -12; + +// The maximum detent that can be counted to +const int MAX_DETENT = +12; + +// The maximum drive force (as a percent) to apply when crossing detents +constexpr float MAX_DRIVE_PERCENT = 0.5f; + +// The brightness of the RGB LED +constexpr float BRIGHTNESS = 0.4f; + +// PID values +constexpr float POS_KP = 0.14f; // Position proportional (P) gain +constexpr float POS_KI = 0.0f; // Position integral (I) gain +constexpr float POS_KD = 0.002f; // Position derivative (D) gain -// Create a motor on pin 0 and 1 -Motor m = Motor(motor2040::MOTOR_A); +// 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 the LED, using PIO 1 and State Machine 0 +WS2812 led(motor2040::NUM_LEDS, pio1, 0, motor2040::LED_DATA); + +// Create PID object for position control +PID pos_pid = PID(POS_KP, POS_KI, POS_KD, UPDATE_RATE); + +int current_detent = 0; + + +// Function to deal with a detent change +void detent_change(int change) { + // Update the current detent and pid setpoint + current_detent += change; + + // Update the motor position setpoint + pos_pid.setpoint = (current_detent * DETENT_SIZE); + printf("Detent = %d\n", current_detent); + + // Convert the current detent to a hue and set the onboard led to it + float hue = (float)(current_detent - MIN_DETENT) / (float)(MAX_DETENT - MIN_DETENT); + led.set_hsv(0, hue, 1.0, BRIGHTNESS); +} int main() { stdio_init_all(); - // Initialise the motor + // Initialise the motor and encoder m.init(); + enc.init(); + + // Start updating the LED + led.start(); // Enable the motor m.enable(); - sleep_ms(2000); - // Go at full neative - m.full_negative(); - sleep_ms(2000); + // Call the function once to set the setpoint and print the value + detent_change(0); - // Go at full positive - m.full_positive(); - sleep_ms(2000); + // Continually move the motor until the user button is pressed + while(!user_sw.raw()) { - // Stop the motor - m.stop(); - sleep_ms(2000); + // Capture the state of the encoder + Encoder::Capture capture = enc.capture(); - // 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); + // Get the current detent's centre angle + float detent_angle = ((float)current_detent * DETENT_SIZE); + + // Is the current angle above the region of this detent? + if(capture.degrees() > detent_angle + (DETENT_SIZE / 2)) { + // Is there another detent we can move to? + if(current_detent < MAX_DETENT) { + detent_change(1); // Increment to the next detent + } + } + // Is the current angle below the region of this detent? + else if(capture.degrees() < detent_angle - (DETENT_SIZE / 2)) { + // Is there another detent we can move to? + if(current_detent > MIN_DETENT) { + detent_change(-1); // Decrement to the next detent + } } - } - // 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); + // Calculate the velocity to move the motor closer to the position setpoint + float vel = pos_pid.calculate(capture.degrees(), capture.degrees_per_second()); + + // If the current angle is within the detent range, limit the max vel + // (aka feedback force) that the user will feel when turning the motor between detents + if((capture.degrees() >= MIN_DETENT * DETENT_SIZE) && (capture.degrees() <= MAX_DETENT * DETENT_SIZE)) { + vel = CLAMP(vel, -MAX_DRIVE_PERCENT, MAX_DRIVE_PERCENT); } + + // Set the new motor driving speed + m.speed(vel); + + sleep_ms(UPDATE_RATE * 1000.0f); } // Disable the motor 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_single_motor.cpp b/examples/motor2040/motor2040_single_motor.cpp index bafe03a8..bda6ed44 100644 --- a/examples/motor2040/motor2040_single_motor.cpp +++ b/examples/motor2040/motor2040_single_motor.cpp @@ -18,7 +18,7 @@ const uint STEPS = 10; const uint STEPS_INTERVAL_MS = 500; // How far from zero to move the motor when sweeping -constexpr float SPEED_EXTENT = 10.0f; +constexpr float SPEED_EXTENT = 1.0f; // Create a motor diff --git a/examples/motor2040/motor2040_velocity_control.cmake b/examples/motor2040/motor2040_velocity_control.cmake index be841677..d4ea113b 100644 --- a/examples/motor2040/motor2040_velocity_control.cmake +++ b/examples/motor2040/motor2040_velocity_control.cmake @@ -4,6 +4,8 @@ add_executable(${OUTPUT_NAME} motor2040_velocity_control.cpp) target_link_libraries(${OUTPUT_NAME} pico_stdlib motor2040 + button + pid ) # enable usb output diff --git a/examples/motor2040/motor2040_velocity_control.cpp b/examples/motor2040/motor2040_velocity_control.cpp index 722c7639..09e18509 100644 --- a/examples/motor2040/motor2040_velocity_control.cpp +++ b/examples/motor2040/motor2040_velocity_control.cpp @@ -1,6 +1,9 @@ +#include #include "pico/stdlib.h" #include "motor2040.hpp" +#include "button.hpp" +#include "pid.hpp" /* An example of how to drive a motor smoothly between random speeds, @@ -10,64 +13,138 @@ Press "Boot" to exit the program. */ using namespace motor; +using namespace encoder; -// How many sweeps of the motor to perform -const uint SWEEPS = 3; +// The pins of the motor being profiled +const pin_pair MOTOR_PINS = motor2040::MOTOR_A; -// The number of discrete sweep steps -const uint STEPS = 10; +// The pins of the encoder attached to the profiled motor +const pin_pair ENCODER_PINS = motor2040::ENCODER_A; -// The time in milliseconds between each step of the sequence -const uint STEPS_INTERVAL_MS = 500; +// The gear ratio of the motor +constexpr float GEAR_RATIO = 50.0f; -// How far from zero to move the motor when sweeping -constexpr float SWEEP_EXTENT = 90.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 to travel between each random value, in seconds +constexpr float TIME_FOR_EACH_MOVE = 1.0f; +const uint UPDATES_PER_MOVE = TIME_FOR_EACH_MOVE * UPDATES; + +// How many of the updates should be printed (i.e. 2 would be every other update) +const uint PRINT_DIVIDER = 4; + +// Multipliers for the different printed values, so they appear nicely on the Thonny plotter +constexpr float ACC_PRINT_SCALE = 0.05f; // Acceleration multiplier + +// How far from zero to drive the motor at, in revolutions per second +constexpr float VELOCITY_EXTENT = 3.0f; + +// The interpolating mode between setpoints. STEP (0), LINEAR (1), COSINE (2) +const uint INTERP_MODE = 2; + +// PID values +constexpr float VEL_KP = 30.0f; // Velocity proportional (P) gain +constexpr float VEL_KI = 0.0f; // Velocity integral (I) gain +constexpr float VEL_KD = 0.4f; // Velocity derivative (D) gain -// Create a motor on pin 0 and 1 -Motor m = Motor(motor2040::MOTOR_A); +// 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 velocity control +PID vel_pid = PID(VEL_KP, VEL_KI, VEL_KD, UPDATE_RATE); int main() { stdio_init_all(); - // Initialise the motor + // Initialise the motor and encoder m.init(); + enc.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); + uint update = 0; + uint print_count = 0; - // Stop the motor - m.stop(); - sleep_ms(2000); + // Set the initial value and create a random end value between the extents + float start_value = 0.0f; + float end_value = (((float)rand() / (float)RAND_MAX) * (VELOCITY_EXTENT * 2.0f)) - VELOCITY_EXTENT; - // 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); + // 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 how far along this movement to be + float percent_along = (float)update / (float)UPDATES_PER_MOVE; + + switch(INTERP_MODE) { + case 0: + // Move the motor instantly to the end value + vel_pid.setpoint = end_value; + break; + + case 2: + // Move the motor between values using cosine + vel_pid.setpoint = (((-cosf(percent_along * (float)M_PI) + 1.0) / 2.0) * (end_value - start_value)) + start_value; + break; + + case 1: + default: + // Move the motor linearly between values + vel_pid.setpoint = (percent_along * (end_value - start_value)) + start_value; } - } - // 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); + // Calculate the acceleration to apply to the motor to move it closer to the velocity setpoint + float accel = vel_pid.calculate(capture.revolutions_per_second()); + + // Accelerate or decelerate the motor + m.speed(m.speed() + (accel * UPDATE_RATE)); + + // Print out the current motor values and their setpoints, but only on every multiple + if(print_count == 0) { + printf("Vel = %f, ", capture.revolutions_per_second()); + printf("Vel SP = %f, ", vel_pid.setpoint); + printf("Accel = %f, ", accel * ACC_PRINT_SCALE); + printf("Speed = %f\n", m.speed()); } - for(auto i = 0u; i < STEPS; i++) { - m.to_percent(i, STEPS, 0, 0.0 - SWEEP_EXTENT, SWEEP_EXTENT); - sleep_ms(STEPS_INTERVAL_MS); + + // 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 movement? + if(update >= UPDATES_PER_MOVE) { + update = 0; // Reset the counter + + // 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) * (VELOCITY_EXTENT * 2.0f)) - VELOCITY_EXTENT; } + + sleep_ms(UPDATE_RATE * 1000.0f); } // Disable the motor diff --git a/examples/motor2040/motor2040_velocity_tuning.cmake b/examples/motor2040/motor2040_velocity_tuning.cmake index 076cfc92..a21dd16e 100644 --- a/examples/motor2040/motor2040_velocity_tuning.cmake +++ b/examples/motor2040/motor2040_velocity_tuning.cmake @@ -5,6 +5,7 @@ target_link_libraries(${OUTPUT_NAME} pico_stdlib motor2040 button + pid ) # enable usb output diff --git a/examples/motor2040/motor2040_velocity_tuning.cpp b/examples/motor2040/motor2040_velocity_tuning.cpp index 7ee2fa93..1348fe54 100644 --- a/examples/motor2040/motor2040_velocity_tuning.cpp +++ b/examples/motor2040/motor2040_velocity_tuning.cpp @@ -1,6 +1,9 @@ +#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 @@ -12,64 +15,116 @@ Press "Boot" to exit the program. */ using namespace motor; +using namespace encoder; -// How many sweeps of the motor to perform -const uint SWEEPS = 2; +// The pins of the motor being profiled +const pin_pair MOTOR_PINS = motor2040::MOTOR_A; -// The number of discrete sweep steps -const uint STEPS = 10; +// The pins of the encoder attached to the profiled motor +const pin_pair ENCODER_PINS = motor2040::ENCODER_A; -// The time in milliseconds between each step of the sequence -const uint STEPS_INTERVAL_MS = 500; +// The gear ratio of the motor +constexpr float GEAR_RATIO = 50.0f; -// How far from zero to drive the motor when sweeping -constexpr float SWEEP_EXTENT = 90.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 ACC_PRINT_SCALE = 0.01f; // Acceleration multiplier + +// How far from zero to drive the motor at, in revolutions per second +constexpr float VELOCITY_EXTENT = 3.0f; + +// PID values +constexpr float VEL_KP = 30.0f; // Velocity proportional (P) gain +constexpr float VEL_KI = 0.0f; // Velocity integral (I) gain +constexpr float VEL_KD = 0.4f; // Velocity derivative (D) gain -// Create a motor on pin 0 and 1 -Motor m = Motor(motor2040::MOTOR_A); +// 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 velocity control +PID vel_pid = PID(VEL_KP, VEL_KI, VEL_KD, UPDATE_RATE); int main() { stdio_init_all(); - // Initialise the motor + // Initialise the motor and encoder m.init(); + enc.init(); // Enable the motor m.enable(); - sleep_ms(2000); - // Go at full neative - m.full_negative(); - sleep_ms(2000); + // Set the initial setpoint velocity + vel_pid.setpoint = VELOCITY_EXTENT; - // Go at full positive - m.full_positive(); - sleep_ms(2000); - // Stop the motor - m.stop(); - sleep_ms(2000); + uint update = 0; + uint print_count = 0; - // 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); + // 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 acceleration to apply to the motor to move it closer to the velocity setpoint + float accel = vel_pid.calculate(capture.revolutions_per_second()); + + // Accelerate or decelerate the motor + m.speed(m.speed() + (accel * UPDATE_RATE)); + + // Print out the current motor values and their setpoints, + // but only for the first few updates and only every multiple + if(update < (uint)(PRINT_WINDOW * UPDATES) && print_count == 0) { + printf("Vel = %f, ", capture.revolutions_per_second()); + printf("Vel SP = %f, ", vel_pid.setpoint); + printf("Accel = %f, ", accel * ACC_PRINT_SCALE); + printf("Speed = %f\n", m.speed()); } - } - // 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); + // 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 >= (uint)(MOVEMENT_WINDOW * UPDATES)) { + update = 0; // Reset the counter + + // Set the new velocity setpoint to be the inverse of the current setpoint + vel_pid.setpoint = 0.0f - vel_pid.setpoint; } + + sleep_ms(UPDATE_RATE * 1000.0f); } // Disable the motor diff --git a/libraries/motor2040/motor2040.hpp b/libraries/motor2040/motor2040.hpp index 7ee5a688..0e6180c1 100644 --- a/libraries/motor2040/motor2040.hpp +++ b/libraries/motor2040/motor2040.hpp @@ -70,8 +70,8 @@ namespace motor { const uint NUM_SENSORS = 2; constexpr float SHUNT_RESISTOR = 0.47f; - constexpr float CURRENT_GAIN = 5; + constexpr float CURRENT_GAIN = 1; constexpr float VOLTAGE_GAIN = 3.9f / 13.9f; - constexpr float CURRENT_OFFSET = -0.02f; + constexpr float CURRENT_OFFSET = -0.005f; } } \ No newline at end of file diff --git a/micropython/examples/motor2040/motor_profiler.py b/micropython/examples/motor2040/motor_profiler.py index 30d2896e..6f501629 100644 --- a/micropython/examples/motor2040/motor_profiler.py +++ b/micropython/examples/motor2040/motor_profiler.py @@ -16,6 +16,8 @@ COUNTS_PER_REV = MMME_CPR * GEAR_RATIO # The counts per revolution of the motor DIRECTION = NORMAL_DIR # The direction to spin the motor in. NORMAL_DIR (0), REVERSED_DIR (1) SPEED_SCALE = 5.4 # The scaling to apply to the motor's speed. Set this to the maximum measured speed +ZERO_POINT = 0.0 # The duty cycle that corresponds with zero speed when plotting the motor's speed as a straight line +DEAD_ZONE = 0.0 # The duty cycle below which the motor's friction prevents it from moving DUTY_STEPS = 100 # How many duty cycle steps to sample the speed of SETTLE_TIME = 0.1 # How long to wait after changing motor duty cycle @@ -25,7 +27,7 @@ CAPTURE_TIME = 0.2 # How long to capture the motor's speed gc.collect() # Create a motor and set its speed scale, and give it a zero deadzone -m = Motor(MOTOR_PINS, direction=DIRECTION, speed_scale=SPEED_SCALE, zeropoint=0.0, deadzone=0.0) +m = Motor(MOTOR_PINS, direction=DIRECTION, speed_scale=SPEED_SCALE, zeropoint=ZERO_POINT, deadzone=DEAD_ZONE) # Create an encoder, using PIO 0 and State Machine 0 enc = Encoder(0, 0, ENCODER_PINS, direction=DIRECTION, counts_per_rev=COUNTS_PER_REV, count_microsteps=True) diff --git a/micropython/examples/motor2040/position_control.py b/micropython/examples/motor2040/position_control.py index 7d656527..8a514738 100644 --- a/micropython/examples/motor2040/position_control.py +++ b/micropython/examples/motor2040/position_control.py @@ -43,7 +43,7 @@ POS_KD = 0.0022 # Position derivative (D) gain gc.collect() # Create a motor and set its speed scale -m = Motor(MOTOR_PINS, direction=DIRECTION, speed_scale=SPEED_SCALE, deadzone=0.05) +m = Motor(MOTOR_PINS, direction=DIRECTION, speed_scale=SPEED_SCALE) # Create an encoder, using PIO 0 and State Machine 0 enc = Encoder(0, 0, ENCODER_PINS, direction=DIRECTION, counts_per_rev=COUNTS_PER_REV, count_microsteps=True) diff --git a/micropython/examples/motor2040/position_on_velocity_control.py b/micropython/examples/motor2040/position_on_velocity_control.py index 718f6e93..87e5e065 100644 --- a/micropython/examples/motor2040/position_on_velocity_control.py +++ b/micropython/examples/motor2040/position_on_velocity_control.py @@ -64,9 +64,6 @@ vel_pid = PID(VEL_KP, VEL_KI, VEL_KD, UPDATE_RATE) # Enable the motor to get started m.enable() -# Set the initial setpoint position -pos_pid.setpoint = POSITION_EXTENT - update = 0 print_count = 0 diff --git a/micropython/examples/motor2040/position_on_velocity_tuning.py b/micropython/examples/motor2040/position_on_velocity_tuning.py index 62778644..993b248a 100644 --- a/micropython/examples/motor2040/position_on_velocity_tuning.py +++ b/micropython/examples/motor2040/position_on_velocity_tuning.py @@ -48,7 +48,7 @@ VEL_KD = 0.4 # Velocity derivative (D) gain gc.collect() # Create a motor and set its speed scale -m = Motor(MOTOR_PINS, direction=DIRECTION, speed_scale=SPEED_SCALE, deadzone=0.0) +m = Motor(MOTOR_PINS, direction=DIRECTION, speed_scale=SPEED_SCALE) # Create an encoder, using PIO 0 and State Machine 0 enc = Encoder(0, 0, ENCODER_PINS, direction=DIRECTION, counts_per_rev=COUNTS_PER_REV, count_microsteps=True) diff --git a/micropython/examples/motor2040/quad_position_wave.py b/micropython/examples/motor2040/quad_position_wave.py index 3ef04108..c9a0d6d2 100644 --- a/micropython/examples/motor2040/quad_position_wave.py +++ b/micropython/examples/motor2040/quad_position_wave.py @@ -25,7 +25,7 @@ UPDATES_PER_MOVE = TIME_FOR_EACH_MOVE * UPDATES PRINT_DIVIDER = 4 # How many of the updates should be printed (i.e. 2 would be every other update) # LED constant -BRIGHTNESS = 0.4 # The brightness of the LEDs +BRIGHTNESS = 0.4 # The brightness of the RGB LED # PID values POS_KP = 0.14 # Position proportional (P) gain @@ -38,7 +38,7 @@ gc.collect() # Create a list of motors with a given speed scale MOTOR_PINS = [motor2040.MOTOR_A, motor2040.MOTOR_B, motor2040.MOTOR_C, motor2040.MOTOR_D] -motors = [Motor(pins, speed_scale=SPEED_SCALE, deadzone=0.05) for pins in MOTOR_PINS] +motors = [Motor(pins, speed_scale=SPEED_SCALE) for pins in MOTOR_PINS] # Create a list of encoders, using PIO 0, with the given counts per revolution ENCODER_PINS = [motor2040.ENCODER_A, motor2040.ENCODER_B, motor2040.ENCODER_C, motor2040.ENCODER_D] @@ -63,7 +63,7 @@ pos_pids = [PID(POS_KP, POS_KI, POS_KD, UPDATE_RATE) for i in range(motor2040.NU # Start updating the LED led.start() -# Enable the motor to get started +# Enable all motors for m in motors: m.enable() @@ -102,7 +102,7 @@ while user_sw.raw() is not True: # Print out the current motor values and their setpoints, but only on every multiple if print_count == 0: - for i in range(len(motors)): + for i in range(motor2040.NUM_MOTORS): print(ENCODER_NAMES[i], "=", captures[i].degrees, end=", ") print() diff --git a/micropython/examples/motor2040/quad_velocity_sequence.py b/micropython/examples/motor2040/quad_velocity_sequence.py index e9fc46a2..65183356 100644 --- a/micropython/examples/motor2040/quad_velocity_sequence.py +++ b/micropython/examples/motor2040/quad_velocity_sequence.py @@ -11,6 +11,12 @@ sequence of velocities, with the help of their attached encoders and PID control Press "Boot" to exit the program. """ +# Wheel friendly names +FL = 2 +FR = 3 +RL = 1 +RR = 0 + GEAR_RATIO = 50 # The gear ratio of the motors COUNTS_PER_REV = MMME_CPR * GEAR_RATIO # The counts per revolution of each motor's output shaft @@ -22,6 +28,8 @@ TIME_FOR_EACH_MOVE = 2 # The time to travel between each value UPDATES_PER_MOVE = TIME_FOR_EACH_MOVE * UPDATES PRINT_DIVIDER = 4 # How many of the updates should be printed (i.e. 2 would be every other update) +DRIVING_SPEED = 1.0 # The speed to drive the wheels at + # PID values VEL_KP = 30.0 # Velocity proportional (P) gain VEL_KI = 0.0 # Velocity integral (I) gain @@ -40,12 +48,6 @@ ENCODER_PINS = [motor2040.ENCODER_A, motor2040.ENCODER_B, motor2040.ENCODER_C, m ENCODER_NAMES = ["RR", "RL", "FL", "FR"] encoders = [Encoder(0, i, ENCODER_PINS[i], counts_per_rev=COUNTS_PER_REV, count_microsteps=True) for i in range(motor2040.NUM_MOTORS)] -# Wheel friendly names -FL = 2 -FR = 3 -RL = 1 -RR = 0 - # Reverse the direction of the B and D motors and encoders motors[FL].direction(REVERSED_DIR) motors[RL].direction(REVERSED_DIR) @@ -58,14 +60,6 @@ user_sw = Button(motor2040.USER_SW) # Create PID objects for position control vel_pids = [PID(VEL_KP, VEL_KI, VEL_KD, UPDATE_RATE) for i in range(motor2040.NUM_MOTORS)] -# Enable the motor to get started -for m in motors: - m.enable() - - -update = 0 -print_count = 0 - # Helper functions for driving in common directions def drive_forward(speed): @@ -96,6 +90,13 @@ def stop(): vel_pids[RR].setpoint = 0 +# Enable the motor to get started +for m in motors: + m.enable() + + +update = 0 +print_count = 0 sequence = 0 captures = [None] * motor2040.NUM_MOTORS @@ -107,9 +108,6 @@ while user_sw.raw() is not True: for i in range(motor2040.NUM_MOTORS): captures[i] = encoders[i].capture() - # Calculate how far along this movement to be - percent_along = min(update / UPDATES_PER_MOVE, 1.0) - for i in range(motor2040.NUM_MOTORS): # Calculate the acceleration to apply to the motor to move it closer to the velocity setpoint accel = vel_pids[i].calculate(captures[i].revolutions_per_second) @@ -119,7 +117,7 @@ while user_sw.raw() is not True: # Print out the current motor values, but only on every multiple if print_count == 0: - for i in range(len(motors)): + for i in range(motor2040.NUM_MOTORS): print(ENCODER_NAMES[i], "=", captures[i].revolutions_per_second, end=", ") print() @@ -141,17 +139,17 @@ while user_sw.raw() is not True: # Set the motor speeds, based on the sequence if sequence == 0: - drive_forward(1.0) + drive_forward(DRIVING_SPEED) elif sequence == 1: - drive_forward(-1.0) + drive_forward(-DRIVING_SPEED) elif sequence == 2: - turn_right(1.0) + turn_right(DRIVING_SPEED) elif sequence == 3: - turn_right(-1.0) + turn_right(-DRIVING_SPEED) elif sequence == 4: - strafe_right(1.0) + strafe_right(DRIVING_SPEED) elif sequence == 5: - strafe_right(-1.0) + strafe_right(-DRIVING_SPEED) elif sequence == 6: stop() diff --git a/micropython/examples/motor2040/reactive_encoder.py b/micropython/examples/motor2040/reactive_encoder.py index e5a2983b..4f1cb7fd 100644 --- a/micropython/examples/motor2040/reactive_encoder.py +++ b/micropython/examples/motor2040/reactive_encoder.py @@ -37,13 +37,13 @@ MIN_DETENT = -12 # The minimum detent that can be counted MAX_DETENT = +12 # The maximum detent that can be counted to MAX_DRIVE_PERCENT = 0.5 # The maximum drive force (as a percent) to apply when crossing detents +BRIGHTNESS = 0.4 # The brightness of the RGB LED + # PID values POS_KP = 0.14 # Position proportional (P) gain POS_KI = 0.0 # Position integral (I) gain POS_KD = 0.0022 # Position derivative (D) gain -BRIGHTNESS = 0.4 # The brightness of the LEDs - # Free up hardware resources ahead of creating a new Encoder gc.collect() diff --git a/micropython/examples/motor2040/read_sensors.py b/micropython/examples/motor2040/read_sensors.py index 15dd6335..7ca4909c 100644 --- a/micropython/examples/motor2040/read_sensors.py +++ b/micropython/examples/motor2040/read_sensors.py @@ -47,7 +47,7 @@ while user_sw.raw() is not True: # Read the current sense's of each motor and print the value for i in range(motor2040.NUM_MOTORS): mux.select(i + motor2040.CURRENT_SENSE_A_ADDR) - print("C", i + 1, "=", cur_adc.read_current(), sep="", end=", ") + print("C", i + 1, " = ", cur_adc.read_current(), sep="", end=", ") # Read the fault sense and print the value mux.select(motor2040.FAULT_SENSE_ADDR) diff --git a/micropython/examples/motor2040/velocity_control.py b/micropython/examples/motor2040/velocity_control.py index db05fa1b..53d47668 100644 --- a/micropython/examples/motor2040/velocity_control.py +++ b/micropython/examples/motor2040/velocity_control.py @@ -43,7 +43,7 @@ VEL_KD = 0.4 # Velocity derivative (D) gain gc.collect() # Create a motor and set its speed scale -m = Motor(MOTOR_PINS, direction=DIRECTION, speed_scale=SPEED_SCALE, deadzone=0.05) +m = Motor(MOTOR_PINS, direction=DIRECTION, speed_scale=SPEED_SCALE) # Create an encoder, using PIO 0 and State Machine 0 enc = Encoder(0, 0, ENCODER_PINS, direction=DIRECTION, counts_per_rev=COUNTS_PER_REV, count_microsteps=True) @@ -57,9 +57,6 @@ vel_pid = PID(VEL_KP, VEL_KI, VEL_KD, UPDATE_RATE) # Enable the motor to get started m.enable() -# Set the initial setpoint velocity -vel_pid.setpoint = VELOCITY_EXTENT - update = 0 print_count = 0 diff --git a/micropython/examples/motor2040/velocity_tuning.py b/micropython/examples/motor2040/velocity_tuning.py index e913e7ea..6f275f3b 100644 --- a/micropython/examples/motor2040/velocity_tuning.py +++ b/micropython/examples/motor2040/velocity_tuning.py @@ -23,9 +23,9 @@ SPEED_SCALE = 5.4 # The scaling to apply to the motor's sp UPDATES = 100 # How many times to update the motor per second UPDATE_RATE = 1 / UPDATES -PRINT_WINDOW = 1.0 # The time (in seconds) after a new setpoint, to display print out motor values +PRINT_WINDOW = 0.25 # The time (in seconds) after a new setpoint, to display print out motor values MOVEMENT_WINDOW = 2.0 # The time (in seconds) between each new setpoint being set -PRINT_DIVIDER = 4 # How many of the updates should be printed (i.e. 2 would be every other update) +PRINT_DIVIDER = 1 # How many of the updates should be printed (i.e. 2 would be every other update) # Multipliers for the different printed values, so they appear nicely on the Thonny plotter ACC_PRINT_SCALE = 0.01 # Acceleration multiplier @@ -42,7 +42,7 @@ VEL_KD = 0.4 # Velocity derivative (D) gain gc.collect() # Create a motor and set its speed scale -m = Motor(MOTOR_PINS, direction=DIRECTION, speed_scale=SPEED_SCALE, deadzone=0.0) +m = Motor(MOTOR_PINS, direction=DIRECTION, speed_scale=SPEED_SCALE) # Create an encoder, using PIO 0 and State Machine 0 enc = Encoder(0, 0, ENCODER_PINS, direction=DIRECTION, counts_per_rev=COUNTS_PER_REV, count_microsteps=True)