mirror
/
pimoroni-pico
kopia lustrzana https://github.com/pimoroni/pimoroni-pico
1
0
Forkuj 0
Kod Zgłoszenia Projekty Wydania Wiki Aktywność

Finished C++ examples and tweaked MP examples

motor-and-encoder
ZodiusInfuser 2022-04-28 21:23:18 +01:00
rodzic 415a1e559a
commit 15e5eaa890
30 zmienionych plików z 1156 dodań i 398 usunięć
Pokaż statystyki Ściągnij plik aktualizacji Ściągnij plik porównania Expand all files Collapse all files

4
drivers/analogmux/analogmux.cpp
Wyświetl plik

@ -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) {

5
drivers/pid/pid.cpp
Wyświetl plik

@ -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) {

1
drivers/pid/pid.hpp
Wyświetl plik

@ -8,6 +8,7 @@ namespace pimoroni {
class PID {
public:
PID();
PID(float kp, float ki, float kd, float sample_rate);
float calculate(float value);

3
examples/motor2040/motor2040_motor_cluster.cpp
Wyświetl plik

@ -76,4 +76,7 @@ int main() {
// Disable the motors
motors.disable_all();
// Sleep a short time so the disable takes effect
sleep_ms(100);
}

136
examples/motor2040/motor2040_motor_profiler.cpp
Wyświetl plik

@ -1,3 +1,4 @@
#include <cstdio>
#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();
}

2
examples/motor2040/motor2040_motor_wave.cpp
Wyświetl plik

@ -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);
}

147
examples/motor2040/motor2040_position_control.cpp
Wyświetl plik

@ -1,6 +1,9 @@
#include <cstdio>
#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

164
examples/motor2040/motor2040_position_on_velocity_control.cpp
Wyświetl plik

@ -1,6 +1,9 @@
#include <cstdio>
#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

144
examples/motor2040/motor2040_position_on_velocity_tuning.cpp
Wyświetl plik

@ -1,6 +1,9 @@
#include <cstdio>
#include "pico/stdlib.h"
#include "motor2040.hpp"
#include "button.hpp"
#include "pid.hpp"
/*
A program to aid in the discovery and tuning of motor PID
@ -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

12
examples/motor2040/motor2040_position_tuning.cpp
Wyświetl plik

@ -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);

184
examples/motor2040/motor2040_quad_position_wave.cpp
Wyświetl plik

@ -1,6 +1,9 @@
#include <cstdio>
#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);
}

224
examples/motor2040/motor2040_quad_velocity_sequence.cpp
Wyświetl plik

@ -1,6 +1,9 @@
#include <cstdio>
#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();
}
}

2
examples/motor2040/motor2040_reactive_encoder.cmake
Wyświetl plik

@ -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

161
examples/motor2040/motor2040_reactive_encoder.cpp
Wyświetl plik

@ -1,6 +1,9 @@
#include <cstdio>
#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);
}

2
examples/motor2040/motor2040_single_motor.cpp
Wyświetl plik

@ -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

2
examples/motor2040/motor2040_velocity_control.cmake
Wyświetl plik

@ -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

147
examples/motor2040/motor2040_velocity_control.cpp
Wyświetl plik

@ -1,6 +1,9 @@
#include <cstdio>
#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

1
examples/motor2040/motor2040_velocity_tuning.cmake
Wyświetl plik

@ -5,6 +5,7 @@ target_link_libraries(${OUTPUT_NAME}
pico_stdlib
motor2040
button
pid
)
# enable usb output

127
examples/motor2040/motor2040_velocity_tuning.cpp
Wyświetl plik

@ -1,6 +1,9 @@
#include <cstdio>
#include "pico/stdlib.h"
#include "motor2040.hpp"
#include "button.hpp"
#include "pid.hpp"
/*
A program to aid in the discovery and tuning of motor PID
@ -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

4
libraries/motor2040/motor2040.hpp
Wyświetl plik

@ -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;
}
}

4
micropython/examples/motor2040/motor_profiler.py
Wyświetl plik

@ -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)

2
micropython/examples/motor2040/position_control.py
Wyświetl plik

@ -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)

3
micropython/examples/motor2040/position_on_velocity_control.py
Wyświetl plik

@ -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

2
micropython/examples/motor2040/position_on_velocity_tuning.py
Wyświetl plik

@ -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)

8
micropython/examples/motor2040/quad_position_wave.py
Wyświetl plik

@ -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()

46
micropython/examples/motor2040/quad_velocity_sequence.py
Wyświetl plik

@ -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()

4
micropython/examples/motor2040/reactive_encoder.py
Wyświetl plik

@ -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()

2
micropython/examples/motor2040/read_sensors.py
Wyświetl plik

@ -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)

5
micropython/examples/motor2040/velocity_control.py
Wyświetl plik

@ -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

6
micropython/examples/motor2040/velocity_tuning.py
Wyświetl plik

@ -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)