Merge pull request #352 from pimoroni/motor-and-encoder
Added support for motors and encoders
This commit is contained in:
Philip Howard
GitHub
commit
cbf1aba919
|
|
@ -6,6 +6,12 @@
|
|||
#define PIMORONI_I2C_DEFAULT_INSTANCE i2c0
|
||||
#define PIMORONI_SPI_DEFAULT_INSTANCE spi0
|
||||
|
||||
// Macro to return a value clamped between a minimum and maximum
|
||||
#ifndef CLAMP
|
||||
#define CLAMP(a, mn, mx) ((a)<(mx)?((a)>(mn)?(a):(mn)):(mx))
|
||||
#endif
|
||||
|
||||
|
||||
namespace pimoroni {
|
||||
static const unsigned int PIN_UNUSED = INT_MAX; // Intentionally INT_MAX to avoid overflowing MicroPython's int type
|
||||
|
||||
|
|
@ -53,6 +59,11 @@ namespace pimoroni {
|
|||
ACTIVE_HIGH = 1
|
||||
};
|
||||
|
||||
enum Direction {
|
||||
NORMAL_DIR = 0,
|
||||
REVERSED_DIR = 1,
|
||||
};
|
||||
|
||||
inline uint32_t millis() {
|
||||
return to_ms_since_boot(get_absolute_time());
|
||||
}
|
||||
|
|
@ -74,4 +85,36 @@ namespace pimoroni {
|
|||
162, 163, 165, 167, 169, 170, 172, 174, 176, 178, 179, 181, 183, 185, 187, 189,
|
||||
191, 193, 194, 196, 198, 200, 202, 204, 206, 208, 210, 212, 214, 216, 218, 220,
|
||||
222, 224, 227, 229, 231, 233, 235, 237, 239, 241, 244, 246, 248, 250, 252, 255};
|
||||
|
||||
struct pin_pair {
|
||||
union {
|
||||
uint8_t first;
|
||||
uint8_t a;
|
||||
uint8_t positive;
|
||||
uint8_t phase;
|
||||
};
|
||||
union {
|
||||
uint8_t second;
|
||||
uint8_t b;
|
||||
uint8_t negative;
|
||||
uint8_t enable;
|
||||
};
|
||||
|
||||
pin_pair() : first(0), second(0) {}
|
||||
pin_pair(uint8_t first, uint8_t second) : first(first), second(second) {}
|
||||
};
|
||||
|
||||
struct bool_pair {
|
||||
union {
|
||||
bool first;
|
||||
bool a;
|
||||
};
|
||||
union {
|
||||
bool second;
|
||||
bool b;
|
||||
};
|
||||
|
||||
bool_pair() : first(false), second(false) {}
|
||||
bool_pair(bool first, bool second) : first(first), second(second) {}
|
||||
};
|
||||
}
|
||||
|
|
@ -21,6 +21,7 @@ add_subdirectory(bme68x)
|
|||
add_subdirectory(bmp280)
|
||||
add_subdirectory(bme280)
|
||||
add_subdirectory(button)
|
||||
add_subdirectory(pid)
|
||||
add_subdirectory(plasma)
|
||||
add_subdirectory(rgbled)
|
||||
add_subdirectory(icp10125)
|
||||
|
|
@ -29,4 +30,6 @@ add_subdirectory(hub75)
|
|||
add_subdirectory(uc8151)
|
||||
add_subdirectory(pwm)
|
||||
add_subdirectory(servo)
|
||||
add_subdirectory(encoder)
|
||||
add_subdirectory(motor)
|
||||
add_subdirectory(vl53l5cx)
|
||||
|
|
|
|||
|
|
@ -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) {
|
||||
|
|
@ -77,4 +81,11 @@ namespace pimoroni {
|
|||
pull_downs &= ~(1u << address);
|
||||
}
|
||||
}
|
||||
|
||||
bool AnalogMux::read() {
|
||||
if(muxed_pin != PIN_UNUSED) {
|
||||
return gpio_get(muxed_pin);
|
||||
}
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
|
@ -14,6 +14,7 @@ namespace pimoroni {
|
|||
void select(uint8_t address);
|
||||
void disable();
|
||||
void configure_pulls(uint8_t address, bool pullup, bool pulldown);
|
||||
bool read();
|
||||
|
||||
private:
|
||||
uint addr0_pin;
|
||||
|
|
|
|||
|
|
@ -0,0 +1 @@
|
|||
include(encoder.cmake)
|
||||
|
|
@ -0,0 +1,16 @@
|
|||
set(DRIVER_NAME encoder)
|
||||
add_library(${DRIVER_NAME} INTERFACE)
|
||||
|
||||
target_sources(${DRIVER_NAME} INTERFACE
|
||||
${CMAKE_CURRENT_LIST_DIR}/encoder.cpp
|
||||
)
|
||||
|
||||
pico_generate_pio_header(${DRIVER_NAME} ${CMAKE_CURRENT_LIST_DIR}/encoder.pio)
|
||||
|
||||
target_include_directories(${DRIVER_NAME} INTERFACE ${CMAKE_CURRENT_LIST_DIR})
|
||||
|
||||
# Pull in pico libraries that we need
|
||||
target_link_libraries(${DRIVER_NAME} INTERFACE
|
||||
pico_stdlib
|
||||
hardware_pio
|
||||
)
|
||||
|
|
@ -0,0 +1,446 @@
|
|||
#include <math.h>
|
||||
#include <cfloat>
|
||||
#include <climits>
|
||||
#include "hardware/irq.h"
|
||||
#include "hardware/clocks.h"
|
||||
#include "encoder.hpp"
|
||||
#include "encoder.pio.h"
|
||||
|
||||
#define LAST_STATE(state) ((state) & 0b0011)
|
||||
#define CURR_STATE(state) (((state) & 0b1100) >> 2)
|
||||
|
||||
namespace encoder {
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
// STATICS
|
||||
////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
Encoder* Encoder::encoders[][NUM_PIO_STATE_MACHINES] = { { nullptr, nullptr, nullptr, nullptr }, { nullptr, nullptr, nullptr, nullptr } };
|
||||
uint8_t Encoder::claimed_sms[] = { 0x0, 0x0 };
|
||||
uint Encoder::pio_program_offset[] = { 0, 0 };
|
||||
|
||||
|
||||
Encoder::Capture::Capture()
|
||||
: captured_count(0), captured_delta(0), captured_frequency(0.0f), counts_per_rev(INT32_MAX) {
|
||||
}
|
||||
|
||||
Encoder::Capture::Capture(int32_t count, int32_t delta, float frequency, float counts_per_rev)
|
||||
: captured_count(count), captured_delta(delta), captured_frequency(frequency)
|
||||
, counts_per_rev(MAX(counts_per_rev, FLT_EPSILON)) { //Clamp counts_per_rev to avoid potential NaN
|
||||
}
|
||||
|
||||
int32_t Encoder::Capture::count() const {
|
||||
return captured_count;
|
||||
}
|
||||
|
||||
int32_t Encoder::Capture::delta() const {
|
||||
return captured_delta;
|
||||
}
|
||||
|
||||
float Encoder::Capture::frequency() const {
|
||||
return captured_frequency;
|
||||
}
|
||||
|
||||
float Encoder::Capture::revolutions() const {
|
||||
return (float)captured_count / counts_per_rev;
|
||||
}
|
||||
|
||||
float Encoder::Capture::degrees() const {
|
||||
return revolutions() * 360.0f;
|
||||
}
|
||||
|
||||
float Encoder::Capture::radians() const {
|
||||
return revolutions() * M_TWOPI;
|
||||
}
|
||||
|
||||
float Encoder::Capture::revolutions_delta() const {
|
||||
return (float)captured_delta / counts_per_rev;
|
||||
}
|
||||
|
||||
float Encoder::Capture::degrees_delta() const {
|
||||
return revolutions_delta() * 360.0f;
|
||||
}
|
||||
|
||||
float Encoder::Capture::radians_delta() const {
|
||||
return revolutions_delta() * M_TWOPI;
|
||||
}
|
||||
|
||||
float Encoder::Capture::revolutions_per_second() const {
|
||||
return captured_frequency / counts_per_rev;
|
||||
}
|
||||
|
||||
float Encoder::Capture::revolutions_per_minute() const {
|
||||
return revolutions_per_second() * 60.0f;
|
||||
}
|
||||
|
||||
float Encoder::Capture::degrees_per_second() const {
|
||||
return revolutions_per_second() * 360.0f;
|
||||
}
|
||||
|
||||
float Encoder::Capture::radians_per_second() const {
|
||||
return revolutions_per_second() * M_TWOPI;
|
||||
}
|
||||
|
||||
Encoder::Encoder(PIO pio, uint sm, const pin_pair &pins, uint common_pin, Direction direction,
|
||||
float counts_per_rev, bool count_microsteps, uint16_t freq_divider)
|
||||
: pio(pio)
|
||||
, sm(sm)
|
||||
, enc_pins(pins)
|
||||
, enc_common_pin(common_pin)
|
||||
, enc_direction(direction)
|
||||
, enc_counts_per_rev(MAX(counts_per_rev, FLT_EPSILON))
|
||||
, count_microsteps(count_microsteps)
|
||||
, freq_divider(freq_divider)
|
||||
, clocks_per_time((float)(clock_get_hz(clk_sys) / (ENC_LOOP_CYCLES * freq_divider))) {
|
||||
}
|
||||
|
||||
Encoder::~Encoder() {
|
||||
if(initialised) {
|
||||
pio_sm_set_enabled(pio, sm, false);
|
||||
pio_sm_unclaim(pio, sm);
|
||||
|
||||
uint pio_idx = pio_get_index(pio);
|
||||
encoders[pio_idx][sm] = nullptr;
|
||||
claimed_sms[pio_idx] &= ~(1u << sm);
|
||||
|
||||
hw_clear_bits(&pio->inte1, PIO_IRQ1_INTE_SM0_RXNEMPTY_BITS << sm);
|
||||
|
||||
//If there are no more SMs using the encoder program, then we can remove it from the PIO
|
||||
if(claimed_sms[pio_idx] == 0) {
|
||||
pio_remove_program(pio, &encoder_program, pio_program_offset[pio_idx]);
|
||||
|
||||
if(pio_idx == 0) {
|
||||
irq_remove_handler(PIO0_IRQ_1, pio0_interrupt_handler);
|
||||
}
|
||||
else {
|
||||
irq_remove_handler(PIO1_IRQ_1, pio1_interrupt_handler);
|
||||
}
|
||||
}
|
||||
|
||||
// Reset all the pins this PWM will control back to an unused state
|
||||
gpio_set_function(enc_pins.a, GPIO_FUNC_NULL);
|
||||
gpio_set_function(enc_pins.b, GPIO_FUNC_NULL);
|
||||
|
||||
if(enc_common_pin != PIN_UNUSED) {
|
||||
gpio_set_function(enc_common_pin, GPIO_FUNC_NULL);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void Encoder::pio_interrupt_handler(uint pio_idx) {
|
||||
// Go through each SM on the PIO to see which triggered this interrupt,
|
||||
// and if there's an associated encoder, have it update its state
|
||||
for(uint8_t sm = 0; sm < NUM_PIO_STATE_MACHINES; sm++) {
|
||||
if(encoders[pio_idx][sm] != nullptr) {
|
||||
encoders[pio_idx][sm]->process_steps();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void Encoder::pio0_interrupt_handler() {
|
||||
pio_interrupt_handler(0);
|
||||
}
|
||||
|
||||
void Encoder::pio1_interrupt_handler() {
|
||||
pio_interrupt_handler(1);
|
||||
}
|
||||
|
||||
bool Encoder::init() {
|
||||
if(!initialised && !pio_sm_is_claimed(pio, sm)) {
|
||||
// Are the pins we want to use actually valid?
|
||||
if((enc_pins.a < NUM_BANK0_GPIOS) && (enc_pins.b < NUM_BANK0_GPIOS)) {
|
||||
|
||||
// If a Pin C was defined, and valid, set it as a GND to pull the other two pins down
|
||||
if((enc_common_pin != PIN_UNUSED) && (enc_common_pin < NUM_BANK0_GPIOS)) {
|
||||
gpio_init(enc_common_pin);
|
||||
gpio_set_dir(enc_common_pin, GPIO_OUT);
|
||||
gpio_put(enc_common_pin, false);
|
||||
}
|
||||
|
||||
pio_sm_claim(pio, sm);
|
||||
uint pio_idx = pio_get_index(pio);
|
||||
|
||||
// If this is the first time using an encoder on this PIO, add the program to the PIO memory
|
||||
if(claimed_sms[pio_idx] == 0) {
|
||||
pio_program_offset[pio_idx] = pio_add_program(pio, &encoder_program);
|
||||
}
|
||||
|
||||
// Initialise the A and B pins of this encoder
|
||||
pio_gpio_init(pio, enc_pins.a);
|
||||
pio_gpio_init(pio, enc_pins.b);
|
||||
gpio_pull_up(enc_pins.a);
|
||||
gpio_pull_up(enc_pins.b);
|
||||
|
||||
// Set their default direction
|
||||
pio_sm_set_consecutive_pindirs(pio, sm, enc_pins.a, 1, false);
|
||||
pio_sm_set_consecutive_pindirs(pio, sm, enc_pins.b, 1, false);
|
||||
|
||||
pio_sm_config c = encoder_program_get_default_config(pio_program_offset[pio_idx]);
|
||||
sm_config_set_jmp_pin(&c, enc_pins.a);
|
||||
sm_config_set_in_pins(&c, enc_pins.b);
|
||||
|
||||
sm_config_set_in_shift(&c, false, false, 1);
|
||||
sm_config_set_fifo_join(&c, PIO_FIFO_JOIN_RX);
|
||||
|
||||
sm_config_set_clkdiv_int_frac(&c, freq_divider, 0);
|
||||
|
||||
pio_sm_init(pio, sm, pio_program_offset[pio_idx], &c);
|
||||
|
||||
hw_set_bits(&pio->inte1, PIO_IRQ1_INTE_SM0_RXNEMPTY_BITS << sm);
|
||||
if(claimed_sms[pio_idx] == 0) {
|
||||
// Configure the processor to run pio_handler() when PIO IRQ 0 is asserted
|
||||
if(pio_idx == 0) {
|
||||
irq_add_shared_handler(PIO0_IRQ_1, pio0_interrupt_handler, PICO_SHARED_IRQ_HANDLER_DEFAULT_ORDER_PRIORITY);
|
||||
irq_set_enabled(PIO0_IRQ_1, true);
|
||||
}
|
||||
else {
|
||||
irq_add_shared_handler(PIO1_IRQ_1, pio1_interrupt_handler, PICO_SHARED_IRQ_HANDLER_DEFAULT_ORDER_PRIORITY);
|
||||
irq_set_enabled(PIO1_IRQ_1, true);
|
||||
}
|
||||
}
|
||||
|
||||
//Keep a record of this encoder for the interrupt callback
|
||||
encoders[pio_idx][sm] = this;
|
||||
claimed_sms[pio_idx] |= 1u << sm;
|
||||
|
||||
enc_state_a = gpio_get(enc_pins.a);
|
||||
enc_state_b = gpio_get(enc_pins.b);
|
||||
pio_sm_exec(pio, sm, pio_encode_set(pio_x, (uint)enc_state_a << 1 | (uint)enc_state_b));
|
||||
pio_sm_set_enabled(pio, sm, true);
|
||||
|
||||
initialised = true;
|
||||
}
|
||||
}
|
||||
|
||||
return initialised;
|
||||
}
|
||||
|
||||
pin_pair Encoder::pins() const {
|
||||
return enc_pins;
|
||||
}
|
||||
|
||||
uint Encoder::common_pin() const {
|
||||
return enc_common_pin;
|
||||
}
|
||||
|
||||
bool_pair Encoder::state() const {
|
||||
return bool_pair(enc_state_a, enc_state_b);
|
||||
}
|
||||
|
||||
int32_t Encoder::count() const {
|
||||
return enc_count;
|
||||
}
|
||||
|
||||
int32_t Encoder::delta() {
|
||||
int32_t count = enc_count; // Store a local copy of enc_count to avoid two reads
|
||||
|
||||
// Determine the change in counts since the last time this function was performed
|
||||
int32_t change = count - last_count;
|
||||
last_count = count;
|
||||
|
||||
return change;
|
||||
}
|
||||
|
||||
void Encoder::zero() {
|
||||
enc_count = 0;
|
||||
enc_cumulative_time = 0;
|
||||
enc_step = 0;
|
||||
enc_turn = 0;
|
||||
|
||||
microstep_time = 0;
|
||||
step_dir = NO_DIR; // may not be wanted?
|
||||
|
||||
last_count = 0;
|
||||
last_capture_count = 0;
|
||||
}
|
||||
|
||||
int16_t Encoder::step() const {
|
||||
return enc_step;
|
||||
}
|
||||
|
||||
int16_t Encoder::turn() const {
|
||||
return enc_turn;
|
||||
}
|
||||
|
||||
float Encoder::revolutions() const {
|
||||
return (float)count() / enc_counts_per_rev;
|
||||
}
|
||||
|
||||
float Encoder::degrees() const {
|
||||
return revolutions() * 360.0f;
|
||||
}
|
||||
|
||||
float Encoder::radians() const {
|
||||
return revolutions() * M_TWOPI;
|
||||
}
|
||||
|
||||
Direction Encoder::direction() const {
|
||||
return enc_direction;
|
||||
}
|
||||
|
||||
void Encoder::direction(Direction direction) {
|
||||
enc_direction = direction;
|
||||
}
|
||||
|
||||
float Encoder::counts_per_rev() const {
|
||||
return enc_counts_per_rev;
|
||||
}
|
||||
|
||||
void Encoder::counts_per_rev(float counts_per_rev) {
|
||||
enc_counts_per_rev = MAX(counts_per_rev, FLT_EPSILON);
|
||||
}
|
||||
|
||||
Encoder::Capture Encoder::capture() {
|
||||
// Take a capture of the current values
|
||||
int32_t count = enc_count;
|
||||
int32_t cumulative_time = enc_cumulative_time;
|
||||
enc_cumulative_time = 0;
|
||||
|
||||
// Determine the change in counts since the last capture was taken
|
||||
int32_t change = count - last_capture_count;
|
||||
last_capture_count = count;
|
||||
|
||||
// Calculate the average frequency of steps
|
||||
float frequency = 0.0f;
|
||||
if(change != 0 && cumulative_time != INT32_MAX) {
|
||||
frequency = (clocks_per_time * (float)change) / (float)cumulative_time;
|
||||
}
|
||||
|
||||
return Capture(count, change, frequency, enc_counts_per_rev);
|
||||
}
|
||||
|
||||
void Encoder::process_steps() {
|
||||
while(pio->ints1 & (PIO_IRQ1_INTS_SM0_RXNEMPTY_BITS << sm)) {
|
||||
uint32_t received = pio_sm_get(pio, sm);
|
||||
|
||||
// Extract the current and last encoder states from the received value
|
||||
enc_state_a = (bool)(received & STATE_A_MASK);
|
||||
enc_state_b = (bool)(received & STATE_B_MASK);
|
||||
uint8_t states = (received & STATES_MASK) >> 28;
|
||||
|
||||
// Extract the time (in cycles) it has been since the last received
|
||||
int32_t time_received = (received & TIME_MASK) + ENC_DEBOUNCE_TIME;
|
||||
|
||||
// For rotary encoders, only every fourth step is cared about, causing an inaccurate time value
|
||||
// To address this we accumulate the times received and zero it when a step is counted
|
||||
if(!count_microsteps) {
|
||||
if(time_received + microstep_time < time_received) // Check to avoid integer overflow
|
||||
time_received = INT32_MAX;
|
||||
else
|
||||
time_received += microstep_time;
|
||||
microstep_time = time_received;
|
||||
}
|
||||
|
||||
bool up = (enc_direction == NORMAL_DIR);
|
||||
|
||||
// Determine what step occurred
|
||||
switch(LAST_STATE(states)) {
|
||||
//--------------------------------------------------
|
||||
case MICROSTEP_0:
|
||||
switch(CURR_STATE(states)) {
|
||||
// A ____|‾‾‾‾
|
||||
// B _________
|
||||
case MICROSTEP_1:
|
||||
if(count_microsteps)
|
||||
microstep(time_received, up);
|
||||
break;
|
||||
|
||||
// A _________
|
||||
// B ____|‾‾‾‾
|
||||
case MICROSTEP_3:
|
||||
if(count_microsteps)
|
||||
microstep(time_received, !up);
|
||||
break;
|
||||
}
|
||||
break;
|
||||
|
||||
//--------------------------------------------------
|
||||
case MICROSTEP_1:
|
||||
switch(CURR_STATE(states)) {
|
||||
// A ‾‾‾‾‾‾‾‾‾
|
||||
// B ____|‾‾‾‾
|
||||
case MICROSTEP_2:
|
||||
if(count_microsteps || step_dir == INCREASING)
|
||||
microstep(time_received, up);
|
||||
|
||||
step_dir = NO_DIR; // Finished increasing
|
||||
break;
|
||||
|
||||
// A ‾‾‾‾|____
|
||||
// B _________
|
||||
case MICROSTEP_0:
|
||||
if(count_microsteps)
|
||||
microstep(time_received, !up);
|
||||
break;
|
||||
}
|
||||
break;
|
||||
|
||||
//--------------------------------------------------
|
||||
case MICROSTEP_2:
|
||||
switch(CURR_STATE(states)) {
|
||||
// A ‾‾‾‾|____
|
||||
// B ‾‾‾‾‾‾‾‾‾
|
||||
case MICROSTEP_3:
|
||||
if(count_microsteps)
|
||||
microstep(time_received, up);
|
||||
|
||||
step_dir = INCREASING; // Started increasing
|
||||
break;
|
||||
|
||||
// A ‾‾‾‾‾‾‾‾‾
|
||||
// B ‾‾‾‾|____
|
||||
case MICROSTEP_1:
|
||||
if(count_microsteps)
|
||||
microstep(time_received, !up);
|
||||
|
||||
step_dir = DECREASING; // Started decreasing
|
||||
break;
|
||||
}
|
||||
break;
|
||||
|
||||
//--------------------------------------------------
|
||||
case MICROSTEP_3:
|
||||
switch(CURR_STATE(states)) {
|
||||
// A _________
|
||||
// B ‾‾‾‾|____
|
||||
case MICROSTEP_0:
|
||||
if(count_microsteps)
|
||||
microstep(time_received, up);
|
||||
break;
|
||||
|
||||
// A ____|‾‾‾‾
|
||||
// B ‾‾‾‾‾‾‾‾‾
|
||||
case MICROSTEP_2:
|
||||
if(count_microsteps || step_dir == DECREASING)
|
||||
microstep(time_received, !up);
|
||||
|
||||
step_dir = NO_DIR; // Finished decreasing
|
||||
break;
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void Encoder::microstep(int32_t time, bool up) {
|
||||
if(up) {
|
||||
enc_count++;
|
||||
if(++enc_step >= (int16_t)enc_counts_per_rev) {
|
||||
enc_step -= (int16_t)enc_counts_per_rev;
|
||||
enc_turn++;
|
||||
}
|
||||
}
|
||||
else {
|
||||
enc_count--;
|
||||
if(--enc_step < 0) {
|
||||
enc_step += (int16_t)enc_counts_per_rev;
|
||||
enc_turn--;
|
||||
}
|
||||
}
|
||||
microstep_time = 0;
|
||||
|
||||
if(time + enc_cumulative_time < time) // Check to avoid integer overflow
|
||||
enc_cumulative_time = INT32_MAX;
|
||||
else
|
||||
enc_cumulative_time += time;
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,189 @@
|
|||
#pragma once
|
||||
|
||||
#include "hardware/pio.h"
|
||||
#include "common/pimoroni_common.hpp"
|
||||
|
||||
using namespace pimoroni;
|
||||
|
||||
namespace encoder {
|
||||
static constexpr float MMME_CPR = 12.0f;
|
||||
static constexpr float ROTARY_CPR = 24.0f;
|
||||
|
||||
class Encoder {
|
||||
//--------------------------------------------------
|
||||
// Constants
|
||||
//--------------------------------------------------
|
||||
public:
|
||||
static constexpr float DEFAULT_COUNTS_PER_REV = ROTARY_CPR;
|
||||
static const bool DEFAULT_COUNT_MICROSTEPS = false;
|
||||
static const uint16_t DEFAULT_FREQ_DIVIDER = 1;
|
||||
|
||||
private:
|
||||
static const uint32_t STATE_A_MASK = 0x80000000;
|
||||
static const uint32_t STATE_B_MASK = 0x40000000;
|
||||
static const uint32_t STATE_A_LAST_MASK = 0x20000000;
|
||||
static const uint32_t STATE_B_LAST_MASK = 0x10000000;
|
||||
|
||||
static const uint32_t STATES_MASK = STATE_A_MASK | STATE_B_MASK |
|
||||
STATE_A_LAST_MASK | STATE_B_LAST_MASK;
|
||||
|
||||
static const uint32_t TIME_MASK = 0x0fffffff;
|
||||
|
||||
|
||||
//--------------------------------------------------
|
||||
// Enums
|
||||
//--------------------------------------------------
|
||||
private:
|
||||
enum StepDir {
|
||||
NO_DIR = 0,
|
||||
INCREASING = 1,
|
||||
DECREASING = -1,
|
||||
};
|
||||
|
||||
enum MicroStep : uint8_t {
|
||||
MICROSTEP_0 = 0b00,
|
||||
MICROSTEP_1 = 0b10,
|
||||
MICROSTEP_2 = 0b11,
|
||||
MICROSTEP_3 = 0b01,
|
||||
};
|
||||
|
||||
|
||||
//--------------------------------------------------
|
||||
// Substructures
|
||||
//--------------------------------------------------
|
||||
public:
|
||||
class Capture {
|
||||
//--------------------------------------------------
|
||||
// Variables
|
||||
//--------------------------------------------------
|
||||
private:
|
||||
int32_t captured_count;
|
||||
int32_t captured_delta;
|
||||
float captured_frequency;
|
||||
float counts_per_rev;
|
||||
|
||||
|
||||
//--------------------------------------------------
|
||||
// Constructors
|
||||
//--------------------------------------------------
|
||||
public:
|
||||
Capture();
|
||||
Capture(int32_t count, int32_t delta, float frequency, float counts_per_rev);
|
||||
|
||||
|
||||
//--------------------------------------------------
|
||||
// Methods
|
||||
//--------------------------------------------------
|
||||
public:
|
||||
int32_t count() const;
|
||||
int32_t delta() const;
|
||||
float frequency() const;
|
||||
|
||||
float revolutions() const;
|
||||
float degrees() const;
|
||||
float radians() const;
|
||||
|
||||
float revolutions_delta() const;
|
||||
float degrees_delta() const;
|
||||
float radians_delta() const;
|
||||
|
||||
float revolutions_per_second() const;
|
||||
float revolutions_per_minute() const;
|
||||
float degrees_per_second() const;
|
||||
float radians_per_second() const;
|
||||
};
|
||||
|
||||
|
||||
//--------------------------------------------------
|
||||
// Variables
|
||||
//--------------------------------------------------
|
||||
private:
|
||||
PIO pio;
|
||||
uint sm;
|
||||
pin_pair enc_pins;
|
||||
uint enc_common_pin;
|
||||
Direction enc_direction;
|
||||
float enc_counts_per_rev;
|
||||
|
||||
const bool count_microsteps;
|
||||
const uint16_t freq_divider;
|
||||
const float clocks_per_time;
|
||||
|
||||
//--------------------------------------------------
|
||||
|
||||
volatile bool enc_state_a = false;
|
||||
volatile bool enc_state_b = false;
|
||||
volatile int32_t enc_count = 0;
|
||||
volatile int32_t enc_cumulative_time = 0;
|
||||
volatile int16_t enc_step = 0;
|
||||
volatile int16_t enc_turn = 0;
|
||||
|
||||
volatile int32_t microstep_time = 0;
|
||||
volatile StepDir step_dir = NO_DIR;
|
||||
|
||||
int32_t last_count = 0;
|
||||
int32_t last_capture_count = 0;
|
||||
|
||||
bool initialised = false;
|
||||
|
||||
|
||||
//--------------------------------------------------
|
||||
// Statics
|
||||
//--------------------------------------------------
|
||||
static Encoder* encoders[NUM_PIOS][NUM_PIO_STATE_MACHINES];
|
||||
static uint8_t claimed_sms[NUM_PIOS];
|
||||
static uint pio_program_offset[NUM_PIOS];
|
||||
static void pio_interrupt_handler(uint pio_idx);
|
||||
static void pio0_interrupt_handler();
|
||||
static void pio1_interrupt_handler();
|
||||
|
||||
|
||||
//--------------------------------------------------
|
||||
// Constructors/Destructor
|
||||
//--------------------------------------------------
|
||||
public:
|
||||
Encoder(PIO pio, uint sm, const pin_pair &pins, uint common_pin = PIN_UNUSED, Direction direction = NORMAL_DIR,
|
||||
float counts_per_rev = DEFAULT_COUNTS_PER_REV, bool count_microsteps = DEFAULT_COUNT_MICROSTEPS,
|
||||
uint16_t freq_divider = DEFAULT_FREQ_DIVIDER);
|
||||
~Encoder();
|
||||
|
||||
|
||||
//--------------------------------------------------
|
||||
// Methods
|
||||
//--------------------------------------------------
|
||||
public:
|
||||
bool init();
|
||||
|
||||
// For print access in micropython
|
||||
pin_pair pins() const;
|
||||
uint common_pin() const;
|
||||
|
||||
bool_pair state() const;
|
||||
int32_t count() const;
|
||||
int32_t delta();
|
||||
void zero();
|
||||
|
||||
int16_t step() const;
|
||||
int16_t turn() const;
|
||||
|
||||
float revolutions() const;
|
||||
float degrees() const;
|
||||
float radians() const;
|
||||
|
||||
//--------------------------------------------------
|
||||
|
||||
Direction direction() const;
|
||||
void direction(Direction direction);
|
||||
|
||||
float counts_per_rev() const;
|
||||
void counts_per_rev(float counts_per_rev);
|
||||
|
||||
Capture capture();
|
||||
|
||||
//--------------------------------------------------
|
||||
private:
|
||||
void process_steps();
|
||||
void microstep(int32_t time_since, bool up);
|
||||
};
|
||||
|
||||
}
|
||||
|
|
@ -0,0 +1,100 @@
|
|||
; --------------------------------------------------
|
||||
; Quadrature Encoder reader using PIO
|
||||
; by Christopher (@ZodiusInfuser) Parrott
|
||||
; --------------------------------------------------
|
||||
;
|
||||
; Watches any two pins (i.e. do not need to be
|
||||
; consecutive) for when their state changes, and
|
||||
; pushes that new state along with the old state,
|
||||
; and time since the last change.
|
||||
;
|
||||
; - X is used for storing the last state
|
||||
; - Y is used as a general scratch register
|
||||
; and for storing the current state
|
||||
; - OSR is used for storing the state-change timer
|
||||
;
|
||||
; After data is pushed into the system, a long delay
|
||||
; takes place as a form of switch debounce to deal
|
||||
; with rotary encoder dials. This is currently set
|
||||
; to 500 cycles, but can be changed using the debounce
|
||||
; constants below, as well as adjusting the frequency
|
||||
; the PIO state machine runs at. E.g. a freq_divider
|
||||
; of 250 gives a 1ms debounce.
|
||||
|
||||
|
||||
; Debounce Constants
|
||||
; --------------------------------------------------
|
||||
.define SET_CYCLES 10
|
||||
.define ITERATIONS 30
|
||||
.define JMP_CYCLES 16
|
||||
.define public ENC_DEBOUNCE_CYCLES (SET_CYCLES + (JMP_CYCLES * ITERATIONS))
|
||||
|
||||
; Ensure that ENC_DEBOUNCE_CYCLES is a multiple of the
|
||||
; number of cycles the wrap takes, which is currently
|
||||
; 10 cycles, otherwise timing may be inaccurate
|
||||
|
||||
|
||||
; Encoder Program
|
||||
; --------------------------------------------------
|
||||
.program encoder
|
||||
|
||||
.wrap_target
|
||||
loop:
|
||||
; Copy the state-change timer from OSR,
|
||||
; decrement it, and save it back
|
||||
mov y, osr
|
||||
jmp y-- osr_dec
|
||||
osr_dec:
|
||||
mov osr, y
|
||||
; takes 3 cycles
|
||||
|
||||
; Read the state of both encoder pins and check
|
||||
; if they are different from the last state
|
||||
jmp pin enc_a_was_high
|
||||
mov isr, null
|
||||
jmp read_enc_b
|
||||
enc_a_was_high:
|
||||
set y, 1
|
||||
mov isr, y
|
||||
read_enc_b:
|
||||
in pins, 1
|
||||
mov y, isr
|
||||
jmp x!=y state_changed [1]
|
||||
; takes 7 cycles on both paths
|
||||
.wrap
|
||||
|
||||
state_changed:
|
||||
; Put the last state and the timer value into
|
||||
; ISR alongside the current state, and push that
|
||||
; state to the system. Then override the last
|
||||
; state with the current state
|
||||
in x, 2
|
||||
mov x, ~osr ; invert the timer value to give
|
||||
; a sensible value to the system
|
||||
in x, 28
|
||||
push noblock ; this also clears isr
|
||||
mov x, y
|
||||
|
||||
; Perform a delay to debounce switch inputs
|
||||
set y, (ITERATIONS - 1) [SET_CYCLES - 1]
|
||||
debounce_loop:
|
||||
jmp y-- debounce_loop [JMP_CYCLES - 1]
|
||||
|
||||
; Initialise the timer, as an inverse, and decrement
|
||||
; it to account for the time this setup takes
|
||||
mov y, ~null
|
||||
jmp y-- y_dec
|
||||
y_dec:
|
||||
mov osr, y
|
||||
jmp loop [1]
|
||||
;takes 10 cycles, not counting whatever the debounce adds
|
||||
|
||||
|
||||
; Initialisation Code
|
||||
; --------------------------------------------------
|
||||
% c-sdk {
|
||||
static const uint8_t ENC_LOOP_CYCLES = encoder_wrap - encoder_wrap_target;
|
||||
|
||||
// The time that the debounce takes, as the number of wrap loops that the debounce is equivalent to
|
||||
static const uint8_t ENC_DEBOUNCE_TIME = ENC_DEBOUNCE_CYCLES / ENC_LOOP_CYCLES;
|
||||
%}
|
||||
|
|
@ -0,0 +1,2 @@
|
|||
include(motor.cmake)
|
||||
include(motor_cluster.cmake)
|
||||
|
|
@ -0,0 +1,15 @@
|
|||
set(DRIVER_NAME motor)
|
||||
add_library(${DRIVER_NAME} INTERFACE)
|
||||
|
||||
target_sources(${DRIVER_NAME} INTERFACE
|
||||
${CMAKE_CURRENT_LIST_DIR}/motor.cpp
|
||||
${CMAKE_CURRENT_LIST_DIR}/motor_state.cpp
|
||||
)
|
||||
|
||||
target_include_directories(${DRIVER_NAME} INTERFACE ${CMAKE_CURRENT_LIST_DIR})
|
||||
|
||||
target_link_libraries(${DRIVER_NAME} INTERFACE
|
||||
pico_stdlib
|
||||
hardware_pwm
|
||||
pwm
|
||||
)
|
||||
|
|
@ -0,0 +1,313 @@
|
|||
#include "motor.hpp"
|
||||
#include "hardware/clocks.h"
|
||||
#include "pwm.hpp"
|
||||
#include "math.h"
|
||||
|
||||
namespace motor {
|
||||
Motor::Driver::Driver(const pin_pair &pins) : motor_pins(pins), pwm_period(1) {
|
||||
}
|
||||
|
||||
Motor::Driver::~Driver() {
|
||||
gpio_set_function(motor_pins.first, GPIO_FUNC_NULL);
|
||||
gpio_set_function(motor_pins.second, GPIO_FUNC_NULL);
|
||||
}
|
||||
|
||||
const pin_pair& Motor::Driver::pins() const {
|
||||
return motor_pins;
|
||||
}
|
||||
|
||||
void Motor::DualPWMDriver::init(pwm_config *pwm_cfg, uint16_t period) {
|
||||
pwm_period = period;
|
||||
|
||||
// Set up the positive and negative pins
|
||||
pwm_init(pwm_gpio_to_slice_num(motor_pins.positive), pwm_cfg, true);
|
||||
pwm_init(pwm_gpio_to_slice_num(motor_pins.negative), pwm_cfg, true);
|
||||
|
||||
gpio_set_function(motor_pins.positive, GPIO_FUNC_PWM);
|
||||
gpio_set_function(motor_pins.negative, GPIO_FUNC_PWM);
|
||||
|
||||
pwm_set_gpio_level(motor_pins.positive, 0);
|
||||
pwm_set_gpio_level(motor_pins.negative, 0);
|
||||
}
|
||||
|
||||
void Motor::DualPWMDriver::update_frequency(uint8_t div, uint8_t mod, uint16_t period, float duty, DecayMode mode) {
|
||||
// Record if the new period will be larger or smaller.
|
||||
// This is used to apply new pwm speeds either before or after the wrap is applied,
|
||||
// to avoid momentary blips in PWM output on SLOW_DECAY
|
||||
bool pre_update_pwm = (period > pwm_period);
|
||||
pwm_period = period;
|
||||
|
||||
uint pos_pin_slice = pwm_gpio_to_slice_num(motor_pins.positive);
|
||||
uint neg_pin_slice = pwm_gpio_to_slice_num(motor_pins.negative);
|
||||
|
||||
// Apply the new divider
|
||||
pwm_set_clkdiv_int_frac(pos_pin_slice, div, mod);
|
||||
if((neg_pin_slice != pos_pin_slice))
|
||||
pwm_set_clkdiv_int_frac(neg_pin_slice, div, mod);
|
||||
|
||||
// If the period is larger, update the pwm before setting the new wraps
|
||||
if(pre_update_pwm) {
|
||||
apply_duty(duty, mode);
|
||||
}
|
||||
|
||||
// Set the new wrap (should be 1 less than the period to get full 0 to 100%)
|
||||
pwm_set_wrap(pos_pin_slice, pwm_period - 1);
|
||||
if(neg_pin_slice != pos_pin_slice)
|
||||
pwm_set_wrap(neg_pin_slice, pwm_period - 1);
|
||||
|
||||
// If the period is smaller, update the pwm after setting the new wraps
|
||||
if(!pre_update_pwm) {
|
||||
apply_duty(duty, mode);
|
||||
}
|
||||
}
|
||||
|
||||
void Motor::DualPWMDriver::apply_duty(float duty, DecayMode mode) {
|
||||
if(isfinite(duty)) {
|
||||
int32_t signed_level = MotorState::duty_to_level(duty, pwm_period);
|
||||
|
||||
switch(mode) {
|
||||
case SLOW_DECAY: //aka 'Braking'
|
||||
if(signed_level >= 0) {
|
||||
pwm_set_gpio_level(motor_pins.positive, pwm_period);
|
||||
pwm_set_gpio_level(motor_pins.negative, pwm_period - signed_level);
|
||||
}
|
||||
else {
|
||||
pwm_set_gpio_level(motor_pins.positive, pwm_period + signed_level);
|
||||
pwm_set_gpio_level(motor_pins.negative, pwm_period);
|
||||
}
|
||||
break;
|
||||
|
||||
case FAST_DECAY: //aka 'Coasting'
|
||||
default:
|
||||
if(signed_level >= 0) {
|
||||
pwm_set_gpio_level(motor_pins.positive, signed_level);
|
||||
pwm_set_gpio_level(motor_pins.negative, 0);
|
||||
}
|
||||
else {
|
||||
pwm_set_gpio_level(motor_pins.positive, 0);
|
||||
pwm_set_gpio_level(motor_pins.negative, 0 - signed_level);
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
else {
|
||||
pwm_set_gpio_level(motor_pins.positive, 0);
|
||||
pwm_set_gpio_level(motor_pins.negative, 0);
|
||||
}
|
||||
}
|
||||
|
||||
void Motor::PhEnDriver::init(pwm_config *pwm_cfg, uint16_t period) {
|
||||
pwm_period = period;
|
||||
|
||||
// Set up the phase and enable pins
|
||||
gpio_init(motor_pins.phase);
|
||||
pwm_init(pwm_gpio_to_slice_num(motor_pins.enable), pwm_cfg, true);
|
||||
|
||||
gpio_set_dir(motor_pins.phase, true);
|
||||
gpio_set_function(motor_pins.enable, GPIO_FUNC_PWM);
|
||||
|
||||
gpio_put(motor_pins.phase, false);
|
||||
pwm_set_gpio_level(motor_pins.enable, 0);
|
||||
}
|
||||
|
||||
|
||||
void Motor::PhEnDriver::update_frequency(uint8_t div, uint8_t mod, uint16_t period, float duty, DecayMode mode) {
|
||||
// Record if the new period will be larger or smaller.
|
||||
// This is used to apply new pwm speeds either before or after the wrap is applied,
|
||||
// to avoid momentary blips in PWM output on SLOW_DECAY
|
||||
bool pre_update_pwm = (period > pwm_period);
|
||||
pwm_period = period;
|
||||
|
||||
uint en_pin_slice = pwm_gpio_to_slice_num(motor_pins.enable);
|
||||
|
||||
// Apply the new divider
|
||||
pwm_set_clkdiv_int_frac(en_pin_slice, div, mod);
|
||||
|
||||
// If the period is larger, update the pwm before setting the new wraps
|
||||
if(pre_update_pwm) {
|
||||
apply_duty(duty, mode);
|
||||
}
|
||||
|
||||
// Set the new wrap (should be 1 less than the period to get full 0 to 100%)
|
||||
pwm_set_wrap(en_pin_slice, pwm_period - 1);
|
||||
|
||||
// If the period is smaller, update the pwm after setting the new wraps
|
||||
if(!pre_update_pwm) {
|
||||
apply_duty(duty, mode);
|
||||
}
|
||||
}
|
||||
|
||||
void Motor::PhEnDriver::apply_duty(float duty, DecayMode mode) {
|
||||
if(isfinite(duty)) {
|
||||
int32_t signed_level = MotorState::duty_to_level(duty, pwm_period);
|
||||
|
||||
if(signed_level >= 0) {
|
||||
gpio_put(motor_pins.phase, true);
|
||||
pwm_set_gpio_level(motor_pins.enable, signed_level);
|
||||
}
|
||||
else {
|
||||
gpio_put(motor_pins.phase, false);
|
||||
pwm_set_gpio_level(motor_pins.enable, 0 - signed_level);
|
||||
}
|
||||
}
|
||||
else {
|
||||
gpio_put(motor_pins.phase, false);
|
||||
pwm_set_gpio_level(motor_pins.enable, 0);
|
||||
}
|
||||
}
|
||||
|
||||
Motor::Motor(const pin_pair &pins, Direction direction, float speed_scale, float zeropoint, float deadzone, float freq, DecayMode mode, bool ph_en_driver)
|
||||
: driver(ph_en_driver ? (Driver*)new PhEnDriver(pins) : (Driver*)new DualPWMDriver(pins))
|
||||
, state(direction, speed_scale, zeropoint, deadzone), pwm_frequency(freq), motor_mode(mode) {
|
||||
}
|
||||
|
||||
Motor::~Motor() {
|
||||
delete driver;
|
||||
}
|
||||
|
||||
bool Motor::init() {
|
||||
bool success = false;
|
||||
|
||||
uint16_t period; uint16_t div16;
|
||||
if(pimoroni::calculate_pwm_factors(pwm_frequency, period, div16)) {
|
||||
pwm_cfg = pwm_get_default_config();
|
||||
|
||||
// Set the new wrap (should be 1 less than the period to get full 0 to 100%)
|
||||
pwm_config_set_wrap(&pwm_cfg, period - 1);
|
||||
|
||||
// Apply the divider
|
||||
pwm_config_set_clkdiv(&pwm_cfg, (float)div16 / 16.0f); // There's no 'pwm_config_set_clkdiv_int_frac' for some reason...
|
||||
|
||||
// Set up the pin driver
|
||||
driver->init(&pwm_cfg, period);
|
||||
|
||||
success = true;
|
||||
}
|
||||
return success;
|
||||
}
|
||||
|
||||
pin_pair Motor::pins() const {
|
||||
return driver->pins();
|
||||
}
|
||||
|
||||
void Motor::enable() {
|
||||
driver->apply_duty(state.enable_with_return(), motor_mode);
|
||||
}
|
||||
|
||||
void Motor::disable() {
|
||||
driver->apply_duty(state.disable_with_return(), motor_mode);
|
||||
}
|
||||
|
||||
bool Motor::is_enabled() const {
|
||||
return state.is_enabled();
|
||||
}
|
||||
|
||||
float Motor::duty() const {
|
||||
return state.get_duty();
|
||||
}
|
||||
|
||||
void Motor::duty(float duty) {
|
||||
driver->apply_duty(state.set_duty_with_return(duty), motor_mode);
|
||||
}
|
||||
|
||||
float Motor::speed() const {
|
||||
return state.get_speed();
|
||||
}
|
||||
|
||||
void Motor::speed(float speed) {
|
||||
driver->apply_duty(state.set_speed_with_return(speed), motor_mode);
|
||||
}
|
||||
|
||||
float Motor::frequency() const {
|
||||
return pwm_frequency;
|
||||
}
|
||||
|
||||
bool Motor::frequency(float freq) {
|
||||
bool success = false;
|
||||
|
||||
if((freq >= MotorState::MIN_FREQUENCY) && (freq <= MotorState::MAX_FREQUENCY)) {
|
||||
// Calculate a suitable pwm wrap period for this frequency
|
||||
uint16_t period; uint16_t div16;
|
||||
if(pimoroni::calculate_pwm_factors(freq, period, div16)) {
|
||||
|
||||
pwm_frequency = freq;
|
||||
|
||||
uint8_t div = div16 >> 4;
|
||||
uint8_t mod = div16 % 16;
|
||||
driver->update_frequency(div, mod, period, state.get_deadzoned_duty(), motor_mode);
|
||||
|
||||
success = true;
|
||||
}
|
||||
}
|
||||
return success;
|
||||
}
|
||||
|
||||
void Motor::stop() {
|
||||
driver->apply_duty(state.stop_with_return(), motor_mode);
|
||||
}
|
||||
|
||||
void Motor::coast() {
|
||||
driver->apply_duty(state.stop_with_return(), FAST_DECAY);
|
||||
}
|
||||
|
||||
void Motor::brake() {
|
||||
driver->apply_duty(state.stop_with_return(), SLOW_DECAY);
|
||||
}
|
||||
|
||||
void Motor::full_negative() {
|
||||
driver->apply_duty(state.full_negative_with_return(), motor_mode);
|
||||
}
|
||||
|
||||
void Motor::full_positive() {
|
||||
driver->apply_duty(state.full_positive_with_return(), motor_mode);
|
||||
}
|
||||
|
||||
void Motor::to_percent(float in, float in_min, float in_max) {
|
||||
driver->apply_duty(state.to_percent_with_return(in, in_min, in_max), motor_mode);
|
||||
}
|
||||
|
||||
void Motor::to_percent(float in, float in_min, float in_max, float speed_min, float speed_max) {
|
||||
driver->apply_duty(state.to_percent_with_return(in, in_min, in_max, speed_min, speed_max), motor_mode);
|
||||
}
|
||||
|
||||
Direction Motor::direction() const {
|
||||
return state.get_direction();
|
||||
}
|
||||
|
||||
void Motor::direction(Direction direction) {
|
||||
state.set_direction(direction);
|
||||
}
|
||||
|
||||
float Motor::speed_scale() const {
|
||||
return state.get_speed_scale();
|
||||
}
|
||||
|
||||
void Motor::speed_scale(float speed_scale) {
|
||||
state.set_speed_scale(speed_scale);
|
||||
}
|
||||
|
||||
float Motor::zeropoint() const {
|
||||
return state.get_zeropoint();
|
||||
}
|
||||
|
||||
void Motor::zeropoint(float zeropoint) {
|
||||
state.set_zeropoint(zeropoint);
|
||||
}
|
||||
|
||||
float Motor::deadzone() const {
|
||||
return state.get_deadzone();
|
||||
}
|
||||
|
||||
void Motor::deadzone(float deadzone) {
|
||||
driver->apply_duty(state.set_deadzone_with_return(deadzone), motor_mode);
|
||||
}
|
||||
|
||||
DecayMode Motor::decay_mode() {
|
||||
return motor_mode;
|
||||
}
|
||||
|
||||
void Motor::decay_mode(DecayMode mode) {
|
||||
motor_mode = mode;
|
||||
driver->apply_duty(state.get_deadzoned_duty(), motor_mode);
|
||||
}
|
||||
};
|
||||
|
|
@ -0,0 +1,152 @@
|
|||
#pragma once
|
||||
|
||||
#include "pico/stdlib.h"
|
||||
#include "hardware/pwm.h"
|
||||
#include "common/pimoroni_common.hpp"
|
||||
#include "motor_state.hpp"
|
||||
|
||||
using namespace pimoroni;
|
||||
|
||||
namespace motor {
|
||||
|
||||
class Motor {
|
||||
//--------------------------------------------------
|
||||
// Subclasses
|
||||
//--------------------------------------------------
|
||||
private:
|
||||
class Driver {
|
||||
//--------------------------------------------------
|
||||
// Variables
|
||||
//--------------------------------------------------
|
||||
protected:
|
||||
pin_pair motor_pins;
|
||||
uint16_t pwm_period;
|
||||
|
||||
|
||||
//--------------------------------------------------
|
||||
// Constructors/Destructor
|
||||
//--------------------------------------------------
|
||||
public:
|
||||
Driver(const pin_pair &pins);
|
||||
virtual ~Driver();
|
||||
|
||||
|
||||
//--------------------------------------------------
|
||||
// Methods
|
||||
//--------------------------------------------------
|
||||
public:
|
||||
const pin_pair& pins() const;
|
||||
virtual void init(pwm_config *pwm_cfg, uint16_t period) = 0;
|
||||
virtual void update_frequency(uint8_t div, uint8_t mod, uint16_t period, float duty, DecayMode mode) = 0;
|
||||
virtual void apply_duty(float duty, DecayMode mode) = 0;
|
||||
};
|
||||
|
||||
class DualPWMDriver : public Driver {
|
||||
//--------------------------------------------------
|
||||
// Constructors/Destructor
|
||||
//--------------------------------------------------
|
||||
public:
|
||||
DualPWMDriver(pin_pair pins) : Driver(pins) {}
|
||||
virtual ~DualPWMDriver() {}
|
||||
|
||||
|
||||
//--------------------------------------------------
|
||||
// Methods
|
||||
//--------------------------------------------------
|
||||
public:
|
||||
virtual void init(pwm_config *pwm_cfg, uint16_t period);
|
||||
virtual void update_frequency(uint8_t div, uint8_t mod, uint16_t period, float duty, DecayMode mode);
|
||||
virtual void apply_duty(float duty, DecayMode mode);
|
||||
};
|
||||
|
||||
class PhEnDriver : public Driver {
|
||||
//--------------------------------------------------
|
||||
// Methods
|
||||
//--------------------------------------------------
|
||||
public:
|
||||
PhEnDriver(pin_pair pins) : Driver(pins) {}
|
||||
virtual ~PhEnDriver() {}
|
||||
|
||||
|
||||
//--------------------------------------------------
|
||||
// Methods
|
||||
//--------------------------------------------------
|
||||
public:
|
||||
virtual void init(pwm_config *pwm_cfg, uint16_t period);
|
||||
virtual void update_frequency(uint8_t div, uint8_t mod, uint16_t period, float duty, DecayMode mode);
|
||||
virtual void apply_duty(float duty, DecayMode mode);
|
||||
};
|
||||
|
||||
|
||||
//--------------------------------------------------
|
||||
// Variables
|
||||
//--------------------------------------------------
|
||||
private:
|
||||
Driver *driver;
|
||||
MotorState state;
|
||||
pwm_config pwm_cfg;
|
||||
float pwm_frequency;
|
||||
DecayMode motor_mode;
|
||||
|
||||
|
||||
//--------------------------------------------------
|
||||
// Constructors/Destructor
|
||||
//--------------------------------------------------
|
||||
public:
|
||||
Motor(const pin_pair &pins, Direction direction = NORMAL_DIR, float speed_scale = MotorState::DEFAULT_SPEED_SCALE, float zeropoint = MotorState::DEFAULT_ZEROPOINT,
|
||||
float deadzone = MotorState::DEFAULT_DEADZONE, float freq = MotorState::DEFAULT_FREQUENCY, DecayMode mode = MotorState::DEFAULT_DECAY_MODE,
|
||||
bool ph_en_driver = false);
|
||||
~Motor();
|
||||
|
||||
|
||||
//--------------------------------------------------
|
||||
// Methods
|
||||
//--------------------------------------------------
|
||||
public:
|
||||
bool init();
|
||||
|
||||
// For print access in micropython
|
||||
pin_pair pins() const;
|
||||
|
||||
void enable();
|
||||
void disable();
|
||||
bool is_enabled() const;
|
||||
|
||||
float duty() const;
|
||||
void duty(float duty);
|
||||
|
||||
float speed() const;
|
||||
void speed(float speed);
|
||||
|
||||
float frequency() const;
|
||||
bool frequency(float freq);
|
||||
|
||||
//--------------------------------------------------
|
||||
|
||||
void stop();
|
||||
void coast();
|
||||
void brake();
|
||||
void full_negative();
|
||||
void full_positive();
|
||||
void to_percent(float in, float in_min = MotorState::ZERO_PERCENT, float in_max = MotorState::ONEHUNDRED_PERCENT);
|
||||
void to_percent(float in, float in_min, float in_max, float speed_min, float speed_max);
|
||||
|
||||
//--------------------------------------------------
|
||||
|
||||
Direction direction() const;
|
||||
void direction(Direction direction);
|
||||
|
||||
float speed_scale() const;
|
||||
void speed_scale(float speed_scale);
|
||||
|
||||
float zeropoint() const;
|
||||
void zeropoint(float zeropoint);
|
||||
|
||||
float deadzone() const;
|
||||
void deadzone(float deadzone);
|
||||
|
||||
DecayMode decay_mode();
|
||||
void decay_mode(DecayMode mode);
|
||||
};
|
||||
|
||||
}
|
||||
|
|
@ -0,0 +1,14 @@
|
|||
set(DRIVER_NAME motor_cluster)
|
||||
add_library(${DRIVER_NAME} INTERFACE)
|
||||
|
||||
target_sources(${DRIVER_NAME} INTERFACE
|
||||
${CMAKE_CURRENT_LIST_DIR}/motor_cluster.cpp
|
||||
${CMAKE_CURRENT_LIST_DIR}/motor_state.cpp
|
||||
)
|
||||
|
||||
target_include_directories(${DRIVER_NAME} INTERFACE ${CMAKE_CURRENT_LIST_DIR})
|
||||
|
||||
target_link_libraries(${DRIVER_NAME} INTERFACE
|
||||
pico_stdlib
|
||||
pwm_cluster
|
||||
)
|
||||
|
|
@ -0,0 +1,726 @@
|
|||
#include "motor_cluster.hpp"
|
||||
#include "pwm.hpp"
|
||||
#include <cstdio>
|
||||
#include "math.h"
|
||||
|
||||
#define POS_MOTOR(motor) (PWMCluster::channel_from_pair(motor))
|
||||
#define NEG_MOTOR(motor) (PWMCluster::channel_from_pair(motor) + 1)
|
||||
|
||||
namespace motor {
|
||||
MotorCluster::MotorCluster(PIO pio, uint sm, uint pin_base, uint pin_pair_count, Direction direction,
|
||||
float speed_scale, float zeropoint, float deadzone, float freq, DecayMode mode,
|
||||
bool auto_phase, PWMCluster::Sequence *seq_buffer, PWMCluster::TransitionData *dat_buffer)
|
||||
: pwms(pio, sm, pin_base, (pin_pair_count * 2), seq_buffer, dat_buffer, false), pwm_frequency(freq) {
|
||||
create_motor_states(direction, speed_scale, zeropoint, deadzone, mode, auto_phase);
|
||||
}
|
||||
|
||||
MotorCluster::MotorCluster(PIO pio, uint sm, const pin_pair *pin_pairs, uint32_t length, Direction direction,
|
||||
float speed_scale, float zeropoint, float deadzone, float freq, DecayMode mode,
|
||||
bool auto_phase, PWMCluster::Sequence *seq_buffer, PWMCluster::TransitionData *dat_buffer)
|
||||
: pwms(pio, sm, pin_pairs, length, seq_buffer, dat_buffer, false), pwm_frequency(freq) {
|
||||
create_motor_states(direction, speed_scale, zeropoint, deadzone, mode, auto_phase);
|
||||
}
|
||||
|
||||
MotorCluster::MotorCluster(PIO pio, uint sm, std::initializer_list<pin_pair> pin_pairs, Direction direction,
|
||||
float speed_scale, float zeropoint, float deadzone, float freq, DecayMode mode,
|
||||
bool auto_phase, PWMCluster::Sequence *seq_buffer, PWMCluster::TransitionData *dat_buffer)
|
||||
: pwms(pio, sm, pin_pairs, seq_buffer, dat_buffer, false), pwm_frequency(freq) {
|
||||
create_motor_states(direction, speed_scale, zeropoint, deadzone, mode, auto_phase);
|
||||
}
|
||||
|
||||
MotorCluster::~MotorCluster() {
|
||||
delete[] states;
|
||||
delete[] configs;
|
||||
}
|
||||
|
||||
bool MotorCluster::init() {
|
||||
bool success = false;
|
||||
|
||||
if(pwms.init()) {
|
||||
// Calculate a suitable pwm wrap period for this frequency
|
||||
uint32_t period; uint32_t div256;
|
||||
if(pimoroni::PWMCluster::calculate_pwm_factors(pwm_frequency, period, div256)) {
|
||||
pwm_period = period;
|
||||
|
||||
// Update the pwm before setting the new wrap
|
||||
uint8_t motor_count = pwms.get_chan_pair_count();
|
||||
for(uint8_t motor = 0; motor < motor_count; motor++) {
|
||||
pwms.set_chan_level(POS_MOTOR(motor), 0, false);
|
||||
pwms.set_chan_level(NEG_MOTOR(motor), 0, false);
|
||||
pwms.set_chan_offset(POS_MOTOR(motor), (uint32_t)(configs[motor].phase * (float)pwm_period), false);
|
||||
pwms.set_chan_offset(NEG_MOTOR(motor), (uint32_t)(configs[motor].phase * (float)pwm_period), false);
|
||||
}
|
||||
|
||||
// Set the new wrap (should be 1 less than the period to get full 0 to 100%)
|
||||
pwms.set_wrap(pwm_period, true); // NOTE Minus 1 not needed here. Maybe should change Wrap behaviour so it is needed, for consistency with hardware pwm?
|
||||
|
||||
// Apply the new divider
|
||||
// This is done after loading new PWM speeds to avoid a lockup condition
|
||||
uint8_t div = div256 >> 8;
|
||||
uint8_t mod = div256 % 256;
|
||||
pwms.set_clkdiv_int_frac(div, mod);
|
||||
|
||||
success = true;
|
||||
}
|
||||
}
|
||||
|
||||
return success;
|
||||
}
|
||||
|
||||
uint8_t MotorCluster::count() const {
|
||||
return pwms.get_chan_pair_count();
|
||||
}
|
||||
|
||||
pin_pair MotorCluster::pins(uint8_t motor) const {
|
||||
assert(motor < pwms.get_chan_pair_count());
|
||||
return pwms.get_chan_pin_pair(motor);
|
||||
}
|
||||
|
||||
void MotorCluster::enable(uint8_t motor, bool load) {
|
||||
assert(motor < pwms.get_chan_pair_count());
|
||||
float new_duty = states[motor].enable_with_return();
|
||||
apply_duty(motor, new_duty, configs[motor].mode, load);
|
||||
}
|
||||
|
||||
void MotorCluster::enable(const uint8_t *motors, uint8_t length, bool load) {
|
||||
assert(motors != nullptr);
|
||||
for(uint8_t i = 0; i < length; i++) {
|
||||
enable(motors[i], false);
|
||||
}
|
||||
if(load)
|
||||
pwms.load_pwm();
|
||||
}
|
||||
|
||||
void MotorCluster::enable(std::initializer_list<uint8_t> motors, bool load) {
|
||||
for(auto motor : motors) {
|
||||
enable(motor, false);
|
||||
}
|
||||
if(load)
|
||||
pwms.load_pwm();
|
||||
}
|
||||
|
||||
void MotorCluster::enable_all(bool load) {
|
||||
uint8_t motor_count = pwms.get_chan_pair_count();
|
||||
for(uint8_t motor = 0; motor < motor_count; motor++) {
|
||||
enable(motor, false);
|
||||
}
|
||||
if(load)
|
||||
pwms.load_pwm();
|
||||
}
|
||||
|
||||
void MotorCluster::disable(uint8_t motor, bool load) {
|
||||
assert(motor < pwms.get_chan_pair_count());
|
||||
float new_duty = states[motor].disable_with_return();
|
||||
apply_duty(motor, new_duty, configs[motor].mode, load);
|
||||
}
|
||||
|
||||
void MotorCluster::disable(const uint8_t *motors, uint8_t length, bool load) {
|
||||
assert(motors != nullptr);
|
||||
for(uint8_t i = 0; i < length; i++) {
|
||||
disable(motors[i], false);
|
||||
}
|
||||
if(load)
|
||||
pwms.load_pwm();
|
||||
}
|
||||
|
||||
void MotorCluster::disable(std::initializer_list<uint8_t> motors, bool load) {
|
||||
for(auto motor : motors) {
|
||||
disable(motor, false);
|
||||
}
|
||||
if(load)
|
||||
pwms.load_pwm();
|
||||
}
|
||||
|
||||
void MotorCluster::disable_all(bool load) {
|
||||
uint8_t motor_count = pwms.get_chan_pair_count();
|
||||
for(uint8_t motor = 0; motor < motor_count; motor++) {
|
||||
disable(motor, false);
|
||||
}
|
||||
if(load)
|
||||
pwms.load_pwm();
|
||||
}
|
||||
|
||||
bool MotorCluster::is_enabled(uint8_t motor) const {
|
||||
assert(motor < pwms.get_chan_pair_count());
|
||||
return states[motor].is_enabled();
|
||||
}
|
||||
|
||||
float MotorCluster::duty(uint8_t motor) const {
|
||||
assert(motor < pwms.get_chan_pair_count());
|
||||
return states[motor].get_duty();
|
||||
}
|
||||
|
||||
void MotorCluster::duty(uint8_t motor, float duty, bool load) {
|
||||
assert(motor < pwms.get_chan_pair_count());
|
||||
float new_duty = states[motor].set_duty_with_return(duty);
|
||||
apply_duty(motor, new_duty, configs[motor].mode, load);
|
||||
}
|
||||
|
||||
void MotorCluster::duty(const uint8_t *motors, uint8_t length, float duty, bool load) {
|
||||
assert(motors != nullptr);
|
||||
for(uint8_t i = 0; i < length; i++) {
|
||||
this->duty(motors[i], duty, false);
|
||||
}
|
||||
if(load)
|
||||
pwms.load_pwm();
|
||||
}
|
||||
|
||||
void MotorCluster::duty(std::initializer_list<uint8_t> motors, float duty, bool load) {
|
||||
for(auto motor : motors) {
|
||||
this->duty(motor, duty, false);
|
||||
}
|
||||
if(load)
|
||||
pwms.load_pwm();
|
||||
}
|
||||
|
||||
void MotorCluster::all_to_duty(float duty, bool load) {
|
||||
uint8_t motor_count = pwms.get_chan_pair_count();
|
||||
for(uint8_t motor = 0; motor < motor_count; motor++) {
|
||||
this->duty(motor, duty, false);
|
||||
}
|
||||
if(load)
|
||||
pwms.load_pwm();
|
||||
}
|
||||
|
||||
float MotorCluster::speed(uint8_t motor) const {
|
||||
assert(motor < pwms.get_chan_pair_count());
|
||||
return states[motor].get_speed();
|
||||
}
|
||||
|
||||
void MotorCluster::speed(uint8_t motor, float speed, bool load) {
|
||||
assert(motor < pwms.get_chan_pair_count());
|
||||
float new_duty = states[motor].set_speed_with_return(speed);
|
||||
apply_duty(motor, new_duty, configs[motor].mode, load);
|
||||
}
|
||||
|
||||
void MotorCluster::speed(const uint8_t *motors, uint8_t length, float speed, bool load) {
|
||||
assert(motors != nullptr);
|
||||
for(uint8_t i = 0; i < length; i++) {
|
||||
this->speed(motors[i], speed, false);
|
||||
}
|
||||
if(load)
|
||||
pwms.load_pwm();
|
||||
}
|
||||
|
||||
void MotorCluster::speed(std::initializer_list<uint8_t> motors, float speed, bool load) {
|
||||
for(auto motor : motors) {
|
||||
this->speed(motor, speed, false);
|
||||
}
|
||||
if(load)
|
||||
pwms.load_pwm();
|
||||
}
|
||||
|
||||
void MotorCluster::all_to_speed(float speed, bool load) {
|
||||
uint8_t motor_count = pwms.get_chan_pair_count();
|
||||
for(uint8_t motor = 0; motor < motor_count; motor++) {
|
||||
this->speed(motor, speed, false);
|
||||
}
|
||||
if(load)
|
||||
pwms.load_pwm();
|
||||
}
|
||||
|
||||
float MotorCluster::phase(uint8_t motor) const {
|
||||
assert(motor < pwms.get_chan_pair_count());
|
||||
return configs[motor].phase;
|
||||
}
|
||||
|
||||
void MotorCluster::phase(uint8_t motor, float phase, bool load) {
|
||||
assert(motor < pwms.get_chan_pair_count());
|
||||
configs[motor].phase = MIN(MAX(phase, 0.0f), 1.0f);
|
||||
pwms.set_chan_offset(motor, (uint32_t)(configs[motor].phase * (float)pwms.get_wrap()), load);
|
||||
}
|
||||
|
||||
void MotorCluster::phase(const uint8_t *motors, uint8_t length, float phase, bool load) {
|
||||
assert(motors != nullptr);
|
||||
for(uint8_t i = 0; i < length; i++) {
|
||||
this->phase(motors[i], phase, false);
|
||||
}
|
||||
if(load)
|
||||
pwms.load_pwm();
|
||||
}
|
||||
|
||||
void MotorCluster::phase(std::initializer_list<uint8_t> motors, float phase, bool load) {
|
||||
for(auto motor : motors) {
|
||||
this->phase(motor, phase, false);
|
||||
}
|
||||
if(load)
|
||||
pwms.load_pwm();
|
||||
}
|
||||
|
||||
void MotorCluster::all_to_phase(float phase, bool load) {
|
||||
uint8_t motor_count = pwms.get_chan_pair_count();
|
||||
for(uint8_t motor = 0; motor < motor_count; motor++) {
|
||||
this->phase(motor, phase, false);
|
||||
}
|
||||
if(load)
|
||||
pwms.load_pwm();
|
||||
}
|
||||
|
||||
float MotorCluster::frequency() const {
|
||||
return pwm_frequency;
|
||||
}
|
||||
|
||||
bool MotorCluster::frequency(float freq) {
|
||||
bool success = false;
|
||||
|
||||
if((freq >= MotorState::MIN_FREQUENCY) && (freq <= MotorState::MAX_FREQUENCY)) {
|
||||
// Calculate a suitable pwm wrap period for this frequency
|
||||
uint32_t period; uint32_t div256;
|
||||
if(pimoroni::PWMCluster::calculate_pwm_factors(freq, period, div256)) {
|
||||
|
||||
pwm_period = period;
|
||||
pwm_frequency = freq;
|
||||
|
||||
// Update the pwm before setting the new wrap
|
||||
uint8_t motor_count = pwms.get_chan_pair_count();
|
||||
for(uint motor = 0; motor < motor_count; motor++) {
|
||||
apply_duty(motor, states[motor].get_deadzoned_duty(), configs[motor].mode, false);
|
||||
pwms.set_chan_offset(POS_MOTOR(motor), (uint32_t)(configs[motor].phase * (float)pwm_period), false);
|
||||
pwms.set_chan_offset(NEG_MOTOR(motor), (uint32_t)(configs[motor].phase * (float)pwm_period), false);
|
||||
}
|
||||
|
||||
// Set the new wrap (should be 1 less than the period to get full 0 to 100%)
|
||||
pwms.set_wrap(pwm_period, true);
|
||||
|
||||
// Apply the new divider
|
||||
uint16_t div = div256 >> 8;
|
||||
uint8_t mod = div256 % 256;
|
||||
pwms.set_clkdiv_int_frac(div, mod);
|
||||
|
||||
success = true;
|
||||
}
|
||||
}
|
||||
return success;
|
||||
}
|
||||
|
||||
void MotorCluster::stop(uint8_t motor, bool load) {
|
||||
assert(motor < pwms.get_chan_pair_count());
|
||||
float new_duty = states[motor].stop_with_return();
|
||||
apply_duty(motor, new_duty, configs[motor].mode, load);
|
||||
}
|
||||
|
||||
void MotorCluster::stop(const uint8_t *motors, uint8_t length, bool load) {
|
||||
assert(motors != nullptr);
|
||||
for(uint8_t i = 0; i < length; i++) {
|
||||
this->stop(motors[i], false);
|
||||
}
|
||||
if(load)
|
||||
pwms.load_pwm();
|
||||
}
|
||||
|
||||
void MotorCluster::stop(std::initializer_list<uint8_t> motors, bool load) {
|
||||
for(auto motor : motors) {
|
||||
this->stop(motor, false);
|
||||
}
|
||||
if(load)
|
||||
pwms.load_pwm();
|
||||
}
|
||||
|
||||
void MotorCluster::stop_all(bool load) {
|
||||
uint8_t motor_count = pwms.get_chan_pair_count();
|
||||
for(uint8_t motor = 0; motor < motor_count; motor++) {
|
||||
this->stop(motor, false);
|
||||
}
|
||||
if(load)
|
||||
pwms.load_pwm();
|
||||
}
|
||||
|
||||
void MotorCluster::coast(uint8_t motor, bool load) {
|
||||
assert(motor < pwms.get_chan_pair_count());
|
||||
float new_duty = states[motor].stop_with_return();
|
||||
apply_duty(motor, new_duty, FAST_DECAY, load);
|
||||
}
|
||||
|
||||
void MotorCluster::coast(const uint8_t *motors, uint8_t length, bool load) {
|
||||
assert(motors != nullptr);
|
||||
for(uint8_t i = 0; i < length; i++) {
|
||||
this->coast(motors[i], false);
|
||||
}
|
||||
if(load)
|
||||
pwms.load_pwm();
|
||||
}
|
||||
|
||||
void MotorCluster::coast(std::initializer_list<uint8_t> motors, bool load) {
|
||||
for(auto motor : motors) {
|
||||
this->coast(motor, false);
|
||||
}
|
||||
if(load)
|
||||
pwms.load_pwm();
|
||||
}
|
||||
|
||||
void MotorCluster::coast_all(bool load) {
|
||||
uint8_t motor_count = pwms.get_chan_pair_count();
|
||||
for(uint8_t motor = 0; motor < motor_count; motor++) {
|
||||
this->coast(motor, false);
|
||||
}
|
||||
if(load)
|
||||
pwms.load_pwm();
|
||||
}
|
||||
|
||||
void MotorCluster::brake(uint8_t motor, bool load) {
|
||||
assert(motor < pwms.get_chan_pair_count());
|
||||
float new_duty = states[motor].stop_with_return();
|
||||
apply_duty(motor, new_duty, SLOW_DECAY, load);
|
||||
}
|
||||
|
||||
void MotorCluster::brake(const uint8_t *motors, uint8_t length, bool load) {
|
||||
assert(motors != nullptr);
|
||||
for(uint8_t i = 0; i < length; i++) {
|
||||
this->brake(motors[i], false);
|
||||
}
|
||||
if(load)
|
||||
pwms.load_pwm();
|
||||
}
|
||||
|
||||
void MotorCluster::brake(std::initializer_list<uint8_t> motors, bool load) {
|
||||
for(auto motor : motors) {
|
||||
this->brake(motor, false);
|
||||
}
|
||||
if(load)
|
||||
pwms.load_pwm();
|
||||
}
|
||||
|
||||
void MotorCluster::brake_all(bool load) {
|
||||
uint8_t motor_count = pwms.get_chan_pair_count();
|
||||
for(uint8_t motor = 0; motor < motor_count; motor++) {
|
||||
this->brake(motor, false);
|
||||
}
|
||||
if(load)
|
||||
pwms.load_pwm();
|
||||
}
|
||||
|
||||
void MotorCluster::full_negative(uint8_t motor, bool load) {
|
||||
assert(motor < pwms.get_chan_pair_count());
|
||||
float new_duty = states[motor].full_negative_with_return();
|
||||
apply_duty(motor, new_duty, configs[motor].mode, load);
|
||||
}
|
||||
|
||||
void MotorCluster::full_negative(const uint8_t *motors, uint8_t length, bool load) {
|
||||
assert(motors != nullptr);
|
||||
for(uint8_t i = 0; i < length; i++) {
|
||||
this->full_negative(motors[i], false);
|
||||
}
|
||||
if(load)
|
||||
pwms.load_pwm();
|
||||
}
|
||||
|
||||
void MotorCluster::full_negative(std::initializer_list<uint8_t> motors, bool load) {
|
||||
for(auto motor : motors) {
|
||||
this->full_negative(motor, false);
|
||||
}
|
||||
if(load)
|
||||
pwms.load_pwm();
|
||||
}
|
||||
|
||||
void MotorCluster::all_full_negative(bool load) {
|
||||
uint8_t motor_count = pwms.get_chan_pair_count();
|
||||
for(uint8_t motor = 0; motor < motor_count; motor++) {
|
||||
this->full_negative(motor, false);
|
||||
}
|
||||
if(load)
|
||||
pwms.load_pwm();
|
||||
}
|
||||
|
||||
void MotorCluster::full_positive(uint8_t motor, bool load) {
|
||||
assert(motor < pwms.get_chan_pair_count());
|
||||
float new_duty = states[motor].full_positive_with_return();
|
||||
apply_duty(motor, new_duty, configs[motor].mode, load);
|
||||
}
|
||||
|
||||
void MotorCluster::full_positive(const uint8_t *motors, uint8_t length, bool load) {
|
||||
assert(motors != nullptr);
|
||||
for(uint8_t i = 0; i < length; i++) {
|
||||
this->full_positive(motors[i], false);
|
||||
}
|
||||
if(load)
|
||||
pwms.load_pwm();
|
||||
}
|
||||
|
||||
void MotorCluster::full_positive(std::initializer_list<uint8_t> motors, bool load) {
|
||||
for(auto motor : motors) {
|
||||
this->full_positive(motor, false);
|
||||
}
|
||||
if(load)
|
||||
pwms.load_pwm();
|
||||
}
|
||||
|
||||
void MotorCluster::all_full_positive(bool load) {
|
||||
uint8_t motor_count = pwms.get_chan_pair_count();
|
||||
for(uint8_t motor = 0; motor < motor_count; motor++) {
|
||||
this->full_positive(motor, false);
|
||||
}
|
||||
if(load)
|
||||
pwms.load_pwm();
|
||||
}
|
||||
|
||||
void MotorCluster::to_percent(uint8_t motor, float in, float in_min, float in_max, bool load) {
|
||||
assert(motor < pwms.get_chan_pair_count());
|
||||
float new_duty = states[motor].to_percent_with_return(in, in_min, in_max);
|
||||
apply_duty(motor, new_duty, configs[motor].mode, load);
|
||||
}
|
||||
|
||||
void MotorCluster::to_percent(const uint8_t *motors, uint8_t length, float in, float in_min, float in_max, bool load) {
|
||||
assert(motors != nullptr);
|
||||
for(uint8_t i = 0; i < length; i++) {
|
||||
to_percent(motors[i], in, in_min, in_max, false);
|
||||
}
|
||||
if(load)
|
||||
pwms.load_pwm();
|
||||
}
|
||||
|
||||
void MotorCluster::to_percent(std::initializer_list<uint8_t> motors, float in, float in_min, float in_max, bool load) {
|
||||
for(auto motor : motors) {
|
||||
to_percent(motor, in, in_min, in_max, false);
|
||||
}
|
||||
if(load)
|
||||
pwms.load_pwm();
|
||||
}
|
||||
|
||||
void MotorCluster::all_to_percent(float in, float in_min, float in_max, bool load) {
|
||||
uint8_t motor_count = pwms.get_chan_pair_count();
|
||||
for(uint8_t motor = 0; motor < motor_count; motor++) {
|
||||
to_percent(motor, in, in_min, in_max, false);
|
||||
}
|
||||
if(load)
|
||||
pwms.load_pwm();
|
||||
}
|
||||
|
||||
void MotorCluster::to_percent(uint8_t motor, float in, float in_min, float in_max, float speed_min, float speed_max, bool load) {
|
||||
assert(motor < pwms.get_chan_pair_count());
|
||||
float new_duty = states[motor].to_percent_with_return(in, in_min, in_max, speed_min, speed_max);
|
||||
apply_duty(motor, new_duty, configs[motor].mode, load);
|
||||
}
|
||||
|
||||
void MotorCluster::to_percent(const uint8_t *motors, uint8_t length, float in, float in_min, float in_max, float speed_min, float speed_max, bool load) {
|
||||
assert(motors != nullptr);
|
||||
for(uint8_t i = 0; i < length; i++) {
|
||||
to_percent(motors[i], in, in_min, in_max, speed_min, speed_max, false);
|
||||
}
|
||||
if(load)
|
||||
pwms.load_pwm();
|
||||
}
|
||||
|
||||
void MotorCluster::to_percent(std::initializer_list<uint8_t> motors, float in, float in_min, float in_max, float speed_min, float speed_max, bool load) {
|
||||
for(auto motor : motors) {
|
||||
to_percent(motor, in, in_min, in_max, speed_min, speed_max, false);
|
||||
}
|
||||
if(load)
|
||||
pwms.load_pwm();
|
||||
}
|
||||
|
||||
void MotorCluster::all_to_percent(float in, float in_min, float in_max, float speed_min, float speed_max, bool load) {
|
||||
uint8_t motor_count = pwms.get_chan_pair_count();
|
||||
for(uint8_t motor = 0; motor < motor_count; motor++) {
|
||||
to_percent(motor, in, in_min, in_max, speed_min, speed_max, false);
|
||||
}
|
||||
if(load)
|
||||
pwms.load_pwm();
|
||||
}
|
||||
|
||||
void MotorCluster::load() {
|
||||
pwms.load_pwm();
|
||||
}
|
||||
|
||||
Direction MotorCluster::direction(uint8_t motor) const {
|
||||
assert(motor < pwms.get_chan_pair_count());
|
||||
return states[motor].get_direction();
|
||||
}
|
||||
|
||||
void MotorCluster::direction(uint8_t motor, Direction direction) {
|
||||
assert(motor < pwms.get_chan_pair_count());
|
||||
states[motor].set_direction(direction);
|
||||
}
|
||||
|
||||
void MotorCluster::direction(const uint8_t *motors, uint8_t length, Direction direction) {
|
||||
assert(motors != nullptr);
|
||||
for(uint8_t i = 0; i < length; i++) {
|
||||
this->direction(motors[i], direction);
|
||||
}
|
||||
}
|
||||
|
||||
void MotorCluster::direction(std::initializer_list<uint8_t> motors, Direction direction) {
|
||||
for(auto motor : motors) {
|
||||
this->direction(motor, direction);
|
||||
}
|
||||
}
|
||||
|
||||
void MotorCluster::all_directions(Direction direction) {
|
||||
uint8_t motor_count = pwms.get_chan_pair_count();
|
||||
for(uint8_t motor = 0; motor < motor_count; motor++) {
|
||||
this->direction(motor, direction);
|
||||
}
|
||||
}
|
||||
|
||||
float MotorCluster::speed_scale(uint8_t motor) const {
|
||||
assert(motor < pwms.get_chan_pair_count());
|
||||
return states[motor].get_speed_scale();
|
||||
}
|
||||
|
||||
void MotorCluster::speed_scale(uint8_t motor, float speed_scale) {
|
||||
assert(motor < pwms.get_chan_pair_count());
|
||||
states[motor].set_speed_scale(speed_scale);
|
||||
}
|
||||
|
||||
void MotorCluster::speed_scale(const uint8_t *motors, uint8_t length, float speed_scale) {
|
||||
assert(motors != nullptr);
|
||||
for(uint8_t i = 0; i < length; i++) {
|
||||
this->speed_scale(motors[i], speed_scale);
|
||||
}
|
||||
}
|
||||
|
||||
void MotorCluster::speed_scale(std::initializer_list<uint8_t> motors, float speed_scale) {
|
||||
for(auto motor : motors) {
|
||||
this->speed_scale(motor, speed_scale);
|
||||
}
|
||||
}
|
||||
|
||||
void MotorCluster::all_speed_scales(float speed_scale) {
|
||||
uint8_t motor_count = pwms.get_chan_pair_count();
|
||||
for(uint8_t motor = 0; motor < motor_count; motor++) {
|
||||
this->speed_scale(motor, speed_scale);
|
||||
}
|
||||
}
|
||||
|
||||
float MotorCluster::zeropoint(uint8_t motor) const {
|
||||
assert(motor < pwms.get_chan_pair_count());
|
||||
return states[motor].get_zeropoint();
|
||||
}
|
||||
|
||||
void MotorCluster::zeropoint(uint8_t motor, float zeropoint, bool load) {
|
||||
assert(motor < pwms.get_chan_pair_count());
|
||||
states[motor].set_zeropoint(zeropoint);
|
||||
}
|
||||
|
||||
void MotorCluster::zeropoint(const uint8_t *motors, uint8_t length, float zeropoint, bool load) {
|
||||
assert(motors != nullptr);
|
||||
for(uint8_t i = 0; i < length; i++) {
|
||||
this->zeropoint(motors[i], zeropoint);
|
||||
}
|
||||
}
|
||||
|
||||
void MotorCluster::zeropoint(std::initializer_list<uint8_t> motors, float zeropoint, bool load) {
|
||||
for(auto motor : motors) {
|
||||
this->zeropoint(motor, zeropoint);
|
||||
}
|
||||
}
|
||||
|
||||
void MotorCluster::all_zeropoints(float zeropoint, bool load) {
|
||||
uint8_t motor_count = pwms.get_chan_pair_count();
|
||||
for(uint8_t motor = 0; motor < motor_count; motor++) {
|
||||
this->zeropoint(motor, zeropoint);
|
||||
}
|
||||
}
|
||||
|
||||
float MotorCluster::deadzone(uint8_t motor) const {
|
||||
assert(motor < pwms.get_chan_pair_count());
|
||||
return states[motor].get_deadzone();
|
||||
}
|
||||
|
||||
void MotorCluster::deadzone(uint8_t motor, float deadzone, bool load) {
|
||||
assert(motor < pwms.get_chan_pair_count());
|
||||
float new_duty = states[motor].set_deadzone_with_return(deadzone);
|
||||
apply_duty(motor, new_duty, configs[motor].mode, load);
|
||||
}
|
||||
|
||||
void MotorCluster::deadzone(const uint8_t *motors, uint8_t length, float deadzone, bool load) {
|
||||
assert(motors != nullptr);
|
||||
for(uint8_t i = 0; i < length; i++) {
|
||||
this->deadzone(motors[i], deadzone);
|
||||
}
|
||||
}
|
||||
|
||||
void MotorCluster::deadzone(std::initializer_list<uint8_t> motors, float deadzone, bool load) {
|
||||
for(auto motor : motors) {
|
||||
this->deadzone(motor, deadzone);
|
||||
}
|
||||
}
|
||||
|
||||
void MotorCluster::all_deadzones(float deadzone, bool load) {
|
||||
uint8_t motor_count = pwms.get_chan_pair_count();
|
||||
for(uint8_t motor = 0; motor < motor_count; motor++) {
|
||||
this->deadzone(motor, deadzone);
|
||||
}
|
||||
}
|
||||
|
||||
DecayMode MotorCluster::decay_mode(uint8_t motor) const {
|
||||
assert(motor < pwms.get_chan_pair_count());
|
||||
return configs[motor].mode;
|
||||
}
|
||||
|
||||
void MotorCluster::decay_mode(uint8_t motor, DecayMode mode, bool load) {
|
||||
assert(motor < pwms.get_chan_pair_count());
|
||||
configs[motor].mode = mode;
|
||||
apply_duty(motor, states[motor].get_deadzoned_duty(), configs[motor].mode, false);
|
||||
}
|
||||
|
||||
void MotorCluster::decay_mode(const uint8_t *motors, uint8_t length, DecayMode mode, bool load) {
|
||||
assert(motors != nullptr);
|
||||
for(uint8_t i = 0; i < length; i++) {
|
||||
this->decay_mode(motors[i], mode);
|
||||
}
|
||||
}
|
||||
|
||||
void MotorCluster::decay_mode(std::initializer_list<uint8_t> motors, DecayMode mode, bool load) {
|
||||
for(auto motor : motors) {
|
||||
this->decay_mode(motor, mode);
|
||||
}
|
||||
}
|
||||
|
||||
void MotorCluster::all_decay_modes(DecayMode mode, bool load) {
|
||||
uint8_t motor_count = pwms.get_chan_pair_count();
|
||||
for(uint8_t motor = 0; motor < motor_count; motor++) {
|
||||
this->decay_mode(motor, mode);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void MotorCluster::apply_duty(uint8_t motor, float duty, DecayMode mode, bool load) {
|
||||
if(isfinite(duty)) {
|
||||
int32_t signed_level = MotorState::duty_to_level(duty, pwm_period);
|
||||
|
||||
switch(mode) {
|
||||
case SLOW_DECAY: //aka 'Braking'
|
||||
if(signed_level >= 0) {
|
||||
pwms.set_chan_level(POS_MOTOR(motor), pwm_period, false);
|
||||
pwms.set_chan_level(NEG_MOTOR(motor), pwm_period - signed_level, load);
|
||||
}
|
||||
else {
|
||||
pwms.set_chan_level(POS_MOTOR(motor), pwm_period + signed_level, false);
|
||||
pwms.set_chan_level(NEG_MOTOR(motor), pwm_period, load);
|
||||
}
|
||||
break;
|
||||
|
||||
case FAST_DECAY: //aka 'Coasting'
|
||||
default:
|
||||
if(signed_level >= 0) {
|
||||
pwms.set_chan_level(POS_MOTOR(motor), signed_level, false);
|
||||
pwms.set_chan_level(NEG_MOTOR(motor), 0, load);
|
||||
}
|
||||
else {
|
||||
pwms.set_chan_level(POS_MOTOR(motor), 0, false);
|
||||
pwms.set_chan_level(NEG_MOTOR(motor), 0 - signed_level, load);
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
else {
|
||||
pwms.set_chan_level(POS_MOTOR(motor), 0, false);
|
||||
pwms.set_chan_level(NEG_MOTOR(motor), 0, load);
|
||||
}
|
||||
}
|
||||
|
||||
void MotorCluster::create_motor_states(Direction direction, float speed_scale, float zeropoint,
|
||||
float deadzone, DecayMode mode, bool auto_phase) {
|
||||
uint8_t motor_count = pwms.get_chan_pair_count();
|
||||
if(motor_count > 0) {
|
||||
states = new MotorState[motor_count];
|
||||
configs = new motor_config[motor_count];
|
||||
|
||||
for(uint motor = 0; motor < motor_count; motor++) {
|
||||
states[motor] = MotorState(direction, speed_scale, zeropoint, deadzone);
|
||||
configs[motor].phase = (auto_phase) ? (float)motor / (float)motor_count : 0.0f;
|
||||
configs[motor].mode = mode;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,168 @@
|
|||
#pragma once
|
||||
|
||||
#include "pico/stdlib.h"
|
||||
#include "pwm_cluster.hpp"
|
||||
#include "motor_state.hpp"
|
||||
|
||||
using namespace pimoroni;
|
||||
|
||||
namespace motor {
|
||||
|
||||
class MotorCluster {
|
||||
//--------------------------------------------------
|
||||
// Substructures
|
||||
//--------------------------------------------------
|
||||
private:
|
||||
struct motor_config {
|
||||
float phase;
|
||||
DecayMode mode;
|
||||
};
|
||||
|
||||
|
||||
//--------------------------------------------------
|
||||
// Variables
|
||||
//--------------------------------------------------
|
||||
private:
|
||||
PWMCluster pwms;
|
||||
uint32_t pwm_period;
|
||||
float pwm_frequency;
|
||||
MotorState* states;
|
||||
motor_config* configs;
|
||||
|
||||
|
||||
//--------------------------------------------------
|
||||
// Constructors/Destructor
|
||||
//--------------------------------------------------
|
||||
public:
|
||||
MotorCluster(PIO pio, uint sm, uint pin_base, uint pin_pair_count, Direction direction = NORMAL_DIR, float speed_scale = MotorState::DEFAULT_SPEED_SCALE,
|
||||
float zeropoint = MotorState::DEFAULT_ZEROPOINT, float deadzone = MotorState::DEFAULT_DEADZONE, float freq = MotorState::DEFAULT_FREQUENCY, DecayMode mode = MotorState::DEFAULT_DECAY_MODE,
|
||||
bool auto_phase = true, PWMCluster::Sequence *seq_buffer = nullptr, PWMCluster::TransitionData *dat_buffer = nullptr);
|
||||
MotorCluster(PIO pio, uint sm, const pin_pair *pin_pairs, uint32_t length, Direction direction = NORMAL_DIR, float speed_scale = MotorState::DEFAULT_SPEED_SCALE,
|
||||
float zeropoint = MotorState::DEFAULT_ZEROPOINT, float deadzone = MotorState::DEFAULT_DEADZONE, float freq = MotorState::DEFAULT_FREQUENCY, DecayMode mode = MotorState::DEFAULT_DECAY_MODE,
|
||||
bool auto_phase = true, PWMCluster::Sequence *seq_buffer = nullptr, PWMCluster::TransitionData *dat_buffer = nullptr);
|
||||
MotorCluster(PIO pio, uint sm, std::initializer_list<pin_pair> pin_pairs, Direction direction = NORMAL_DIR, float speed_scale = MotorState::DEFAULT_SPEED_SCALE,
|
||||
float zeropoint = MotorState::DEFAULT_ZEROPOINT, float deadzone = MotorState::DEFAULT_DEADZONE, float freq = MotorState::DEFAULT_FREQUENCY, DecayMode mode = MotorState::DEFAULT_DECAY_MODE,
|
||||
bool auto_phase = true, PWMCluster::Sequence *seq_buffer = nullptr, PWMCluster::TransitionData *dat_buffer = nullptr);
|
||||
~MotorCluster();
|
||||
|
||||
|
||||
//--------------------------------------------------
|
||||
// Methods
|
||||
//--------------------------------------------------
|
||||
public:
|
||||
bool init();
|
||||
|
||||
uint8_t count() const;
|
||||
pin_pair pins(uint8_t motor) const;
|
||||
|
||||
void enable(uint8_t motor, bool load = true);
|
||||
void enable(const uint8_t *motors, uint8_t length, bool load = true);
|
||||
void enable(std::initializer_list<uint8_t> motors, bool load = true);
|
||||
void enable_all(bool load = true);
|
||||
|
||||
void disable(uint8_t motor, bool load = true);
|
||||
void disable(const uint8_t *motors, uint8_t length, bool load = true);
|
||||
void disable(std::initializer_list<uint8_t> motors, bool load = true);
|
||||
void disable_all(bool load = true);
|
||||
|
||||
bool is_enabled(uint8_t motor) const;
|
||||
|
||||
float duty(uint8_t motor) const;
|
||||
void duty(uint8_t motor, float duty, bool load = true);
|
||||
void duty(const uint8_t *motors, uint8_t length, float duty, bool load = true);
|
||||
void duty(std::initializer_list<uint8_t> motors, float duty, bool load = true);
|
||||
void all_to_duty(float duty, bool load = true);
|
||||
|
||||
float speed(uint8_t motor) const;
|
||||
void speed(uint8_t motor, float speed, bool load = true);
|
||||
void speed(const uint8_t *motors, uint8_t length, float speed, bool load = true);
|
||||
void speed(std::initializer_list<uint8_t> motors, float speed, bool load = true);
|
||||
void all_to_speed(float speed, bool load = true);
|
||||
|
||||
float phase(uint8_t motor) const;
|
||||
void phase(uint8_t motor, float phase, bool load = true);
|
||||
void phase(const uint8_t *motors, uint8_t length, float phase, bool load = true);
|
||||
void phase(std::initializer_list<uint8_t> motors, float phase, bool load = true);
|
||||
void all_to_phase(float phase, bool load = true);
|
||||
|
||||
float frequency() const;
|
||||
bool frequency(float freq);
|
||||
|
||||
//--------------------------------------------------
|
||||
void stop(uint8_t motor, bool load = true);
|
||||
void stop(const uint8_t *motors, uint8_t length, bool load = true);
|
||||
void stop(std::initializer_list<uint8_t> motors, bool load = true);
|
||||
void stop_all(bool load = true);
|
||||
|
||||
void coast(uint8_t motor, bool load = true);
|
||||
void coast(const uint8_t *motors, uint8_t length, bool load = true);
|
||||
void coast(std::initializer_list<uint8_t> motors, bool load = true);
|
||||
void coast_all(bool load = true);
|
||||
|
||||
void brake(uint8_t motor, bool load = true);
|
||||
void brake(const uint8_t *motors, uint8_t length, bool load = true);
|
||||
void brake(std::initializer_list<uint8_t> motors, bool load = true);
|
||||
void brake_all(bool load = true);
|
||||
|
||||
void full_negative(uint8_t motor, bool load = true);
|
||||
void full_negative(const uint8_t *motors, uint8_t length, bool load = true);
|
||||
void full_negative(std::initializer_list<uint8_t> motors, bool load = true);
|
||||
void all_full_negative(bool load = true);
|
||||
|
||||
void full_positive(uint8_t motor, bool load = true);
|
||||
void full_positive(const uint8_t *motors, uint8_t length, bool load = true);
|
||||
void full_positive(std::initializer_list<uint8_t> motors, bool load = true);
|
||||
void all_full_positive(bool load = true);
|
||||
|
||||
void to_percent(uint8_t motor, float in, float in_min = MotorState::ZERO_PERCENT, float in_max = MotorState::ONEHUNDRED_PERCENT, bool load = true);
|
||||
void to_percent(const uint8_t *motors, uint8_t length, float in, float in_min = MotorState::ZERO_PERCENT, float in_max = MotorState::ONEHUNDRED_PERCENT, bool load = true);
|
||||
void to_percent(std::initializer_list<uint8_t> motors, float in, float in_min = MotorState::ZERO_PERCENT, float in_max = MotorState::ONEHUNDRED_PERCENT, bool load = true);
|
||||
void all_to_percent(float in, float in_min = MotorState::ZERO_PERCENT, float in_max = MotorState::ONEHUNDRED_PERCENT, bool load = true);
|
||||
|
||||
void to_percent(uint8_t motor, float in, float in_min, float in_max, float speed_min, float speed_max, bool load = true);
|
||||
void to_percent(const uint8_t *motors, uint8_t length, float in, float in_min, float in_max, float speed_min, float speed_max, bool load = true);
|
||||
void to_percent(std::initializer_list<uint8_t> motors, float in, float in_min, float in_max, float speed_min, float speed_max, bool load = true);
|
||||
void all_to_percent(float in, float in_min, float in_max, float speed_min, float speed_max, bool load = true);
|
||||
|
||||
void load();
|
||||
|
||||
//--------------------------------------------------
|
||||
|
||||
Direction direction(uint8_t motor) const;
|
||||
void direction(uint8_t motor, Direction direction);
|
||||
void direction(const uint8_t *motors, uint8_t length, Direction direction);
|
||||
void direction(std::initializer_list<uint8_t> motors, Direction direction);
|
||||
void all_directions(Direction direction);
|
||||
|
||||
float speed_scale(uint8_t motor) const;
|
||||
void speed_scale(uint8_t motor, float speed_scale);
|
||||
void speed_scale(const uint8_t *motors, uint8_t length, float speed_scale);
|
||||
void speed_scale(std::initializer_list<uint8_t> motors, float speed_scale);
|
||||
void all_speed_scales(float speed_scale);
|
||||
|
||||
float zeropoint(uint8_t motor) const;
|
||||
void zeropoint(uint8_t motor, float zeropoint, bool load = true);
|
||||
void zeropoint(const uint8_t *motors, uint8_t length, float zeropoint, bool load = true);
|
||||
void zeropoint(std::initializer_list<uint8_t> motors, float zeropoint, bool load = true);
|
||||
void all_zeropoints(float zeropoint, bool load = true);
|
||||
|
||||
float deadzone(uint8_t motor) const;
|
||||
void deadzone(uint8_t motor, float deadzone, bool load = true);
|
||||
void deadzone(const uint8_t *motors, uint8_t length, float deadzone, bool load = true);
|
||||
void deadzone(std::initializer_list<uint8_t> motors, float deadzone, bool load = true);
|
||||
void all_deadzones(float deadzone, bool load = true);
|
||||
|
||||
DecayMode decay_mode(uint8_t motor) const;
|
||||
void decay_mode(uint8_t motor, DecayMode mode, bool load = true);
|
||||
void decay_mode(const uint8_t *motors, uint8_t length, DecayMode mode, bool load = true);
|
||||
void decay_mode(std::initializer_list<uint8_t> motors, DecayMode mode, bool load = true);
|
||||
void all_decay_modes(DecayMode mode, bool load = true);
|
||||
|
||||
//--------------------------------------------------
|
||||
private:
|
||||
void apply_duty(uint8_t motor, float duty, DecayMode mode, bool load);
|
||||
void create_motor_states(Direction direction, float speed_scale, float zeropoint,
|
||||
float deadzone, DecayMode mode, bool auto_phase);
|
||||
};
|
||||
|
||||
}
|
||||
|
|
@ -0,0 +1,168 @@
|
|||
#include "motor_state.hpp"
|
||||
#include "common/pimoroni_common.hpp"
|
||||
#include "float.h"
|
||||
#include "math.h"
|
||||
|
||||
namespace motor {
|
||||
MotorState::MotorState()
|
||||
: motor_speed(0.0f), last_enabled_duty(0.0f), enabled(false)
|
||||
, motor_direction(NORMAL_DIR), motor_scale(DEFAULT_SPEED_SCALE)
|
||||
, motor_zeropoint(DEFAULT_ZEROPOINT), motor_deadzone(DEFAULT_DEADZONE) {
|
||||
}
|
||||
|
||||
MotorState::MotorState(Direction direction, float speed_scale, float zeropoint, float deadzone)
|
||||
: motor_speed(0.0f), last_enabled_duty(0.0f), enabled(false)
|
||||
, motor_direction(direction) , motor_scale(MAX(speed_scale, FLT_EPSILON))
|
||||
, motor_zeropoint(CLAMP(zeropoint, 0.0f, 1.0f - FLT_EPSILON)), motor_deadzone(CLAMP(deadzone, 0.0f, 1.0f)) {
|
||||
}
|
||||
|
||||
float MotorState::enable_with_return() {
|
||||
enabled = true;
|
||||
return get_deadzoned_duty();
|
||||
}
|
||||
|
||||
float MotorState::disable_with_return() {
|
||||
enabled = false;
|
||||
return NAN;
|
||||
}
|
||||
|
||||
bool MotorState::is_enabled() const {
|
||||
return enabled;
|
||||
}
|
||||
|
||||
float MotorState::get_duty() const {
|
||||
return (motor_direction == NORMAL_DIR) ? last_enabled_duty : 0.0f - last_enabled_duty;
|
||||
}
|
||||
|
||||
float MotorState::get_deadzoned_duty() const {
|
||||
float duty = 0.0f;
|
||||
if((last_enabled_duty <= 0.0f - motor_deadzone) || (last_enabled_duty >= motor_deadzone)) {
|
||||
duty = last_enabled_duty;
|
||||
}
|
||||
|
||||
if(enabled)
|
||||
return duty;
|
||||
else
|
||||
return NAN;
|
||||
}
|
||||
|
||||
float MotorState::set_duty_with_return(float duty) {
|
||||
// Invert provided speed if the motor direction is reversed
|
||||
if(motor_direction == REVERSED_DIR)
|
||||
duty = 0.0f - duty;
|
||||
|
||||
// Clamp the duty between the hard limits
|
||||
last_enabled_duty = CLAMP(duty, -1.0f, 1.0f);
|
||||
|
||||
// Calculate the corresponding speed
|
||||
motor_speed = duty_to_speed(last_enabled_duty, motor_zeropoint, motor_scale);
|
||||
|
||||
return enable_with_return();
|
||||
}
|
||||
|
||||
float MotorState::get_speed() const {
|
||||
return (motor_direction == NORMAL_DIR) ? motor_speed : 0.0f - motor_speed;
|
||||
}
|
||||
|
||||
float MotorState::set_speed_with_return(float speed) {
|
||||
// Invert provided speed if the motor direction is reversed
|
||||
if(motor_direction == REVERSED_DIR)
|
||||
speed = 0.0f - speed;
|
||||
|
||||
// Clamp the speed between the hard limits
|
||||
motor_speed = CLAMP(speed, 0.0f - motor_scale, motor_scale);
|
||||
|
||||
// Calculate the corresponding duty cycle
|
||||
last_enabled_duty = speed_to_duty(motor_speed, motor_zeropoint, motor_scale);
|
||||
|
||||
return enable_with_return();
|
||||
}
|
||||
|
||||
float MotorState::stop_with_return() {
|
||||
return set_duty_with_return(0.0f);
|
||||
}
|
||||
|
||||
float MotorState::full_negative_with_return() {
|
||||
return set_duty_with_return(-1.0f);
|
||||
}
|
||||
|
||||
float MotorState::full_positive_with_return() {
|
||||
return set_duty_with_return(1.0f);
|
||||
}
|
||||
|
||||
float MotorState::to_percent_with_return(float in, float in_min, float in_max) {
|
||||
float speed = MotorState::map_float(in, in_min, in_max, 0.0f - motor_scale, motor_scale);
|
||||
return set_speed_with_return(speed);
|
||||
}
|
||||
|
||||
float MotorState::to_percent_with_return(float in, float in_min, float in_max, float speed_min, float speed_max) {
|
||||
float speed = MotorState::map_float(in, in_min, in_max, speed_min, speed_max);
|
||||
return set_speed_with_return(speed);
|
||||
}
|
||||
|
||||
Direction MotorState::get_direction() const {
|
||||
return motor_direction;
|
||||
}
|
||||
|
||||
void MotorState::set_direction(Direction direction) {
|
||||
motor_direction = direction;
|
||||
}
|
||||
|
||||
float MotorState::get_speed_scale() const {
|
||||
return motor_scale;
|
||||
}
|
||||
|
||||
void MotorState::set_speed_scale(float speed_scale) {
|
||||
motor_scale = MAX(speed_scale, FLT_EPSILON);
|
||||
motor_speed = duty_to_speed(last_enabled_duty, motor_zeropoint, motor_scale);
|
||||
}
|
||||
|
||||
float MotorState::get_zeropoint() const {
|
||||
return motor_scale;
|
||||
}
|
||||
|
||||
void MotorState::set_zeropoint(float zeropoint) {
|
||||
motor_zeropoint = CLAMP(zeropoint, 0.0f, 1.0f - FLT_EPSILON);
|
||||
motor_speed = duty_to_speed(last_enabled_duty, motor_zeropoint, motor_scale);
|
||||
}
|
||||
|
||||
float MotorState::get_deadzone() const {
|
||||
return motor_deadzone;
|
||||
}
|
||||
|
||||
float MotorState::set_deadzone_with_return(float deadzone) {
|
||||
motor_deadzone = CLAMP(deadzone, 0.0f, 1.0f);
|
||||
return get_deadzoned_duty();
|
||||
}
|
||||
|
||||
int32_t MotorState::duty_to_level(float duty, uint32_t resolution) {
|
||||
return (int32_t)(duty * (float)resolution);
|
||||
}
|
||||
|
||||
float MotorState::map_float(float in, float in_min, float in_max, float out_min, float out_max) {
|
||||
return (((in - in_min) * (out_max - out_min)) / (in_max - in_min)) + out_min;
|
||||
}
|
||||
|
||||
float MotorState::duty_to_speed(float duty, float zeropoint, float scale) {
|
||||
float speed = 0.0f;
|
||||
if(duty > zeropoint) {
|
||||
speed = map_float(duty, zeropoint, 1.0f, 0.0f, scale);
|
||||
}
|
||||
else if(duty < -zeropoint) {
|
||||
speed = map_float(duty, -zeropoint, -1.0f, 0.0f, -scale);
|
||||
}
|
||||
return speed;
|
||||
}
|
||||
|
||||
float MotorState::speed_to_duty(float speed, float zeropoint, float scale) {
|
||||
float duty = 0.0f;
|
||||
if(speed > 0.0f) {
|
||||
duty = map_float(speed, 0.0f, scale, zeropoint, 1.0f);
|
||||
}
|
||||
else if(speed < 0.0f) {
|
||||
duty = map_float(speed, 0.0f, -scale, -zeropoint, -1.0f);
|
||||
}
|
||||
return duty;
|
||||
}
|
||||
|
||||
};
|
||||
|
|
@ -0,0 +1,102 @@
|
|||
#pragma once
|
||||
|
||||
#include "pico/stdlib.h"
|
||||
#include "common/pimoroni_common.hpp"
|
||||
|
||||
using namespace pimoroni;
|
||||
|
||||
namespace motor {
|
||||
|
||||
enum DecayMode {
|
||||
FAST_DECAY = 0, //aka 'Coasting'
|
||||
SLOW_DECAY = 1, //aka 'Braking'
|
||||
};
|
||||
|
||||
class MotorState {
|
||||
//--------------------------------------------------
|
||||
// Constants
|
||||
//--------------------------------------------------
|
||||
public:
|
||||
static constexpr float DEFAULT_SPEED_SCALE = 1.0f; // The standard motor speed scale
|
||||
static constexpr float DEFAULT_ZEROPOINT = 0.0f; // The standard motor zeropoint
|
||||
static constexpr float DEFAULT_DEADZONE = 0.05f; // The standard motor deadzone
|
||||
|
||||
static const DecayMode DEFAULT_DECAY_MODE = SLOW_DECAY; // The standard motor decay behaviour
|
||||
static constexpr float DEFAULT_FREQUENCY = 25000.0f; // The standard motor update rate
|
||||
static constexpr float MIN_FREQUENCY = 10.0f;
|
||||
static constexpr float MAX_FREQUENCY = 400000.0f;
|
||||
|
||||
static constexpr float ZERO_PERCENT = 0.0f;
|
||||
static constexpr float ONEHUNDRED_PERCENT = 1.0f;
|
||||
|
||||
|
||||
//--------------------------------------------------
|
||||
// Variables
|
||||
//--------------------------------------------------
|
||||
private:
|
||||
float motor_speed;
|
||||
float last_enabled_duty;
|
||||
bool enabled;
|
||||
|
||||
// Customisation variables
|
||||
Direction motor_direction;
|
||||
float motor_scale;
|
||||
float motor_zeropoint;
|
||||
float motor_deadzone;
|
||||
|
||||
|
||||
//--------------------------------------------------
|
||||
// Constructors/Destructor
|
||||
//--------------------------------------------------
|
||||
public:
|
||||
MotorState();
|
||||
MotorState(Direction direction, float speed_scale, float zeropoint, float deadzone);
|
||||
|
||||
|
||||
//--------------------------------------------------
|
||||
// Methods
|
||||
//--------------------------------------------------
|
||||
public:
|
||||
float enable_with_return();
|
||||
float disable_with_return();
|
||||
bool is_enabled() const;
|
||||
|
||||
float get_duty() const;
|
||||
float get_deadzoned_duty() const;
|
||||
float set_duty_with_return(float duty);
|
||||
|
||||
float get_speed() const;
|
||||
float set_speed_with_return(float speed);
|
||||
|
||||
//--------------------------------------------------
|
||||
|
||||
float stop_with_return();
|
||||
float full_negative_with_return();
|
||||
float full_positive_with_return();
|
||||
float to_percent_with_return(float in, float in_min = ZERO_PERCENT, float in_max = ONEHUNDRED_PERCENT);
|
||||
float to_percent_with_return(float in, float in_min, float in_max, float speed_min, float speed_max);
|
||||
|
||||
//--------------------------------------------------
|
||||
|
||||
Direction get_direction() const;
|
||||
void set_direction(Direction direction);
|
||||
|
||||
float get_speed_scale() const;
|
||||
void set_speed_scale(float speed_scale);
|
||||
|
||||
float get_zeropoint() const;
|
||||
void set_zeropoint(float zeropoint);
|
||||
|
||||
float get_deadzone() const;
|
||||
float set_deadzone_with_return(float deadzone);
|
||||
|
||||
//--------------------------------------------------
|
||||
static int32_t duty_to_level(float duty, uint32_t resolution);
|
||||
|
||||
static float map_float(float in, float in_min, float in_max, float out_min, float out_max);
|
||||
private:
|
||||
static float duty_to_speed(float duty, float zeropoint, float scale);
|
||||
static float speed_to_duty(float speed, float zeropoint, float scale);
|
||||
};
|
||||
|
||||
}
|
||||
|
|
@ -0,0 +1 @@
|
|||
include(pid.cmake)
|
||||
|
|
@ -0,0 +1,11 @@
|
|||
set(DRIVER_NAME pid)
|
||||
add_library(${DRIVER_NAME} INTERFACE)
|
||||
|
||||
target_sources(${DRIVER_NAME} INTERFACE
|
||||
${CMAKE_CURRENT_LIST_DIR}/${DRIVER_NAME}.cpp
|
||||
)
|
||||
|
||||
target_include_directories(${DRIVER_NAME} INTERFACE ${CMAKE_CURRENT_LIST_DIR})
|
||||
|
||||
# Pull in pico libraries that we need
|
||||
target_link_libraries(${DRIVER_NAME} INTERFACE pico_stdlib)
|
||||
|
|
@ -0,0 +1,30 @@
|
|||
#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) {
|
||||
}
|
||||
|
||||
float PID::calculate(float value) {
|
||||
float error = setpoint - value;
|
||||
error_sum += error * sample_rate;
|
||||
float rate_error = (value - last_value) / sample_rate;
|
||||
last_value = value;
|
||||
|
||||
return (error * kp) + (error_sum * ki) - (rate_error * kd);
|
||||
}
|
||||
|
||||
float PID::calculate(float value, float value_change) {
|
||||
float error = setpoint - value;
|
||||
error_sum += error * sample_rate;
|
||||
last_value = value;
|
||||
|
||||
return (error * kp) + (error_sum * ki) - (value_change * kd);
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,28 @@
|
|||
#pragma once
|
||||
|
||||
//#include <stdint.h>
|
||||
#include "pico/stdlib.h"
|
||||
//include "common/pimoroni_common.hpp"
|
||||
|
||||
namespace pimoroni {
|
||||
|
||||
class PID {
|
||||
public:
|
||||
PID();
|
||||
PID(float kp, float ki, float kd, float sample_rate);
|
||||
|
||||
float calculate(float value);
|
||||
float calculate(float value, float value_change);
|
||||
|
||||
public:
|
||||
float kp;
|
||||
float ki;
|
||||
float kd;
|
||||
float setpoint;
|
||||
private:
|
||||
float error_sum;
|
||||
float last_value;
|
||||
float sample_rate;
|
||||
};
|
||||
|
||||
}
|
||||
|
|
@ -22,13 +22,14 @@ uint8_t PWMCluster::claimed_sms[] = { 0x0, 0x0 };
|
|||
uint PWMCluster::pio_program_offset = 0;
|
||||
|
||||
|
||||
PWMCluster::PWMCluster(PIO pio, uint sm, uint pin_mask, Sequence *seq_buffer, TransitionData *dat_buffer)
|
||||
PWMCluster::PWMCluster(PIO pio, uint sm, uint pin_mask, Sequence *seq_buffer, TransitionData *dat_buffer, bool loading_zone)
|
||||
: pio(pio)
|
||||
, sm(sm)
|
||||
, pin_mask(pin_mask & ((1u << NUM_BANK0_GPIOS) - 1))
|
||||
, channel_count(0)
|
||||
, channels(nullptr)
|
||||
, wrap_level(0) {
|
||||
, wrap_level(0)
|
||||
, loading_zone(loading_zone) {
|
||||
|
||||
// Create the channel mapping
|
||||
for(uint pin = 0; pin < NUM_BANK0_GPIOS; pin++) {
|
||||
|
|
@ -42,13 +43,14 @@ PWMCluster::PWMCluster(PIO pio, uint sm, uint pin_mask, Sequence *seq_buffer, Tr
|
|||
}
|
||||
|
||||
|
||||
PWMCluster::PWMCluster(PIO pio, uint sm, uint pin_base, uint pin_count, Sequence *seq_buffer, TransitionData *dat_buffer)
|
||||
PWMCluster::PWMCluster(PIO pio, uint sm, uint pin_base, uint pin_count, Sequence *seq_buffer, TransitionData *dat_buffer, bool loading_zone)
|
||||
: pio(pio)
|
||||
, sm(sm)
|
||||
, pin_mask(0x00000000)
|
||||
, channel_count(0)
|
||||
, channels(nullptr)
|
||||
, wrap_level(0) {
|
||||
, wrap_level(0)
|
||||
, loading_zone(loading_zone) {
|
||||
|
||||
// Create the pin mask and channel mapping
|
||||
uint pin_end = MIN(pin_count + pin_base, NUM_BANK0_GPIOS);
|
||||
|
|
@ -61,13 +63,14 @@ PWMCluster::PWMCluster(PIO pio, uint sm, uint pin_base, uint pin_count, Sequence
|
|||
constructor_common(seq_buffer, dat_buffer);
|
||||
}
|
||||
|
||||
PWMCluster::PWMCluster(PIO pio, uint sm, const uint8_t *pins, uint32_t length, Sequence *seq_buffer, TransitionData *dat_buffer)
|
||||
PWMCluster::PWMCluster(PIO pio, uint sm, const uint8_t *pins, uint32_t length, Sequence *seq_buffer, TransitionData *dat_buffer, bool loading_zone)
|
||||
: pio(pio)
|
||||
, sm(sm)
|
||||
, pin_mask(0x00000000)
|
||||
, channel_count(0)
|
||||
, channels(nullptr)
|
||||
, wrap_level(0) {
|
||||
, wrap_level(0)
|
||||
, loading_zone(loading_zone) {
|
||||
|
||||
// Create the pin mask and channel mapping
|
||||
for(uint i = 0; i < length; i++) {
|
||||
|
|
@ -82,13 +85,14 @@ PWMCluster::PWMCluster(PIO pio, uint sm, const uint8_t *pins, uint32_t length, S
|
|||
constructor_common(seq_buffer, dat_buffer);
|
||||
}
|
||||
|
||||
PWMCluster::PWMCluster(PIO pio, uint sm, std::initializer_list<uint8_t> pins, Sequence *seq_buffer, TransitionData *dat_buffer)
|
||||
PWMCluster::PWMCluster(PIO pio, uint sm, std::initializer_list<uint8_t> pins, Sequence *seq_buffer, TransitionData *dat_buffer, bool loading_zone)
|
||||
: pio(pio)
|
||||
, sm(sm)
|
||||
, pin_mask(0x00000000)
|
||||
, channel_count(0)
|
||||
, channels(nullptr)
|
||||
, wrap_level(0) {
|
||||
, wrap_level(0)
|
||||
, loading_zone(loading_zone) {
|
||||
|
||||
// Create the pin mask and channel mapping
|
||||
for(auto pin : pins) {
|
||||
|
|
@ -102,6 +106,57 @@ PWMCluster::PWMCluster(PIO pio, uint sm, std::initializer_list<uint8_t> pins, Se
|
|||
constructor_common(seq_buffer, dat_buffer);
|
||||
}
|
||||
|
||||
PWMCluster::PWMCluster(PIO pio, uint sm, const pin_pair *pin_pairs, uint32_t length, Sequence *seq_buffer, TransitionData *dat_buffer, bool loading_zone)
|
||||
: pio(pio)
|
||||
, sm(sm)
|
||||
, pin_mask(0x00000000)
|
||||
, channel_count(0)
|
||||
, channels(nullptr)
|
||||
, wrap_level(0)
|
||||
, loading_zone(loading_zone) {
|
||||
|
||||
// Create the pin mask and channel mapping
|
||||
for(uint i = 0; i < length; i++) {
|
||||
pin_pair pair = pin_pairs[i];
|
||||
if((pair.first < NUM_BANK0_GPIOS) && (pair.second < NUM_BANK0_GPIOS)) {
|
||||
pin_mask |= (1u << pair.first);
|
||||
channel_to_pin_map[channel_count] = pair.first;
|
||||
channel_count++;
|
||||
|
||||
pin_mask |= (1u << pair.second);
|
||||
channel_to_pin_map[channel_count] = pair.second;
|
||||
channel_count++;
|
||||
}
|
||||
}
|
||||
|
||||
constructor_common(seq_buffer, dat_buffer);
|
||||
}
|
||||
|
||||
PWMCluster::PWMCluster(PIO pio, uint sm, std::initializer_list<pin_pair> pin_pairs, Sequence *seq_buffer, TransitionData *dat_buffer, bool loading_zone)
|
||||
: pio(pio)
|
||||
, sm(sm)
|
||||
, pin_mask(0x00000000)
|
||||
, channel_count(0)
|
||||
, channels(nullptr)
|
||||
, wrap_level(0)
|
||||
, loading_zone(loading_zone) {
|
||||
|
||||
// Create the pin mask and channel mapping
|
||||
for(auto pair : pin_pairs) {
|
||||
if((pair.first < NUM_BANK0_GPIOS) && (pair.second < NUM_BANK0_GPIOS)) {
|
||||
pin_mask |= (1u << pair.first);
|
||||
channel_to_pin_map[channel_count] = pair.first;
|
||||
channel_count++;
|
||||
|
||||
pin_mask |= (1u << pair.second);
|
||||
channel_to_pin_map[channel_count] = pair.second;
|
||||
channel_count++;
|
||||
}
|
||||
}
|
||||
|
||||
constructor_common(seq_buffer, dat_buffer);
|
||||
}
|
||||
|
||||
void PWMCluster::constructor_common(Sequence *seq_buffer, TransitionData *dat_buffer) {
|
||||
// Initialise all the channels this PWM will control
|
||||
if(channel_count > 0) {
|
||||
|
|
@ -332,11 +387,25 @@ uint8_t PWMCluster::get_chan_count() const {
|
|||
return channel_count;
|
||||
}
|
||||
|
||||
uint8_t PWMCluster::get_chan_pair_count() const {
|
||||
return (channel_count / 2);
|
||||
}
|
||||
|
||||
uint8_t PWMCluster::get_chan_pin(uint8_t channel) const {
|
||||
assert(channel < channel_count);
|
||||
return channel_to_pin_map[channel];
|
||||
}
|
||||
|
||||
pin_pair PWMCluster::get_chan_pin_pair(uint8_t channel_pair) const {
|
||||
assert(channel_pair < get_chan_pair_count());
|
||||
uint8_t channel_base = channel_from_pair(channel_pair);
|
||||
return pin_pair(channel_to_pin_map[channel_base], channel_to_pin_map[channel_base + 1]);
|
||||
}
|
||||
|
||||
uint8_t PWMCluster::channel_from_pair(uint8_t channel_pair) {
|
||||
return (channel_pair * 2);
|
||||
}
|
||||
|
||||
uint32_t PWMCluster::get_chan_level(uint8_t channel) const {
|
||||
assert(channel < channel_count);
|
||||
return channels[channel].level;
|
||||
|
|
@ -472,12 +541,14 @@ void PWMCluster::load_pwm() {
|
|||
gpio_put(WRITE_GPIO, false);
|
||||
#endif
|
||||
|
||||
// Introduce "Loading Zone" transitions to the end of the sequence to
|
||||
// prevent the DMA interrupt firing many milliseconds before the sequence ends.
|
||||
uint32_t zone_inserts = MIN(LOADING_ZONE_SIZE, wrap_level - LOADING_ZONE_POSITION);
|
||||
for(uint32_t i = zone_inserts + LOADING_ZONE_POSITION; i > LOADING_ZONE_POSITION; i--) {
|
||||
PWMCluster::sorted_insert(transitions, data_size, TransitionData(wrap_level - i));
|
||||
PWMCluster::sorted_insert(looping_transitions, looping_data_size, TransitionData(wrap_level - i));
|
||||
if(loading_zone) {
|
||||
// Introduce "Loading Zone" transitions to the end of the sequence to
|
||||
// prevent the DMA interrupt firing many milliseconds before the sequence ends.
|
||||
uint32_t zone_inserts = MIN(LOADING_ZONE_SIZE, wrap_level - LOADING_ZONE_POSITION);
|
||||
for(uint32_t i = zone_inserts + LOADING_ZONE_POSITION; i > LOADING_ZONE_POSITION; i--) {
|
||||
PWMCluster::sorted_insert(transitions, data_size, TransitionData(wrap_level - i));
|
||||
PWMCluster::sorted_insert(looping_transitions, looping_data_size, TransitionData(wrap_level - i));
|
||||
}
|
||||
}
|
||||
|
||||
#ifdef DEBUG_MULTI_PWM
|
||||
|
|
|
|||
|
|
@ -4,6 +4,7 @@
|
|||
#include "hardware/pio.h"
|
||||
#include "hardware/dma.h"
|
||||
#include "hardware/irq.h"
|
||||
#include "common/pimoroni_common.hpp"
|
||||
#include <initializer_list>
|
||||
|
||||
namespace pimoroni {
|
||||
|
|
@ -14,11 +15,12 @@ namespace pimoroni {
|
|||
// Constants
|
||||
//--------------------------------------------------
|
||||
private:
|
||||
static const uint64_t MAX_PWM_CLUSTER_WRAP = UINT16_MAX; // UINT32_MAX works too, but seems to produce less accurate counters
|
||||
static const uint32_t LOADING_ZONE_SIZE = 3; // The number of dummy transitions to insert into the data to delay the DMA interrupt (if zero then no zone is used)
|
||||
static const uint32_t LOADING_ZONE_POSITION = 55; // The number of levels before the wrap level to insert the load zone
|
||||
// Smaller values will make the DMA interrupt trigger closer to the time the data is needed,
|
||||
// but risks stalling the PIO if the interrupt takes longer due to other processes
|
||||
static const uint64_t MAX_PWM_CLUSTER_WRAP = UINT16_MAX; // UINT32_MAX works too, but seems to produce less accurate counters
|
||||
static const uint32_t LOADING_ZONE_SIZE = 3; // The number of dummy transitions to insert into the data to delay the DMA interrupt (if zero then no zone is used)
|
||||
static const uint32_t LOADING_ZONE_POSITION = 55; // The number of levels before the wrap level to insert the load zone
|
||||
// Smaller values will make the DMA interrupt trigger closer to the time the data is needed,
|
||||
// but risks stalling the PIO if the interrupt takes longer due to other processes
|
||||
static const bool DEFAULT_USE_LOADING_ZONE = true; // Whether or not the default behaviour of PWMCluster is to use the loading zone
|
||||
public:
|
||||
static const uint BUFFER_SIZE = 64; // Set to 64, the maximum number of single rises and falls for 32 channels within a looping time period
|
||||
static const uint NUM_BUFFERS = 3;
|
||||
|
|
@ -118,6 +120,7 @@ namespace pimoroni {
|
|||
volatile uint last_written_index = 0;
|
||||
|
||||
bool initialised = false;
|
||||
bool loading_zone = true;
|
||||
|
||||
|
||||
//--------------------------------------------------
|
||||
|
|
@ -133,10 +136,13 @@ namespace pimoroni {
|
|||
// Constructors/Destructor
|
||||
//--------------------------------------------------
|
||||
public:
|
||||
PWMCluster(PIO pio, uint sm, uint pin_mask, Sequence *seq_buffer = nullptr, TransitionData *dat_buffer = nullptr);
|
||||
PWMCluster(PIO pio, uint sm, uint pin_base, uint pin_count, Sequence *seq_buffer = nullptr, TransitionData *dat_buffer = nullptr);
|
||||
PWMCluster(PIO pio, uint sm, const uint8_t *pins, uint32_t length, Sequence *seq_buffer = nullptr, TransitionData *dat_buffer = nullptr);
|
||||
PWMCluster(PIO pio, uint sm, std::initializer_list<uint8_t> pins, Sequence *seq_buffer = nullptr, TransitionData *dat_buffer = nullptr);
|
||||
PWMCluster(PIO pio, uint sm, uint pin_mask, Sequence *seq_buffer = nullptr, TransitionData *dat_buffer = nullptr, bool loading_zone = DEFAULT_USE_LOADING_ZONE);
|
||||
PWMCluster(PIO pio, uint sm, uint pin_base, uint pin_count, Sequence *seq_buffer = nullptr, TransitionData *dat_buffer = nullptr, bool loading_zone = DEFAULT_USE_LOADING_ZONE);
|
||||
PWMCluster(PIO pio, uint sm, const uint8_t *pins, uint32_t length, Sequence *seq_buffer = nullptr, TransitionData *dat_buffer = nullptr, bool loading_zone = DEFAULT_USE_LOADING_ZONE);
|
||||
PWMCluster(PIO pio, uint sm, std::initializer_list<uint8_t> pins, Sequence *seq_buffer = nullptr, TransitionData *dat_buffer = nullptr, bool loading_zone = DEFAULT_USE_LOADING_ZONE);
|
||||
|
||||
PWMCluster(PIO pio, uint sm, const pin_pair *pin_pairs, uint32_t length, Sequence *seq_buffer = nullptr, TransitionData *dat_buffer = nullptr, bool loading_zone = DEFAULT_USE_LOADING_ZONE);
|
||||
PWMCluster(PIO pio, uint sm, std::initializer_list<pin_pair> pin_pairs, Sequence *seq_buffer = nullptr, TransitionData *dat_buffer = nullptr, bool loading_zone = DEFAULT_USE_LOADING_ZONE);
|
||||
~PWMCluster();
|
||||
|
||||
private:
|
||||
|
|
@ -150,7 +156,10 @@ namespace pimoroni {
|
|||
bool init();
|
||||
|
||||
uint8_t get_chan_count() const;
|
||||
uint8_t get_chan_pair_count() const;
|
||||
uint8_t get_chan_pin(uint8_t channel) const;
|
||||
pin_pair get_chan_pin_pair(uint8_t channel_pair) const;
|
||||
static uint8_t channel_from_pair(uint8_t channel_pair);
|
||||
|
||||
uint32_t get_chan_level(uint8_t channel) const;
|
||||
void set_chan_level(uint8_t channel, uint32_t level, bool load = true);
|
||||
|
|
|
|||
|
|
@ -31,6 +31,8 @@ add_subdirectory(pico_scroll)
|
|||
add_subdirectory(pico_enc_explorer)
|
||||
add_subdirectory(pico_explorer)
|
||||
add_subdirectory(pico_pot_explorer)
|
||||
add_subdirectory(pico_explorer_encoder)
|
||||
add_subdirectory(pico_motor_shim)
|
||||
add_subdirectory(pico_rgb_keypad)
|
||||
add_subdirectory(pico_rtc_display)
|
||||
add_subdirectory(pico_tof_display)
|
||||
|
|
@ -42,3 +44,5 @@ add_subdirectory(plasma2040)
|
|||
add_subdirectory(badger2040)
|
||||
add_subdirectory(interstate75)
|
||||
add_subdirectory(servo2040)
|
||||
add_subdirectory(motor2040)
|
||||
add_subdirectory(encoder)
|
||||
|
|
|
|||
|
|
@ -0,0 +1,5 @@
|
|||
include(encoder_item_selector.cmake)
|
||||
include(encoder_read_change.cmake)
|
||||
include(encoder_read_counts.cmake)
|
||||
include(encoder_read_speed.cmake)
|
||||
include(encoder_value_dial.cmake)
|
||||
|
|
@ -0,0 +1,40 @@
|
|||
# Encoder C++ Examples <!-- omit in toc -->
|
||||
|
||||
- [Examples](#examples)
|
||||
- [Read Counts](#read-counts)
|
||||
- [Read Change](#read-change)
|
||||
- [Read Speed](#read-speed)
|
||||
- [Value Dial](#value-dial)
|
||||
- [Item Selector](#item-selector)
|
||||
|
||||
|
||||
## Examples
|
||||
|
||||
### Read Counts
|
||||
[encoder_read_counts.cpp](encoder_read_counts.cpp)
|
||||
|
||||
An example of how to read a mechanical rotary encoder.
|
||||
|
||||
|
||||
### Read Change
|
||||
[encoder_read_change.cpp](encoder_read_change.cpp)
|
||||
|
||||
An example of how to read a mechanical rotary encoder, only when a change has occurred.
|
||||
|
||||
|
||||
### Read Speed
|
||||
[encoder_read_speed.cpp](encoder_read_speed.cpp)
|
||||
|
||||
An example of how to read the speed a mechanical rotary encoder is being turned at.
|
||||
|
||||
|
||||
### Value Dial
|
||||
[encoder_value_dial.cpp](encoder_value_dial.cpp)
|
||||
|
||||
A demonstration of a rotary encoder being used to control a value.
|
||||
|
||||
|
||||
### Item Selector
|
||||
[encoder_item_selector.cpp](encoder_item_selector.cpp)
|
||||
|
||||
A demonstration of a rotary encoder being used to select items based on its physical position.
|
||||
|
|
@ -0,0 +1,12 @@
|
|||
set(OUTPUT_NAME encoder_item_selector)
|
||||
add_executable(${OUTPUT_NAME} encoder_item_selector.cpp)
|
||||
|
||||
target_link_libraries(${OUTPUT_NAME}
|
||||
pico_stdlib
|
||||
encoder
|
||||
)
|
||||
|
||||
# enable usb output
|
||||
pico_enable_stdio_usb(${OUTPUT_NAME} 1)
|
||||
|
||||
pico_add_extra_outputs(${OUTPUT_NAME})
|
||||
|
|
@ -0,0 +1,54 @@
|
|||
#include <cstdio>
|
||||
#include "pico/stdlib.h"
|
||||
|
||||
#include "encoder.hpp"
|
||||
|
||||
/*
|
||||
A demonstration of a rotary encoder being used to
|
||||
select items based on its physical position.
|
||||
|
||||
This requires that the encoder is positioned in the same
|
||||
direction at the start of every program run (e.g. upwards).
|
||||
"""
|
||||
*/
|
||||
|
||||
using namespace encoder;
|
||||
|
||||
// Create an encoder on the 3 ADC pins, using PIO 0 and State Machine 0
|
||||
const uint PIN_A = 26; // The A channel pin
|
||||
const uint PIN_B = 28; // The B channel pin
|
||||
const uint PIN_C = 27; // The common pin
|
||||
Encoder enc(pio0, 0, {PIN_A, PIN_B}, PIN_C);
|
||||
|
||||
|
||||
// An array of items, up to the encoder's counts_per_rev
|
||||
const char* ITEMS[] = {"Red", "Orange", "Yellow", "Green", "Blue", "Indigo", "Violet", "Black", "White"};
|
||||
const int16_t NUM_ITEMS = count_of(ITEMS);
|
||||
|
||||
int16_t last_step = -1;
|
||||
|
||||
|
||||
int main() {
|
||||
stdio_init_all();
|
||||
|
||||
// Sleep 8 seconds to give enough time to connect up a terminal
|
||||
sleep_ms(8000);
|
||||
|
||||
// Uncomment the below line to reverse the counting direction
|
||||
// enc.direction(REVERSED_DIR);
|
||||
|
||||
// Initialise the encoder
|
||||
enc.init();
|
||||
|
||||
// Loop forever
|
||||
while(true) {
|
||||
int16_t step = enc.step();
|
||||
if(step != last_step) {
|
||||
if(step < NUM_ITEMS)
|
||||
printf("%d/%d: %s\n", step, (int)enc.counts_per_rev() - 1, ITEMS[step]);
|
||||
else
|
||||
printf("%d/%d: %s\n", step, (int)enc.counts_per_rev() - 1, "Undefined");
|
||||
last_step = step;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,12 @@
|
|||
set(OUTPUT_NAME encoder_read_change)
|
||||
add_executable(${OUTPUT_NAME} encoder_read_change.cpp)
|
||||
|
||||
target_link_libraries(${OUTPUT_NAME}
|
||||
pico_stdlib
|
||||
encoder
|
||||
)
|
||||
|
||||
# enable usb output
|
||||
pico_enable_stdio_usb(${OUTPUT_NAME} 1)
|
||||
|
||||
pico_add_extra_outputs(${OUTPUT_NAME})
|
||||
|
|
@ -0,0 +1,45 @@
|
|||
#include <cstdio>
|
||||
#include "pico/stdlib.h"
|
||||
|
||||
#include "encoder.hpp"
|
||||
|
||||
/*
|
||||
An example of how to read a mechanical rotary encoder, only when a change has occurred.
|
||||
*/
|
||||
|
||||
using namespace encoder;
|
||||
|
||||
// Create an encoder on the 3 ADC pins, using PIO 0 and State Machine 0
|
||||
const uint PIN_A = 26; // The A channel pin
|
||||
const uint PIN_B = 28; // The B channel pin
|
||||
const uint PIN_C = 27; // The common pin
|
||||
Encoder enc(pio0, 0, {PIN_A, PIN_B}, PIN_C);
|
||||
|
||||
|
||||
int main() {
|
||||
stdio_init_all();
|
||||
|
||||
// Sleep 8 seconds to give enough time to connect up a terminal
|
||||
sleep_ms(8000);
|
||||
|
||||
// Uncomment the below line to reverse the counting direction
|
||||
// enc.direction(REVERSED_DIR);
|
||||
|
||||
// Initialise the encoder
|
||||
enc.init();
|
||||
|
||||
// Print out the initial count, step, and turn (they should all be zero)
|
||||
printf("Count = %ld, ", enc.count());
|
||||
printf("Step = %d, ", enc.step());
|
||||
printf("Turn = %d\n", enc.turn());
|
||||
|
||||
// Loop forever
|
||||
while(true) {
|
||||
if(enc.delta() != 0) {
|
||||
// Print out the new count, step, and turn
|
||||
printf("Count = %ld, ", enc.count());
|
||||
printf("Step = %d, ", enc.step());
|
||||
printf("Turn = %d\n", enc.turn());
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,12 @@
|
|||
set(OUTPUT_NAME encoder_read_counts)
|
||||
add_executable(${OUTPUT_NAME} encoder_read_counts.cpp)
|
||||
|
||||
target_link_libraries(${OUTPUT_NAME}
|
||||
pico_stdlib
|
||||
encoder
|
||||
)
|
||||
|
||||
# enable usb output
|
||||
pico_enable_stdio_usb(${OUTPUT_NAME} 1)
|
||||
|
||||
pico_add_extra_outputs(${OUTPUT_NAME})
|
||||
|
|
@ -0,0 +1,38 @@
|
|||
#include <cstdio>
|
||||
#include "pico/stdlib.h"
|
||||
|
||||
#include "encoder.hpp"
|
||||
|
||||
/*
|
||||
An example of how to read a mechanical rotary encoder.
|
||||
*/
|
||||
|
||||
using namespace encoder;
|
||||
|
||||
// Create an encoder on the 3 ADC pins, using PIO 0 and State Machine 0
|
||||
const uint PIN_A = 26; // The A channel pin
|
||||
const uint PIN_B = 28; // The B channel pin
|
||||
const uint PIN_C = 27; // The common pin
|
||||
Encoder enc(pio0, 0, {PIN_A, PIN_B}, PIN_C);
|
||||
|
||||
|
||||
int main() {
|
||||
stdio_init_all();
|
||||
|
||||
// Uncomment the below line to reverse the counting direction
|
||||
// enc.direction(REVERSED_DIR);
|
||||
|
||||
// Initialise the encoder
|
||||
enc.init();
|
||||
|
||||
// Loop forever
|
||||
while(true) {
|
||||
// Print out the count, delta, step, and turn
|
||||
printf("Count = %ld, ", enc.count());
|
||||
printf("Delta = %ld, ", enc.delta());
|
||||
printf("Step = %d, ", enc.step());
|
||||
printf("Turn = %d\n", enc.turn());
|
||||
|
||||
sleep_ms(100);
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,12 @@
|
|||
set(OUTPUT_NAME encoder_read_speed)
|
||||
add_executable(${OUTPUT_NAME} encoder_read_speed.cpp)
|
||||
|
||||
target_link_libraries(${OUTPUT_NAME}
|
||||
pico_stdlib
|
||||
encoder
|
||||
)
|
||||
|
||||
# enable usb output
|
||||
pico_enable_stdio_usb(${OUTPUT_NAME} 1)
|
||||
|
||||
pico_add_extra_outputs(${OUTPUT_NAME})
|
||||
|
|
@ -0,0 +1,39 @@
|
|||
#include <cstdio>
|
||||
#include "pico/stdlib.h"
|
||||
|
||||
#include "encoder.hpp"
|
||||
|
||||
/*
|
||||
An example of how to read the speed a mechanical rotary encoder is being turned at.
|
||||
*/
|
||||
|
||||
using namespace encoder;
|
||||
|
||||
// Create an encoder on the 3 ADC pins, using PIO 0 and State Machine 0
|
||||
const uint PIN_A = 26; // The A channel pin
|
||||
const uint PIN_B = 28; // The B channel pin
|
||||
const uint PIN_C = 27; // The common pin
|
||||
Encoder enc(pio0, 0, {PIN_A, PIN_B}, PIN_C);
|
||||
|
||||
|
||||
int main() {
|
||||
stdio_init_all();
|
||||
|
||||
// Uncomment the below line to reverse the counting direction
|
||||
// enc.direction(REVERSED_DIR);
|
||||
|
||||
// Initialise the encoder
|
||||
enc.init();
|
||||
|
||||
// Loop forever
|
||||
while(true) {
|
||||
Encoder::Capture capture = enc.capture();
|
||||
|
||||
printf("Count = %ld, ", capture.count());
|
||||
printf("Angle = %f, ", capture.degrees());
|
||||
printf("Freq = %f, ", capture.frequency());
|
||||
printf("Speed = %f\n", capture.degrees_per_second());
|
||||
|
||||
sleep_ms(100);
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,12 @@
|
|||
set(OUTPUT_NAME encoder_value_dial)
|
||||
add_executable(${OUTPUT_NAME} encoder_value_dial.cpp)
|
||||
|
||||
target_link_libraries(${OUTPUT_NAME}
|
||||
pico_stdlib
|
||||
encoder
|
||||
)
|
||||
|
||||
# enable usb output
|
||||
pico_enable_stdio_usb(${OUTPUT_NAME} 1)
|
||||
|
||||
pico_add_extra_outputs(${OUTPUT_NAME})
|
||||
|
|
@ -0,0 +1,54 @@
|
|||
#include <cstdio>
|
||||
#include "pico/stdlib.h"
|
||||
|
||||
#include "encoder.hpp"
|
||||
|
||||
/*
|
||||
A demonstration of a rotary encoder being used to control a value.
|
||||
*/
|
||||
|
||||
using namespace encoder;
|
||||
|
||||
// Create an encoder on the 3 ADC pins, using PIO 0 and State Machine 0
|
||||
const uint PIN_A = 26; // The A channel pin
|
||||
const uint PIN_B = 28; // The B channel pin
|
||||
const uint PIN_C = 27; // The common pin
|
||||
Encoder enc(pio0, 0, {PIN_A, PIN_B}, PIN_C);
|
||||
|
||||
|
||||
// The min and max value
|
||||
const int MIN_VALUE = 0;
|
||||
const int MAX_VALUE = 100;
|
||||
|
||||
int value = 50;
|
||||
|
||||
|
||||
int main() {
|
||||
stdio_init_all();
|
||||
|
||||
// Sleep 8 seconds to give enough time to connect up a terminal
|
||||
sleep_ms(8000);
|
||||
|
||||
// Uncomment the below line to reverse the counting direction
|
||||
// enc.direction(REVERSED_DIR);
|
||||
|
||||
// Initialise the encoder
|
||||
enc.init();
|
||||
|
||||
// Print out the initial value
|
||||
printf("Value = %d\n", value);
|
||||
|
||||
// Loop forever
|
||||
while(true) {
|
||||
int32_t delta = enc.delta();
|
||||
if(delta != 0) {
|
||||
if(delta > 0)
|
||||
value = MIN(value + 1, MAX_VALUE);
|
||||
else
|
||||
value = MAX(value - 1, MIN_VALUE);
|
||||
|
||||
// Print out the new value
|
||||
printf("Value = %d\n", value);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,17 @@
|
|||
include(motor2040_led_rainbow.cmake)
|
||||
include(motor2040_motor_cluster.cmake)
|
||||
include(motor2040_motor_profiler.cmake)
|
||||
include(motor2040_motor_wave.cmake)
|
||||
include(motor2040_multiple_motors.cmake)
|
||||
include(motor2040_position_control.cmake)
|
||||
include(motor2040_position_tuning.cmake)
|
||||
include(motor2040_position_on_velocity_control.cmake)
|
||||
include(motor2040_position_on_velocity_tuning.cmake)
|
||||
include(motor2040_quad_position_wave.cmake)
|
||||
include(motor2040_quad_velocity_sequence.cmake)
|
||||
include(motor2040_reactive_encoder.cmake)
|
||||
include(motor2040_read_encoders.cmake)
|
||||
include(motor2040_read_sensors.cmake)
|
||||
include(motor2040_single_motor.cmake)
|
||||
include(motor2040_velocity_control.cmake)
|
||||
include(motor2040_velocity_tuning.cmake)
|
||||
|
|
@ -0,0 +1,135 @@
|
|||
# Motor 2040 C++ Examples <!-- omit in toc -->
|
||||
|
||||
- [Motor Examples](#motor-examples)
|
||||
- [Single Motor](#single-motor)
|
||||
- [Multiple Motors](#multiple-motors)
|
||||
- [Motor Cluster](#motor-cluster)
|
||||
- [Motor Wave](#motor-wave)
|
||||
- [Function Examples](#function-examples)
|
||||
- [Read Sensors](#read-sensors)
|
||||
- [Read Encoders](#read-encoders)
|
||||
- [Motor Profiler](#motor-profiler)
|
||||
- [LED Rainbow](#led-rainbow)
|
||||
- [Turn Off LEDs](#turn-off-leds)
|
||||
- [Control Examples](#control-examples)
|
||||
- [Position Control](#position-control)
|
||||
- [Velocity Control](#velocity-control)
|
||||
- [Position on Velocity Control](#position-on-velocity-control)
|
||||
- [Reactive Encoder](#reactive-encoder)
|
||||
- [Quad Position Wave](#quad-position-wave)
|
||||
- [Quad Velocity Sequence](#quad-velocity-sequence)
|
||||
- [Tuning Examples](#tuning-examples)
|
||||
- [Position Tuning](#position-tuning)
|
||||
- [Velocity Tuning](#velocity-tuning)
|
||||
- [Position on Velocity Tuning](#position-on-velocity-tuning)
|
||||
|
||||
|
||||
## Motor Examples
|
||||
|
||||
### Single Motor
|
||||
[motor2040_single_motor.cpp](motor2040_single_motor.cpp)
|
||||
|
||||
Demonstrates how to create a Motor object and control it.
|
||||
|
||||
|
||||
### Multiple Motors
|
||||
[motor2040_multiple_motors.cpp](motor2040_multiple_motors.cpp)
|
||||
|
||||
Demonstrates how to create multiple Motor objects and control them together.
|
||||
|
||||
|
||||
### Motor Cluster
|
||||
[motor2040_motor_cluster.cpp](motor2040_motor_cluster.cpp)
|
||||
|
||||
Demonstrates how to create a MotorCluster object to control multiple motors at once.
|
||||
|
||||
|
||||
### Motor Wave
|
||||
[motor2040_motor_wave.cpp](motor2040_motor_wave.cpp)
|
||||
|
||||
An example of applying a wave pattern to a group of motors and the LEDs.
|
||||
|
||||
|
||||
## Function Examples
|
||||
|
||||
### Read Sensors
|
||||
[motor2040_read_sensors.cpp](motor2040_read_sensors.cpp)
|
||||
|
||||
Shows how to initialise and read the 2 external and 6 internal sensors of Motor 2040.
|
||||
|
||||
|
||||
### Read Encoders
|
||||
[motor2040_read_encoders.cpp](motor2040_read_encoders.cpp)
|
||||
|
||||
Demonstrates how to read the angles of Motor 2040's four encoders.
|
||||
|
||||
|
||||
### Motor Profiler
|
||||
[motor2040_motor_profiler.cpp](motor2040_motor_profiler.cpp)
|
||||
|
||||
A program that profiles the speed of a motor across its PWM
|
||||
duty cycle range using the attached encoder for feedback.
|
||||
|
||||
|
||||
### LED Rainbow
|
||||
[motor2040_led_rainbow.cpp](motor2040_led_rainbow.cpp)
|
||||
|
||||
Displays a rotating rainbow pattern on the Motor 2040's onboard LED.
|
||||
|
||||
|
||||
## Control Examples
|
||||
|
||||
### Position Control
|
||||
[motor2040_position_control.cpp](motor2040_position_control.cpp)
|
||||
|
||||
An example of how to move a motor smoothly between random positions, with the help of it's attached encoder and PID control.
|
||||
|
||||
|
||||
### Velocity Control
|
||||
[motor2040_velocity_control.cpp](motor2040_velocity_control.cpp)
|
||||
|
||||
An example of how to drive a motor smoothly between random speeds, with the help of it's attached encoder and PID control.
|
||||
|
||||
|
||||
### Position on Velocity Control
|
||||
[motor2040_position_on_velocity_control.cpp](motor2040_position_on_velocity_control.cpp)
|
||||
|
||||
An example of how to move a motor smoothly between random positions, with velocity limits, with the help of it's attached encoder and PID control.
|
||||
|
||||
|
||||
### Reactive Encoder
|
||||
[motor2040_reactive_encoder.cpp](motor2040_reactive_encoder.cpp)
|
||||
|
||||
A demonstration of how a motor with an encoder can be used as a programmable rotary encoder for user input, with force-feedback for arbitrary detents and end stops.
|
||||
|
||||
|
||||
### Quad Position Wave
|
||||
[motor2040_quad_position_wave.cpp](motor2040_quad_position_wave.cpp)
|
||||
|
||||
A demonstration of driving all four of Motor 2040's motor outputs between positions, with the help of their attached encoders and PID control.
|
||||
|
||||
|
||||
### Quad Velocity Sequence
|
||||
[motor2040_quad_velocity_sequence.cpp](motor2040_quad_velocity_sequence.cpp)
|
||||
|
||||
A demonstration of driving all four of Motor 2040's motor outputs through a sequence of velocities, with the help of their attached encoders and PID control.
|
||||
|
||||
|
||||
## Tuning Examples
|
||||
|
||||
### Position Tuning
|
||||
[motor2040_position_tuning.cpp](motor2040_position_tuning.cpp)
|
||||
|
||||
A program to aid in the discovery and tuning of motor PID values for position control. It does this by commanding the motor to move repeatedly between two setpoint angles and plots the measured response.
|
||||
|
||||
|
||||
### Velocity Tuning
|
||||
[motor2040_velocity_tuning.cpp](motor2040_velocity_tuning.cpp)
|
||||
|
||||
A program to aid in the discovery and tuning of motor PID values for velocity control. It does this by commanding the motor to drive repeatedly between two setpoint speeds and plots the measured response.
|
||||
|
||||
|
||||
### Position on Velocity Tuning
|
||||
[motor2040_position_on_velocity_tuning.cpp](motor2040_position_on_velocity_tuning.cpp)
|
||||
|
||||
A program to aid in the discovery and tuning of motor PID values for position on velocity control. It does this by commanding the motor to move repeatedly between two setpoint angles and plots the measured response.
|
||||
|
|
@ -0,0 +1,13 @@
|
|||
set(OUTPUT_NAME motor2040_led_rainbow)
|
||||
add_executable(${OUTPUT_NAME} motor2040_led_rainbow.cpp)
|
||||
|
||||
target_link_libraries(${OUTPUT_NAME}
|
||||
pico_stdlib
|
||||
motor2040
|
||||
button
|
||||
)
|
||||
|
||||
# enable usb output
|
||||
pico_enable_stdio_usb(${OUTPUT_NAME} 1)
|
||||
|
||||
pico_add_extra_outputs(${OUTPUT_NAME})
|
||||
|
|
@ -0,0 +1,56 @@
|
|||
#include "pico/stdlib.h"
|
||||
|
||||
#include "motor2040.hpp"
|
||||
#include "button.hpp"
|
||||
|
||||
/*
|
||||
Displays a rotating rainbow pattern on the Motor 2040's onboard LED.
|
||||
|
||||
Press "Boot" to exit the program.
|
||||
*/
|
||||
|
||||
using namespace plasma;
|
||||
using namespace motor;
|
||||
|
||||
// The speed that the LED will cycle at
|
||||
const uint SPEED = 5;
|
||||
|
||||
// The brightness of the LED
|
||||
constexpr float BRIGHTNESS = 0.4f;
|
||||
|
||||
// How many times the LED will be updated per second
|
||||
const uint UPDATES = 50;
|
||||
|
||||
|
||||
// Create the LED, using PIO 1 and State Machine 0
|
||||
WS2812 led(motor2040::NUM_LEDS, pio1, 0, motor2040::LED_DATA);
|
||||
|
||||
// Create the user button
|
||||
Button user_sw(motor2040::USER_SW);
|
||||
|
||||
|
||||
int main() {
|
||||
stdio_init_all();
|
||||
|
||||
// Start updating the LED
|
||||
led.start();
|
||||
|
||||
float hue = 0.0f;
|
||||
|
||||
// Make rainbows until the user button is pressed
|
||||
while(!user_sw.raw()) {
|
||||
|
||||
hue += (float)SPEED / 1000.0f;
|
||||
|
||||
// Update the LED
|
||||
led.set_hsv(0, hue, 1.0f, BRIGHTNESS);
|
||||
|
||||
sleep_ms(1000 / UPDATES);
|
||||
}
|
||||
|
||||
// Turn off the LED
|
||||
led.clear();
|
||||
|
||||
// Sleep a short time so the clear takes effect
|
||||
sleep_ms(100);
|
||||
}
|
||||
|
|
@ -0,0 +1,13 @@
|
|||
set(OUTPUT_NAME motor2040_motor_cluster)
|
||||
add_executable(${OUTPUT_NAME} motor2040_motor_cluster.cpp)
|
||||
|
||||
target_link_libraries(${OUTPUT_NAME}
|
||||
pico_stdlib
|
||||
motor2040
|
||||
button
|
||||
)
|
||||
|
||||
# enable usb output
|
||||
pico_enable_stdio_usb(${OUTPUT_NAME} 1)
|
||||
|
||||
pico_add_extra_outputs(${OUTPUT_NAME})
|
||||
|
|
@ -0,0 +1,82 @@
|
|||
#include "pico/stdlib.h"
|
||||
|
||||
#include "motor2040.hpp"
|
||||
|
||||
/*
|
||||
Demonstrates how to create multiple Motor objects and control them together.
|
||||
*/
|
||||
|
||||
using namespace motor;
|
||||
|
||||
// How many sweeps of the motors to perform
|
||||
const uint SWEEPS = 2;
|
||||
|
||||
// The number of discrete sweep steps
|
||||
const uint STEPS = 10;
|
||||
|
||||
// The time in milliseconds between each step of the sequence
|
||||
const uint STEPS_INTERVAL_MS = 500;
|
||||
|
||||
// How far from zero to drive the motors when sweeping
|
||||
constexpr float SPEED_EXTENT = 1.0f;
|
||||
|
||||
// Create a motor cluster, using PIO 0 and State Machine 0
|
||||
const pin_pair motor_pins[] = {motor2040::MOTOR_A, motor2040::MOTOR_B,
|
||||
motor2040::MOTOR_C, motor2040::MOTOR_D};
|
||||
const uint NUM_MOTORS = count_of(motor_pins);
|
||||
MotorCluster motors = MotorCluster(pio0, 0, motor_pins, NUM_MOTORS);
|
||||
|
||||
|
||||
int main() {
|
||||
stdio_init_all();
|
||||
|
||||
// Initialise the motor cluster
|
||||
motors.init();
|
||||
|
||||
// Enable all motors
|
||||
motors.enable_all();
|
||||
sleep_ms(2000);
|
||||
|
||||
// Drive at full positive
|
||||
motors.all_full_positive();
|
||||
sleep_ms(2000);
|
||||
|
||||
// Stop all moving
|
||||
motors.stop_all();
|
||||
sleep_ms(2000);
|
||||
|
||||
// Drive at full negative
|
||||
motors.all_full_negative();
|
||||
sleep_ms(2000);
|
||||
|
||||
// Coast all to a gradual stop
|
||||
motors.coast_all();
|
||||
sleep_ms(2000);
|
||||
|
||||
// Do a sine speed sweep
|
||||
for(auto j = 0u; j < SWEEPS; j++) {
|
||||
for(auto i = 0u; i < 360; i++) {
|
||||
float speed = sin(((float)i * (float)M_PI) / 180.0f) * SPEED_EXTENT;
|
||||
motors.all_to_speed(speed);
|
||||
sleep_ms(20);
|
||||
}
|
||||
}
|
||||
|
||||
// Do a stepped speed sweep
|
||||
for(auto j = 0u; j < SWEEPS; j++) {
|
||||
for(auto i = 0u; i < STEPS; i++) {
|
||||
motors.all_to_percent(i, 0, STEPS, 0.0 - SPEED_EXTENT, SPEED_EXTENT);
|
||||
sleep_ms(STEPS_INTERVAL_MS);
|
||||
}
|
||||
for(auto i = 0u; i < STEPS; i++) {
|
||||
motors.all_to_percent(i, STEPS, 0, 0.0 - SPEED_EXTENT, SPEED_EXTENT);
|
||||
sleep_ms(STEPS_INTERVAL_MS);
|
||||
}
|
||||
}
|
||||
|
||||
// Disable the motors
|
||||
motors.disable_all();
|
||||
|
||||
// Sleep a short time so the disable takes effect
|
||||
sleep_ms(100);
|
||||
}
|
||||
|
|
@ -0,0 +1,12 @@
|
|||
set(OUTPUT_NAME motor2040_motor_profiler)
|
||||
add_executable(${OUTPUT_NAME} motor2040_motor_profiler.cpp)
|
||||
|
||||
target_link_libraries(${OUTPUT_NAME}
|
||||
pico_stdlib
|
||||
motor2040
|
||||
)
|
||||
|
||||
# enable usb output
|
||||
pico_enable_stdio_usb(${OUTPUT_NAME} 1)
|
||||
|
||||
pico_add_extra_outputs(${OUTPUT_NAME})
|
||||
|
|
@ -0,0 +1,127 @@
|
|||
#include <cstdio>
|
||||
#include "pico/stdlib.h"
|
||||
|
||||
#include "motor2040.hpp"
|
||||
|
||||
/*
|
||||
A program that profiles the speed of a motor across its PWM
|
||||
duty cycle range using the attached encoder for feedback.
|
||||
*/
|
||||
|
||||
using namespace motor;
|
||||
using namespace encoder;
|
||||
|
||||
// The pins of the motor being profiled
|
||||
const pin_pair MOTOR_PINS = motor2040::MOTOR_A;
|
||||
|
||||
// The pins of the encoder attached to the profiled motor
|
||||
const pin_pair ENCODER_PINS = motor2040::ENCODER_A;
|
||||
|
||||
// The gear ratio of the motor
|
||||
constexpr float GEAR_RATIO = 50;
|
||||
|
||||
// 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 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();
|
||||
|
||||
// Give some time to connect up a serial terminal
|
||||
sleep_ms(10000);
|
||||
|
||||
// Initialise the motor and enable it
|
||||
m.init();
|
||||
m.enable();
|
||||
|
||||
// Initialise the encoder
|
||||
enc.init();
|
||||
|
||||
printf("Profiler Starting...\n");
|
||||
|
||||
// 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);
|
||||
}
|
||||
|
||||
// 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);
|
||||
}
|
||||
|
||||
// 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();
|
||||
}
|
||||
|
|
@ -0,0 +1,13 @@
|
|||
set(OUTPUT_NAME motor2040_motor_wave)
|
||||
add_executable(${OUTPUT_NAME} motor2040_motor_wave.cpp)
|
||||
|
||||
target_link_libraries(${OUTPUT_NAME}
|
||||
pico_stdlib
|
||||
motor2040
|
||||
button
|
||||
)
|
||||
|
||||
# enable usb output
|
||||
pico_enable_stdio_usb(${OUTPUT_NAME} 1)
|
||||
|
||||
pico_add_extra_outputs(${OUTPUT_NAME})
|
||||
|
|
@ -0,0 +1,82 @@
|
|||
#include "pico/stdlib.h"
|
||||
|
||||
#include "motor2040.hpp"
|
||||
#include "button.hpp"
|
||||
|
||||
/*
|
||||
An example of applying a wave pattern to a group of motors and the LED.
|
||||
|
||||
Press "Boot" to exit the program.
|
||||
*/
|
||||
|
||||
using namespace plasma;
|
||||
using namespace motor;
|
||||
|
||||
// The speed that the LEDs and motors will cycle at
|
||||
const uint SPEED = 5;
|
||||
|
||||
// The brightness of the LEDs
|
||||
constexpr float BRIGHTNESS = 0.4;
|
||||
|
||||
// How many times to update LEDs and motors per second
|
||||
const uint UPDATES = 50;
|
||||
|
||||
// How far from zero to drive the motors
|
||||
constexpr float SPEED_EXTENT = 1.0f;
|
||||
|
||||
|
||||
// Create an array of motor pointers
|
||||
const pin_pair motor_pins[] = {motor2040::MOTOR_A, motor2040::MOTOR_B,
|
||||
motor2040::MOTOR_C, motor2040::MOTOR_D};
|
||||
const uint NUM_MOTORS = count_of(motor_pins);
|
||||
Motor *motors[NUM_MOTORS];
|
||||
|
||||
// Create the LED, using PIO 1 and State Machine 0
|
||||
WS2812 led(motor2040::NUM_LEDS, pio1, 0, motor2040::LED_DATA);
|
||||
|
||||
// Create the user button
|
||||
Button user_sw(motor2040::USER_SW);
|
||||
|
||||
|
||||
int main() {
|
||||
stdio_init_all();
|
||||
|
||||
// Fill the array of motors, and initialise them. Up to 8 motors can be created
|
||||
for(auto m = 0u; m < NUM_MOTORS; m++) {
|
||||
motors[m] = new Motor(motor_pins[m]);
|
||||
motors[m]->init();
|
||||
}
|
||||
|
||||
// Start updating the LED
|
||||
led.start();
|
||||
|
||||
float offset = 0.0f;
|
||||
|
||||
// Make rainbows until the user button is pressed
|
||||
while(!user_sw.raw()) {
|
||||
|
||||
offset += (float)SPEED / 1000.0f;
|
||||
|
||||
// Update the LED
|
||||
led.set_hsv(0, offset / 2.0f, 1.0f, BRIGHTNESS);
|
||||
|
||||
// Update all the motors
|
||||
for(auto m = 0u; m < NUM_MOTORS; m++) {
|
||||
float angle = (((float)m / (float)NUM_MOTORS) + offset) * (float)M_PI;
|
||||
motors[m]->speed(sin(angle) * SPEED_EXTENT);
|
||||
}
|
||||
|
||||
sleep_ms(1000 / UPDATES);
|
||||
}
|
||||
|
||||
// Stop all the motors
|
||||
for(auto m = 0u; m < NUM_MOTORS; m++) {
|
||||
motors[m]->disable();
|
||||
}
|
||||
|
||||
// Turn off the LED
|
||||
led.clear();
|
||||
|
||||
// Sleep a short time so the clear takes effect
|
||||
sleep_ms(100);
|
||||
}
|
||||
|
|
@ -0,0 +1,12 @@
|
|||
set(OUTPUT_NAME motor2040_multiple_motors)
|
||||
add_executable(${OUTPUT_NAME} motor2040_multiple_motors.cpp)
|
||||
|
||||
target_link_libraries(${OUTPUT_NAME}
|
||||
pico_stdlib
|
||||
motor2040
|
||||
)
|
||||
|
||||
# enable usb output
|
||||
pico_enable_stdio_usb(${OUTPUT_NAME} 1)
|
||||
|
||||
pico_add_extra_outputs(${OUTPUT_NAME})
|
||||
|
|
@ -0,0 +1,100 @@
|
|||
#include "pico/stdlib.h"
|
||||
|
||||
#include "motor2040.hpp"
|
||||
|
||||
/*
|
||||
Demonstrates how to create multiple Motor objects and control them together.
|
||||
*/
|
||||
|
||||
using namespace motor;
|
||||
|
||||
// How many sweeps of the motors to perform
|
||||
const uint SWEEPS = 2;
|
||||
|
||||
// The number of discrete sweep steps
|
||||
const uint STEPS = 10;
|
||||
|
||||
// The time in milliseconds between each step of the sequence
|
||||
const uint STEPS_INTERVAL_MS = 500;
|
||||
|
||||
// How far from zero to drive the motors when sweeping
|
||||
constexpr float SPEED_EXTENT = 1.0f;
|
||||
|
||||
// Create an array of motor pointers
|
||||
const pin_pair motor_pins[] = {motor2040::MOTOR_A, motor2040::MOTOR_B,
|
||||
motor2040::MOTOR_C, motor2040::MOTOR_D};
|
||||
const uint NUM_MOTORS = count_of(motor_pins);
|
||||
Motor *motors[NUM_MOTORS];
|
||||
|
||||
|
||||
int main() {
|
||||
stdio_init_all();
|
||||
|
||||
// Fill the array of motors, and initialise them. Up to 8 motors can be created
|
||||
for(auto m = 0u; m < NUM_MOTORS; m++) {
|
||||
motors[m] = new Motor(motor_pins[m]);
|
||||
motors[m]->init();
|
||||
}
|
||||
|
||||
// Enable all motors
|
||||
for(auto m = 0u; m < NUM_MOTORS; m++) {
|
||||
motors[m]->enable();
|
||||
}
|
||||
sleep_ms(2000);
|
||||
|
||||
// Drive at full positive
|
||||
for(auto m = 0u; m < NUM_MOTORS; m++) {
|
||||
motors[m]->full_positive();
|
||||
}
|
||||
sleep_ms(2000);
|
||||
|
||||
// Stop all moving
|
||||
for(auto m = 0u; m < NUM_MOTORS; m++) {
|
||||
motors[m]->stop();
|
||||
}
|
||||
sleep_ms(2000);
|
||||
|
||||
// Drive at full negative
|
||||
for(auto m = 0u; m < NUM_MOTORS; m++) {
|
||||
motors[m]->full_negative();
|
||||
}
|
||||
sleep_ms(2000);
|
||||
|
||||
// Coast all to a gradual stop
|
||||
for(auto m = 0u; m < NUM_MOTORS; m++) {
|
||||
motors[m]->coast();
|
||||
}
|
||||
sleep_ms(2000);
|
||||
|
||||
// Do a sine speed sweep
|
||||
for(auto j = 0u; j < SWEEPS; j++) {
|
||||
for(auto i = 0u; i < 360; i++) {
|
||||
float speed = sin(((float)i * (float)M_PI) / 180.0f) * SPEED_EXTENT;
|
||||
for(auto m = 0u; m < NUM_MOTORS; m++) {
|
||||
motors[m]->speed(speed);
|
||||
}
|
||||
sleep_ms(20);
|
||||
}
|
||||
}
|
||||
|
||||
// Do a stepped speed sweep
|
||||
for(auto j = 0u; j < SWEEPS; j++) {
|
||||
for(auto i = 0u; i < STEPS; i++) {
|
||||
for(auto m = 0u; m < NUM_MOTORS; m++) {
|
||||
motors[m]->to_percent(i, 0, STEPS, 0.0 - SPEED_EXTENT, SPEED_EXTENT);
|
||||
}
|
||||
sleep_ms(STEPS_INTERVAL_MS);
|
||||
}
|
||||
for(auto i = 0u; i < STEPS; i++) {
|
||||
for(auto m = 0u; m < NUM_MOTORS; m++) {
|
||||
motors[m]->to_percent(i, STEPS, 0, 0.0 - SPEED_EXTENT, SPEED_EXTENT);
|
||||
}
|
||||
sleep_ms(STEPS_INTERVAL_MS);
|
||||
}
|
||||
}
|
||||
|
||||
// Disable the motors
|
||||
for(auto m = 0u; m < NUM_MOTORS; m++) {
|
||||
motors[m]->disable();
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,14 @@
|
|||
set(OUTPUT_NAME motor2040_position_control)
|
||||
add_executable(${OUTPUT_NAME} motor2040_position_control.cpp)
|
||||
|
||||
target_link_libraries(${OUTPUT_NAME}
|
||||
pico_stdlib
|
||||
motor2040
|
||||
button
|
||||
pid
|
||||
)
|
||||
|
||||
# enable usb output
|
||||
pico_enable_stdio_usb(${OUTPUT_NAME} 1)
|
||||
|
||||
pico_add_extra_outputs(${OUTPUT_NAME})
|
||||
|
|
@ -0,0 +1,152 @@
|
|||
#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,
|
||||
with the help of it's attached encoder and PID control.
|
||||
|
||||
Press "Boot" to exit the program.
|
||||
*/
|
||||
|
||||
using namespace motor;
|
||||
using namespace encoder;
|
||||
|
||||
// The pins of the motor being profiled
|
||||
const pin_pair MOTOR_PINS = motor2040::MOTOR_A;
|
||||
|
||||
// The pins of the encoder attached to the profiled motor
|
||||
const pin_pair ENCODER_PINS = motor2040::ENCODER_A;
|
||||
|
||||
// The gear ratio of the motor
|
||||
constexpr float GEAR_RATIO = 50.0f;
|
||||
|
||||
// The counts per revolution of the motor's output shaft
|
||||
constexpr float COUNTS_PER_REV = MMME_CPR * GEAR_RATIO;
|
||||
|
||||
// The direction to spin the motor in. NORMAL_DIR (0), REVERSED_DIR (1)
|
||||
const Direction DIRECTION = NORMAL_DIR;
|
||||
|
||||
// The scaling to apply to the motor's speed to match its real-world speed
|
||||
constexpr 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;
|
||||
|
||||
|
||||
// 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 and encoder
|
||||
m.init();
|
||||
enc.init();
|
||||
|
||||
// Enable the motor
|
||||
m.enable();
|
||||
|
||||
|
||||
uint update = 0;
|
||||
uint print_count = 0;
|
||||
|
||||
// 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;
|
||||
|
||||
// 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;
|
||||
}
|
||||
|
||||
// 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);
|
||||
}
|
||||
|
||||
// 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
|
||||
m.disable();
|
||||
}
|
||||
|
|
@ -0,0 +1,14 @@
|
|||
set(OUTPUT_NAME motor2040_position_on_velocity_control)
|
||||
add_executable(${OUTPUT_NAME} motor2040_position_on_velocity_control.cpp)
|
||||
|
||||
target_link_libraries(${OUTPUT_NAME}
|
||||
pico_stdlib
|
||||
motor2040
|
||||
button
|
||||
pid
|
||||
)
|
||||
|
||||
# enable usb output
|
||||
pico_enable_stdio_usb(${OUTPUT_NAME} 1)
|
||||
|
||||
pico_add_extra_outputs(${OUTPUT_NAME})
|
||||
|
|
@ -0,0 +1,169 @@
|
|||
#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,
|
||||
with velocity limits, with the help of it's attached encoder and PID control.
|
||||
|
||||
Press "Boot" to exit the program.
|
||||
*/
|
||||
|
||||
using namespace motor;
|
||||
using namespace encoder;
|
||||
|
||||
// The pins of the motor being profiled
|
||||
const pin_pair MOTOR_PINS = motor2040::MOTOR_A;
|
||||
|
||||
// The pins of the encoder attached to the profiled motor
|
||||
const pin_pair ENCODER_PINS = motor2040::ENCODER_A;
|
||||
|
||||
// The gear ratio of the motor
|
||||
constexpr float GEAR_RATIO = 50.0f;
|
||||
|
||||
// The counts per revolution of the motor's output shaft
|
||||
constexpr float COUNTS_PER_REV = MMME_CPR * GEAR_RATIO;
|
||||
|
||||
// The direction to spin the motor in. NORMAL_DIR (0), REVERSED_DIR (1)
|
||||
const Direction DIRECTION = NORMAL_DIR;
|
||||
|
||||
// The scaling to apply to the motor's speed to match its real-world speed
|
||||
constexpr 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 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 and encoder
|
||||
m.init();
|
||||
enc.init();
|
||||
|
||||
// Enable the motor
|
||||
m.enable();
|
||||
|
||||
|
||||
uint update = 0;
|
||||
uint print_count = 0;
|
||||
|
||||
// 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;
|
||||
|
||||
// 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;
|
||||
}
|
||||
|
||||
// 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());
|
||||
}
|
||||
|
||||
// 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
|
||||
m.disable();
|
||||
}
|
||||
|
|
@ -0,0 +1,14 @@
|
|||
set(OUTPUT_NAME motor2040_position_on_velocity_tuning)
|
||||
add_executable(${OUTPUT_NAME} motor2040_position_on_velocity_tuning.cpp)
|
||||
|
||||
target_link_libraries(${OUTPUT_NAME}
|
||||
pico_stdlib
|
||||
motor2040
|
||||
button
|
||||
pid
|
||||
)
|
||||
|
||||
# enable usb output
|
||||
pico_enable_stdio_usb(${OUTPUT_NAME} 1)
|
||||
|
||||
pico_add_extra_outputs(${OUTPUT_NAME})
|
||||
|
|
@ -0,0 +1,149 @@
|
|||
#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
|
||||
values for position on velocity control. It does this by
|
||||
commanding the motor to move repeatedly between two setpoint
|
||||
angles and plots the measured response.
|
||||
|
||||
Press "Boot" to exit the program.
|
||||
*/
|
||||
|
||||
using namespace motor;
|
||||
using namespace encoder;
|
||||
|
||||
// The pins of the motor being profiled
|
||||
const pin_pair MOTOR_PINS = motor2040::MOTOR_A;
|
||||
|
||||
// The pins of the encoder attached to the profiled motor
|
||||
const pin_pair ENCODER_PINS = motor2040::ENCODER_A;
|
||||
|
||||
// The gear ratio of the motor
|
||||
constexpr float GEAR_RATIO = 50.0f;
|
||||
|
||||
// The counts per revolution of the motor's output shaft
|
||||
constexpr float COUNTS_PER_REV = MMME_CPR * GEAR_RATIO;
|
||||
|
||||
// The direction to spin the motor in. NORMAL_DIR (0), REVERSED_DIR (1)
|
||||
const Direction DIRECTION = NORMAL_DIR;
|
||||
|
||||
// The scaling to apply to the motor's speed to match its real-world speed
|
||||
constexpr 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 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 and encoder
|
||||
m.init();
|
||||
enc.init();
|
||||
|
||||
// Enable the motor
|
||||
m.enable();
|
||||
|
||||
// Set the initial setpoint position
|
||||
pos_pid.setpoint = POSITION_EXTENT;
|
||||
|
||||
|
||||
uint update = 0;
|
||||
uint print_count = 0;
|
||||
|
||||
// Continually move the motor until the user button is pressed
|
||||
while(!user_sw.raw()) {
|
||||
|
||||
// Capture the state of the encoder
|
||||
Encoder::Capture capture = enc.capture();
|
||||
|
||||
// Calculate the velocity to move the motor closer to the position setpoint
|
||||
float vel = pos_pid.calculate(capture.degrees(), capture.degrees_per_second());
|
||||
|
||||
// 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());
|
||||
}
|
||||
|
||||
// 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
|
||||
m.disable();
|
||||
}
|
||||
|
|
@ -0,0 +1,14 @@
|
|||
set(OUTPUT_NAME motor2040_position_tuning)
|
||||
add_executable(${OUTPUT_NAME} motor2040_position_tuning.cpp)
|
||||
|
||||
target_link_libraries(${OUTPUT_NAME}
|
||||
pico_stdlib
|
||||
motor2040
|
||||
button
|
||||
pid
|
||||
)
|
||||
|
||||
# enable usb output
|
||||
pico_enable_stdio_usb(${OUTPUT_NAME} 1)
|
||||
|
||||
pico_add_extra_outputs(${OUTPUT_NAME})
|
||||
|
|
@ -0,0 +1,131 @@
|
|||
#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
|
||||
values for position control. It does this by commanding the
|
||||
motor to move repeatedly between two setpoint angles and
|
||||
plots the measured response.
|
||||
|
||||
Press "Boot" to exit the program.
|
||||
*/
|
||||
|
||||
using namespace motor;
|
||||
using namespace encoder;
|
||||
|
||||
// The pins of the motor being profiled
|
||||
const pin_pair MOTOR_PINS = motor2040::MOTOR_A;
|
||||
|
||||
// The pins of the encoder attached to the profiled motor
|
||||
const pin_pair ENCODER_PINS = motor2040::ENCODER_A;
|
||||
|
||||
// The gear ratio of the motor
|
||||
constexpr float GEAR_RATIO = 50.0f;
|
||||
|
||||
// The counts per revolution of the motor's output shaft
|
||||
constexpr float COUNTS_PER_REV = MMME_CPR * GEAR_RATIO;
|
||||
|
||||
// The direction to spin the motor in. NORMAL_DIR (0), REVERSED_DIR (1)
|
||||
const Direction DIRECTION = NORMAL_DIR;
|
||||
|
||||
// The scaling to apply to the motor's speed to match its real-world speed
|
||||
constexpr float SPEED_SCALE = 5.4f;
|
||||
|
||||
// How many times to update the motor per second
|
||||
const uint UPDATES = 100;
|
||||
constexpr float UPDATE_RATE = 1.0f / (float)UPDATES;
|
||||
|
||||
// The time (in seconds) after a new setpoint, to display print out motor values
|
||||
constexpr float PRINT_WINDOW = 0.25f;
|
||||
|
||||
// The time (in seconds) between each new setpoint being set
|
||||
constexpr float MOVEMENT_WINDOW = 2.0f;
|
||||
|
||||
// How many of the updates should be printed (i.e. 2 would be every other update)
|
||||
const uint PRINT_DIVIDER = 1;
|
||||
|
||||
// Multipliers for the different printed values, so they appear nicely on the Thonny plotter
|
||||
constexpr float SPD_PRINT_SCALE = 10.0f; // Driving Speed multipler
|
||||
|
||||
// How far from zero to move the motor, in degrees
|
||||
constexpr float POSITION_EXTENT = 90.0f;
|
||||
|
||||
// PID values
|
||||
constexpr float POS_KP = 0.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 and encoder
|
||||
m.init();
|
||||
enc.init();
|
||||
|
||||
// Enable the motor
|
||||
m.enable();
|
||||
|
||||
// Set the initial setpoint position
|
||||
pos_pid.setpoint = POSITION_EXTENT;
|
||||
|
||||
|
||||
uint update = 0;
|
||||
uint print_count = 0;
|
||||
|
||||
// Continually move the motor until the user button is pressed
|
||||
while(!user_sw.raw()) {
|
||||
|
||||
// Capture the state of the encoder
|
||||
Encoder::Capture capture = enc.capture();
|
||||
|
||||
// Calculate the velocity to move the motor closer to the position setpoint
|
||||
float vel = pos_pid.calculate(capture.degrees(), capture.degrees_per_second());
|
||||
|
||||
// Set the new motor driving speed
|
||||
m.speed(vel);
|
||||
|
||||
// Print out the current motor values and their setpoints,
|
||||
// but only for the first few updates and only every multiple
|
||||
if(update < (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);
|
||||
}
|
||||
|
||||
// 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.0f - pos_pid.setpoint;
|
||||
}
|
||||
|
||||
sleep_ms(UPDATE_RATE * 1000.0f);
|
||||
}
|
||||
|
||||
// Disable the motor
|
||||
m.disable();
|
||||
}
|
||||
|
|
@ -0,0 +1,14 @@
|
|||
set(OUTPUT_NAME motor2040_quad_position_wave)
|
||||
add_executable(${OUTPUT_NAME} motor2040_quad_position_wave.cpp)
|
||||
|
||||
target_link_libraries(${OUTPUT_NAME}
|
||||
pico_stdlib
|
||||
motor2040
|
||||
button
|
||||
pid
|
||||
)
|
||||
|
||||
# enable usb output
|
||||
pico_enable_stdio_usb(${OUTPUT_NAME} 1)
|
||||
|
||||
pico_add_extra_outputs(${OUTPUT_NAME})
|
||||
|
|
@ -0,0 +1,169 @@
|
|||
#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
|
||||
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;
|
||||
|
||||
// The gear ratio of the motor
|
||||
constexpr float GEAR_RATIO = 50.0f;
|
||||
|
||||
// The counts per revolution of the motor's output shaft
|
||||
constexpr float COUNTS_PER_REV = MMME_CPR * GEAR_RATIO;
|
||||
|
||||
// The scaling to apply to the motor's speed to match its real-world speed
|
||||
constexpr 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 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 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();
|
||||
|
||||
// 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();
|
||||
|
||||
encoders[i] = new Encoder(pio0, i, encoder_pins[i], PIN_UNUSED, NORMAL_DIR, COUNTS_PER_REV, true);
|
||||
encoders[i]->init();
|
||||
|
||||
pos_pids[i] = PID(POS_KP, POS_KI, POS_KD, UPDATE_RATE);
|
||||
}
|
||||
|
||||
// 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();
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
|
|
@ -0,0 +1,14 @@
|
|||
set(OUTPUT_NAME motor2040_quad_velocity_sequence)
|
||||
add_executable(${OUTPUT_NAME} motor2040_quad_velocity_sequence.cpp)
|
||||
|
||||
target_link_libraries(${OUTPUT_NAME}
|
||||
pico_stdlib
|
||||
motor2040
|
||||
button
|
||||
pid
|
||||
)
|
||||
|
||||
# enable usb output
|
||||
pico_enable_stdio_usb(${OUTPUT_NAME} 1)
|
||||
|
||||
pico_add_extra_outputs(${OUTPUT_NAME})
|
||||
|
|
@ -0,0 +1,209 @@
|
|||
#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
|
||||
sequence of velocities, with the help of their attached encoders and PID control.
|
||||
|
||||
Press "Boot" to exit the program.
|
||||
*/
|
||||
|
||||
using namespace motor;
|
||||
using namespace encoder;
|
||||
|
||||
enum Wheels {
|
||||
FL = 2,
|
||||
FR = 3,
|
||||
RL = 1,
|
||||
RR = 0,
|
||||
};
|
||||
|
||||
// The gear ratio of the motor
|
||||
constexpr float GEAR_RATIO = 50.0f;
|
||||
|
||||
// The counts per revolution of the motor's output shaft
|
||||
constexpr float COUNTS_PER_REV = MMME_CPR * GEAR_RATIO;
|
||||
|
||||
// The scaling to apply to the motor's speed to match its real-world speed
|
||||
constexpr 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 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();
|
||||
|
||||
// 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();
|
||||
|
||||
encoders[i] = new Encoder(pio0, i, encoder_pins[i], PIN_UNUSED, NORMAL_DIR, COUNTS_PER_REV, true);
|
||||
encoders[i]->init();
|
||||
|
||||
vel_pids[i] = PID(VEL_KP, VEL_KI, VEL_KD, UPDATE_RATE);
|
||||
}
|
||||
|
||||
// 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();
|
||||
}
|
||||
|
||||
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();
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,14 @@
|
|||
set(OUTPUT_NAME motor2040_reactive_encoder)
|
||||
add_executable(${OUTPUT_NAME} motor2040_reactive_encoder.cpp)
|
||||
|
||||
target_link_libraries(${OUTPUT_NAME}
|
||||
pico_stdlib
|
||||
motor2040
|
||||
button
|
||||
pid
|
||||
)
|
||||
|
||||
# enable usb output
|
||||
pico_enable_stdio_usb(${OUTPUT_NAME} 1)
|
||||
|
||||
pico_add_extra_outputs(${OUTPUT_NAME})
|
||||
|
|
@ -0,0 +1,165 @@
|
|||
#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
|
||||
as a programmable rotary encoder for user input, with
|
||||
force-feedback for arbitrary detents and end stops.
|
||||
|
||||
Press "Boot" to exit the program.
|
||||
*/
|
||||
|
||||
using namespace plasma;
|
||||
using namespace motor;
|
||||
using namespace encoder;
|
||||
|
||||
// The pins of the motor being profiled
|
||||
const pin_pair MOTOR_PINS = motor2040::MOTOR_A;
|
||||
|
||||
// The pins of the encoder attached to the profiled motor
|
||||
const pin_pair ENCODER_PINS = motor2040::ENCODER_A;
|
||||
|
||||
// The gear ratio of the motor
|
||||
constexpr float GEAR_RATIO = 50.0f;
|
||||
|
||||
// The counts per revolution of the motor's output shaft
|
||||
constexpr float COUNTS_PER_REV = MMME_CPR * GEAR_RATIO;
|
||||
|
||||
// The direction to spin the motor in. NORMAL_DIR (0), REVERSED_DIR (1)
|
||||
const Direction DIRECTION = NORMAL_DIR;
|
||||
|
||||
// The scaling to apply to the motor's speed to match its real-world speed
|
||||
constexpr 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 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 and encoder
|
||||
m.init();
|
||||
enc.init();
|
||||
|
||||
// Start updating the LED
|
||||
led.start();
|
||||
|
||||
// Enable the motor
|
||||
m.enable();
|
||||
|
||||
// Call the function once to set the setpoint and print the value
|
||||
detent_change(0);
|
||||
|
||||
// Continually move the motor until the user button is pressed
|
||||
while(!user_sw.raw()) {
|
||||
|
||||
// Capture the state of the encoder
|
||||
Encoder::Capture capture = enc.capture();
|
||||
|
||||
// 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
|
||||
}
|
||||
}
|
||||
|
||||
// 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);
|
||||
}
|
||||
|
|
@ -0,0 +1,13 @@
|
|||
set(OUTPUT_NAME motor2040_read_encoders)
|
||||
add_executable(${OUTPUT_NAME} motor2040_read_encoders.cpp)
|
||||
|
||||
target_link_libraries(${OUTPUT_NAME}
|
||||
pico_stdlib
|
||||
motor2040
|
||||
button
|
||||
)
|
||||
|
||||
# enable usb output
|
||||
pico_enable_stdio_usb(${OUTPUT_NAME} 1)
|
||||
|
||||
pico_add_extra_outputs(${OUTPUT_NAME})
|
||||
|
|
@ -0,0 +1,60 @@
|
|||
#include <cstdio>
|
||||
#include "pico/stdlib.h"
|
||||
|
||||
#include "motor2040.hpp"
|
||||
#include "button.hpp"
|
||||
|
||||
/*
|
||||
Demonstrates how to read the angles of Motor 2040's four encoders.
|
||||
|
||||
Press "Boot" to exit the program.
|
||||
*/
|
||||
|
||||
using namespace motor;
|
||||
using namespace encoder;
|
||||
|
||||
// The gear ratio of the motor
|
||||
constexpr float GEAR_RATIO = 50.0f;
|
||||
|
||||
// The counts per revolution of the motor's output shaft
|
||||
constexpr float COUNTS_PER_REV = MMME_CPR * GEAR_RATIO;
|
||||
|
||||
|
||||
// Create an array of encoder pointers
|
||||
const pin_pair encoder_pins[] = {motor2040::ENCODER_A, motor2040::ENCODER_B,
|
||||
motor2040::ENCODER_C, motor2040::ENCODER_D};
|
||||
const char* ENCODER_NAMES[] = {"A", "B", "C", "D"};
|
||||
const uint NUM_ENCODERS = count_of(encoder_pins);
|
||||
Encoder *encoders[NUM_ENCODERS];
|
||||
|
||||
// Create the user button
|
||||
Button user_sw(motor2040::USER_SW);
|
||||
|
||||
|
||||
int main() {
|
||||
stdio_init_all();
|
||||
|
||||
// Fill the array of motors, and initialise them. Up to 8 motors can be created
|
||||
for(auto e = 0u; e < NUM_ENCODERS; e++) {
|
||||
encoders[e] = new Encoder(pio0, e, encoder_pins[e], PIN_UNUSED, NORMAL_DIR, COUNTS_PER_REV, true);
|
||||
encoders[e]->init();
|
||||
}
|
||||
|
||||
// Uncomment the below lines to reverse
|
||||
// the counting direction of an encoder
|
||||
// encoders[0].direction(REVERSED_DIR);
|
||||
// encoders[1].direction(REVERSED_DIR);
|
||||
// encoders[2].direction(REVERSED_DIR);
|
||||
// encoders[3].direction(REVERSED_DIR);
|
||||
|
||||
// Read the encoders until the user button is pressed
|
||||
while(!user_sw.raw()) {
|
||||
// Print out the angle of each encoder
|
||||
for(auto e = 0u; e < NUM_ENCODERS; e++) {
|
||||
printf("%s = %f, ", ENCODER_NAMES[e], encoders[e]->degrees());
|
||||
}
|
||||
printf("\n");
|
||||
|
||||
sleep_ms(100);
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,15 @@
|
|||
set(OUTPUT_NAME motor2040_read_sensors)
|
||||
add_executable(${OUTPUT_NAME} motor2040_read_sensors.cpp)
|
||||
|
||||
target_link_libraries(${OUTPUT_NAME}
|
||||
pico_stdlib
|
||||
motor2040
|
||||
analogmux
|
||||
analog
|
||||
button
|
||||
)
|
||||
|
||||
# enable usb output
|
||||
pico_enable_stdio_usb(${OUTPUT_NAME} 1)
|
||||
|
||||
pico_add_extra_outputs(${OUTPUT_NAME})
|
||||
|
|
@ -0,0 +1,68 @@
|
|||
#include <cstdio>
|
||||
#include "pico/stdlib.h"
|
||||
|
||||
#include "motor2040.hpp"
|
||||
#include "analogmux.hpp"
|
||||
#include "analog.hpp"
|
||||
#include "button.hpp"
|
||||
|
||||
/*
|
||||
Shows how to initialise and read the 2 external
|
||||
and 6 internal sensors of Motor 2040.
|
||||
|
||||
Press "Boot" to exit the program.
|
||||
*/
|
||||
|
||||
using namespace motor;
|
||||
|
||||
// Set up the shared analog inputs
|
||||
Analog sen_adc = Analog(motor2040::SHARED_ADC);
|
||||
Analog vol_adc = Analog(motor2040::SHARED_ADC, motor2040::VOLTAGE_GAIN);
|
||||
Analog cur_adc = Analog(motor2040::SHARED_ADC, motor2040::CURRENT_GAIN,
|
||||
motor2040::SHUNT_RESISTOR, motor2040::CURRENT_OFFSET);
|
||||
|
||||
// Set up the analog multiplexer, including the pin for controlling pull-up/pull-down
|
||||
AnalogMux mux = AnalogMux(motor2040::ADC_ADDR_0, motor2040::ADC_ADDR_1, motor2040::ADC_ADDR_2,
|
||||
PIN_UNUSED, motor2040::SHARED_ADC);
|
||||
|
||||
// Create the user button
|
||||
Button user_sw(motor2040::USER_SW);
|
||||
|
||||
|
||||
int main() {
|
||||
stdio_init_all();
|
||||
|
||||
// Set up the sensor addresses with pull downs
|
||||
for(auto i = 0u; i < motor2040::NUM_SENSORS; i++) {
|
||||
mux.configure_pulls(motor2040::SENSOR_1_ADDR + i, false, true);
|
||||
}
|
||||
|
||||
// Set up the pull-up for the fault sense
|
||||
mux.configure_pulls(motor2040::FAULT_SENSE_ADDR, true, false);
|
||||
|
||||
// Read sensors until the user button is pressed
|
||||
while(!user_sw.raw()) {
|
||||
|
||||
// Read each sensor in turn and print its voltage
|
||||
for(auto i = 0u; i < motor2040::NUM_SENSORS; i++) {
|
||||
mux.select(motor2040::SENSOR_1_ADDR + i);
|
||||
printf("S%d = %f, ", i + 1, sen_adc.read_voltage());
|
||||
}
|
||||
|
||||
// Read the voltage sense and print the value
|
||||
mux.select(motor2040::VOLTAGE_SENSE_ADDR);
|
||||
printf("Voltage = %f, ", vol_adc.read_voltage());
|
||||
|
||||
// Read the current sense's of each motor and print the value
|
||||
for(auto i = 0u; i < motor2040::NUM_MOTORS; i++) {
|
||||
mux.select(motor2040::CURRENT_SENSE_A_ADDR + i);
|
||||
printf("C%d = %f, ", i + 1, cur_adc.read_current());
|
||||
}
|
||||
|
||||
// Read the fault sense and print the value
|
||||
mux.select(motor2040::FAULT_SENSE_ADDR);
|
||||
printf("Fault = %s\n", mux.read() ? "false" : "true");
|
||||
|
||||
sleep_ms(500);
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,12 @@
|
|||
set(OUTPUT_NAME motor2040_single_motor)
|
||||
add_executable(${OUTPUT_NAME} motor2040_single_motor.cpp)
|
||||
|
||||
target_link_libraries(${OUTPUT_NAME}
|
||||
pico_stdlib
|
||||
motor2040
|
||||
)
|
||||
|
||||
# enable usb output
|
||||
pico_enable_stdio_usb(${OUTPUT_NAME} 1)
|
||||
|
||||
pico_add_extra_outputs(${OUTPUT_NAME})
|
||||
|
|
@ -0,0 +1,77 @@
|
|||
#include "pico/stdlib.h"
|
||||
|
||||
#include "motor2040.hpp"
|
||||
|
||||
/*
|
||||
Demonstrates how to create a Motor object and control it.
|
||||
*/
|
||||
|
||||
using namespace motor;
|
||||
|
||||
// How many sweeps of the motor to perform
|
||||
const uint SWEEPS = 2;
|
||||
|
||||
// The number of discrete sweep steps
|
||||
const uint STEPS = 10;
|
||||
|
||||
// The time in milliseconds between each step of the sequence
|
||||
const uint STEPS_INTERVAL_MS = 500;
|
||||
|
||||
// How far from zero to drive the motor when sweeping
|
||||
constexpr float SPEED_EXTENT = 1.0f;
|
||||
|
||||
|
||||
// Create a motor
|
||||
Motor m = Motor(motor2040::MOTOR_A);
|
||||
|
||||
|
||||
int main() {
|
||||
stdio_init_all();
|
||||
|
||||
// Initialise the motor
|
||||
m.init();
|
||||
|
||||
// Enable the motor
|
||||
m.enable();
|
||||
sleep_ms(2000);
|
||||
|
||||
// Drive at full positive
|
||||
m.full_positive();
|
||||
sleep_ms(2000);
|
||||
|
||||
// Stop moving
|
||||
m.stop();
|
||||
sleep_ms(2000);
|
||||
|
||||
// Drive at full negative
|
||||
m.full_negative();
|
||||
sleep_ms(2000);
|
||||
|
||||
// Coast to a gradual stop
|
||||
m.coast();
|
||||
sleep_ms(2000);
|
||||
|
||||
|
||||
// Do a sine speed sweep
|
||||
for(auto j = 0u; j < SWEEPS; j++) {
|
||||
for(auto i = 0u; i < 360; i++) {
|
||||
m.speed(sin(((float)i * (float)M_PI) / 180.0f) * SPEED_EXTENT);
|
||||
sleep_ms(20);
|
||||
}
|
||||
}
|
||||
|
||||
// Do a stepped speed sweep
|
||||
for(auto j = 0u; j < SWEEPS; j++) {
|
||||
for(auto i = 0u; i < STEPS; i++) {
|
||||
m.to_percent(i, 0, STEPS, 0.0 - SPEED_EXTENT, SPEED_EXTENT);
|
||||
sleep_ms(STEPS_INTERVAL_MS);
|
||||
}
|
||||
for(auto i = 0u; i < STEPS; i++) {
|
||||
m.to_percent(i, STEPS, 0, 0.0 - SPEED_EXTENT, SPEED_EXTENT);
|
||||
sleep_ms(STEPS_INTERVAL_MS);
|
||||
}
|
||||
}
|
||||
|
||||
// Disable the motor
|
||||
m.disable();
|
||||
}
|
||||
|
|
@ -0,0 +1,14 @@
|
|||
set(OUTPUT_NAME motor2040_velocity_control)
|
||||
add_executable(${OUTPUT_NAME} motor2040_velocity_control.cpp)
|
||||
|
||||
target_link_libraries(${OUTPUT_NAME}
|
||||
pico_stdlib
|
||||
motor2040
|
||||
button
|
||||
pid
|
||||
)
|
||||
|
||||
# enable usb output
|
||||
pico_enable_stdio_usb(${OUTPUT_NAME} 1)
|
||||
|
||||
pico_add_extra_outputs(${OUTPUT_NAME})
|
||||
|
|
@ -0,0 +1,152 @@
|
|||
#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,
|
||||
with the help of it's attached encoder and PID control.
|
||||
|
||||
Press "Boot" to exit the program.
|
||||
*/
|
||||
|
||||
using namespace motor;
|
||||
using namespace encoder;
|
||||
|
||||
// The pins of the motor being profiled
|
||||
const pin_pair MOTOR_PINS = motor2040::MOTOR_A;
|
||||
|
||||
// The pins of the encoder attached to the profiled motor
|
||||
const pin_pair ENCODER_PINS = motor2040::ENCODER_A;
|
||||
|
||||
// The gear ratio of the motor
|
||||
constexpr float GEAR_RATIO = 50.0f;
|
||||
|
||||
// The counts per revolution of the motor's output shaft
|
||||
constexpr float COUNTS_PER_REV = MMME_CPR * GEAR_RATIO;
|
||||
|
||||
// The direction to spin the motor in. NORMAL_DIR (0), REVERSED_DIR (1)
|
||||
const Direction DIRECTION = NORMAL_DIR;
|
||||
|
||||
// The scaling to apply to the motor's speed to match its real-world speed
|
||||
constexpr 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 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 and encoder
|
||||
m.init();
|
||||
enc.init();
|
||||
|
||||
// Enable the motor
|
||||
m.enable();
|
||||
|
||||
|
||||
uint update = 0;
|
||||
uint print_count = 0;
|
||||
|
||||
// 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;
|
||||
|
||||
// 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;
|
||||
}
|
||||
|
||||
// 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());
|
||||
}
|
||||
|
||||
// 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
|
||||
m.disable();
|
||||
}
|
||||
|
|
@ -0,0 +1,14 @@
|
|||
set(OUTPUT_NAME motor2040_velocity_tuning)
|
||||
add_executable(${OUTPUT_NAME} motor2040_velocity_tuning.cpp)
|
||||
|
||||
target_link_libraries(${OUTPUT_NAME}
|
||||
pico_stdlib
|
||||
motor2040
|
||||
button
|
||||
pid
|
||||
)
|
||||
|
||||
# enable usb output
|
||||
pico_enable_stdio_usb(${OUTPUT_NAME} 1)
|
||||
|
||||
pico_add_extra_outputs(${OUTPUT_NAME})
|
||||
|
|
@ -0,0 +1,132 @@
|
|||
#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
|
||||
values for velocity control. It does this by commanding the
|
||||
motor to drive repeatedly between two setpoint speeds and
|
||||
plots the measured response.
|
||||
|
||||
Press "Boot" to exit the program.
|
||||
*/
|
||||
|
||||
using namespace motor;
|
||||
using namespace encoder;
|
||||
|
||||
// The pins of the motor being profiled
|
||||
const pin_pair MOTOR_PINS = motor2040::MOTOR_A;
|
||||
|
||||
// The pins of the encoder attached to the profiled motor
|
||||
const pin_pair ENCODER_PINS = motor2040::ENCODER_A;
|
||||
|
||||
// The gear ratio of the motor
|
||||
constexpr float GEAR_RATIO = 50.0f;
|
||||
|
||||
// The counts per revolution of the motor's output shaft
|
||||
constexpr float COUNTS_PER_REV = MMME_CPR * GEAR_RATIO;
|
||||
|
||||
// The direction to spin the motor in. NORMAL_DIR (0), REVERSED_DIR (1)
|
||||
const Direction DIRECTION = NORMAL_DIR;
|
||||
|
||||
// The scaling to apply to the motor's speed to match its real-world speed
|
||||
constexpr 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 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 and encoder
|
||||
m.init();
|
||||
enc.init();
|
||||
|
||||
// Enable the motor
|
||||
m.enable();
|
||||
|
||||
// Set the initial setpoint velocity
|
||||
vel_pid.setpoint = VELOCITY_EXTENT;
|
||||
|
||||
|
||||
uint update = 0;
|
||||
uint print_count = 0;
|
||||
|
||||
// Continually move the motor until the user button is pressed
|
||||
while(!user_sw.raw()) {
|
||||
|
||||
// Capture the state of the encoder
|
||||
Encoder::Capture capture = enc.capture();
|
||||
|
||||
// Calculate the 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());
|
||||
}
|
||||
|
||||
// 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
|
||||
m.disable();
|
||||
}
|
||||
|
|
@ -0,0 +1,12 @@
|
|||
add_executable(
|
||||
explorerencoder
|
||||
demo.cpp
|
||||
)
|
||||
|
||||
pico_generate_pio_header(explorerencoder ${CMAKE_CURRENT_LIST_DIR}/quadrature_out.pio)
|
||||
|
||||
# Pull in pico libraries that we need
|
||||
target_link_libraries(explorerencoder pico_stdlib pico_explorer encoder)
|
||||
|
||||
# create map/bin/hex file etc.
|
||||
pico_add_extra_outputs(explorerencoder)
|
||||
|
|
@ -0,0 +1,382 @@
|
|||
#include <iomanip>
|
||||
#include <sstream>
|
||||
#include "pico_explorer.hpp"
|
||||
#include "pico/stdlib.h"
|
||||
#include "encoder.hpp"
|
||||
#include "quadrature_out.pio.h"
|
||||
|
||||
/*
|
||||
An interactive demo of how rotary encoders work.
|
||||
|
||||
Connect up an encoder (be it rotary or magnetic) as detailed below
|
||||
and see the resulting signals and stats on the Pico Explorer's display.
|
||||
|
||||
Connections:
|
||||
- A to GP0
|
||||
- B to GP1
|
||||
- C (if present) to GP2
|
||||
- Switch (if present) to GP3
|
||||
|
||||
Buttons
|
||||
- A is 'Zoom Out'
|
||||
- X is 'Zoom In'
|
||||
- B is 'Motor 1 Forward'
|
||||
- Y is 'Motor 1 Reverse'
|
||||
- Switch is 'Zero the Count'
|
||||
|
||||
If you do not have an encoder and wish to try out
|
||||
this example, simulated A and B encoder signals can
|
||||
be used by jumping GP0 to GP6 and GP1 to GP7.
|
||||
*/
|
||||
|
||||
using namespace pimoroni;
|
||||
using namespace encoder;
|
||||
|
||||
//--------------------------------------------------
|
||||
// Constants
|
||||
//--------------------------------------------------
|
||||
|
||||
// The pins used by the encoder
|
||||
static const pin_pair ENCODER_PINS = {0, 1};
|
||||
static const uint ENCODER_COMMON_PIN = 2;
|
||||
static const uint ENCODER_SWITCH_PIN = 3;
|
||||
|
||||
// The counts per revolution of the encoder's output shaft
|
||||
static constexpr float COUNTS_PER_REV = encoder::ROTARY_CPR;
|
||||
|
||||
// Set to true if using a motor with a magnetic encoder
|
||||
static const bool COUNT_MICROSTEPS = false;
|
||||
|
||||
// Increase this to deal with switch bounce. 250 Gives a 1ms debounce
|
||||
static const uint16_t FREQ_DIVIDER = 1;
|
||||
|
||||
// Time between each sample, in microseconds
|
||||
static const int32_t TIME_BETWEEN_SAMPLES_US = 100;
|
||||
|
||||
// The full time window that will be stored
|
||||
static const int32_t WINDOW_DURATION_US = 1000000;
|
||||
|
||||
static const int32_t READINGS_SIZE = WINDOW_DURATION_US / TIME_BETWEEN_SAMPLES_US;
|
||||
static const int32_t SCRATCH_SIZE = READINGS_SIZE / 10; // A smaller value, for temporarily storing readings during screen drawing
|
||||
|
||||
// Whether to output a synthetic quadrature signal
|
||||
static const bool QUADRATURE_OUT_ENABLED = true;
|
||||
|
||||
// The frequency the quadrature output will run at (note that counting microsteps will show 4x this value)
|
||||
static constexpr float QUADRATURE_OUT_FREQ = 800;
|
||||
|
||||
// Which first pin to output the quadrature signal to (e.g. GP6 and GP7)
|
||||
static const float QUADRATURE_OUT_1ST_PIN = 6;
|
||||
|
||||
// How long there should be in microseconds between each screen refresh
|
||||
static const uint64_t MAIN_LOOP_TIME_US = 50000;
|
||||
|
||||
// The zoom level beyond which edge alignment will be enabled to make viewing encoder patterns look nice
|
||||
static const uint16_t EDGE_ALIGN_ABOVE_ZOOM = 4;
|
||||
|
||||
|
||||
|
||||
//--------------------------------------------------
|
||||
// Enums
|
||||
//--------------------------------------------------
|
||||
enum DrawState {
|
||||
DRAW_LOW = 0,
|
||||
DRAW_HIGH,
|
||||
DRAW_TRANSITION,
|
||||
};
|
||||
|
||||
|
||||
|
||||
//--------------------------------------------------
|
||||
// Variables
|
||||
//--------------------------------------------------
|
||||
uint16_t buffer[PicoExplorer::WIDTH * PicoExplorer::HEIGHT];
|
||||
PicoExplorer pico_explorer(buffer);
|
||||
|
||||
Encoder enc(pio0, 0, ENCODER_PINS, ENCODER_COMMON_PIN, NORMAL_DIR, COUNTS_PER_REV, COUNT_MICROSTEPS, FREQ_DIVIDER);
|
||||
|
||||
volatile bool enc_a_readings[READINGS_SIZE];
|
||||
volatile bool enc_b_readings[READINGS_SIZE];
|
||||
volatile bool enc_a_scratch[SCRATCH_SIZE];
|
||||
volatile bool enc_b_scratch[SCRATCH_SIZE];
|
||||
volatile uint32_t next_reading_index = 0;
|
||||
volatile uint32_t next_scratch_index = 0;
|
||||
volatile bool drawing_to_screen = false;
|
||||
uint16_t current_zoom_level = 1;
|
||||
|
||||
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
// FUNCTIONS
|
||||
////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
uint32_t draw_plot(Point p1, Point p2, volatile bool (&readings)[READINGS_SIZE], uint32_t reading_pos, bool edge_align) {
|
||||
uint32_t reading_window = READINGS_SIZE / current_zoom_level;
|
||||
uint32_t start_index_no_modulus = (reading_pos + (READINGS_SIZE - reading_window));
|
||||
uint32_t start_index = start_index_no_modulus % READINGS_SIZE;
|
||||
int32_t screen_window = std::min(p2.x, (int32_t)PicoExplorer::WIDTH) - p1.x;
|
||||
|
||||
bool last_reading = readings[start_index % READINGS_SIZE];
|
||||
|
||||
uint32_t alignment_offset = 0;
|
||||
if(edge_align) {
|
||||
// Perform edge alignment by first seeing if there is a window of readings available (will be at anything other than x1 zoom)
|
||||
uint32_t align_window = (start_index_no_modulus - reading_pos);
|
||||
|
||||
// Then go backwards through that window
|
||||
for(uint32_t i = 1; i < align_window; i++) {
|
||||
uint32_t align_index = (start_index + (READINGS_SIZE - i)) % READINGS_SIZE;
|
||||
bool align_reading = readings[align_index];
|
||||
|
||||
// Has a transition from high to low been detected?
|
||||
if(!align_reading && align_reading != last_reading) {
|
||||
// Set the new start index from which to draw from and break out of the search
|
||||
start_index = align_index;
|
||||
alignment_offset = i;
|
||||
break;
|
||||
}
|
||||
last_reading = align_reading;
|
||||
}
|
||||
|
||||
last_reading = readings[start_index % READINGS_SIZE];
|
||||
}
|
||||
|
||||
// Go through each X pixel within the screen window
|
||||
uint32_t reading_window_start = 0;
|
||||
for(int32_t x = 0; x < screen_window; x++) {
|
||||
uint32_t reading_window_end = ((x + 1) * reading_window) / screen_window;
|
||||
|
||||
// Set the draw state to be whatever the last reading was
|
||||
DrawState draw_state = last_reading ? DRAW_HIGH : DRAW_LOW;
|
||||
|
||||
// Go through the readings in this window to see if a transition from low to high or high to low occurs
|
||||
if(reading_window_end > reading_window_start) {
|
||||
for(uint32_t i = reading_window_start; i < reading_window_end; i++) {
|
||||
bool reading = readings[(i + start_index) % READINGS_SIZE];
|
||||
if(reading != last_reading) {
|
||||
draw_state = DRAW_TRANSITION;
|
||||
break; // A transition occurred, so no need to continue checking readings
|
||||
}
|
||||
last_reading = reading;
|
||||
}
|
||||
last_reading = readings[((reading_window_end - 1) + start_index) % READINGS_SIZE];
|
||||
}
|
||||
reading_window_start = reading_window_end;
|
||||
|
||||
// Draw a pixel in a high or low position, or a line between the two if a transition
|
||||
switch(draw_state) {
|
||||
case DRAW_TRANSITION:
|
||||
for(uint8_t y = p1.y; y < p2.y; y++)
|
||||
pico_explorer.pixel(Point(x + p1.x, y));
|
||||
break;
|
||||
case DRAW_HIGH:
|
||||
pico_explorer.pixel(Point(x + p1.x, p1.y));
|
||||
break;
|
||||
case DRAW_LOW:
|
||||
pico_explorer.pixel(Point(x + p1.x, p2.y - 1));
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// Return the alignment offset so subsequent encoder channel plots can share the alignment
|
||||
return alignment_offset;
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
bool repeating_timer_callback(struct repeating_timer *t) {
|
||||
bool_pair state = enc.state();
|
||||
if(drawing_to_screen && next_scratch_index < SCRATCH_SIZE) {
|
||||
enc_a_scratch[next_scratch_index] = state.a;
|
||||
enc_b_scratch[next_scratch_index] = state.b;
|
||||
next_scratch_index++;
|
||||
}
|
||||
else {
|
||||
enc_a_readings[next_reading_index] = state.a;
|
||||
enc_b_readings[next_reading_index] = state.b;
|
||||
|
||||
next_reading_index++;
|
||||
if(next_reading_index >= READINGS_SIZE) {
|
||||
next_reading_index = 0;
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void setup() {
|
||||
stdio_init_all();
|
||||
|
||||
gpio_init(PICO_DEFAULT_LED_PIN);
|
||||
gpio_set_dir(PICO_DEFAULT_LED_PIN, GPIO_OUT);
|
||||
|
||||
if(ENCODER_SWITCH_PIN != PIN_UNUSED) {
|
||||
gpio_init(ENCODER_SWITCH_PIN);
|
||||
gpio_set_dir(ENCODER_SWITCH_PIN, GPIO_IN);
|
||||
gpio_pull_down(ENCODER_SWITCH_PIN);
|
||||
}
|
||||
|
||||
pico_explorer.init();
|
||||
pico_explorer.set_pen(0);
|
||||
pico_explorer.clear();
|
||||
pico_explorer.update();
|
||||
|
||||
enc.init();
|
||||
|
||||
bool_pair state = enc.state();
|
||||
for(uint i = 0; i < READINGS_SIZE; i++) {
|
||||
enc_a_readings[i] = state.a;
|
||||
enc_b_readings[i] = state.b;
|
||||
}
|
||||
|
||||
if(QUADRATURE_OUT_ENABLED) {
|
||||
// Set up the quadrature encoder output
|
||||
PIO pio = pio1;
|
||||
uint offset = pio_add_program(pio, &quadrature_out_program);
|
||||
uint sm = pio_claim_unused_sm(pio, true);
|
||||
quadrature_out_program_init(pio, sm, offset, QUADRATURE_OUT_1ST_PIN, QUADRATURE_OUT_FREQ);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
// MAIN
|
||||
////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
int main() {
|
||||
|
||||
// Perform the main setup for the demo
|
||||
setup();
|
||||
|
||||
// Begin the timer that will take readings of the coder at regular intervals
|
||||
struct repeating_timer timer;
|
||||
add_repeating_timer_us(-TIME_BETWEEN_SAMPLES_US, repeating_timer_callback, NULL, &timer);
|
||||
|
||||
bool button_latch_a = false;
|
||||
bool button_latch_x = false;
|
||||
uint64_t last_time = time_us_64();
|
||||
|
||||
while(true) {
|
||||
|
||||
// Has enough time elapsed since we last refreshed the screen?
|
||||
uint64_t current_time = time_us_64();
|
||||
if(current_time > last_time + MAIN_LOOP_TIME_US) {
|
||||
last_time = current_time;
|
||||
|
||||
gpio_put(PICO_DEFAULT_LED_PIN, true); // Show the screen refresh has stated
|
||||
|
||||
// If the user has wired up their encoder switch, and it is pressed, set the encoder count to zero
|
||||
if(ENCODER_SWITCH_PIN != PIN_UNUSED && gpio_get(ENCODER_SWITCH_PIN)) {
|
||||
enc.zero();
|
||||
}
|
||||
|
||||
// Capture the encoder state
|
||||
Encoder::Capture capture = enc.capture();
|
||||
|
||||
// Spin Motor 1 either clockwise or counterclockwise depending on if B or Y are pressed
|
||||
if(pico_explorer.is_pressed(PicoExplorer::B) && !pico_explorer.is_pressed(PicoExplorer::Y)) {
|
||||
pico_explorer.set_motor(PicoExplorer::MOTOR1, PicoExplorer::FORWARD, 1.0f);
|
||||
}
|
||||
else if(pico_explorer.is_pressed(PicoExplorer::Y) && !pico_explorer.is_pressed(PicoExplorer::B)) {
|
||||
pico_explorer.set_motor(PicoExplorer::MOTOR1, PicoExplorer::REVERSE, 0.2f);
|
||||
}
|
||||
else {
|
||||
pico_explorer.set_motor(PicoExplorer::MOTOR1, PicoExplorer::STOP);
|
||||
}
|
||||
|
||||
// If A has been pressed, zoom the view out to a min of x1
|
||||
if(pico_explorer.is_pressed(PicoExplorer::A)) {
|
||||
if(!button_latch_a) {
|
||||
button_latch_a = true;
|
||||
current_zoom_level = std::max(current_zoom_level / 2, 1);
|
||||
}
|
||||
}
|
||||
else {
|
||||
button_latch_a = false;
|
||||
}
|
||||
|
||||
// If X has been pressed, zoom the view in to the max of x512
|
||||
if(pico_explorer.is_pressed(PicoExplorer::X)) {
|
||||
if(!button_latch_x) {
|
||||
button_latch_x = true;
|
||||
current_zoom_level = std::min(current_zoom_level * 2, 512);
|
||||
}
|
||||
}
|
||||
else {
|
||||
button_latch_x = false;
|
||||
}
|
||||
|
||||
//--------------------------------------------------
|
||||
// Draw the encoder readings to the screen as a signal plot
|
||||
|
||||
pico_explorer.set_pen(0, 0, 0);
|
||||
pico_explorer.clear();
|
||||
|
||||
drawing_to_screen = true;
|
||||
|
||||
pico_explorer.set_pen(255, 255, 0);
|
||||
uint32_t local_pos = next_reading_index;
|
||||
uint32_t alignment_offset = draw_plot(Point(0, 10), Point(PicoExplorer::WIDTH, 10 + 50), enc_a_readings, local_pos, current_zoom_level > EDGE_ALIGN_ABOVE_ZOOM);
|
||||
|
||||
pico_explorer.set_pen(0, 255, 255);
|
||||
draw_plot(Point(0, 80), Point(PicoExplorer::WIDTH, 80 + 50), enc_b_readings, (local_pos + (READINGS_SIZE - alignment_offset)) % READINGS_SIZE, false);
|
||||
|
||||
// Copy values that may have been stored in the scratch buffers, back into the main buffers
|
||||
for(uint16_t i = 0; i < next_scratch_index; i++) {
|
||||
enc_a_readings[next_reading_index] = enc_a_scratch[i];
|
||||
enc_b_readings[next_reading_index] = enc_b_scratch[i];
|
||||
|
||||
next_reading_index++;
|
||||
if(next_reading_index >= READINGS_SIZE)
|
||||
next_reading_index = 0;
|
||||
}
|
||||
|
||||
drawing_to_screen = false;
|
||||
next_scratch_index = 0;
|
||||
|
||||
pico_explorer.set_pen(255, 255, 255);
|
||||
pico_explorer.character('A', Point(5, 10 + 15), 3);
|
||||
pico_explorer.character('B', Point(5, 80 + 15), 3);
|
||||
|
||||
if(current_zoom_level < 10)
|
||||
pico_explorer.text("x" + std::to_string(current_zoom_level), Point(220, 62), 200, 2);
|
||||
else if(current_zoom_level < 100)
|
||||
pico_explorer.text("x" + std::to_string(current_zoom_level), Point(210, 62), 200, 2);
|
||||
else
|
||||
pico_explorer.text("x" + std::to_string(current_zoom_level), Point(200, 62), 200, 2);
|
||||
|
||||
|
||||
//--------------------------------------------------
|
||||
// Write out the count, frequency and rpm of the encoder
|
||||
|
||||
pico_explorer.set_pen(8, 8, 8);
|
||||
pico_explorer.rectangle(Rect(0, 140, PicoExplorer::WIDTH, PicoExplorer::HEIGHT - 140));
|
||||
|
||||
pico_explorer.set_pen(64, 64, 64);
|
||||
pico_explorer.rectangle(Rect(0, 140, PicoExplorer::WIDTH, 2));
|
||||
|
||||
{
|
||||
std::stringstream sstream;
|
||||
sstream << capture.count();
|
||||
pico_explorer.set_pen(255, 255, 255); pico_explorer.text("Count:", Point(10, 150), 200, 3);
|
||||
pico_explorer.set_pen(255, 128, 255); pico_explorer.text(sstream.str(), Point(110, 150), 200, 3);
|
||||
}
|
||||
|
||||
{
|
||||
std::stringstream sstream;
|
||||
sstream << std::fixed << std::setprecision(1) << capture.frequency() << "hz";
|
||||
pico_explorer.set_pen(255, 255, 255); pico_explorer.text("Freq: ", Point(10, 180), 220, 3);
|
||||
pico_explorer.set_pen(128, 255, 255); pico_explorer.text(sstream.str(), Point(90, 180), 220, 3);
|
||||
}
|
||||
|
||||
{
|
||||
std::stringstream sstream;
|
||||
sstream << std::fixed << std::setprecision(1) << capture.revolutions_per_minute();
|
||||
pico_explorer.set_pen(255, 255, 255); pico_explorer.text("RPM: ", Point(10, 210), 220, 3);
|
||||
pico_explorer.set_pen(255, 255, 128); pico_explorer.text(sstream.str(), Point(80, 210), 220, 3);
|
||||
}
|
||||
|
||||
pico_explorer.update(); // Refresh the screen
|
||||
gpio_put(PICO_DEFAULT_LED_PIN, false); // Show the screen refresh has ended
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,44 @@
|
|||
; --------------------------------------------------
|
||||
; Quadrature Output using PIO
|
||||
; by Christopher (@ZodiusInfuser) Parrott
|
||||
; --------------------------------------------------
|
||||
;
|
||||
; A simple PIO that will create a quadrature output signal
|
||||
; for use with testing quadrature decoder code
|
||||
|
||||
|
||||
; Constants
|
||||
; --------------------------------------------------
|
||||
.define public QUAD_OUT_SET_CYCLES 10
|
||||
|
||||
|
||||
.program quadrature_out
|
||||
.wrap_target
|
||||
set pins, 0 [QUAD_OUT_SET_CYCLES - 1]
|
||||
set pins, 1 [QUAD_OUT_SET_CYCLES - 1]
|
||||
set pins, 3 [QUAD_OUT_SET_CYCLES - 1]
|
||||
set pins, 2 [QUAD_OUT_SET_CYCLES - 1]
|
||||
.wrap
|
||||
|
||||
|
||||
; Initialisation Code
|
||||
; --------------------------------------------------
|
||||
% c-sdk {
|
||||
#include "hardware/clocks.h"
|
||||
|
||||
void quadrature_out_program_init(PIO pio, uint sm, uint offset, uint pin, float freq)
|
||||
{
|
||||
pio_gpio_init(pio, pin);
|
||||
pio_gpio_init(pio, pin + 1);
|
||||
pio_sm_set_consecutive_pindirs(pio, sm, pin, 2, true);
|
||||
pio_sm_config c = quadrature_out_program_get_default_config(offset);
|
||||
sm_config_set_set_pins(&c, pin, 2);
|
||||
|
||||
int cycles_per_bit = QUAD_OUT_SET_CYCLES * 4;
|
||||
float div = clock_get_hz(clk_sys) / (freq * cycles_per_bit);
|
||||
sm_config_set_clkdiv(&c, div);
|
||||
|
||||
pio_sm_init(pio, sm, offset, &c);
|
||||
pio_sm_set_enabled(pio, sm, true);
|
||||
}
|
||||
%}
|
||||
|
|
@ -0,0 +1,4 @@
|
|||
include(motorshim_dual_motors.cmake)
|
||||
include(motorshim_movements.cmake)
|
||||
include(motorshim_motor_song.cmake)
|
||||
include(motorshim_single_motor.cmake)
|
||||
|
|
@ -0,0 +1,33 @@
|
|||
# Pico Motor Shim C++ Examples <!-- omit in toc -->
|
||||
|
||||
- [Examples](#examples)
|
||||
- [Single Motor](#single-motor)
|
||||
- [Dual Motors](#dual-motors)
|
||||
- [Movements](#movements)
|
||||
- [Motor Song](#motor-song)
|
||||
|
||||
|
||||
## Examples
|
||||
|
||||
### Single Motor
|
||||
[motorshim_single_motor.cpp](motorshim_single_motor.cpp)
|
||||
|
||||
Demonstrates how to create a Motor object and control it.
|
||||
|
||||
|
||||
### Dual Motors
|
||||
[motorshim_dual_motors.cpp](motorshim_dual_motors.cpp)
|
||||
|
||||
Demonstrates how to create two Motor objects and control them together.
|
||||
|
||||
|
||||
### Movements
|
||||
[motorshim_movements.cpp](motorshim_movements.cpp)
|
||||
|
||||
An example of how to perform simple movements of a 2-wheeled driving robot.
|
||||
|
||||
|
||||
### Motor Song
|
||||
[motorshim_motor_song.cpp](motorshim_motor_song.cpp)
|
||||
|
||||
A fun example of how to change a motor's frequency to have it play a song.
|
||||
|
|
@ -0,0 +1,12 @@
|
|||
set(OUTPUT_NAME motorshim_dual_motors)
|
||||
add_executable(${OUTPUT_NAME} motorshim_dual_motors.cpp)
|
||||
|
||||
target_link_libraries(${OUTPUT_NAME}
|
||||
pico_stdlib
|
||||
pico_motor_shim
|
||||
)
|
||||
|
||||
# enable usb output
|
||||
pico_enable_stdio_usb(${OUTPUT_NAME} 1)
|
||||
|
||||
pico_add_extra_outputs(${OUTPUT_NAME})
|
||||
|
|
@ -0,0 +1,105 @@
|
|||
#include <math.h>
|
||||
#include "pico/stdlib.h"
|
||||
|
||||
#include "pico_motor_shim.hpp"
|
||||
|
||||
/*
|
||||
Demonstrates how to create two Motor objects and control them together.
|
||||
*/
|
||||
|
||||
using namespace motor;
|
||||
|
||||
// How many sweeps of the motors to perform
|
||||
const uint SWEEPS = 2;
|
||||
|
||||
// The number of discrete sweep steps
|
||||
const uint STEPS = 10;
|
||||
|
||||
// The time in milliseconds between each step of the sequence
|
||||
const uint STEPS_INTERVAL_MS = 500;
|
||||
|
||||
// How far from zero to drive the motors when sweeping
|
||||
constexpr float SPEED_EXTENT = 1.0f;
|
||||
|
||||
// Create an array of motor pointers
|
||||
const pin_pair motor_pins[] = {pico_motor_shim::MOTOR_1, pico_motor_shim::MOTOR_2};
|
||||
const uint NUM_MOTORS = count_of(motor_pins);
|
||||
Motor *motors[NUM_MOTORS];
|
||||
|
||||
|
||||
int main() {
|
||||
stdio_init_all();
|
||||
|
||||
// Fill the array of motors, and initialise them. Up to 8 motors can be created
|
||||
for(auto m = 0u; m < NUM_MOTORS; m++) {
|
||||
motors[m] = new Motor(motor_pins[m]);
|
||||
motors[m]->init();
|
||||
}
|
||||
|
||||
// Uncomment the below lines to reverse
|
||||
// the driving direction of a motor
|
||||
// motors[0].direction(REVERSED_DIR);
|
||||
// motors[1].direction(REVERSED_DIR);
|
||||
|
||||
// Enable all motors
|
||||
for(auto m = 0u; m < NUM_MOTORS; m++) {
|
||||
motors[m]->enable();
|
||||
}
|
||||
sleep_ms(2000);
|
||||
|
||||
// Drive at full positive
|
||||
for(auto m = 0u; m < NUM_MOTORS; m++) {
|
||||
motors[m]->full_positive();
|
||||
}
|
||||
sleep_ms(2000);
|
||||
|
||||
// Stop all moving
|
||||
for(auto m = 0u; m < NUM_MOTORS; m++) {
|
||||
motors[m]->stop();
|
||||
}
|
||||
sleep_ms(2000);
|
||||
|
||||
// Drive at full negative
|
||||
for(auto m = 0u; m < NUM_MOTORS; m++) {
|
||||
motors[m]->full_negative();
|
||||
}
|
||||
sleep_ms(2000);
|
||||
|
||||
// Coast all to a gradual stop
|
||||
for(auto m = 0u; m < NUM_MOTORS; m++) {
|
||||
motors[m]->coast();
|
||||
}
|
||||
sleep_ms(2000);
|
||||
|
||||
// Do a sine speed sweep
|
||||
for(auto j = 0u; j < SWEEPS; j++) {
|
||||
for(auto i = 0u; i < 360; i++) {
|
||||
float speed = sin(((float)i * (float)M_PI) / 180.0f) * SPEED_EXTENT;
|
||||
for(auto m = 0u; m < NUM_MOTORS; m++) {
|
||||
motors[m]->speed(speed);
|
||||
}
|
||||
sleep_ms(20);
|
||||
}
|
||||
}
|
||||
|
||||
// Do a stepped speed sweep
|
||||
for(auto j = 0u; j < SWEEPS; j++) {
|
||||
for(auto i = 0u; i < STEPS; i++) {
|
||||
for(auto m = 0u; m < NUM_MOTORS; m++) {
|
||||
motors[m]->to_percent(i, 0, STEPS, 0.0 - SPEED_EXTENT, SPEED_EXTENT);
|
||||
}
|
||||
sleep_ms(STEPS_INTERVAL_MS);
|
||||
}
|
||||
for(auto i = 0u; i < STEPS; i++) {
|
||||
for(auto m = 0u; m < NUM_MOTORS; m++) {
|
||||
motors[m]->to_percent(i, STEPS, 0, 0.0 - SPEED_EXTENT, SPEED_EXTENT);
|
||||
}
|
||||
sleep_ms(STEPS_INTERVAL_MS);
|
||||
}
|
||||
}
|
||||
|
||||
// Disable the motors
|
||||
for(auto m = 0u; m < NUM_MOTORS; m++) {
|
||||
motors[m]->disable();
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,13 @@
|
|||
set(OUTPUT_NAME motorshim_motor_song)
|
||||
add_executable(${OUTPUT_NAME} motorshim_motor_song.cpp)
|
||||
|
||||
target_link_libraries(${OUTPUT_NAME}
|
||||
pico_stdlib
|
||||
pico_motor_shim
|
||||
button
|
||||
)
|
||||
|
||||
# enable usb output
|
||||
pico_enable_stdio_usb(${OUTPUT_NAME} 1)
|
||||
|
||||
pico_add_extra_outputs(${OUTPUT_NAME})
|
||||
|
|
@ -0,0 +1,128 @@
|
|||
#include <stdio.h>
|
||||
#include "pico/stdlib.h"
|
||||
|
||||
#include "pico_motor_shim.hpp"
|
||||
#include "button.hpp"
|
||||
|
||||
/*
|
||||
A fun example of how to change a motor's frequency to have it play a song.
|
||||
|
||||
Press "A" to start or stop the song.
|
||||
*/
|
||||
|
||||
using namespace pimoroni;
|
||||
using namespace motor;
|
||||
|
||||
// List frequencies (in hz) to play in sequence here. Use zero for when silence or a pause is wanted
|
||||
// Song from PicoExplorer noise.py
|
||||
constexpr float SONG[] = {1397, 1397, 1319, 1397, 698, 0, 698, 0, 1047, 932,
|
||||
880, 1047, 1397, 0, 1397, 0, 1568, 1480, 1568, 784,
|
||||
0, 784, 0, 1568, 1397, 1319, 1175, 1047, 0, 1047,
|
||||
0, 1175, 1319, 1397, 1319, 1175, 1047, 1175, 1047, 932,
|
||||
880, 932, 880, 784, 698, 784, 698, 659, 587, 523,
|
||||
587, 659, 698, 784, 932, 880, 784, 880, 698, 0, 698};
|
||||
constexpr uint SONG_LENGTH = count_of(SONG);
|
||||
|
||||
// The time (in seconds) to play each note for. Change this to make the song play faster or slower
|
||||
const uint NOTE_DURATION_MS = 150;
|
||||
|
||||
// The time (in microseconds) between each direction switch of the motor when using STATIONARY_PLAYBACK
|
||||
const uint STATIONARY_TOGGLE_US = 2000;
|
||||
|
||||
// Whether to play the song with or without the motors spinning
|
||||
const bool STATIONARY_PLAYBACK = false;
|
||||
|
||||
// The motor decay mode to use, either FAST_DECAY (0) or SLOW_DECAY (1). Affects the song's quality
|
||||
const DecayMode DECAY_MODE = SLOW_DECAY;
|
||||
|
||||
|
||||
// Create two motor objects with a given decay mode
|
||||
Motor motor_1(pico_motor_shim::MOTOR_1);
|
||||
Motor motor_2(pico_motor_shim::MOTOR_2);
|
||||
|
||||
// Create the user button
|
||||
Button button_a(pico_motor_shim::BUTTON_A, Polarity::ACTIVE_LOW, 0);
|
||||
|
||||
// Variable for recording if the button has been toggled
|
||||
// Starting as true makes the song play automatically
|
||||
static bool button_toggle = true;
|
||||
|
||||
// Checks if the button has been pressed, toggling a value that is also returned.
|
||||
bool check_button_toggle() {
|
||||
if(button_a.read()) {
|
||||
button_toggle = !button_toggle;
|
||||
}
|
||||
return button_toggle;
|
||||
}
|
||||
|
||||
|
||||
int main() {
|
||||
stdio_init_all();
|
||||
|
||||
//Initialise the LED. We use this to indicate that the sequence is running.
|
||||
gpio_init(PICO_DEFAULT_LED_PIN);
|
||||
gpio_set_dir(PICO_DEFAULT_LED_PIN, GPIO_OUT);
|
||||
|
||||
// Se the two motor's decay modes
|
||||
motor_1.decay_mode(DECAY_MODE);
|
||||
motor_2.decay_mode(DECAY_MODE);
|
||||
|
||||
// Initialise the motors
|
||||
if(!motor_1.init() || !motor_2.init()) {
|
||||
printf("Cannot initialise the motors. Check the provided parameters\n");
|
||||
return 0;
|
||||
}
|
||||
|
||||
while(true) {
|
||||
// Has the button been toggled?
|
||||
if(check_button_toggle()) {
|
||||
// Turn the Pico's LED on to show that the song has started
|
||||
gpio_put(PICO_DEFAULT_LED_PIN, true);
|
||||
|
||||
// Play the song
|
||||
for(uint i = 0; i < SONG_LENGTH && check_button_toggle(); i++) {
|
||||
// Get the frequency of the note and attempt to set the motors to it
|
||||
float freq = SONG[i];
|
||||
if(motor_1.frequency(freq) && motor_2.frequency(freq)) {
|
||||
if(STATIONARY_PLAYBACK) {
|
||||
// Set the motors to 50% duty cycle to play the note, but alternate
|
||||
// the direction so that the motor does not actually spin
|
||||
uint t = 0;
|
||||
while(t < NOTE_DURATION_MS * 1000) {
|
||||
motor_1.duty(0.5f);
|
||||
motor_2.duty(0.5f);
|
||||
sleep_us(STATIONARY_TOGGLE_US);
|
||||
t += STATIONARY_TOGGLE_US;
|
||||
|
||||
motor_1.duty(-0.5f);
|
||||
motor_2.duty(-0.5f);
|
||||
sleep_us(STATIONARY_TOGGLE_US);
|
||||
t += STATIONARY_TOGGLE_US;
|
||||
}
|
||||
}
|
||||
else {
|
||||
// Set the motors to 50% duty cycle to play the note
|
||||
motor_1.duty(0.5f);
|
||||
motor_2.duty(0.5f);
|
||||
sleep_ms(NOTE_DURATION_MS);
|
||||
}
|
||||
}
|
||||
else {
|
||||
// The frequency was invalid, so we are treating that to mean this is a pause note
|
||||
motor_1.stop();
|
||||
motor_2.stop();
|
||||
sleep_ms(NOTE_DURATION_MS);
|
||||
}
|
||||
}
|
||||
button_toggle = false;
|
||||
|
||||
// The song has finished, so turn off the Pico's LED and disable the motors
|
||||
gpio_put(PICO_DEFAULT_LED_PIN, false);
|
||||
motor_1.disable();
|
||||
motor_2.disable();
|
||||
}
|
||||
|
||||
sleep_ms(10);
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -0,0 +1,12 @@
|
|||
set(OUTPUT_NAME motorshim_movements)
|
||||
add_executable(${OUTPUT_NAME} motorshim_movements.cpp)
|
||||
|
||||
target_link_libraries(${OUTPUT_NAME}
|
||||
pico_stdlib
|
||||
pico_motor_shim
|
||||
)
|
||||
|
||||
# enable usb output
|
||||
pico_enable_stdio_usb(${OUTPUT_NAME} 1)
|
||||
|
||||
pico_add_extra_outputs(${OUTPUT_NAME})
|
||||
|
|
@ -0,0 +1,126 @@
|
|||
#include <stdio.h>
|
||||
#include "pico/stdlib.h"
|
||||
|
||||
#include "pico_motor_shim.hpp"
|
||||
|
||||
/*
|
||||
An example of how to perform simple movements of a 2-wheeled driving robot.
|
||||
*/
|
||||
|
||||
using namespace motor;
|
||||
|
||||
// The scaling to apply to each motor's speed to match its real-world speed
|
||||
constexpr float SPEED_SCALE = 5.4f;
|
||||
|
||||
// The speed to drive the wheels at, from 0.0 to SPEED_SCALE
|
||||
constexpr float DRIVING_SPEED = SPEED_SCALE;
|
||||
|
||||
|
||||
// Create the left and right motors with a given speed scale
|
||||
// Swap the numbers and directions if this is different to your setup
|
||||
Motor left(pico_motor_shim::MOTOR_1, NORMAL_DIR, SPEED_SCALE);
|
||||
Motor right(pico_motor_shim::MOTOR_2, REVERSED_DIR, SPEED_SCALE);
|
||||
|
||||
|
||||
// Helper functions for driving in common directions
|
||||
void forward(float speed=DRIVING_SPEED) {
|
||||
left.speed(speed);
|
||||
right.speed(speed);
|
||||
}
|
||||
|
||||
void backward(float speed=DRIVING_SPEED) {
|
||||
left.speed(-speed);
|
||||
right.speed(-speed);
|
||||
}
|
||||
|
||||
void turn_left(float speed=DRIVING_SPEED) {
|
||||
left.speed(-speed);
|
||||
right.speed(speed);
|
||||
}
|
||||
|
||||
void turn_right(float speed=DRIVING_SPEED) {
|
||||
left.speed(speed);
|
||||
right.speed(-speed);
|
||||
}
|
||||
|
||||
void curve_forward_left(float speed=DRIVING_SPEED) {
|
||||
left.speed(0.0);
|
||||
right.speed(speed);
|
||||
}
|
||||
|
||||
void curve_forward_right(float speed=DRIVING_SPEED) {
|
||||
left.speed(speed);
|
||||
right.speed(0.0);
|
||||
}
|
||||
|
||||
void curve_backward_left(float speed=DRIVING_SPEED) {
|
||||
left.speed(0.0);
|
||||
right.speed(-speed);
|
||||
}
|
||||
|
||||
void curve_backward_right(float speed=DRIVING_SPEED) {
|
||||
left.speed(-speed);
|
||||
right.speed(0.0);
|
||||
}
|
||||
|
||||
void stop() {
|
||||
left.stop();
|
||||
right.stop();
|
||||
}
|
||||
|
||||
void coast() {
|
||||
left.coast();
|
||||
right.coast();
|
||||
}
|
||||
|
||||
|
||||
int main() {
|
||||
stdio_init_all();
|
||||
|
||||
// Initialise the motors
|
||||
if(!left.init() || !right.init()) {
|
||||
printf("Cannot initialise the motors. Check the provided parameters\n");
|
||||
return 0;
|
||||
}
|
||||
|
||||
// Demo each of the move methods
|
||||
forward();
|
||||
sleep_ms(1000);
|
||||
|
||||
backward();
|
||||
sleep_ms(1000);
|
||||
|
||||
curve_forward_right();
|
||||
sleep_ms(1000);
|
||||
|
||||
curve_forward_left();
|
||||
sleep_ms(1000);
|
||||
|
||||
turn_right();
|
||||
sleep_ms(1000);
|
||||
|
||||
forward(0.5 * DRIVING_SPEED); // Half speed
|
||||
sleep_ms(1000);
|
||||
|
||||
turn_left(0.5 * DRIVING_SPEED); // Half speed
|
||||
sleep_ms(1000);
|
||||
|
||||
curve_backward_right(0.75 * DRIVING_SPEED); // Three quarters speed
|
||||
sleep_ms(1000);
|
||||
|
||||
forward(); // Full speed
|
||||
sleep_ms(500);
|
||||
|
||||
coast(); // Come to a halt gently
|
||||
sleep_ms(1000);
|
||||
|
||||
forward();
|
||||
sleep_ms(500);
|
||||
|
||||
stop(); // Apply the brakes
|
||||
sleep_ms(1000);
|
||||
|
||||
// Disable the motors
|
||||
left.disable();
|
||||
right.disable();
|
||||
}
|
||||
|
|
@ -0,0 +1,12 @@
|
|||
set(OUTPUT_NAME motorshim_single_motor)
|
||||
add_executable(${OUTPUT_NAME} motorshim_single_motor.cpp)
|
||||
|
||||
target_link_libraries(${OUTPUT_NAME}
|
||||
pico_stdlib
|
||||
pico_motor_shim
|
||||
)
|
||||
|
||||
# enable usb output
|
||||
pico_enable_stdio_usb(${OUTPUT_NAME} 1)
|
||||
|
||||
pico_add_extra_outputs(${OUTPUT_NAME})
|
||||
|
|
@ -0,0 +1,78 @@
|
|||
#include <math.h>
|
||||
#include "pico/stdlib.h"
|
||||
|
||||
#include "pico_motor_shim.hpp"
|
||||
|
||||
/*
|
||||
Demonstrates how to create a Motor object and control it.
|
||||
*/
|
||||
|
||||
using namespace motor;
|
||||
|
||||
// How many sweeps of the motor to perform
|
||||
const uint SWEEPS = 2;
|
||||
|
||||
// The number of discrete sweep steps
|
||||
const uint STEPS = 10;
|
||||
|
||||
// The time in milliseconds between each step of the sequence
|
||||
const uint STEPS_INTERVAL_MS = 500;
|
||||
|
||||
// How far from zero to drive the motor when sweeping
|
||||
constexpr float SPEED_EXTENT = 1.0f;
|
||||
|
||||
|
||||
// Create a motor
|
||||
Motor m = Motor(pico_motor_shim::MOTOR_1);
|
||||
|
||||
|
||||
int main() {
|
||||
stdio_init_all();
|
||||
|
||||
// Initialise the motor
|
||||
m.init();
|
||||
|
||||
// Enable the motor
|
||||
m.enable();
|
||||
sleep_ms(2000);
|
||||
|
||||
// Drive at full positive
|
||||
m.full_positive();
|
||||
sleep_ms(2000);
|
||||
|
||||
// Stop moving
|
||||
m.stop();
|
||||
sleep_ms(2000);
|
||||
|
||||
// Drive at full negative
|
||||
m.full_negative();
|
||||
sleep_ms(2000);
|
||||
|
||||
// Coast to a gradual stop
|
||||
m.coast();
|
||||
sleep_ms(2000);
|
||||
|
||||
|
||||
// Do a sine speed sweep
|
||||
for(auto j = 0u; j < SWEEPS; j++) {
|
||||
for(auto i = 0u; i < 360; i++) {
|
||||
m.speed(sin(((float)i * (float)M_PI) / 180.0f) * SPEED_EXTENT);
|
||||
sleep_ms(20);
|
||||
}
|
||||
}
|
||||
|
||||
// Do a stepped speed sweep
|
||||
for(auto j = 0u; j < SWEEPS; j++) {
|
||||
for(auto i = 0u; i < STEPS; i++) {
|
||||
m.to_percent(i, 0, STEPS, 0.0 - SPEED_EXTENT, SPEED_EXTENT);
|
||||
sleep_ms(STEPS_INTERVAL_MS);
|
||||
}
|
||||
for(auto i = 0u; i < STEPS; i++) {
|
||||
m.to_percent(i, STEPS, 0, 0.0 - SPEED_EXTENT, SPEED_EXTENT);
|
||||
sleep_ms(STEPS_INTERVAL_MS);
|
||||
}
|
||||
}
|
||||
|
||||
// Disable the motor
|
||||
m.disable();
|
||||
}
|
||||
|
|
@ -25,8 +25,10 @@ add_subdirectory(pico_display_2)
|
|||
add_subdirectory(pico_unicorn)
|
||||
add_subdirectory(pico_scroll)
|
||||
add_subdirectory(pico_explorer)
|
||||
add_subdirectory(pico_motor_shim)
|
||||
add_subdirectory(pico_rgb_keypad)
|
||||
add_subdirectory(pico_wireless)
|
||||
add_subdirectory(plasma2040)
|
||||
add_subdirectory(badger2040)
|
||||
add_subdirectory(servo2040)
|
||||
add_subdirectory(motor2040)
|
||||
|
|
|
|||
|
|
@ -0,0 +1 @@
|
|||
include(motor2040.cmake)
|
||||
|
|
@ -0,0 +1,6 @@
|
|||
add_library(motor2040 INTERFACE)
|
||||
|
||||
target_include_directories(motor2040 INTERFACE ${CMAKE_CURRENT_LIST_DIR})
|
||||
|
||||
# Pull in pico libraries that we need
|
||||
target_link_libraries(motor2040 INTERFACE pico_stdlib plasma motor motor_cluster encoder)
|
||||
|
|
@ -0,0 +1,77 @@
|
|||
#pragma once
|
||||
#include "pico/stdlib.h"
|
||||
|
||||
#include "ws2812.hpp"
|
||||
#include "motor.hpp"
|
||||
#include "motor_cluster.hpp"
|
||||
#include "encoder.hpp"
|
||||
|
||||
namespace motor {
|
||||
namespace motor2040 {
|
||||
const uint MOTOR_A_P = 4;
|
||||
const uint MOTOR_A_N = 5;
|
||||
const uint MOTOR_B_P = 6;
|
||||
const uint MOTOR_B_N = 7;
|
||||
const uint MOTOR_C_P = 8;
|
||||
const uint MOTOR_C_N = 9;
|
||||
const uint MOTOR_D_P = 10;
|
||||
const uint MOTOR_D_N = 11;
|
||||
|
||||
const pin_pair MOTOR_A(MOTOR_A_P, MOTOR_A_N);
|
||||
const pin_pair MOTOR_B(MOTOR_B_P, MOTOR_B_N);
|
||||
const pin_pair MOTOR_C(MOTOR_C_P, MOTOR_C_N);
|
||||
const pin_pair MOTOR_D(MOTOR_D_P, MOTOR_D_N);
|
||||
const uint NUM_MOTORS = 4;
|
||||
|
||||
const uint ENCODER_A_A = 0;
|
||||
const uint ENCODER_A_B = 1;
|
||||
const uint ENCODER_B_A = 2;
|
||||
const uint ENCODER_B_B = 3;
|
||||
const uint ENCODER_C_A = 12;
|
||||
const uint ENCODER_C_B = 13;
|
||||
const uint ENCODER_D_A = 14;
|
||||
const uint ENCODER_D_B = 15;
|
||||
|
||||
// Although encoder A and B channels are arbitrary, our MMME Encoders
|
||||
// that accompany Motor2040 count down when the motors are diving in a
|
||||
// positive direction, so these pin pairs are set as B and A instead
|
||||
const pin_pair ENCODER_A(ENCODER_A_B, ENCODER_A_A);
|
||||
const pin_pair ENCODER_B(ENCODER_B_B, ENCODER_B_A);
|
||||
const pin_pair ENCODER_C(ENCODER_C_B, ENCODER_C_A);
|
||||
const pin_pair ENCODER_D(ENCODER_D_B, ENCODER_D_A);
|
||||
const uint NUM_ENCODERS = 4;
|
||||
|
||||
const uint TX_TRIG = 16;
|
||||
const uint RX_ECHO = 17;
|
||||
|
||||
const uint LED_DATA = 18;
|
||||
const uint NUM_LEDS = 1;
|
||||
|
||||
const uint USER_SW = 23;
|
||||
|
||||
const uint ADC_ADDR_0 = 22;
|
||||
const uint ADC_ADDR_1 = 24;
|
||||
const uint ADC_ADDR_2 = 25;
|
||||
|
||||
const uint ADC0 = 26;
|
||||
const uint ADC1 = 27;
|
||||
const uint ADC2 = 28;
|
||||
const uint SHARED_ADC = 29; // The pin used for the board's sensing features
|
||||
|
||||
const uint CURRENT_SENSE_A_ADDR = 0b000;
|
||||
const uint CURRENT_SENSE_B_ADDR = 0b001;
|
||||
const uint CURRENT_SENSE_C_ADDR = 0b010;
|
||||
const uint CURRENT_SENSE_D_ADDR = 0b011;
|
||||
const uint VOLTAGE_SENSE_ADDR = 0b100;
|
||||
const uint FAULT_SENSE_ADDR = 0b101;
|
||||
|
||||
const uint SENSOR_1_ADDR = 0b110;
|
||||
const uint SENSOR_2_ADDR = 0b111;
|
||||
const uint NUM_SENSORS = 2;
|
||||
|
||||
constexpr float SHUNT_RESISTOR = 0.47f;
|
||||
constexpr float CURRENT_GAIN = 1;
|
||||
constexpr float VOLTAGE_GAIN = 3.9f / 13.9f;
|
||||
constexpr float CURRENT_OFFSET = -0.005f;
|
||||
}
|
||||
}
|
||||
|
|
@ -35,7 +35,7 @@ namespace pimoroni {
|
|||
|
||||
// setup motor pins
|
||||
pwm_config motor_pwm_cfg = pwm_get_default_config();
|
||||
pwm_config_set_wrap(&motor_pwm_cfg, 255);
|
||||
pwm_config_set_wrap(&motor_pwm_cfg, 5500);
|
||||
|
||||
pwm_init(pwm_gpio_to_slice_num(MOTOR1N), &motor_pwm_cfg, true);
|
||||
gpio_set_function(MOTOR1N, GPIO_FUNC_PWM);
|
||||
|
|
@ -76,20 +76,20 @@ namespace pimoroni {
|
|||
|
||||
switch(action) {
|
||||
case FORWARD: {
|
||||
pwm_set_gpio_level(p, speed * 255);
|
||||
pwm_set_gpio_level(n, 0);
|
||||
pwm_set_gpio_level(n, (1 - speed) * 5500);
|
||||
pwm_set_gpio_level(p, 5500);
|
||||
break;
|
||||
}
|
||||
|
||||
case REVERSE: {
|
||||
pwm_set_gpio_level(p, 0);
|
||||
pwm_set_gpio_level(n, speed * 255);
|
||||
pwm_set_gpio_level(n, 5500);
|
||||
pwm_set_gpio_level(p, (1 - speed) * 5500);
|
||||
break;
|
||||
}
|
||||
|
||||
case STOP: {
|
||||
pwm_set_gpio_level(p, 0);
|
||||
pwm_set_gpio_level(n, 0);
|
||||
pwm_set_gpio_level(p, 5500);
|
||||
pwm_set_gpio_level(n, 5500);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
|
@ -116,4 +116,4 @@ namespace pimoroni {
|
|||
}
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -0,0 +1 @@
|
|||
include(pico_motor_shim.cmake)
|
||||
|
|
@ -0,0 +1,6 @@
|
|||
add_library(pico_motor_shim INTERFACE)
|
||||
|
||||
target_include_directories(pico_motor_shim INTERFACE ${CMAKE_CURRENT_LIST_DIR})
|
||||
|
||||
# Pull in pico libraries that we need
|
||||
target_link_libraries(pico_motor_shim INTERFACE pico_stdlib motor)
|
||||
|
|
@ -0,0 +1,19 @@
|
|||
#pragma once
|
||||
#include "pico/stdlib.h"
|
||||
|
||||
#include "motor.hpp"
|
||||
|
||||
namespace motor {
|
||||
namespace pico_motor_shim {
|
||||
const uint BUTTON_A = 2;
|
||||
|
||||
const uint MOTOR_1_P = 6;
|
||||
const uint MOTOR_1_N = 7;
|
||||
const uint MOTOR_2_P = 27;
|
||||
const uint MOTOR_2_N = 26;
|
||||
|
||||
const pin_pair MOTOR_1(MOTOR_1_P, MOTOR_1_N);
|
||||
const pin_pair MOTOR_2(MOTOR_2_P, MOTOR_2_N);
|
||||
const uint NUM_MOTORS = 2;
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,40 @@
|
|||
# Encoder MicroPython Examples <!-- omit in toc -->
|
||||
|
||||
- [Examples](#examples)
|
||||
- [Read Counts](#read-counts)
|
||||
- [Read Change](#read-change)
|
||||
- [Read Speed](#read-speed)
|
||||
- [Value Dial](#value-dial)
|
||||
- [Item Selector](#item-selector)
|
||||
|
||||
|
||||
## Examples
|
||||
|
||||
### Read Counts
|
||||
[read_counts.py](read_counts.py)
|
||||
|
||||
An example of how to read a mechanical rotary encoder.
|
||||
|
||||
|
||||
### Read Change
|
||||
[read_change.py](read_change.py)
|
||||
|
||||
An example of how to read a mechanical rotary encoder, only when a change has occurred.
|
||||
|
||||
|
||||
### Read Speed
|
||||
[read_speed.py](read_speed.py)
|
||||
|
||||
An example of how to read the speed a mechanical rotary encoder is being turned at.
|
||||
|
||||
|
||||
### Value Dial
|
||||
[value_dial.py](value_dial.py)
|
||||
|
||||
A demonstration of a rotary encoder being used to control a value.
|
||||
|
||||
|
||||
### Item Selector
|
||||
[item_selector.py](item_selector.py)
|
||||
|
||||
A demonstration of a rotary encoder being used to select items based on its physical position.
|
||||
|
|
@ -0,0 +1,39 @@
|
|||
import gc
|
||||
from encoder import Encoder
|
||||
# from encoder import REVERSED_DIR
|
||||
|
||||
"""
|
||||
A demonstration of a rotary encoder being used to
|
||||
select items based on its physical position.
|
||||
|
||||
This requires that the encoder is positioned in the same
|
||||
direction at the start of every program run (e.g. upwards).
|
||||
"""
|
||||
|
||||
# Free up hardware resources ahead of creating a new Encoder
|
||||
gc.collect()
|
||||
|
||||
# Create an encoder on the 3 ADC pins, using PIO 0 and State Machine 0
|
||||
PIN_A = 26 # The A channel pin
|
||||
PIN_B = 28 # The B channel pin
|
||||
PIN_C = 27 # The common pin
|
||||
enc = Encoder(0, 0, (PIN_A, PIN_B), PIN_C)
|
||||
|
||||
# Uncomment the below line (and the top import) to reverse the counting direction
|
||||
# enc.direction(REVERSED_DIR)
|
||||
|
||||
|
||||
# A list of items, up to the encoder's counts_per_rev
|
||||
ITEMS = ["Red", "Orange", "Yellow", "Green", "Blue", "Indigo", "Violet", "Black", "White"]
|
||||
|
||||
last_step = -1
|
||||
|
||||
# Loop forever
|
||||
while True:
|
||||
step = enc.step()
|
||||
if step != last_step:
|
||||
if step < len(ITEMS):
|
||||
print(step, "/", int(enc.counts_per_rev()) - 1, ": ", ITEMS[step], sep="")
|
||||
else:
|
||||
print(step, "/", int(enc.counts_per_rev()) - 1, ": ", "Undefined", sep="")
|
||||
last_step = step
|
||||
|
|
@ -0,0 +1,33 @@
|
|||
import gc
|
||||
from encoder import Encoder
|
||||
# from encoder import REVERSED_DIR
|
||||
|
||||
"""
|
||||
An example of how to read a mechanical rotary encoder, only when a change has occurred.
|
||||
"""
|
||||
|
||||
# Free up hardware resources ahead of creating a new Encoder
|
||||
gc.collect()
|
||||
|
||||
# Create an encoder on the 3 ADC pins, using PIO 0 and State Machine 0
|
||||
PIN_A = 26 # The A channel pin
|
||||
PIN_B = 28 # The B channel pin
|
||||
PIN_C = 27 # The common pin
|
||||
enc = Encoder(0, 0, (PIN_A, PIN_B), PIN_C)
|
||||
|
||||
# Uncomment the below line (and the top import) to reverse the counting direction
|
||||
# enc.direction(REVERSED_DIR)
|
||||
|
||||
|
||||
# Print out the initial count, step, and turn (they should all be zero)
|
||||
print("Count =", enc.count(), end=", ")
|
||||
print("Step =", enc.step(), end=", ")
|
||||
print("Turn =", enc.turn())
|
||||
|
||||
# Loop forever
|
||||
while True:
|
||||
if enc.delta() != 0:
|
||||
# Print out the new count, step, and turn
|
||||
print("Count =", enc.count(), end=", ")
|
||||
print("Step =", enc.step(), end=", ")
|
||||
print("Turn =", enc.turn())
|
||||
|
|
@ -0,0 +1,32 @@
|
|||
import gc
|
||||
import time
|
||||
from encoder import Encoder
|
||||
# from encoder import REVERSED_DIR
|
||||
|
||||
"""
|
||||
An example of how to read a mechanical rotary encoder.
|
||||
"""
|
||||
|
||||
# Free up hardware resources ahead of creating a new Encoder
|
||||
gc.collect()
|
||||
|
||||
# Create an encoder on the 3 ADC pins, using PIO 0 and State Machine 0
|
||||
PIN_A = 26 # The A channel pin
|
||||
PIN_B = 28 # The B channel pin
|
||||
PIN_C = 27 # The common pin
|
||||
enc = Encoder(0, 0, (PIN_A, PIN_B), PIN_C)
|
||||
|
||||
# Uncomment the below line (and the top import) to reverse the counting direction
|
||||
# enc.direction(REVERSED_DIR)
|
||||
|
||||
|
||||
# Loop forever
|
||||
while True:
|
||||
|
||||
# Print out the count, delta, step, and turn
|
||||
print("Count =", enc.count(), end=", ")
|
||||
print("Delta =", enc.delta(), end=", ")
|
||||
print("Step =", enc.step(), end=", ")
|
||||
print("Turn =", enc.turn())
|
||||
|
||||
time.sleep(0.1)
|
||||
|
|
@ -0,0 +1,32 @@
|
|||
import gc
|
||||
import time
|
||||
from encoder import Encoder
|
||||
# from encoder import REVERSED_DIR
|
||||
|
||||
"""
|
||||
An example of how to read the speed a mechanical rotary encoder is being turned at.
|
||||
"""
|
||||
|
||||
# Free up hardware resources ahead of creating a new Encoder
|
||||
gc.collect()
|
||||
|
||||
# Create an encoder on the 3 ADC pins, using PIO 0 and State Machine 0
|
||||
PIN_A = 26 # The A channel pin
|
||||
PIN_B = 28 # The B channel pin
|
||||
PIN_C = 27 # The common pin
|
||||
enc = Encoder(0, 0, (PIN_A, PIN_B), PIN_C)
|
||||
|
||||
# Uncomment the below line (and the top import) to reverse the counting direction
|
||||
# enc.direction(REVERSED_DIR)
|
||||
|
||||
|
||||
# Loop forever
|
||||
while True:
|
||||
capture = enc.capture()
|
||||
|
||||
print("Count =", capture.count, end=", ")
|
||||
print("Angle =", capture.degrees, end=", ")
|
||||
print("Freq =", capture.frequency, end=", ")
|
||||
print("Speed =", capture.degrees_per_second)
|
||||
|
||||
time.sleep(0.1)
|
||||
|
|
@ -0,0 +1,40 @@
|
|||
import gc
|
||||
from encoder import Encoder
|
||||
# from encoder import REVERSED_DIR
|
||||
|
||||
"""
|
||||
A demonstration of a rotary encoder being used to control a value.
|
||||
"""
|
||||
|
||||
# Free up hardware resources ahead of creating a new Encoder
|
||||
gc.collect()
|
||||
|
||||
# Create an encoder on the 3 ADC pins, using PIO 0 and State Machine 0
|
||||
PIN_A = 26 # The A channel pin
|
||||
PIN_B = 28 # The B channel pin
|
||||
PIN_C = 27 # The common pin
|
||||
enc = Encoder(0, 0, (PIN_A, PIN_B), PIN_C)
|
||||
|
||||
# Uncomment the below line (and the top import) to reverse the counting direction
|
||||
# enc.direction(REVERSED_DIR)
|
||||
|
||||
# The min and max value
|
||||
MIN_VALUE = 0
|
||||
MAX_VALUE = 100
|
||||
|
||||
value = 50
|
||||
|
||||
# Print out the initial value
|
||||
print("Value =", value)
|
||||
|
||||
# Loop forever
|
||||
while True:
|
||||
delta = enc.delta()
|
||||
if delta != 0:
|
||||
if delta > 0:
|
||||
value = min(value + 1, MAX_VALUE)
|
||||
else:
|
||||
value = max(value - 1, MIN_VALUE)
|
||||
|
||||
# Print out the new value
|
||||
print("Value =", value)
|
||||
Some files were not shown because too many files have changed in this diff Show More
Loading…
Reference in New Issue