From 8a36102c53e9400a65f8c5cc94992705d5a1cadb Mon Sep 17 00:00:00 2001 From: ZodiusInfuser Date: Mon, 28 Mar 2022 18:32:05 +0100 Subject: [PATCH 01/60] Merged in old motor implementation --- drivers/CMakeLists.txt | 1 + drivers/motor/CMakeLists.txt | 1 + drivers/motor/motor.cmake | 14 ++ drivers/motor/motor.cpp | 147 +++++++++++++++ drivers/motor/motor.hpp | 72 ++++++++ examples/CMakeLists.txt | 1 + examples/pico_motor_shim/CMakeLists.txt | 3 + .../pico_motor_shim/balance/CMakeLists.txt | 16 ++ examples/pico_motor_shim/balance/demo.cpp | 118 ++++++++++++ .../pico_motor_shim/sequence/CMakeLists.txt | 16 ++ examples/pico_motor_shim/sequence/demo.cpp | 169 ++++++++++++++++++ examples/pico_motor_shim/song/CMakeLists.txt | 16 ++ examples/pico_motor_shim/song/demo.cpp | 126 +++++++++++++ libraries/CMakeLists.txt | 1 + libraries/pico_motor_shim/CMakeLists.txt | 1 + .../pico_motor_shim/pico_motor_shim.cmake | 6 + libraries/pico_motor_shim/pico_motor_shim.hpp | 12 ++ micropython/modules/micropython.cmake | 1 + .../modules/pico_motor_shim/micropython.cmake | 18 ++ .../modules/pico_motor_shim/pico_motor_shim.c | 40 +++++ .../modules/pico_motor_shim/pico_motor_shim.h | 3 + micropython/modules_py/pimoroni.py | 73 ++++++++ 22 files changed, 855 insertions(+) create mode 100644 drivers/motor/CMakeLists.txt create mode 100644 drivers/motor/motor.cmake create mode 100644 drivers/motor/motor.cpp create mode 100644 drivers/motor/motor.hpp create mode 100644 examples/pico_motor_shim/CMakeLists.txt create mode 100644 examples/pico_motor_shim/balance/CMakeLists.txt create mode 100644 examples/pico_motor_shim/balance/demo.cpp create mode 100644 examples/pico_motor_shim/sequence/CMakeLists.txt create mode 100644 examples/pico_motor_shim/sequence/demo.cpp create mode 100644 examples/pico_motor_shim/song/CMakeLists.txt create mode 100644 examples/pico_motor_shim/song/demo.cpp create mode 100644 libraries/pico_motor_shim/CMakeLists.txt create mode 100644 libraries/pico_motor_shim/pico_motor_shim.cmake create mode 100644 libraries/pico_motor_shim/pico_motor_shim.hpp create mode 100644 micropython/modules/pico_motor_shim/micropython.cmake create mode 100644 micropython/modules/pico_motor_shim/pico_motor_shim.c create mode 100644 micropython/modules/pico_motor_shim/pico_motor_shim.h diff --git a/drivers/CMakeLists.txt b/drivers/CMakeLists.txt index 32c6169c..68549297 100644 --- a/drivers/CMakeLists.txt +++ b/drivers/CMakeLists.txt @@ -29,3 +29,4 @@ add_subdirectory(hub75) add_subdirectory(uc8151) add_subdirectory(pwm) add_subdirectory(servo) +add_subdirectory(motor) diff --git a/drivers/motor/CMakeLists.txt b/drivers/motor/CMakeLists.txt new file mode 100644 index 00000000..086269fe --- /dev/null +++ b/drivers/motor/CMakeLists.txt @@ -0,0 +1 @@ +include(motor.cmake) \ No newline at end of file diff --git a/drivers/motor/motor.cmake b/drivers/motor/motor.cmake new file mode 100644 index 00000000..7f498ab6 --- /dev/null +++ b/drivers/motor/motor.cmake @@ -0,0 +1,14 @@ +set(DRIVER_NAME motor) +add_library(${DRIVER_NAME} INTERFACE) + +target_sources(${DRIVER_NAME} INTERFACE + ${CMAKE_CURRENT_LIST_DIR}/motor.cpp +) + +target_include_directories(${DRIVER_NAME} INTERFACE ${CMAKE_CURRENT_LIST_DIR}) + +target_link_libraries(${DRIVER_NAME} INTERFACE + pico_stdlib + hardware_pwm + pwm + ) \ No newline at end of file diff --git a/drivers/motor/motor.cpp b/drivers/motor/motor.cpp new file mode 100644 index 00000000..50dbe95b --- /dev/null +++ b/drivers/motor/motor.cpp @@ -0,0 +1,147 @@ +#include "motor.hpp" +#include "pwm.hpp" + +namespace pimoroni { + Motor::Motor(uint pin_pos, uint pin_neg, float freq, DecayMode mode) + : pin_pos(pin_pos), pin_neg(pin_neg), pwm_frequency(freq), motor_decay_mode(mode) { + } + + Motor::~Motor() { + gpio_set_function(pin_pos, GPIO_FUNC_NULL); + gpio_set_function(pin_neg, GPIO_FUNC_NULL); + } + + bool Motor::init() { + bool success = false; + + uint16_t period; uint16_t div16; + if(pimoroni::calculate_pwm_factors(pwm_frequency, period, div16)) { + pwm_period = period; + + 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); + + pwm_init(pwm_gpio_to_slice_num(pin_pos), &pwm_cfg, true); + gpio_set_function(pin_pos, GPIO_FUNC_PWM); + + pwm_init(pwm_gpio_to_slice_num(pin_neg), &pwm_cfg, true); + gpio_set_function(pin_neg, GPIO_FUNC_PWM); + update_pwm(); + + success = true; + } + return success; + } + + float Motor::get_speed() { + return motor_speed; + } + + void Motor::set_speed(float speed) { + motor_speed = MIN(MAX(speed, -1.0f), 1.0f); + update_pwm(); + } + + float Motor::get_frequency() { + return pwm_frequency; + } + + bool Motor::set_frequency(float freq) { + bool success = false; + + //Calculate a suitable pwm wrap period for this frequency + uint16_t period; uint16_t div16; + if(pimoroni::calculate_pwm_factors(freq, period, div16)) { + + //Record if the new period will be larger or smaller. + //This is used to apply new pwm values 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; + pwm_frequency = freq; + + uint pos_num = pwm_gpio_to_slice_num(pin_pos); + uint neg_num = pwm_gpio_to_slice_num(pin_neg); + + //Apply the new divider + uint8_t div = div16 >> 4; + uint8_t mod = div16 % 16; + pwm_set_clkdiv_int_frac(pos_num, div, mod); + if(neg_num != pos_num) { + pwm_set_clkdiv_int_frac(neg_num, div, mod); + } + + //If the the period is larger, update the pwm before setting the new wraps + if(pre_update_pwm) + update_pwm(); + + //Set the new wrap (should be 1 less than the period to get full 0 to 100%) + pwm_set_wrap(pos_num, pwm_period - 1); + if(neg_num != pos_num) { + pwm_set_wrap(neg_num, pwm_period - 1); + } + + //If the the period is smaller, update the pwm after setting the new wraps + if(!pre_update_pwm) + update_pwm(); + + success = true; + } + return success; + } + + Motor::DecayMode Motor::get_decay_mode() { + return motor_decay_mode; + } + + void Motor::set_decay_mode(Motor::DecayMode mode) { + motor_decay_mode = mode; + update_pwm(); + } + + void Motor::stop() { + motor_speed = 0.0f; + update_pwm(); + } + + void Motor::disable() { + motor_speed = 0.0f; + pwm_set_gpio_level(pin_pos, 0); + pwm_set_gpio_level(pin_neg, 0); + } + + void Motor::update_pwm() { + int32_t signed_duty_cycle = (int32_t)(motor_speed * (float)pwm_period); + + switch(motor_decay_mode) { + case SLOW_DECAY: //aka 'Braking' + if(signed_duty_cycle >= 0) { + pwm_set_gpio_level(pin_pos, pwm_period); + pwm_set_gpio_level(pin_neg, pwm_period - signed_duty_cycle); + } + else { + pwm_set_gpio_level(pin_pos, pwm_period + signed_duty_cycle); + pwm_set_gpio_level(pin_neg, pwm_period); + } + break; + + case FAST_DECAY: //aka 'Coasting' + default: + if(signed_duty_cycle >= 0) { + pwm_set_gpio_level(pin_pos, signed_duty_cycle); + pwm_set_gpio_level(pin_neg, 0); + } + else { + pwm_set_gpio_level(pin_pos, 0); + pwm_set_gpio_level(pin_neg, 0 - signed_duty_cycle); + } + break; + } + } +}; \ No newline at end of file diff --git a/drivers/motor/motor.hpp b/drivers/motor/motor.hpp new file mode 100644 index 00000000..eb3d6922 --- /dev/null +++ b/drivers/motor/motor.hpp @@ -0,0 +1,72 @@ +#pragma once + +#include "pico/stdlib.h" +#include "hardware/pwm.h" + +namespace pimoroni { + + class Motor { + //-------------------------------------------------- + // Enums + //-------------------------------------------------- + public: + enum DecayMode { + FAST_DECAY = 0, //aka 'Coasting' + SLOW_DECAY = 1, //aka 'Braking' + }; + + //-------------------------------------------------- + // Constants + //-------------------------------------------------- + public: + static const uint16_t DEFAULT_PWM_FREQUENCY = 25000; // Chose 25KHz because it is outside of hearing + // and divides nicely into the RP2040's 125MHz PWM frequency + static const DecayMode DEFAULT_DECAY_MODE = SLOW_DECAY; + + + //-------------------------------------------------- + // Variables + //-------------------------------------------------- + private: + uint pin_pos; + uint pin_neg; + pwm_config pwm_cfg; + uint16_t pwm_period; + float pwm_frequency = DEFAULT_PWM_FREQUENCY; + + DecayMode motor_decay_mode = DEFAULT_DECAY_MODE; + float motor_speed = 0.0f; + + + //-------------------------------------------------- + // Constructors/Destructor + //-------------------------------------------------- + public: + Motor(uint pin_pos, uint pin_neg, float freq = DEFAULT_PWM_FREQUENCY, DecayMode mode = DEFAULT_DECAY_MODE); + ~Motor(); + + + //-------------------------------------------------- + // Methods + //-------------------------------------------------- + public: + bool init(); + + float get_speed(); + void set_speed(float speed); + + float get_frequency(); + bool set_frequency(float freq); + + DecayMode get_decay_mode(); + void set_decay_mode(DecayMode mode); + + void stop(); + void disable(); + + //-------------------------------------------------- + private: + void update_pwm(); + }; + +} diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index e3293cb9..4cbb1eaf 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -30,6 +30,7 @@ add_subdirectory(pico_scroll) add_subdirectory(pico_enc_explorer) add_subdirectory(pico_explorer) add_subdirectory(pico_pot_explorer) +add_subdirectory(pico_motor_shim) add_subdirectory(pico_rgb_keypad) add_subdirectory(pico_rtc_display) add_subdirectory(pico_tof_display) diff --git a/examples/pico_motor_shim/CMakeLists.txt b/examples/pico_motor_shim/CMakeLists.txt new file mode 100644 index 00000000..91e9e29f --- /dev/null +++ b/examples/pico_motor_shim/CMakeLists.txt @@ -0,0 +1,3 @@ +add_subdirectory(balance) +add_subdirectory(sequence) +add_subdirectory(song) \ No newline at end of file diff --git a/examples/pico_motor_shim/balance/CMakeLists.txt b/examples/pico_motor_shim/balance/CMakeLists.txt new file mode 100644 index 00000000..ee58efe4 --- /dev/null +++ b/examples/pico_motor_shim/balance/CMakeLists.txt @@ -0,0 +1,16 @@ +set(OUTPUT_NAME motor_shim_balance) + +add_executable( + ${OUTPUT_NAME} + demo.cpp +) + +# enable usb output, disable uart output +pico_enable_stdio_usb(${OUTPUT_NAME} 1) +pico_enable_stdio_uart(${OUTPUT_NAME} 1) + +# Pull in pico libraries that we need +target_link_libraries(${OUTPUT_NAME} pico_stdlib pico_motor_shim motor button breakout_msa301) + +# create map/bin/hex file etc. +pico_add_extra_outputs(${OUTPUT_NAME}) diff --git a/examples/pico_motor_shim/balance/demo.cpp b/examples/pico_motor_shim/balance/demo.cpp new file mode 100644 index 00000000..8c964d72 --- /dev/null +++ b/examples/pico_motor_shim/balance/demo.cpp @@ -0,0 +1,118 @@ +#include +#include "pico_motor_shim.hpp" + +#include "common/pimoroni_common.hpp" +#include "motor.hpp" +#include "button.hpp" +#include "breakout_msa301.hpp" +#include + +/* +A very basic balancing robot implementation, using an MSA301 to give accelerating values that are passed to the motors using proportional control. +Press "A" to start and stop the balancer +*/ + +using namespace pimoroni; + +static constexpr float TOP_SPEED = 1.0f; //A value between 0 and 1 +static constexpr float Z_BIAS_CORRECTION = 0.5f; //A magic number that seems to correct the MSA301's Z bias +static constexpr float PROPORTIONAL = 0.03f; + + + +Button button_a(pico_motor_shim::BUTTON_A, Polarity::ACTIVE_LOW, 0); + +Motor motor_1(pico_motor_shim::MOTOR_1_POS, pico_motor_shim::MOTOR_1_NEG);//, Motor::DEFAULT_PWM_FREQUENCY, Motor::DEFAULT_DECAY_MODE); +Motor motor_2(pico_motor_shim::MOTOR_2_POS, pico_motor_shim::MOTOR_2_NEG);//, Motor::DEFAULT_PWM_FREQUENCY, Motor::DEFAULT_DECAY_MODE); + +I2C i2c(BOARD::BREAKOUT_GARDEN); +BreakoutMSA301 msa301(&i2c); + +static bool button_toggle = false; +static float target_angle = 0.0f; + +/** + * Checks if the button has been pressed, toggling a value that is also returned. + */ +bool check_button_toggle() { + bool button_pressed = button_a.read(); + if(button_pressed) { + button_toggle = !button_toggle; + } + return button_toggle; +} + +/** + * Takes and angle and wraps it around so that it stays within a -180 to +180 degree range. + * + * Note, it will only work for values between -540 and +540 degrees. + * This can be resolved by changing the 'if's into 'while's, but for most uses it is unnecessary + */ +float wrap_angle(float angle) { + if(angle <= -180.0f) + angle += 360.0f; + + if(angle > 180.0f) + angle -= 360.0f; + + return angle; +} + +/** + * The entry point of the program. + */ +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); + for(int i = 0; i < 20; i++) { + gpio_put(PICO_DEFAULT_LED_PIN, true); + sleep_ms(250); + gpio_put(PICO_DEFAULT_LED_PIN, false); + sleep_ms(250); + } + + //Initialise the two motors + if(!motor_1.init() || !motor_2.init()) { + printf("Cannot initialise motors. Check the provided parameters\n"); + return 0; + } + + if(!msa301.init()) { + printf("Cannot initialise msa301. Check that it is connected\n"); + return 0; + } + + printf("Ready\n"); + + while(true) { + //Turn the Pico's LED on to show that the sequence has started + gpio_put(PICO_DEFAULT_LED_PIN, true); + sleep_ms(50); + + //Has the user has pressed the button to start the sequence + while(check_button_toggle()) { + float y = msa301.get_y_axis(); + float z = msa301.get_z_axis() + Z_BIAS_CORRECTION; + + float current_angle = (atan2(z, -y) * 180.0f) / M_PI; + float angle_error = wrap_angle(target_angle - current_angle); + printf("Y: %f, Z: %f, AngErr: %f\n", y, z, angle_error); + + float output = angle_error * PROPORTIONAL; //No need to clamp this value as set_speed does this internally + motor_1.set_speed(output); + motor_2.set_speed(-output); + + sleep_ms(1); + } + + //The sequence loop has ended, 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(50); + } + return 0; +} diff --git a/examples/pico_motor_shim/sequence/CMakeLists.txt b/examples/pico_motor_shim/sequence/CMakeLists.txt new file mode 100644 index 00000000..eafeebe8 --- /dev/null +++ b/examples/pico_motor_shim/sequence/CMakeLists.txt @@ -0,0 +1,16 @@ +set(OUTPUT_NAME motor_shim_sequence) + +add_executable( + ${OUTPUT_NAME} + demo.cpp +) + +# enable usb output, disable uart output +pico_enable_stdio_usb(${OUTPUT_NAME} 1) +pico_enable_stdio_uart(${OUTPUT_NAME} 1) + +# Pull in pico libraries that we need +target_link_libraries(${OUTPUT_NAME} pico_stdlib pico_motor_shim motor button) + +# create map/bin/hex file etc. +pico_add_extra_outputs(${OUTPUT_NAME}) diff --git a/examples/pico_motor_shim/sequence/demo.cpp b/examples/pico_motor_shim/sequence/demo.cpp new file mode 100644 index 00000000..0b9767a7 --- /dev/null +++ b/examples/pico_motor_shim/sequence/demo.cpp @@ -0,0 +1,169 @@ +#include +#include "pico_motor_shim.hpp" + +#include "common/pimoroni_common.hpp" +#include "motor.hpp" +#include "button.hpp" + +/* +Program showing how the two motors of the Pico Motor Shim can be perform a sequence of movements. +Press "A" to start and stop the movement sequence +*/ + +using namespace pimoroni; + +static constexpr float TOP_SPEED = 1.0f; //A value between 0 and 1 +static const uint32_t ACCELERATE_TIME_MS = 2000; +static const uint32_t WAIT_TIME_MS = 1000; +static const uint32_t STOP_TIME_MS = 1000; + + +Button button_a(pico_motor_shim::BUTTON_A, Polarity::ACTIVE_LOW, 0); + +Motor motor_1(pico_motor_shim::MOTOR_1_POS, pico_motor_shim::MOTOR_1_NEG);//, Motor::DEFAULT_PWM_FREQUENCY, Motor::DEFAULT_DECAY_MODE); +Motor motor_2(pico_motor_shim::MOTOR_2_POS, pico_motor_shim::MOTOR_2_NEG);//, Motor::DEFAULT_PWM_FREQUENCY, Motor::DEFAULT_DECAY_MODE); + +static bool button_toggle = false; + +/** + * Checks if the button has been pressed, toggling a value that is also returned. + */ +bool check_button_toggle() { + bool button_pressed = button_a.read(); + if(button_pressed) { + button_toggle = !button_toggle; + } + return button_toggle; +} + +/** + * Waits for a given amount of time (in milliseconds). + * Exits early if the user presses the button to stop the sequence, returning false. + */ +bool wait_for(uint32_t duration_ms) { + uint32_t start_time = millis(); + uint32_t ellapsed = 0; + + //Loops until the duration has elapsed, checking the button state every millisecond + while(ellapsed < duration_ms) { + if(!check_button_toggle()) + return false; + + sleep_ms(1); + ellapsed = millis() - start_time; + } + return true; +} + +/** + * Accelerate/Decelerate the motors from their current speed to the target speed over the given amount of time (in milliseconds). + * Exits early if the user presses the button to stop the sequence, returning false. + */ +bool accelerate_over(float left_speed, float right_speed, uint32_t duration_ms) { + uint32_t start_time = millis(); + uint32_t ellapsed = 0; + + //Get the current motor speeds + float last_left = motor_1.get_speed(); + float last_right = motor_2.get_speed(); + + //Loops until the duration has elapsed, checking the button state every millisecond, and updating motor speeds + while(ellapsed <= duration_ms) { + if(!check_button_toggle()) + return false; + + //Calculate and set the new motor speeds + float percentage = (float)ellapsed / (float)duration_ms; + motor_1.set_speed(((left_speed - last_left) * percentage) + last_left); + motor_2.set_speed(((right_speed - last_right) * percentage) + last_right); + + sleep_ms(1); + ellapsed = millis() - start_time; + } + + //Set the final motor speeds as loop may not reach 100% + motor_1.set_speed(left_speed); + motor_2.set_speed(right_speed); + + return true; +} + +/** + * The function that performs the driving sequence. + * Exits early if the user presses the button to stop the sequence, returning false. + */ +bool sequence() { + printf("accelerate forward\n"); + if(!accelerate_over(-TOP_SPEED, TOP_SPEED, ACCELERATE_TIME_MS)) + return false; //Early exit if the button was toggled + + printf("driving forward\n"); + if(!wait_for(WAIT_TIME_MS)) + return false; //Early exit if the button was toggled + + printf("deccelerate forward\n"); + if(!accelerate_over(0.0f, 0.0f, ACCELERATE_TIME_MS)) + return false; //Early exit if the button was toggled + + printf("stop\n"); + motor_1.stop(); + motor_2.stop(); + if(!wait_for(STOP_TIME_MS)) + return false; //Early exit if the button was toggled + + printf("accelerate turn left\n"); + if(!accelerate_over(TOP_SPEED * 0.5f, TOP_SPEED * 0.5f, ACCELERATE_TIME_MS * 0.5f)) + return false; //Early exit if the button was toggled + + printf("turning left\n"); + if(!wait_for(WAIT_TIME_MS)) + return false; //Early exit if the button was toggled + + printf("deccelerate turn left\n"); + if(!accelerate_over(0.0f, 0.0f, ACCELERATE_TIME_MS * 0.5f)) + return false; //Early exit if the button was toggled + + printf("stop\n"); + motor_1.stop(); + motor_2.stop(); + if(!wait_for(STOP_TIME_MS)) + return false; //Early exit if the button was toggled + + //Signal that the sequence completed successfully + return true; +} + +/** + * The entry point of the program. + */ +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); + + //Initialise the two motors + if(!motor_1.init() || !motor_2.init()) { + printf("Cannot initialise motors. Check the provided parameters\n"); + return 0; + } + + while(true) { + //Has the user has pressed the button to start the sequence + if(check_button_toggle()) { + + //Turn the Pico's LED on to show that the sequence has started + gpio_put(PICO_DEFAULT_LED_PIN, true); + + //Run the sequence in a perpetual loop, exiting early if the button is pressed again + while(sequence()); + } + + //The sequence loop has ended, so turn off the Pico's LED and disable the motors + gpio_put(PICO_DEFAULT_LED_PIN, false); + motor_1.disable(); + motor_2.disable(); + } + return 0; +} diff --git a/examples/pico_motor_shim/song/CMakeLists.txt b/examples/pico_motor_shim/song/CMakeLists.txt new file mode 100644 index 00000000..b5adb316 --- /dev/null +++ b/examples/pico_motor_shim/song/CMakeLists.txt @@ -0,0 +1,16 @@ +set(OUTPUT_NAME motor_shim_song) + +add_executable( + ${OUTPUT_NAME} + demo.cpp +) + +# enable usb output, disable uart output +pico_enable_stdio_usb(${OUTPUT_NAME} 1) +pico_enable_stdio_uart(${OUTPUT_NAME} 1) + +# Pull in pico libraries that we need +target_link_libraries(${OUTPUT_NAME} pico_stdlib pico_motor_shim motor button breakout_msa301) + +# create map/bin/hex file etc. +pico_add_extra_outputs(${OUTPUT_NAME}) diff --git a/examples/pico_motor_shim/song/demo.cpp b/examples/pico_motor_shim/song/demo.cpp new file mode 100644 index 00000000..75d920db --- /dev/null +++ b/examples/pico_motor_shim/song/demo.cpp @@ -0,0 +1,126 @@ +#include +#include "pico_motor_shim.hpp" + +#include "common/pimoroni_common.hpp" +#include "motor.hpp" +#include "button.hpp" + +/* +Play a song using a motor! Works by setting the PWM duty cycle to 50% and changing the frequency on the fly. +Plug a motor into connector 1, and press "A" to start the song playing (does not loop). Press the button again will stop the song early. +*/ + +using namespace pimoroni; + +// 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 = sizeof(SONG) / sizeof(float); + +//The time (in milliseconds) to play each note for. Change this to make the song play faster or slower +static const uint NOTE_DURATION_MS = 150; + +//Uncomment this lineto have the song be played without the motor turning +//Note, this will affect the audio quality of the sound produced +//#define STATIONARY_PLAYBACK + +//The time (in microseconds) between each direction switch of the motor when using STATIONARY_PLAYBACK +static const uint STATIONARY_TOGGLE_US = 2000; + +//Uncomment this line to use the fast decay (coasting) motor mode. +//This seems to produce a louder sound with STATIONARY_PLAYBACK enabled, but will make movement poorer when STATIONARY_PLAYBACK is disabled +//#define USE_FAST_DECAY + + +Button button_a(pico_motor_shim::BUTTON_A, Polarity::ACTIVE_LOW, 0); +#ifdef USE_FAST_DECAY + Motor motor_1(pico_motor_shim::MOTOR_1_POS, pico_motor_shim::MOTOR_1_NEG, Motor::DEFAULT_PWM_FREQUENCY, Motor::FAST_DECAY); + Motor motor_2(pico_motor_shim::MOTOR_2_POS, pico_motor_shim::MOTOR_2_NEG, Motor::DEFAULT_PWM_FREQUENCY, Motor::FAST_DECAY); +#else + Motor motor_1(pico_motor_shim::MOTOR_1_POS, pico_motor_shim::MOTOR_1_NEG, Motor::DEFAULT_PWM_FREQUENCY, Motor::SLOW_DECAY); + Motor motor_2(pico_motor_shim::MOTOR_2_POS, pico_motor_shim::MOTOR_2_NEG, Motor::DEFAULT_PWM_FREQUENCY, Motor::SLOW_DECAY); +#endif + +static bool button_toggle = false; + +/** + * Checks if the button has been pressed, toggling a value that is also returned. + */ +bool check_button_toggle() { + bool button_pressed = button_a.read(); + if(button_pressed) { + button_toggle = !button_toggle; + } + return button_toggle; +} + +/** + * The entry point of the program. + */ +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); + + //Initialise the motor + if(!motor_1.init() || !motor_2.init()) { + printf("Cannot initialise the motors. Check the provided parameters\n"); + return 0; + } + + while(true) { + 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++) { + if(motor_1.set_frequency(SONG[i]) && motor_2.set_frequency(SONG[i])) { + #ifdef 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.set_speed(0.5f); + motor_2.set_speed(0.5f); + sleep_us(STATIONARY_TOGGLE_US); + t += STATIONARY_TOGGLE_US; + + motor_1.set_speed(-0.5f); + motor_2.set_speed(-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.set_speed(0.5f); + motor_2.set_speed(0.5f); + sleep_ms(NOTE_DURATION_MS); + #endif + } + 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; +} diff --git a/libraries/CMakeLists.txt b/libraries/CMakeLists.txt index ca810b56..e606e4fd 100644 --- a/libraries/CMakeLists.txt +++ b/libraries/CMakeLists.txt @@ -23,6 +23,7 @@ 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) diff --git a/libraries/pico_motor_shim/CMakeLists.txt b/libraries/pico_motor_shim/CMakeLists.txt new file mode 100644 index 00000000..f1101b9a --- /dev/null +++ b/libraries/pico_motor_shim/CMakeLists.txt @@ -0,0 +1 @@ +include(pico_motor_shim.cmake) \ No newline at end of file diff --git a/libraries/pico_motor_shim/pico_motor_shim.cmake b/libraries/pico_motor_shim/pico_motor_shim.cmake new file mode 100644 index 00000000..d28b6c2f --- /dev/null +++ b/libraries/pico_motor_shim/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) \ No newline at end of file diff --git a/libraries/pico_motor_shim/pico_motor_shim.hpp b/libraries/pico_motor_shim/pico_motor_shim.hpp new file mode 100644 index 00000000..e93d6bf5 --- /dev/null +++ b/libraries/pico_motor_shim/pico_motor_shim.hpp @@ -0,0 +1,12 @@ +#pragma once +#include "pico/stdlib.h" + +namespace pico_motor_shim { + const uint8_t BUTTON_A = 2; + + const uint MOTOR_1_POS = 6; + const uint MOTOR_1_NEG = 7; + + const uint MOTOR_2_POS = 27; + const uint MOTOR_2_NEG = 26; +} \ No newline at end of file diff --git a/micropython/modules/micropython.cmake b/micropython/modules/micropython.cmake index 930d15fe..f9e6314b 100644 --- a/micropython/modules/micropython.cmake +++ b/micropython/modules/micropython.cmake @@ -36,6 +36,7 @@ include(pico_unicorn/micropython) include(pico_display/micropython) include(pico_display_2/micropython) include(pico_explorer/micropython) +include(pico_motor_shim/micropython) include(pico_wireless/micropython) include(plasma/micropython) diff --git a/micropython/modules/pico_motor_shim/micropython.cmake b/micropython/modules/pico_motor_shim/micropython.cmake new file mode 100644 index 00000000..f444ad49 --- /dev/null +++ b/micropython/modules/pico_motor_shim/micropython.cmake @@ -0,0 +1,18 @@ +set(MOD_NAME picomotorshim) +string(TOUPPER ${MOD_NAME} MOD_NAME_UPPER) +add_library(usermod_${MOD_NAME} INTERFACE) + +target_sources(usermod_${MOD_NAME} INTERFACE + ${CMAKE_CURRENT_LIST_DIR}/pico_motor_shim.c +) + +target_include_directories(usermod_${MOD_NAME} INTERFACE + ${CMAKE_CURRENT_LIST_DIR} + ${CMAKE_CURRENT_LIST_DIR}/../../../libraries/pico_motor_shim/ +) + +target_compile_definitions(usermod_${MOD_NAME} INTERFACE + -DMODULE_${MOD_NAME_UPPER}_ENABLED=1 +) + +target_link_libraries(usermod INTERFACE usermod_${MOD_NAME}) \ No newline at end of file diff --git a/micropython/modules/pico_motor_shim/pico_motor_shim.c b/micropython/modules/pico_motor_shim/pico_motor_shim.c new file mode 100644 index 00000000..d860bbdd --- /dev/null +++ b/micropython/modules/pico_motor_shim/pico_motor_shim.c @@ -0,0 +1,40 @@ +#include "pico_motor_shim.h" + +/***** Constants *****/ +enum pins +{ + BUTTON_A = 2, + + MOTOR_1_POS = 6, + MOTOR_1_NEG = 7, + + MOTOR_2_POS = 27, + MOTOR_2_NEG = 26, +}; + + +//////////////////////////////////////////////////////////////////////////////////////////////////// +// picomotorshim Module +//////////////////////////////////////////////////////////////////////////////////////////////////// + +/***** Globals Table *****/ +STATIC const mp_map_elem_t picomotorshim_globals_table[] = { + { MP_ROM_QSTR(MP_QSTR___name__), MP_ROM_QSTR(MP_QSTR_picomotorshim) }, + { MP_ROM_QSTR(MP_QSTR_PIN_BUTTON_A), MP_ROM_INT(BUTTON_A) }, + { MP_ROM_QSTR(MP_QSTR_PIN_MOTOR_1_POS), MP_ROM_INT(MOTOR_1_POS) }, + { MP_ROM_QSTR(MP_QSTR_PIN_MOTOR_1_NEG), MP_ROM_INT(MOTOR_1_NEG) }, + { MP_ROM_QSTR(MP_QSTR_PIN_MOTOR_2_POS), MP_ROM_INT(MOTOR_2_POS) }, + { MP_ROM_QSTR(MP_QSTR_PIN_MOTOR_2_NEG), MP_ROM_INT(MOTOR_2_NEG) }, +}; +STATIC MP_DEFINE_CONST_DICT(mp_module_picomotorshim_globals, picomotorshim_globals_table); + +/***** Module Definition *****/ +const mp_obj_module_t picomotorshim_user_cmodule = { + .base = { &mp_type_module }, + .globals = (mp_obj_dict_t*)&mp_module_picomotorshim_globals, +}; + +//////////////////////////////////////////////////////////////////////////////////////////////////// +MP_REGISTER_MODULE(MP_QSTR_picomotorshim, picomotorshim_user_cmodule, MODULE_PICOMOTORSHIM_ENABLED); +//////////////////////////////////////////////////////////////////////////////////////////////////// +//////////////////////////////////////////////////////////////////////////////////////////////////// \ No newline at end of file diff --git a/micropython/modules/pico_motor_shim/pico_motor_shim.h b/micropython/modules/pico_motor_shim/pico_motor_shim.h new file mode 100644 index 00000000..707d6769 --- /dev/null +++ b/micropython/modules/pico_motor_shim/pico_motor_shim.h @@ -0,0 +1,3 @@ +// Include MicroPython API. +#include "py/runtime.h" +#include "py/objstr.h" \ No newline at end of file diff --git a/micropython/modules_py/pimoroni.py b/micropython/modules_py/pimoroni.py index bc67d0f7..d051833f 100644 --- a/micropython/modules_py/pimoroni.py +++ b/micropython/modules_py/pimoroni.py @@ -141,3 +141,76 @@ class RGBLED: self.led_r.duty_u16(int((r * 65535) / 255)) self.led_g.duty_u16(int((g * 65535) / 255)) self.led_b.duty_u16(int((b * 65535) / 255)) + + +class Motor: + FAST_DECAY = 0 # Recirculation current fast decay mode (coasting) + SLOW_DECAY = 1 # Recirculation current slow decay mode (braking) + + def __init__(self, pos, neg, freq=25000, decay_mode=SLOW_DECAY): + self.speed = 0.0 + self.freq = freq + if decay_mode in (self.FAST_DECAY, self.SLOW_DECAY): + self.decay_mode = decay_mode + else: + raise ValueError("Decay mode value must be either Motor.FAST_DECAY or Motor.SLOW_DECAY") + + self.pos_pwm = PWM(Pin(pos)) + self.pos_pwm.freq(freq) + self.neg_pwm = PWM(Pin(neg)) + self.neg_pwm.freq(freq) + + def get_speed(self): + return self.speed + + def set_speed(self, speed): + if speed > 1.0 or speed < -1.0: + raise ValueError("Speed must be between -1.0 and +1.0") + self.speed = speed + self._update_pwm() + + def get_frequency(self): + return self.freq + + def set_frequency(self, freq): + self.pos_pwm.freq(freq) + self.neg_pwm.freq(freq) + self._update_pwm() + + def get_decay_mode(self): + return self.decay_mode + + def set_decay_mode(self, mode): + if mode in (self.FAST_DECAY, self.SLOW_DECAY): + self.decay_mode = mode + self._update_pwm() + else: + raise ValueError("Decay mode value must be either Motor.FAST_DECAY or Motor.SLOW_DECAY") + + def stop(self): + self.speed = 0.0 + self._update_pwm() + + def disable(self): + self.speed = 0.0 + self.pos_pwm.duty_u16(0) + self.neg_pwm.duty_u16(0) + + def _update_pwm(self): + signed_duty_cycle = int(self.speed * 0xFFFF) + + if self.decay_mode is self.SLOW_DECAY: # aka 'Braking' + if signed_duty_cycle >= 0: + self.pos_pwm.duty_u16(0xFFFF) + self.neg_pwm.duty_u16(0xFFFF - signed_duty_cycle) + else: + self.pos_pwm.duty_u16(0xFFFF + signed_duty_cycle) + self.neg_pwm.duty_u16(0xFFFF) + + else: # aka 'Coasting' + if signed_duty_cycle >= 0: + self.pos_pwm.duty_u16(signed_duty_cycle) + self.neg_pwm.duty_u16(0) + else: + self.pos_pwm.duty_u16(0) + self.neg_pwm.duty_u16(0 - signed_duty_cycle) From e59bdc34c4aa5f1d9c4bec53901d37a3e5d32d75 Mon Sep 17 00:00:00 2001 From: ZodiusInfuser Date: Mon, 28 Mar 2022 22:46:58 +0100 Subject: [PATCH 02/60] Added initial structure of new motor classes --- drivers/motor/CMakeLists.txt | 4 +- drivers/motor/calibration.cpp | 338 +++++++++++ drivers/motor/calibration.hpp | 119 ++++ drivers/motor/motor2.cmake | 15 + drivers/motor/motor2.cpp | 211 +++++++ drivers/motor/motor2.hpp | 79 +++ drivers/motor/motor_cluster.cmake | 14 + drivers/motor/motor_cluster.cpp | 527 ++++++++++++++++++ drivers/motor/motor_cluster.hpp | 123 ++++ drivers/motor/motor_state.cpp | 129 +++++ drivers/motor/motor_state.hpp | 90 +++ examples/CMakeLists.txt | 1 + examples/motor2040/CMakeLists.txt | 10 + .../motor2040/motor2040_single_motor.cmake | 12 + examples/motor2040/motor2040_single_motor.cpp | 72 +++ libraries/CMakeLists.txt | 1 + libraries/motor2040/CMakeLists.txt | 1 + libraries/motor2040/motor2040.cmake | 6 + libraries/motor2040/motor2040.hpp | 60 ++ 19 files changed, 1811 insertions(+), 1 deletion(-) create mode 100644 drivers/motor/calibration.cpp create mode 100644 drivers/motor/calibration.hpp create mode 100644 drivers/motor/motor2.cmake create mode 100644 drivers/motor/motor2.cpp create mode 100644 drivers/motor/motor2.hpp create mode 100644 drivers/motor/motor_cluster.cmake create mode 100644 drivers/motor/motor_cluster.cpp create mode 100644 drivers/motor/motor_cluster.hpp create mode 100644 drivers/motor/motor_state.cpp create mode 100644 drivers/motor/motor_state.hpp create mode 100644 examples/motor2040/CMakeLists.txt create mode 100644 examples/motor2040/motor2040_single_motor.cmake create mode 100644 examples/motor2040/motor2040_single_motor.cpp create mode 100644 libraries/motor2040/CMakeLists.txt create mode 100644 libraries/motor2040/motor2040.cmake create mode 100644 libraries/motor2040/motor2040.hpp diff --git a/drivers/motor/CMakeLists.txt b/drivers/motor/CMakeLists.txt index 086269fe..af7cdfff 100644 --- a/drivers/motor/CMakeLists.txt +++ b/drivers/motor/CMakeLists.txt @@ -1 +1,3 @@ -include(motor.cmake) \ No newline at end of file +include(motor.cmake) +include(motor2.cmake) +include(motor_cluster.cmake) \ No newline at end of file diff --git a/drivers/motor/calibration.cpp b/drivers/motor/calibration.cpp new file mode 100644 index 00000000..64fbd50d --- /dev/null +++ b/drivers/motor/calibration.cpp @@ -0,0 +1,338 @@ +#include "calibration.hpp" + +namespace motor { + Calibration::Pair::Pair() + : duty(0.0f), speed(0.0f) { + } + + Calibration::Pair::Pair(float duty, float speed) + : duty(duty), speed(speed) { + } + + Calibration::Calibration() + : calibration(nullptr), calibration_size(0), limit_lower(true), limit_upper(true) { + } + + Calibration::Calibration(CalibrationType default_type) + : Calibration() { + apply_default_pairs(default_type); + } + + Calibration::Calibration(const Calibration &other) + : calibration(nullptr), calibration_size(0), limit_lower(other.limit_lower), limit_upper(other.limit_upper) { + uint size = other.size(); + apply_blank_pairs(size); + for(uint i = 0; i < size; i++) { + calibration[i] = other.calibration[i]; + } + } + + Calibration::~Calibration() { + if(calibration != nullptr) { + delete[] calibration; + calibration = nullptr; + } + } + + Calibration &Calibration::operator=(const Calibration &other) { + uint size = other.size(); + apply_blank_pairs(size); + for(uint i = 0; i < size; i++) { + calibration[i] = other.calibration[i]; + } + limit_lower = other.limit_lower; + limit_upper = other.limit_upper; + + return *this; + } + + Calibration::Pair &Calibration::operator[](uint8_t index) { + assert(index < calibration_size); + return calibration[index]; + } + + const Calibration::Pair &Calibration::operator[](uint8_t index) const { + assert(index < calibration_size); + return calibration[index]; + } + + void Calibration::apply_blank_pairs(uint size) { + if(calibration != nullptr) { + delete[] calibration; + } + + if(size > 0) { + calibration = new Pair[size]; + calibration_size = size; + } + else { + calibration = nullptr; + calibration_size = 0; + } + } + + void Calibration::apply_two_pairs(float min_duty, float max_duty, float min_speed, float max_speed) { + apply_blank_pairs(2); + calibration[0] = Pair(min_duty, min_speed); + calibration[1] = Pair(max_duty, max_speed); + } + + void Calibration::apply_three_pairs(float min_duty, float mid_duty, float max_duty, float min_speed, float mid_speed, float max_speed) { + apply_blank_pairs(3); + calibration[0] = Pair(min_duty, min_speed); + calibration[1] = Pair(mid_duty, mid_speed); + calibration[2] = Pair(max_duty, max_speed); + } + + void Calibration::apply_uniform_pairs(uint size, float min_duty, float max_duty, float min_speed, float max_speed) { + apply_blank_pairs(size); + if(size > 0) { + float size_minus_one = (float)(size - 1); + for(uint i = 0; i < size; i++) { + float duty = Calibration::map_float((float)i, 0.0f, size_minus_one, min_duty, max_duty); + float speed = Calibration::map_float((float)i, 0.0f, size_minus_one, min_speed, max_speed); + calibration[i] = Pair(duty, speed); + } + } + } + + void Calibration::apply_default_pairs(CalibrationType default_type) { + switch(default_type) { + default: + case ANGULAR: + apply_three_pairs(DEFAULT_MIN_PULSE, DEFAULT_MID_PULSE, DEFAULT_MAX_PULSE, + -90.0f, 0.0f, +90.0f); + break; + case LINEAR: + apply_two_pairs(DEFAULT_MIN_PULSE, DEFAULT_MAX_PULSE, + 0.0f, 1.0f); + break; + case CONTINUOUS: + apply_three_pairs(DEFAULT_MIN_PULSE, DEFAULT_MID_PULSE, DEFAULT_MAX_PULSE, + -1.0f, 0.0f, +1.0f); + break; + } + } + + uint Calibration::size() const { + return calibration_size; + } + + Calibration::Pair &Calibration::pair(uint8_t index) { + assert(index < calibration_size); + return calibration[index]; + } + + const Calibration::Pair &Calibration::pair(uint8_t index) const { + assert(index < calibration_size); + return calibration[index]; + } + + float Calibration::duty(uint8_t index) const { + return pair(index).duty; + } + + void Calibration::duty(uint8_t index, float duty) { + pair(index).duty = duty; + } + + float Calibration::speed(uint8_t index) const { + return pair(index).speed; + } + + void Calibration::speed(uint8_t index, float speed) { + pair(index).speed = speed; + } + + Calibration::Pair &Calibration::first() { + assert(calibration_size > 0); + return calibration[0]; + } + + const Calibration::Pair &Calibration::first() const { + assert(calibration_size > 0); + return calibration[0]; + } + + float Calibration::first_duty() const { + return first().duty; + } + + void Calibration::first_duty(float duty) { + first().duty = duty; + } + + float Calibration::first_speed() const { + return first().speed; + } + + void Calibration::first_speed(float speed) { + first().speed = speed; + } + + Calibration::Pair &Calibration::last() { + assert(calibration_size > 0); + return calibration[calibration_size - 1]; + } + + const Calibration::Pair &Calibration::last() const { + assert(calibration_size > 0); + return calibration[calibration_size - 1]; + } + + float Calibration::last_duty() const { + return last().duty; + } + + void Calibration::last_duty(float duty) { + last().duty = duty; + } + + float Calibration::last_speed() const { + return last().speed; + } + + void Calibration::last_speed(float speed) { + last().speed = speed; + } + + bool Calibration::has_lower_limit() const { + return limit_lower; + } + + bool Calibration::has_upper_limit() const { + return limit_upper; + } + + void Calibration::limit_to_calibration(bool lower, bool upper) { + limit_lower = lower; + limit_upper = upper; + } + + bool Calibration::speed_to_duty(float speed, float &duty_out, float &speed_out) const { + bool success = false; + if(calibration_size >= 2) { + uint8_t last = calibration_size - 1; + + speed_out = speed; + + // Is the speed below the bottom most calibration pair? + if(speed < calibration[0].speed) { + // Should the speed be limited to the calibration or projected below it? + if(limit_lower) { + duty_out = calibration[0].duty; + speed_out = calibration[0].speed; + } + else { + duty_out = map_float(speed, calibration[0].speed, calibration[1].speed, + calibration[0].duty, calibration[1].duty); + } + } + // Is the speed above the top most calibration pair? + else if(speed > calibration[last].speed) { + // Should the speed be limited to the calibration or projected above it? + if(limit_upper) { + duty_out = calibration[last].duty; + speed_out = calibration[last].speed; + } + else { + duty_out = map_float(speed, calibration[last - 1].speed, calibration[last].speed, + calibration[last - 1].duty, calibration[last].duty); + } + } + else { + // The speed must between two calibration pairs, so iterate through them to find which ones + for(uint8_t i = 0; i < last; i++) { + if(speed <= calibration[i + 1].speed) { + duty_out = map_float(speed, calibration[i].speed, calibration[i + 1].speed, + calibration[i].duty, calibration[i + 1].duty); + break; // No need to continue checking so break out of the loop + } + } + } + + // Clamp the duty between the hard limits + if(duty_out < LOWER_HARD_LIMIT || duty_out > UPPER_HARD_LIMIT) { + duty_out = MIN(MAX(duty_out, LOWER_HARD_LIMIT), UPPER_HARD_LIMIT); + + // Is the duty below the bottom most calibration pair? + if(duty_out < calibration[0].duty) { + speed_out = map_float(duty_out, calibration[0].duty, calibration[1].duty, + calibration[0].speed, calibration[1].speed); + } + // Is the duty above the top most calibration pair? + else if(duty_out > calibration[last].duty) { + speed_out = map_float(duty_out, calibration[last - 1].duty, calibration[last].duty, + calibration[last - 1].speed, calibration[last].speed); + } + else { + // The duty must between two calibration pairs, so iterate through them to find which ones + for(uint8_t i = 0; i < last; i++) { + if(duty_out <= calibration[i + 1].duty) { + speed_out = map_float(duty_out, calibration[i].duty, calibration[i + 1].duty, + calibration[i].speed, calibration[i + 1].speed); + break; // No need to continue checking so break out of the loop + } + } + } + } + + success = true; + } + + return success; + } + + bool Calibration::duty_to_speed(float duty, float &speed_out, float &duty_out) const { + bool success = false; + if(calibration_size >= 2) { + uint8_t last = calibration_size - 1; + + // Clamp the duty between the hard limits + duty_out = MIN(MAX(duty, LOWER_HARD_LIMIT), UPPER_HARD_LIMIT); + + // Is the duty below the bottom most calibration pair? + if(duty_out < calibration[0].duty) { + // Should the duty be limited to the calibration or projected below it? + if(limit_lower) { + speed_out = calibration[0].speed; + duty_out = calibration[0].duty; + } + else { + speed_out = map_float(duty, calibration[0].duty, calibration[1].duty, + calibration[0].speed, calibration[1].speed); + } + } + // Is the duty above the top most calibration pair? + else if(duty > calibration[last].duty) { + // Should the duty be limited to the calibration or projected above it? + if(limit_upper) { + speed_out = calibration[last].speed; + duty_out = calibration[last].duty; + } + else { + speed_out = map_float(duty, calibration[last - 1].duty, calibration[last].duty, + calibration[last - 1].speed, calibration[last].speed); + } + } + else { + // The duty must between two calibration pairs, so iterate through them to find which ones + for(uint8_t i = 0; i < last; i++) { + if(duty <= calibration[i + 1].duty) { + speed_out = map_float(duty, calibration[i].duty, calibration[i + 1].duty, + calibration[i].speed, calibration[i + 1].speed); + break; // No need to continue checking so break out of the loop + } + } + } + + success = true; + } + + return success; + } + + float Calibration::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; + } +}; \ No newline at end of file diff --git a/drivers/motor/calibration.hpp b/drivers/motor/calibration.hpp new file mode 100644 index 00000000..447773d2 --- /dev/null +++ b/drivers/motor/calibration.hpp @@ -0,0 +1,119 @@ +#pragma once + +#include "pico/stdlib.h" + +namespace motor { + + enum CalibrationType { + ANGULAR = 0, + LINEAR, + CONTINUOUS + }; + + class Calibration { + //-------------------------------------------------- + // Constants + //-------------------------------------------------- + public: + static constexpr float DEFAULT_MIN_PULSE = 500.0f; // in microseconds + static constexpr float DEFAULT_MID_PULSE = 1500.0f; // in microseconds + static constexpr float DEFAULT_MAX_PULSE = 2500.0f; // in microseconds + + private: + static constexpr float LOWER_HARD_LIMIT = 400.0f; // The minimum microsecond duty to send + static constexpr float UPPER_HARD_LIMIT = 2600.0f; // The maximum microsecond duty to send + + + //-------------------------------------------------- + // Substructures + //-------------------------------------------------- + public: + struct Pair { + //-------------------------------------------------- + // Constructors/Destructor + //-------------------------------------------------- + Pair(); + Pair(float duty, float speed); + + + //-------------------------------------------------- + // Variables + //-------------------------------------------------- + float duty; + float speed; + }; + + + //-------------------------------------------------- + // Constructors/Destructor + //-------------------------------------------------- + public: + Calibration(); + Calibration(CalibrationType default_type); + Calibration(const Calibration &other); + virtual ~Calibration(); + + + //-------------------------------------------------- + // Operators + //-------------------------------------------------- + public: + Calibration &operator=(const Calibration &other); + Pair &operator[](uint8_t index); + const Pair &operator[](uint8_t index) const; + + + //-------------------------------------------------- + // Methods + //-------------------------------------------------- + public: + void apply_blank_pairs(uint size); + void apply_two_pairs(float min_duty, float max_duty, float min_speed, float max_speed); + void apply_three_pairs(float min_duty, float mid_duty, float max_duty, float min_speed, float mid_speed, float max_speed); + void apply_uniform_pairs(uint size, float min_duty, float max_duty, float min_speed, float max_speed); + void apply_default_pairs(CalibrationType default_type); + + uint size() const; + + Pair &pair(uint8_t index); // Ensure the pairs are assigned in ascending speed order + const Pair &pair(uint8_t index) const; // Ensure the pairs are assigned in ascending speed order + float duty(uint8_t index) const; + void duty(uint8_t index, float duty); + float speed(uint8_t index) const; + void speed(uint8_t index, float speed); + + Pair &first(); + const Pair &first() const; + float first_duty() const; + void first_duty(float duty); + float first_speed() const; + void first_speed(float speed); + + Pair &last(); + const Pair &last() const; + float last_duty() const; + void last_duty(float duty); + float last_speed() const; + void last_speed(float speed); + + bool has_lower_limit() const; + bool has_upper_limit() const; + void limit_to_calibration(bool lower, bool upper); + + bool speed_to_duty(float speed, float &duty_out, float &speed_out) const; + bool duty_to_speed(float duty, float &speed_out, float &duty_out) const; + + //static float map_float(float in, float in_min, float in_max, float out_min, float out_max); + + + //-------------------------------------------------- + // Variables + //-------------------------------------------------- + private: + Pair* calibration; + uint calibration_size; + bool limit_lower; + bool limit_upper; + }; + +} \ No newline at end of file diff --git a/drivers/motor/motor2.cmake b/drivers/motor/motor2.cmake new file mode 100644 index 00000000..85ad531d --- /dev/null +++ b/drivers/motor/motor2.cmake @@ -0,0 +1,15 @@ +set(DRIVER_NAME motor2) +add_library(${DRIVER_NAME} INTERFACE) + +target_sources(${DRIVER_NAME} INTERFACE + ${CMAKE_CURRENT_LIST_DIR}/motor2.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 + ) \ No newline at end of file diff --git a/drivers/motor/motor2.cpp b/drivers/motor/motor2.cpp new file mode 100644 index 00000000..441e4957 --- /dev/null +++ b/drivers/motor/motor2.cpp @@ -0,0 +1,211 @@ +#include "motor2.hpp" +#include "hardware/clocks.h" +#include "pwm.hpp" + +namespace motor { + Motor2::Motor2(uint pin_pos, uint pin_neg, float freq, MotorState::DecayMode mode) + : motor_pin_pos(pin_pos), motor_pin_neg(pin_neg), pwm_frequency(freq), motor_decay_mode(mode) { + } + + // Motor2::Motor2(uint pin, /*const Calibration& calibration,*/ float freq) + // : motor_pin_pos(pin), /*state(calibration),*/ pwm_frequency(freq) { + // } + + Motor2::~Motor2() { + gpio_set_function(motor_pin_pos, GPIO_FUNC_NULL); + gpio_set_function(motor_pin_neg, GPIO_FUNC_NULL); + } + + bool Motor2::init() { + bool success = false; + + uint16_t period; uint16_t div16; + if(pimoroni::calculate_pwm_factors(pwm_frequency, period, div16)) { + pwm_period = period; + + 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, pwm_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... + + pwm_init(pwm_gpio_to_slice_num(motor_pin_pos), &pwm_cfg, true); + gpio_set_function(motor_pin_pos, GPIO_FUNC_PWM); + + pwm_init(pwm_gpio_to_slice_num(motor_pin_neg), &pwm_cfg, true); + gpio_set_function(motor_pin_neg, GPIO_FUNC_PWM); + + pwm_set_gpio_level(motor_pin_pos, 0); + pwm_set_gpio_level(motor_pin_neg, 0); + + success = true; + } + return success; + } + + uint Motor2::pin() const { + return motor_pin_pos; + } + + void Motor2::enable() { + apply_duty(state.enable()); + } + + void Motor2::disable() { + apply_duty(state.disable()); + } + + bool Motor2::is_enabled() const { + return state.is_enabled(); + } + + // float Motor2::duty() const { + // return state.get_duty(); + // } + + // void Motor2::duty(float duty) { + // apply_duty(state.set_duty_with_return(duty)); + // } + + float Motor2::speed() const { + return state.get_speed(); + } + + void Motor2::speed(float speed) { + apply_duty(state.set_speed_with_return(speed)); + } + + float Motor2::frequency() const { + return pwm_frequency; + } + + bool Motor2::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)) { + + // 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; + pwm_frequency = freq; + + uint pin_num = pwm_gpio_to_slice_num(motor_pin_pos); + + // Apply the new divider + uint8_t div = div16 >> 4; + uint8_t mod = div16 % 16; + pwm_set_clkdiv_int_frac(pin_num, div, mod); + + // If the the period is larger, update the pwm before setting the new wraps + if(state.is_enabled() && pre_update_pwm) { + apply_duty(state.get_duty()); + } + + // Set the new wrap (should be 1 less than the period to get full 0 to 100%) + pwm_set_wrap(pin_num, pwm_period - 1); + + // If the the period is smaller, update the pwm after setting the new wraps + if(state.is_enabled() && !pre_update_pwm) { + apply_duty(state.get_duty()); + } + + success = true; + } + } + return success; + } + + MotorState::DecayMode Motor2::decay_mode() { + return motor_decay_mode; + } + + void Motor2::decay_mode(MotorState::DecayMode mode) { + motor_decay_mode = mode; + apply_duty(state.get_duty()); + } + + void Motor2::stop() { + apply_duty(state.set_speed_with_return(0.0f)); + } + + void Motor2::coast() { + disable(); + } + + float Motor2::min_speed() const { + return state.get_min_speed(); + } + + // float Motor2::mid_speed() const { + // return state.get_mid_speed(); + // } + + float Motor2::max_speed() const { + return state.get_max_speed(); + } + + void Motor2::to_min() { + apply_duty(state.to_min_with_return()); + } + + // void Motor2::to_mid() { + // apply_duty(state.to_mid_with_return()); + // } + + void Motor2::to_max() { + apply_duty(state.to_max_with_return()); + } + + void Motor2::to_percent(float in, float in_min, float in_max) { + apply_duty(state.to_percent_with_return(in, in_min, in_max)); + } + + void Motor2::to_percent(float in, float in_min, float in_max, float speed_min, float speed_max) { + apply_duty(state.to_percent_with_return(in, in_min, in_max, speed_min, speed_max)); + } + + // Calibration& Motor2::calibration() { + // return state.calibration(); + // } + + // const Calibration& Motor2::calibration() const { + // return state.calibration(); + // } + + void Motor2::apply_duty(float duty) { + int32_t signed_level = MotorState::duty_to_level(duty, pwm_period); + + switch(motor_decay_mode) { + case MotorState::SLOW_DECAY: //aka 'Braking' + if(signed_level >= 0) { + pwm_set_gpio_level(motor_pin_pos, pwm_period); + pwm_set_gpio_level(motor_pin_neg, pwm_period - signed_level); + } + else { + pwm_set_gpio_level(motor_pin_pos, pwm_period + signed_level); + pwm_set_gpio_level(motor_pin_neg, pwm_period); + } + break; + + case MotorState::FAST_DECAY: //aka 'Coasting' + default: + if(signed_level >= 0) { + pwm_set_gpio_level(motor_pin_pos, signed_level); + pwm_set_gpio_level(motor_pin_neg, 0); + } + else { + pwm_set_gpio_level(motor_pin_pos, 0); + pwm_set_gpio_level(motor_pin_neg, 0 - signed_level); + } + break; + } + } +}; \ No newline at end of file diff --git a/drivers/motor/motor2.hpp b/drivers/motor/motor2.hpp new file mode 100644 index 00000000..43310c14 --- /dev/null +++ b/drivers/motor/motor2.hpp @@ -0,0 +1,79 @@ +#pragma once + +#include "pico/stdlib.h" +#include "hardware/pwm.h" +#include "motor_state.hpp" + +namespace motor { + + class Motor2 { + //-------------------------------------------------- + // Variables + //-------------------------------------------------- + private: + uint motor_pin_pos; + uint motor_pin_neg; + MotorState state; + pwm_config pwm_cfg; + uint16_t pwm_period; + float pwm_frequency; + MotorState::DecayMode motor_decay_mode; + + + //-------------------------------------------------- + // Constructors/Destructor + //-------------------------------------------------- + public: + Motor2(uint pin_pos, uint pin_neg, float freq = MotorState::DEFAULT_FREQUENCY, MotorState::DecayMode mode = MotorState::DEFAULT_DECAY_MODE); + //Motor2(uint pin, /*const Calibration& calibration,*/ float freq = MotorState::DEFAULT_FREQUENCY); + ~Motor2(); + + + //-------------------------------------------------- + // Methods + //-------------------------------------------------- + public: + bool init(); + + // For print access in micropython + uint pin() 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); + + MotorState::DecayMode decay_mode(); + void decay_mode(MotorState::DecayMode mode); + + void stop(); + void coast(); // An alias for disable + + //-------------------------------------------------- + float min_speed() const; + //float mid_speed() const; + float max_speed() const; + + void to_min(); + //void to_mid(); + void to_max(); + 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); + + //Calibration& calibration(); + //const Calibration& calibration() const; + + //-------------------------------------------------- + private: + void apply_duty(float duty); + }; + +} \ No newline at end of file diff --git a/drivers/motor/motor_cluster.cmake b/drivers/motor/motor_cluster.cmake new file mode 100644 index 00000000..1bdd0463 --- /dev/null +++ b/drivers/motor/motor_cluster.cmake @@ -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 + ) \ No newline at end of file diff --git a/drivers/motor/motor_cluster.cpp b/drivers/motor/motor_cluster.cpp new file mode 100644 index 00000000..ad97751d --- /dev/null +++ b/drivers/motor/motor_cluster.cpp @@ -0,0 +1,527 @@ +#include "motor_cluster.hpp" +#include "pwm.hpp" +#include + +namespace motor { + MotorCluster::MotorCluster(PIO pio, uint sm, uint pin_mask, CalibrationType default_type, float freq, bool auto_phase, PWMCluster::Sequence *seq_buffer, PWMCluster::TransitionData *dat_buffer) + : pwms(pio, sm, pin_mask, seq_buffer, dat_buffer), pwm_frequency(freq) { + create_motor_states(default_type, auto_phase); + } + + MotorCluster::MotorCluster(PIO pio, uint sm, uint pin_base, uint pin_count, CalibrationType default_type, float freq, bool auto_phase, PWMCluster::Sequence *seq_buffer, PWMCluster::TransitionData *dat_buffer) + : pwms(pio, sm, pin_base, pin_count, seq_buffer, dat_buffer), pwm_frequency(freq) { + create_motor_states(default_type, auto_phase); + } + + MotorCluster::MotorCluster(PIO pio, uint sm, const uint8_t *pins, uint32_t length, CalibrationType default_type, float freq, bool auto_phase, PWMCluster::Sequence *seq_buffer, PWMCluster::TransitionData *dat_buffer) + : pwms(pio, sm, pins, length, seq_buffer, dat_buffer), pwm_frequency(freq) { + create_motor_states(default_type, auto_phase); + } + + MotorCluster::MotorCluster(PIO pio, uint sm, std::initializer_list pins, CalibrationType default_type, float freq, bool auto_phase, PWMCluster::Sequence *seq_buffer, PWMCluster::TransitionData *dat_buffer) + : pwms(pio, sm, pins, seq_buffer, dat_buffer), pwm_frequency(freq) { + create_motor_states(default_type, auto_phase); + } + + MotorCluster::MotorCluster(PIO pio, uint sm, uint pin_mask, const Calibration& calibration, float freq, bool auto_phase, PWMCluster::Sequence *seq_buffer, PWMCluster::TransitionData *dat_buffer) + : pwms(pio, sm, pin_mask, seq_buffer, dat_buffer), pwm_frequency(freq) { + create_motor_states(calibration, auto_phase); + } + + MotorCluster::MotorCluster(PIO pio, uint sm, uint pin_base, uint pin_count, const Calibration& calibration, float freq, bool auto_phase, PWMCluster::Sequence *seq_buffer, PWMCluster::TransitionData *dat_buffer) + : pwms(pio, sm, pin_base, pin_count, seq_buffer, dat_buffer), pwm_frequency(freq) { + create_motor_states(calibration, auto_phase); + } + + MotorCluster::MotorCluster(PIO pio, uint sm, const uint8_t *pins, uint32_t length, const Calibration& calibration, float freq, bool auto_phase, PWMCluster::Sequence *seq_buffer, PWMCluster::TransitionData *dat_buffer) + : pwms(pio, sm, pins, length, seq_buffer, dat_buffer), pwm_frequency(freq) { + create_motor_states(calibration, auto_phase); + } + + MotorCluster::MotorCluster(PIO pio, uint sm, std::initializer_list pins, const Calibration& calibration, float freq, bool auto_phase, PWMCluster::Sequence *seq_buffer, PWMCluster::TransitionData *dat_buffer) + : pwms(pio, sm, pins, seq_buffer, dat_buffer), pwm_frequency(freq) { + create_motor_states(calibration, auto_phase); + } + + MotorCluster::~MotorCluster() { + delete[] states; + delete[] motor_phases; + } + + 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_count(); + for(uint8_t motor = 0; motor < motor_count; motor++) { + pwms.set_chan_level(motor, 0, false); + pwms.set_chan_offset(motor, (uint32_t)(motor_phases[motor] * (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_count(); + } + + uint8_t MotorCluster::pin(uint8_t motor) const { + return pwms.get_chan_pin(motor); + } + + void MotorCluster::enable(uint8_t motor, bool load) { + assert(motor < pwms.get_chan_count()); + float new_pulse = states[motor].enable(); + apply_pulse(motor, new_pulse, 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 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_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_count()); + float new_pulse = states[motor].disable(); + apply_pulse(motor, new_pulse, 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 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_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_count()); + return states[motor].is_enabled(); + } + + float MotorCluster::pulse(uint8_t motor) const { + assert(motor < pwms.get_chan_count()); + return states[motor].get_pulse(); + } + + void MotorCluster::pulse(uint8_t motor, float pulse, bool load) { + assert(motor < pwms.get_chan_count()); + float new_pulse = states[motor].set_pulse_with_return(pulse); + apply_pulse(motor, new_pulse, load); + } + + void MotorCluster::pulse(const uint8_t *motors, uint8_t length, float pulse, bool load) { + assert(motors != nullptr); + for(uint8_t i = 0; i < length; i++) { + this->pulse(motors[i], pulse, false); + } + if(load) + pwms.load_pwm(); + } + + void MotorCluster::pulse(std::initializer_list motors, float pulse, bool load) { + for(auto motor : motors) { + this->pulse(motor, pulse, false); + } + if(load) + pwms.load_pwm(); + } + + void MotorCluster::all_to_pulse(float pulse, bool load) { + uint8_t motor_count = pwms.get_chan_count(); + for(uint8_t motor = 0; motor < motor_count; motor++) { + this->pulse(motor, pulse, false); + } + if(load) + pwms.load_pwm(); + } + + float MotorCluster::speed(uint8_t motor) const { + assert(motor < pwms.get_chan_count()); + return states[motor].get_speed(); + } + + void MotorCluster::speed(uint8_t motor, float speed, bool load) { + assert(motor < pwms.get_chan_count()); + float new_pulse = states[motor].set_speed_with_return(speed); + apply_pulse(motor, new_pulse, 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 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_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_count()); + return motor_phases[motor]; + } + + void MotorCluster::phase(uint8_t motor, float phase, bool load) { + assert(motor < pwms.get_chan_count()); + motor_phases[motor] = MIN(MAX(phase, 0.0f), 1.0f); + pwms.set_chan_offset(motor, (uint32_t)(motor_phases[motor] * (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 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_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_count(); + for(uint motor = 0; motor < motor_count; motor++) { + if(states[motor].is_enabled()) { + apply_pulse(motor, states[motor].get_pulse(), false); + } + pwms.set_chan_offset(motor, (uint32_t)(motor_phases[motor] * (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; + } + + float MotorCluster::min_speed(uint8_t motor) const { + assert(is_assigned(motor)); + return states[motor].get_min_speed(); + } + + float MotorCluster::mid_speed(uint8_t motor) const { + assert(is_assigned(motor)); + return states[motor].get_mid_speed(); + } + + float MotorCluster::max_speed(uint8_t motor) const { + assert(is_assigned(motor)); + return states[motor].get_max_speed(); + } + + void MotorCluster::to_min(uint8_t motor, bool load) { + assert(is_assigned(motor)); + float new_pulse = states[motor].to_min_with_return(); + apply_pulse(motor, new_pulse, load); + } + + void MotorCluster::to_min(const uint8_t *motors, uint8_t length, bool load) { + assert(motors != nullptr); + for(uint8_t i = 0; i < length; i++) { + to_min(motors[i], false); + } + if(load) + pwms.load_pwm(); + } + + void MotorCluster::to_min(std::initializer_list motors, bool load) { + for(auto motor : motors) { + to_min(motor, false); + } + if(load) + pwms.load_pwm(); + } + + void MotorCluster::all_to_min(bool load) { + uint8_t motor_count = pwms.get_chan_count(); + for(uint8_t motor = 0; motor < motor_count; motor++) { + to_min(motor, false); + } + if(load) + pwms.load_pwm(); + } + + void MotorCluster::to_mid(uint8_t motor, bool load) { + assert(is_assigned(motor)); + float new_pulse = states[motor].to_mid_with_return(); + apply_pulse(motor, new_pulse, load); + } + + void MotorCluster::to_mid(const uint8_t *motors, uint8_t length, bool load) { + assert(motors != nullptr); + for(uint8_t i = 0; i < length; i++) { + to_mid(motors[i], false); + } + if(load) + pwms.load_pwm(); + } + + void MotorCluster::to_mid(std::initializer_list motors, bool load) { + for(auto motor : motors) { + to_mid(motor, false); + } + if(load) + pwms.load_pwm(); + } + + void MotorCluster::all_to_mid(bool load) { + uint8_t motor_count = pwms.get_chan_count(); + for(uint8_t motor = 0; motor < motor_count; motor++) { + to_mid(motor, false); + } + if(load) + pwms.load_pwm(); + } + + void MotorCluster::to_max(uint8_t motor, bool load) { + assert(is_assigned(motor)); + float new_pulse = states[motor].to_max_with_return(); + apply_pulse(motor, new_pulse, load); + } + + void MotorCluster::to_max(const uint8_t *motors, uint8_t length, bool load) { + assert(motors != nullptr); + for(uint8_t i = 0; i < length; i++) { + to_max(motors[i], false); + } + if(load) + pwms.load_pwm(); + } + + void MotorCluster::to_max(std::initializer_list motors, bool load) { + for(auto motor : motors) { + to_max(motor, false); + } + if(load) + pwms.load_pwm(); + } + + void MotorCluster::all_to_max(bool load) { + uint8_t motor_count = pwms.get_chan_count(); + for(uint8_t motor = 0; motor < motor_count; motor++) { + to_max(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(is_assigned(motor)); + float new_pulse = states[motor].to_percent_with_return(in, in_min, in_max); + apply_pulse(motor, new_pulse, 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 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_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(is_assigned(motor)); + float new_pulse = states[motor].to_percent_with_return(in, in_min, in_max, speed_min, speed_max); + apply_pulse(motor, new_pulse, 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 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_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(); + } + + Calibration& MotorCluster::calibration(uint8_t motor) { + assert(is_assigned(motor)); + return states[motor].calibration(); + } + + const Calibration& MotorCluster::calibration(uint8_t motor) const { + assert(is_assigned(motor)); + return states[motor].calibration(); + } + + void MotorCluster::load() { + pwms.load_pwm(); + } + + void MotorCluster::apply_pulse(uint8_t motor, float pulse, bool load) { + pwms.set_chan_level(motor, MotorState::pulse_to_level(pulse, pwm_period, pwm_frequency), load); + } + + void MotorCluster::create_motor_states(CalibrationType default_type, bool auto_phase) { + uint8_t motor_count = pwms.get_chan_count(); + if(motor_count > 0) { + states = new MotorState[motor_count]; + motor_phases = new float[motor_count]; + + for(uint motor = 0; motor < motor_count; motor++) { + states[motor] = MotorState(default_type); + motor_phases[motor] = (auto_phase) ? (float)motor / (float)motor_count : 0.0f; + } + } + } + + void MotorCluster::create_motor_states(const Calibration& calibration, bool auto_phase) { + uint8_t motor_count = pwms.get_chan_count(); + if(motor_count > 0) { + states = new MotorState[motor_count]; + motor_phases = new float[motor_count]; + + for(uint motor = 0; motor < motor_count; motor++) { + states[motor] = MotorState(calibration); + motor_phases[motor] = (auto_phase) ? (float)motor / (float)motor_count : 0.0f; + } + } + } +}; \ No newline at end of file diff --git a/drivers/motor/motor_cluster.hpp b/drivers/motor/motor_cluster.hpp new file mode 100644 index 00000000..29c275b3 --- /dev/null +++ b/drivers/motor/motor_cluster.hpp @@ -0,0 +1,123 @@ +#pragma once + +#include "pico/stdlib.h" +#include "pwm_cluster.hpp" +#include "motor_state.hpp" + +using namespace pimoroni; + +namespace motor { + + class MotorCluster { + //-------------------------------------------------- + // Variables + //-------------------------------------------------- + private: + PWMCluster pwms; + uint32_t pwm_period; + float pwm_frequency; + MotorState* states; + float* motor_phases; + + + //-------------------------------------------------- + // Constructors/Destructor + //-------------------------------------------------- + public: + MotorCluster(PIO pio, uint sm, uint pin_mask, CalibrationType default_type = ANGULAR, float freq = MotorState::DEFAULT_FREQUENCY, bool auto_phase = true, PWMCluster::Sequence *seq_buffer = nullptr, PWMCluster::TransitionData *dat_buffer = nullptr); + MotorCluster(PIO pio, uint sm, uint pin_base, uint pin_count, CalibrationType default_type = ANGULAR, float freq = MotorState::DEFAULT_FREQUENCY, bool auto_phase = true, PWMCluster::Sequence *seq_buffer = nullptr, PWMCluster::TransitionData *dat_buffer = nullptr); + MotorCluster(PIO pio, uint sm, const uint8_t *pins, uint32_t length, CalibrationType default_type = ANGULAR, float freq = MotorState::DEFAULT_FREQUENCY, bool auto_phase = true, PWMCluster::Sequence *seq_buffer = nullptr, PWMCluster::TransitionData *dat_buffer = nullptr); + MotorCluster(PIO pio, uint sm, std::initializer_list pins, CalibrationType default_type = ANGULAR, float freq = MotorState::DEFAULT_FREQUENCY, bool auto_phase = true, PWMCluster::Sequence *seq_buffer = nullptr, PWMCluster::TransitionData *dat_buffer = nullptr); + + MotorCluster(PIO pio, uint sm, uint pin_mask, const Calibration& calibration, float freq = MotorState::DEFAULT_FREQUENCY, bool auto_phase = true, PWMCluster::Sequence *seq_buffer = nullptr, PWMCluster::TransitionData *dat_buffer = nullptr); + MotorCluster(PIO pio, uint sm, uint pin_base, uint pin_count, const Calibration& calibration, float freq = MotorState::DEFAULT_FREQUENCY, bool auto_phase = true, PWMCluster::Sequence *seq_buffer = nullptr, PWMCluster::TransitionData *dat_buffer = nullptr); + MotorCluster(PIO pio, uint sm, const uint8_t *pins, uint32_t length, const Calibration& calibration, float freq = MotorState::DEFAULT_FREQUENCY, bool auto_phase = true, PWMCluster::Sequence *seq_buffer = nullptr, PWMCluster::TransitionData *dat_buffer = nullptr); + MotorCluster(PIO pio, uint sm, std::initializer_list pins, const Calibration& calibration, float freq = MotorState::DEFAULT_FREQUENCY, bool auto_phase = true, PWMCluster::Sequence *seq_buffer = nullptr, PWMCluster::TransitionData *dat_buffer = nullptr); + ~MotorCluster(); + + + //-------------------------------------------------- + // Methods + //-------------------------------------------------- + public: + bool init(); + + uint8_t count() const; + uint8_t pin(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 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 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 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 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 motors, float phase, bool load = true); + void all_to_phase(float phase, bool load = true); + + float frequency() const; + bool frequency(float freq); + + //-------------------------------------------------- + float min_speed(uint8_t motor) const; + float mid_speed(uint8_t motor) const; + float max_speed(uint8_t motor) const; + + void to_min(uint8_t motor, bool load = true); + void to_min(const uint8_t *motors, uint8_t length, bool load = true); + void to_min(std::initializer_list motors, bool load = true); + void all_to_min(bool load = true); + + void to_mid(uint8_t motor, bool load = true); + void to_mid(const uint8_t *motors, uint8_t length, bool load = true); + void to_mid(std::initializer_list motors, bool load = true); + void all_to_mid(bool load = true); + + void to_max(uint8_t motor, bool load = true); + void to_max(const uint8_t *motors, uint8_t length, bool load = true); + void to_max(std::initializer_list motors, bool load = true); + void all_to_max(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 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 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); + + Calibration& calibration(uint8_t motor); + const Calibration& calibration(uint8_t motor) const; + + void load(); + + //-------------------------------------------------- + private: + void apply_duty(uint8_t motor, float duty, bool load); + void create_motor_states(CalibrationType default_type, bool auto_phase); + void create_motor_states(const Calibration& calibration, bool auto_phase); + }; + +} \ No newline at end of file diff --git a/drivers/motor/motor_state.cpp b/drivers/motor/motor_state.cpp new file mode 100644 index 00000000..1a9f7d7e --- /dev/null +++ b/drivers/motor/motor_state.cpp @@ -0,0 +1,129 @@ +#include "motor_state.hpp" + +namespace motor { + MotorState::MotorState() { + } + + //MotorState::MotorState(CalibrationType default_type) + // : calib(default_type) { + //} + + //MotorState::MotorState(const Calibration& calibration) + // : calib(calibration) { + //} + + float MotorState::enable() { + return _enable(); + } + + float MotorState::disable() { + enabled = false; + return 0.0f; // A zero duty + } + + float MotorState::_enable() { + enabled = true; + return last_enabled_duty; + } + + bool MotorState::is_enabled() const { + return enabled; + } + + float MotorState::get_duty() const { + return last_enabled_duty; + } + + float MotorState::set_duty_with_return(float duty) { + //TODO + // float speed_out, duty_out; + // if(calib.duty_to_speed(duty, speed_out, duty_out)) { + // motor_speed = speed_out; + // last_enabled_duty = duty_out; + // return _enable(); + // } + return disable(); + } + + float MotorState::get_speed() const { + return motor_speed; + } + + float MotorState::set_speed_with_return(float speed) { + //TODO + // float duty_out, speed_out; + // if(calib.speed_to_duty(speed, duty_out, speed_out)) { + // last_enabled_duty = duty_out; + // motor_speed = speed_out; + // return _enable(); + // } + return disable(); + } + + float MotorState::get_min_speed() const { + float speed = 0.0f; + //TODO + //if(calib.size() > 0) { + // speed = calib.first().speed; + //} + return speed; + } + + // float MotorState::get_mid_speed() const { + // float speed = 0.0f; + // if(calib.size() > 0) { + // const Calibration::Pair &first = calib.first(); + // const Calibration::Pair &last = calib.last(); + // speed = (first.speed + last.speed) / 2.0f; + // } + // return speed; + // } + + float MotorState::get_max_speed() const { + float speed = 0.0f; + //TODO + //if(calib.size() > 0) { + // speed = calib.last().speed; + //} + return speed; + } + + float MotorState::to_min_with_return() { + return set_speed_with_return(get_min_speed()); + } + + // float MotorState::to_mid_with_return() { + // return set_speed_with_return(get_mid_speed()); + // } + + float MotorState::to_max_with_return() { + return set_speed_with_return(get_max_speed()); + } + + float MotorState::to_percent_with_return(float in, float in_min, float in_max) { + float speed = MotorState::map_float(in, in_min, in_max, get_min_speed(), get_max_speed()); + 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); + } + + // Calibration& MotorState::calibration() { + // return calib; + // } + + // const Calibration& MotorState::calibration() const { + // return calib; + // } + + 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; + } + +}; \ No newline at end of file diff --git a/drivers/motor/motor_state.hpp b/drivers/motor/motor_state.hpp new file mode 100644 index 00000000..31107fc8 --- /dev/null +++ b/drivers/motor/motor_state.hpp @@ -0,0 +1,90 @@ +#pragma once + +#include "pico/stdlib.h" +#include "calibration.hpp" + +namespace motor { + + class MotorState { + //-------------------------------------------------- + // Enums + //-------------------------------------------------- + public: + enum DecayMode { + FAST_DECAY = 0, //aka 'Coasting' + SLOW_DECAY = 1, //aka 'Braking' + }; + + + //-------------------------------------------------- + // Constants + //-------------------------------------------------- + public: + static constexpr float DEFAULT_FREQUENCY = 25000.0f; // The standard motor update rate + static const DecayMode DEFAULT_DECAY_MODE = SLOW_DECAY; + static constexpr float MIN_FREQUENCY = 10.0f; // Lowest achievable with hardware PWM with good resolution + static constexpr float MAX_FREQUENCY = 50000.0f; // Highest nice speed + static constexpr float ZERO_PERCENT = 0.0f; + static constexpr float ONEHUNDRED_PERCENT = 1.0f; + + private: + static constexpr float MIN_VALID_DUTY = 1.0f; + + + //-------------------------------------------------- + // Variables + //-------------------------------------------------- + private: + float motor_speed = 0.0f; + float last_enabled_duty = 0.0f; + bool enabled = false; + //Calibration calib; + + + //-------------------------------------------------- + // Constructors/Destructor + //-------------------------------------------------- + public: + MotorState(); + //MotorState(CalibrationType default_type); + //MotorState(const Calibration& calibration); + + + //-------------------------------------------------- + // Methods + //-------------------------------------------------- + public: + float enable(); + float disable(); + bool is_enabled() const; + private: + float _enable(); // Internal version of enable without convenient initialisation to the middle + public: + float get_duty() const; + float set_duty_with_return(float duty); + + float get_speed() const; + float set_speed_with_return(float speed); + + public: + float get_min_speed() const; + //float get_mid_speed() const; + float get_max_speed() const; + + float to_min_with_return(); + //float to_mid_with_return(); + float to_max_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); + + //Calibration& calibration(); + //const Calibration& calibration() const; + + //-------------------------------------------------- + static int32_t duty_to_level(float duty, uint32_t resolution); + + private: + static float map_float(float in, float in_min, float in_max, float out_min, float out_max); + }; + +} \ No newline at end of file diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 4cbb1eaf..e2ecfb85 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -42,3 +42,4 @@ add_subdirectory(plasma2040) add_subdirectory(badger2040) add_subdirectory(interstate75) add_subdirectory(servo2040) +add_subdirectory(motor2040) diff --git a/examples/motor2040/CMakeLists.txt b/examples/motor2040/CMakeLists.txt new file mode 100644 index 00000000..d2009f34 --- /dev/null +++ b/examples/motor2040/CMakeLists.txt @@ -0,0 +1,10 @@ +#include(servo2040_calibration.cmake) +#include(servo2040_current_meter.cmake) +#include(servo2040_led_rainbow.cmake) +#include(servo2040_multiple_servos.cmake) +#include(servo2040_read_sensors.cmake) +#include(servo2040_sensor_feedback.cmake) +#include(servo2040_servo_cluster.cmake) +#include(servo2040_servo_wave.cmake) +#include(servo2040_simple_easing.cmake) +include(motor2040_single_motor.cmake) \ No newline at end of file diff --git a/examples/motor2040/motor2040_single_motor.cmake b/examples/motor2040/motor2040_single_motor.cmake new file mode 100644 index 00000000..a5ad181e --- /dev/null +++ b/examples/motor2040/motor2040_single_motor.cmake @@ -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}) diff --git a/examples/motor2040/motor2040_single_motor.cpp b/examples/motor2040/motor2040_single_motor.cpp new file mode 100644 index 00000000..be32938e --- /dev/null +++ b/examples/motor2040/motor2040_single_motor.cpp @@ -0,0 +1,72 @@ +#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 = 3; + +// The number of discrete sweep steps +const uint STEPS = 10; + +// The time in milliseconds between each step of the sequence +const uint STEPS_INTERVAL_MS = 500; + +// How far from zero to move the motor when sweeping +constexpr float SWEEP_EXTENT = 90.0f; + + +// Create a motor on pin 0 and 1 +Motor2 m = Motor2(motor2040::SERVO_1, motor2040::SERVO_2); + + +int main() { + stdio_init_all(); + + // Initialise the motor + m.init(); + + // Enable the motor (this puts it at the middle) + m.enable(); + sleep_ms(2000); + + // Go to min + m.to_min(); + sleep_ms(2000); + + // Go to max + m.to_max(); + sleep_ms(2000); + + // Go back to mid + //m.to_mid(); + //sleep_ms(2000); + + // Do a sine sweep + for(auto j = 0u; j < SWEEPS; j++) { + for(auto i = 0u; i < 360; i++) { + m.speed(sin(((float)i * (float)M_PI) / 180.0f) * SWEEP_EXTENT); + sleep_ms(20); + } + } + + // Do a stepped sweep + for(auto j = 0u; j < SWEEPS; j++) { + for(auto i = 0u; i < STEPS; i++) { + m.to_percent(i, 0, STEPS, 0.0 - SWEEP_EXTENT, SWEEP_EXTENT); + sleep_ms(STEPS_INTERVAL_MS); + } + for(auto i = 0u; i < STEPS; i++) { + m.to_percent(i, STEPS, 0, 0.0 - SWEEP_EXTENT, SWEEP_EXTENT); + sleep_ms(STEPS_INTERVAL_MS); + } + } + + // Disable the motor + m.disable(); +} diff --git a/libraries/CMakeLists.txt b/libraries/CMakeLists.txt index e606e4fd..9cd8a71e 100644 --- a/libraries/CMakeLists.txt +++ b/libraries/CMakeLists.txt @@ -29,3 +29,4 @@ add_subdirectory(pico_wireless) add_subdirectory(plasma2040) add_subdirectory(badger2040) add_subdirectory(servo2040) +add_subdirectory(motor2040) diff --git a/libraries/motor2040/CMakeLists.txt b/libraries/motor2040/CMakeLists.txt new file mode 100644 index 00000000..a8598a0a --- /dev/null +++ b/libraries/motor2040/CMakeLists.txt @@ -0,0 +1 @@ +include(motor2040.cmake) \ No newline at end of file diff --git a/libraries/motor2040/motor2040.cmake b/libraries/motor2040/motor2040.cmake new file mode 100644 index 00000000..96aa50fa --- /dev/null +++ b/libraries/motor2040/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 motor2) # motor_cluster) \ No newline at end of file diff --git a/libraries/motor2040/motor2040.hpp b/libraries/motor2040/motor2040.hpp new file mode 100644 index 00000000..ee93be86 --- /dev/null +++ b/libraries/motor2040/motor2040.hpp @@ -0,0 +1,60 @@ +#pragma once +#include "pico/stdlib.h" + +#include "ws2812.hpp" +#include "motor2.hpp" +#include "motor_cluster.hpp" + +namespace motor { + namespace motor2040 { + const uint SERVO_1 = 0; + const uint SERVO_2 = 1; + const uint SERVO_3 = 2; + const uint SERVO_4 = 3; + const uint SERVO_5 = 4; + const uint SERVO_6 = 5; + const uint SERVO_7 = 6; + const uint SERVO_8 = 7; + const uint SERVO_9 = 8; + const uint SERVO_10 = 9; + const uint SERVO_11 = 10; + const uint SERVO_12 = 11; + const uint SERVO_13 = 12; + const uint SERVO_14 = 13; + const uint SERVO_15 = 14; + const uint SERVO_16 = 15; + const uint SERVO_17 = 16; + const uint SERVO_18 = 17; + const uint NUM_SERVOS = 18; + + const uint LED_DATA = 18; + const uint 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 SENSOR_1_ADDR = 0b000; + const uint SENSOR_2_ADDR = 0b001; + const uint SENSOR_3_ADDR = 0b010; + const uint SENSOR_4_ADDR = 0b011; + const uint SENSOR_5_ADDR = 0b100; + const uint SENSOR_6_ADDR = 0b101; + const uint NUM_SENSORS = 6; + + const uint VOLTAGE_SENSE_ADDR = 0b110; + const uint CURRENT_SENSE_ADDR = 0b111; + + constexpr float SHUNT_RESISTOR = 0.003f; + constexpr float CURRENT_GAIN = 69; + constexpr float VOLTAGE_GAIN = 3.9f / 13.9f; + constexpr float CURRENT_OFFSET = -0.02f; + } +} \ No newline at end of file From 9a9c3832a21c66a1150bd2fdae52bc9c1c031118 Mon Sep 17 00:00:00 2001 From: ZodiusInfuser Date: Mon, 4 Apr 2022 20:00:03 +0100 Subject: [PATCH 03/60] Various motor work --- drivers/motor/calibration.cpp | 338 ------------------ drivers/motor/calibration.hpp | 119 ------ drivers/motor/motor.cmake | 1 + drivers/motor/motor.cpp | 42 +-- drivers/motor/motor.hpp | 8 +- drivers/motor/motor2.cpp | 119 +++--- drivers/motor/motor2.hpp | 30 +- drivers/motor/motor_cluster.cpp | 193 +++------- drivers/motor/motor_cluster.hpp | 44 +-- drivers/motor/motor_state.cpp | 127 +++---- drivers/motor/motor_state.hpp | 62 ++-- examples/motor2040/motor2040_single_motor.cpp | 18 +- examples/pico_motor_shim/balance/demo.cpp | 5 +- examples/pico_motor_shim/sequence/demo.cpp | 5 +- examples/pico_motor_shim/song/demo.cpp | 9 +- libraries/motor2040/motor2040.cmake | 2 +- libraries/motor2040/motor2040.hpp | 73 ++-- .../pico_motor_shim/pico_motor_shim.cmake | 2 +- libraries/pico_motor_shim/pico_motor_shim.hpp | 19 +- 19 files changed, 332 insertions(+), 884 deletions(-) delete mode 100644 drivers/motor/calibration.cpp delete mode 100644 drivers/motor/calibration.hpp diff --git a/drivers/motor/calibration.cpp b/drivers/motor/calibration.cpp deleted file mode 100644 index 64fbd50d..00000000 --- a/drivers/motor/calibration.cpp +++ /dev/null @@ -1,338 +0,0 @@ -#include "calibration.hpp" - -namespace motor { - Calibration::Pair::Pair() - : duty(0.0f), speed(0.0f) { - } - - Calibration::Pair::Pair(float duty, float speed) - : duty(duty), speed(speed) { - } - - Calibration::Calibration() - : calibration(nullptr), calibration_size(0), limit_lower(true), limit_upper(true) { - } - - Calibration::Calibration(CalibrationType default_type) - : Calibration() { - apply_default_pairs(default_type); - } - - Calibration::Calibration(const Calibration &other) - : calibration(nullptr), calibration_size(0), limit_lower(other.limit_lower), limit_upper(other.limit_upper) { - uint size = other.size(); - apply_blank_pairs(size); - for(uint i = 0; i < size; i++) { - calibration[i] = other.calibration[i]; - } - } - - Calibration::~Calibration() { - if(calibration != nullptr) { - delete[] calibration; - calibration = nullptr; - } - } - - Calibration &Calibration::operator=(const Calibration &other) { - uint size = other.size(); - apply_blank_pairs(size); - for(uint i = 0; i < size; i++) { - calibration[i] = other.calibration[i]; - } - limit_lower = other.limit_lower; - limit_upper = other.limit_upper; - - return *this; - } - - Calibration::Pair &Calibration::operator[](uint8_t index) { - assert(index < calibration_size); - return calibration[index]; - } - - const Calibration::Pair &Calibration::operator[](uint8_t index) const { - assert(index < calibration_size); - return calibration[index]; - } - - void Calibration::apply_blank_pairs(uint size) { - if(calibration != nullptr) { - delete[] calibration; - } - - if(size > 0) { - calibration = new Pair[size]; - calibration_size = size; - } - else { - calibration = nullptr; - calibration_size = 0; - } - } - - void Calibration::apply_two_pairs(float min_duty, float max_duty, float min_speed, float max_speed) { - apply_blank_pairs(2); - calibration[0] = Pair(min_duty, min_speed); - calibration[1] = Pair(max_duty, max_speed); - } - - void Calibration::apply_three_pairs(float min_duty, float mid_duty, float max_duty, float min_speed, float mid_speed, float max_speed) { - apply_blank_pairs(3); - calibration[0] = Pair(min_duty, min_speed); - calibration[1] = Pair(mid_duty, mid_speed); - calibration[2] = Pair(max_duty, max_speed); - } - - void Calibration::apply_uniform_pairs(uint size, float min_duty, float max_duty, float min_speed, float max_speed) { - apply_blank_pairs(size); - if(size > 0) { - float size_minus_one = (float)(size - 1); - for(uint i = 0; i < size; i++) { - float duty = Calibration::map_float((float)i, 0.0f, size_minus_one, min_duty, max_duty); - float speed = Calibration::map_float((float)i, 0.0f, size_minus_one, min_speed, max_speed); - calibration[i] = Pair(duty, speed); - } - } - } - - void Calibration::apply_default_pairs(CalibrationType default_type) { - switch(default_type) { - default: - case ANGULAR: - apply_three_pairs(DEFAULT_MIN_PULSE, DEFAULT_MID_PULSE, DEFAULT_MAX_PULSE, - -90.0f, 0.0f, +90.0f); - break; - case LINEAR: - apply_two_pairs(DEFAULT_MIN_PULSE, DEFAULT_MAX_PULSE, - 0.0f, 1.0f); - break; - case CONTINUOUS: - apply_three_pairs(DEFAULT_MIN_PULSE, DEFAULT_MID_PULSE, DEFAULT_MAX_PULSE, - -1.0f, 0.0f, +1.0f); - break; - } - } - - uint Calibration::size() const { - return calibration_size; - } - - Calibration::Pair &Calibration::pair(uint8_t index) { - assert(index < calibration_size); - return calibration[index]; - } - - const Calibration::Pair &Calibration::pair(uint8_t index) const { - assert(index < calibration_size); - return calibration[index]; - } - - float Calibration::duty(uint8_t index) const { - return pair(index).duty; - } - - void Calibration::duty(uint8_t index, float duty) { - pair(index).duty = duty; - } - - float Calibration::speed(uint8_t index) const { - return pair(index).speed; - } - - void Calibration::speed(uint8_t index, float speed) { - pair(index).speed = speed; - } - - Calibration::Pair &Calibration::first() { - assert(calibration_size > 0); - return calibration[0]; - } - - const Calibration::Pair &Calibration::first() const { - assert(calibration_size > 0); - return calibration[0]; - } - - float Calibration::first_duty() const { - return first().duty; - } - - void Calibration::first_duty(float duty) { - first().duty = duty; - } - - float Calibration::first_speed() const { - return first().speed; - } - - void Calibration::first_speed(float speed) { - first().speed = speed; - } - - Calibration::Pair &Calibration::last() { - assert(calibration_size > 0); - return calibration[calibration_size - 1]; - } - - const Calibration::Pair &Calibration::last() const { - assert(calibration_size > 0); - return calibration[calibration_size - 1]; - } - - float Calibration::last_duty() const { - return last().duty; - } - - void Calibration::last_duty(float duty) { - last().duty = duty; - } - - float Calibration::last_speed() const { - return last().speed; - } - - void Calibration::last_speed(float speed) { - last().speed = speed; - } - - bool Calibration::has_lower_limit() const { - return limit_lower; - } - - bool Calibration::has_upper_limit() const { - return limit_upper; - } - - void Calibration::limit_to_calibration(bool lower, bool upper) { - limit_lower = lower; - limit_upper = upper; - } - - bool Calibration::speed_to_duty(float speed, float &duty_out, float &speed_out) const { - bool success = false; - if(calibration_size >= 2) { - uint8_t last = calibration_size - 1; - - speed_out = speed; - - // Is the speed below the bottom most calibration pair? - if(speed < calibration[0].speed) { - // Should the speed be limited to the calibration or projected below it? - if(limit_lower) { - duty_out = calibration[0].duty; - speed_out = calibration[0].speed; - } - else { - duty_out = map_float(speed, calibration[0].speed, calibration[1].speed, - calibration[0].duty, calibration[1].duty); - } - } - // Is the speed above the top most calibration pair? - else if(speed > calibration[last].speed) { - // Should the speed be limited to the calibration or projected above it? - if(limit_upper) { - duty_out = calibration[last].duty; - speed_out = calibration[last].speed; - } - else { - duty_out = map_float(speed, calibration[last - 1].speed, calibration[last].speed, - calibration[last - 1].duty, calibration[last].duty); - } - } - else { - // The speed must between two calibration pairs, so iterate through them to find which ones - for(uint8_t i = 0; i < last; i++) { - if(speed <= calibration[i + 1].speed) { - duty_out = map_float(speed, calibration[i].speed, calibration[i + 1].speed, - calibration[i].duty, calibration[i + 1].duty); - break; // No need to continue checking so break out of the loop - } - } - } - - // Clamp the duty between the hard limits - if(duty_out < LOWER_HARD_LIMIT || duty_out > UPPER_HARD_LIMIT) { - duty_out = MIN(MAX(duty_out, LOWER_HARD_LIMIT), UPPER_HARD_LIMIT); - - // Is the duty below the bottom most calibration pair? - if(duty_out < calibration[0].duty) { - speed_out = map_float(duty_out, calibration[0].duty, calibration[1].duty, - calibration[0].speed, calibration[1].speed); - } - // Is the duty above the top most calibration pair? - else if(duty_out > calibration[last].duty) { - speed_out = map_float(duty_out, calibration[last - 1].duty, calibration[last].duty, - calibration[last - 1].speed, calibration[last].speed); - } - else { - // The duty must between two calibration pairs, so iterate through them to find which ones - for(uint8_t i = 0; i < last; i++) { - if(duty_out <= calibration[i + 1].duty) { - speed_out = map_float(duty_out, calibration[i].duty, calibration[i + 1].duty, - calibration[i].speed, calibration[i + 1].speed); - break; // No need to continue checking so break out of the loop - } - } - } - } - - success = true; - } - - return success; - } - - bool Calibration::duty_to_speed(float duty, float &speed_out, float &duty_out) const { - bool success = false; - if(calibration_size >= 2) { - uint8_t last = calibration_size - 1; - - // Clamp the duty between the hard limits - duty_out = MIN(MAX(duty, LOWER_HARD_LIMIT), UPPER_HARD_LIMIT); - - // Is the duty below the bottom most calibration pair? - if(duty_out < calibration[0].duty) { - // Should the duty be limited to the calibration or projected below it? - if(limit_lower) { - speed_out = calibration[0].speed; - duty_out = calibration[0].duty; - } - else { - speed_out = map_float(duty, calibration[0].duty, calibration[1].duty, - calibration[0].speed, calibration[1].speed); - } - } - // Is the duty above the top most calibration pair? - else if(duty > calibration[last].duty) { - // Should the duty be limited to the calibration or projected above it? - if(limit_upper) { - speed_out = calibration[last].speed; - duty_out = calibration[last].duty; - } - else { - speed_out = map_float(duty, calibration[last - 1].duty, calibration[last].duty, - calibration[last - 1].speed, calibration[last].speed); - } - } - else { - // The duty must between two calibration pairs, so iterate through them to find which ones - for(uint8_t i = 0; i < last; i++) { - if(duty <= calibration[i + 1].duty) { - speed_out = map_float(duty, calibration[i].duty, calibration[i + 1].duty, - calibration[i].speed, calibration[i + 1].speed); - break; // No need to continue checking so break out of the loop - } - } - } - - success = true; - } - - return success; - } - - float Calibration::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; - } -}; \ No newline at end of file diff --git a/drivers/motor/calibration.hpp b/drivers/motor/calibration.hpp deleted file mode 100644 index 447773d2..00000000 --- a/drivers/motor/calibration.hpp +++ /dev/null @@ -1,119 +0,0 @@ -#pragma once - -#include "pico/stdlib.h" - -namespace motor { - - enum CalibrationType { - ANGULAR = 0, - LINEAR, - CONTINUOUS - }; - - class Calibration { - //-------------------------------------------------- - // Constants - //-------------------------------------------------- - public: - static constexpr float DEFAULT_MIN_PULSE = 500.0f; // in microseconds - static constexpr float DEFAULT_MID_PULSE = 1500.0f; // in microseconds - static constexpr float DEFAULT_MAX_PULSE = 2500.0f; // in microseconds - - private: - static constexpr float LOWER_HARD_LIMIT = 400.0f; // The minimum microsecond duty to send - static constexpr float UPPER_HARD_LIMIT = 2600.0f; // The maximum microsecond duty to send - - - //-------------------------------------------------- - // Substructures - //-------------------------------------------------- - public: - struct Pair { - //-------------------------------------------------- - // Constructors/Destructor - //-------------------------------------------------- - Pair(); - Pair(float duty, float speed); - - - //-------------------------------------------------- - // Variables - //-------------------------------------------------- - float duty; - float speed; - }; - - - //-------------------------------------------------- - // Constructors/Destructor - //-------------------------------------------------- - public: - Calibration(); - Calibration(CalibrationType default_type); - Calibration(const Calibration &other); - virtual ~Calibration(); - - - //-------------------------------------------------- - // Operators - //-------------------------------------------------- - public: - Calibration &operator=(const Calibration &other); - Pair &operator[](uint8_t index); - const Pair &operator[](uint8_t index) const; - - - //-------------------------------------------------- - // Methods - //-------------------------------------------------- - public: - void apply_blank_pairs(uint size); - void apply_two_pairs(float min_duty, float max_duty, float min_speed, float max_speed); - void apply_three_pairs(float min_duty, float mid_duty, float max_duty, float min_speed, float mid_speed, float max_speed); - void apply_uniform_pairs(uint size, float min_duty, float max_duty, float min_speed, float max_speed); - void apply_default_pairs(CalibrationType default_type); - - uint size() const; - - Pair &pair(uint8_t index); // Ensure the pairs are assigned in ascending speed order - const Pair &pair(uint8_t index) const; // Ensure the pairs are assigned in ascending speed order - float duty(uint8_t index) const; - void duty(uint8_t index, float duty); - float speed(uint8_t index) const; - void speed(uint8_t index, float speed); - - Pair &first(); - const Pair &first() const; - float first_duty() const; - void first_duty(float duty); - float first_speed() const; - void first_speed(float speed); - - Pair &last(); - const Pair &last() const; - float last_duty() const; - void last_duty(float duty); - float last_speed() const; - void last_speed(float speed); - - bool has_lower_limit() const; - bool has_upper_limit() const; - void limit_to_calibration(bool lower, bool upper); - - bool speed_to_duty(float speed, float &duty_out, float &speed_out) const; - bool duty_to_speed(float duty, float &speed_out, float &duty_out) const; - - //static float map_float(float in, float in_min, float in_max, float out_min, float out_max); - - - //-------------------------------------------------- - // Variables - //-------------------------------------------------- - private: - Pair* calibration; - uint calibration_size; - bool limit_lower; - bool limit_upper; - }; - -} \ No newline at end of file diff --git a/drivers/motor/motor.cmake b/drivers/motor/motor.cmake index 7f498ab6..38001cf8 100644 --- a/drivers/motor/motor.cmake +++ b/drivers/motor/motor.cmake @@ -3,6 +3,7 @@ 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}) diff --git a/drivers/motor/motor.cpp b/drivers/motor/motor.cpp index 50dbe95b..d1db206e 100644 --- a/drivers/motor/motor.cpp +++ b/drivers/motor/motor.cpp @@ -1,14 +1,14 @@ #include "motor.hpp" #include "pwm.hpp" -namespace pimoroni { - Motor::Motor(uint pin_pos, uint pin_neg, float freq, DecayMode mode) - : pin_pos(pin_pos), pin_neg(pin_neg), pwm_frequency(freq), motor_decay_mode(mode) { +namespace motor { + Motor::Motor(const MotorPins &pins, float freq, DecayMode mode) + : pins(pins), pwm_frequency(freq), motor_decay_mode(mode) { } Motor::~Motor() { - gpio_set_function(pin_pos, GPIO_FUNC_NULL); - gpio_set_function(pin_neg, GPIO_FUNC_NULL); + gpio_set_function(pins.positive, GPIO_FUNC_NULL); + gpio_set_function(pins.negative, GPIO_FUNC_NULL); } bool Motor::init() { @@ -26,11 +26,11 @@ namespace pimoroni { //Apply the divider pwm_config_set_clkdiv(&pwm_cfg, (float)div16 / 16.0f); - pwm_init(pwm_gpio_to_slice_num(pin_pos), &pwm_cfg, true); - gpio_set_function(pin_pos, GPIO_FUNC_PWM); + pwm_init(pwm_gpio_to_slice_num(pins.positive), &pwm_cfg, true); + gpio_set_function(pins.positive, GPIO_FUNC_PWM); - pwm_init(pwm_gpio_to_slice_num(pin_neg), &pwm_cfg, true); - gpio_set_function(pin_neg, GPIO_FUNC_PWM); + pwm_init(pwm_gpio_to_slice_num(pins.negative), &pwm_cfg, true); + gpio_set_function(pins.negative, GPIO_FUNC_PWM); update_pwm(); success = true; @@ -66,8 +66,8 @@ namespace pimoroni { pwm_period = period; pwm_frequency = freq; - uint pos_num = pwm_gpio_to_slice_num(pin_pos); - uint neg_num = pwm_gpio_to_slice_num(pin_neg); + uint pos_num = pwm_gpio_to_slice_num(pins.positive); + uint neg_num = pwm_gpio_to_slice_num(pins.negative); //Apply the new divider uint8_t div = div16 >> 4; @@ -112,8 +112,8 @@ namespace pimoroni { void Motor::disable() { motor_speed = 0.0f; - pwm_set_gpio_level(pin_pos, 0); - pwm_set_gpio_level(pin_neg, 0); + pwm_set_gpio_level(pins.positive, 0); + pwm_set_gpio_level(pins.negative, 0); } void Motor::update_pwm() { @@ -122,24 +122,24 @@ namespace pimoroni { switch(motor_decay_mode) { case SLOW_DECAY: //aka 'Braking' if(signed_duty_cycle >= 0) { - pwm_set_gpio_level(pin_pos, pwm_period); - pwm_set_gpio_level(pin_neg, pwm_period - signed_duty_cycle); + pwm_set_gpio_level(pins.positive, pwm_period); + pwm_set_gpio_level(pins.negative, pwm_period - signed_duty_cycle); } else { - pwm_set_gpio_level(pin_pos, pwm_period + signed_duty_cycle); - pwm_set_gpio_level(pin_neg, pwm_period); + pwm_set_gpio_level(pins.positive, pwm_period + signed_duty_cycle); + pwm_set_gpio_level(pins.negative, pwm_period); } break; case FAST_DECAY: //aka 'Coasting' default: if(signed_duty_cycle >= 0) { - pwm_set_gpio_level(pin_pos, signed_duty_cycle); - pwm_set_gpio_level(pin_neg, 0); + pwm_set_gpio_level(pins.positive, signed_duty_cycle); + pwm_set_gpio_level(pins.negative, 0); } else { - pwm_set_gpio_level(pin_pos, 0); - pwm_set_gpio_level(pin_neg, 0 - signed_duty_cycle); + pwm_set_gpio_level(pins.positive, 0); + pwm_set_gpio_level(pins.negative, 0 - signed_duty_cycle); } break; } diff --git a/drivers/motor/motor.hpp b/drivers/motor/motor.hpp index eb3d6922..a2e7bff3 100644 --- a/drivers/motor/motor.hpp +++ b/drivers/motor/motor.hpp @@ -2,8 +2,9 @@ #include "pico/stdlib.h" #include "hardware/pwm.h" +#include "motor_state.hpp" -namespace pimoroni { +namespace motor { class Motor { //-------------------------------------------------- @@ -28,8 +29,7 @@ namespace pimoroni { // Variables //-------------------------------------------------- private: - uint pin_pos; - uint pin_neg; + MotorPins pins; pwm_config pwm_cfg; uint16_t pwm_period; float pwm_frequency = DEFAULT_PWM_FREQUENCY; @@ -42,7 +42,7 @@ namespace pimoroni { // Constructors/Destructor //-------------------------------------------------- public: - Motor(uint pin_pos, uint pin_neg, float freq = DEFAULT_PWM_FREQUENCY, DecayMode mode = DEFAULT_DECAY_MODE); + Motor(const MotorPins &pins, float freq = DEFAULT_PWM_FREQUENCY, DecayMode mode = DEFAULT_DECAY_MODE); ~Motor(); diff --git a/drivers/motor/motor2.cpp b/drivers/motor/motor2.cpp index 441e4957..2f584796 100644 --- a/drivers/motor/motor2.cpp +++ b/drivers/motor/motor2.cpp @@ -3,17 +3,13 @@ #include "pwm.hpp" namespace motor { - Motor2::Motor2(uint pin_pos, uint pin_neg, float freq, MotorState::DecayMode mode) - : motor_pin_pos(pin_pos), motor_pin_neg(pin_neg), pwm_frequency(freq), motor_decay_mode(mode) { + Motor2::Motor2(const MotorPins &pins, float speed_scale, float freq, MotorState::DecayMode mode) + : motor_pins(pins), state(speed_scale, false), pwm_frequency(freq), motor_decay_mode(mode) { } - // Motor2::Motor2(uint pin, /*const Calibration& calibration,*/ float freq) - // : motor_pin_pos(pin), /*state(calibration),*/ pwm_frequency(freq) { - // } - Motor2::~Motor2() { - gpio_set_function(motor_pin_pos, GPIO_FUNC_NULL); - gpio_set_function(motor_pin_neg, GPIO_FUNC_NULL); + gpio_set_function(motor_pins.positive, GPIO_FUNC_NULL); + gpio_set_function(motor_pins.negative, GPIO_FUNC_NULL); } bool Motor2::init() { @@ -31,43 +27,43 @@ namespace motor { // 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... - pwm_init(pwm_gpio_to_slice_num(motor_pin_pos), &pwm_cfg, true); - gpio_set_function(motor_pin_pos, GPIO_FUNC_PWM); + pwm_init(pwm_gpio_to_slice_num(motor_pins.positive), &pwm_cfg, true); + gpio_set_function(motor_pins.positive, GPIO_FUNC_PWM); - pwm_init(pwm_gpio_to_slice_num(motor_pin_neg), &pwm_cfg, true); - gpio_set_function(motor_pin_neg, GPIO_FUNC_PWM); + pwm_init(pwm_gpio_to_slice_num(motor_pins.negative), &pwm_cfg, true); + gpio_set_function(motor_pins.negative, GPIO_FUNC_PWM); - pwm_set_gpio_level(motor_pin_pos, 0); - pwm_set_gpio_level(motor_pin_neg, 0); + pwm_set_gpio_level(motor_pins.positive, 0); + pwm_set_gpio_level(motor_pins.negative, 0); success = true; } return success; } - uint Motor2::pin() const { - return motor_pin_pos; + MotorPins Motor2::pins() const { + return motor_pins; } void Motor2::enable() { - apply_duty(state.enable()); + apply_duty(state.enable_with_return()); } void Motor2::disable() { - apply_duty(state.disable()); + apply_duty(state.disable_with_return()); } bool Motor2::is_enabled() const { return state.is_enabled(); } - // float Motor2::duty() const { - // return state.get_duty(); - // } + float Motor2::duty() const { + return state.get_duty(); + } - // void Motor2::duty(float duty) { - // apply_duty(state.set_duty_with_return(duty)); - // } + void Motor2::duty(float duty) { + apply_duty(state.set_duty_with_return(duty)); + } float Motor2::speed() const { return state.get_speed(); @@ -77,6 +73,22 @@ namespace motor { apply_duty(state.set_speed_with_return(speed)); } + float Motor2::speed_scale() const { + return state.get_speed_scale(); + } + + void Motor2::speed_scale(float speed_scale) { + state.set_speed_scale(speed_scale); + } + + void Motor2::invert_direction(bool invert) { + state.invert_direction(invert); + } + + bool Motor2::is_inverted() const { + return state.is_inverted(); + } + float Motor2::frequency() const { return pwm_frequency; } @@ -97,12 +109,15 @@ namespace motor { pwm_period = period; pwm_frequency = freq; - uint pin_num = pwm_gpio_to_slice_num(motor_pin_pos); + uint pos_pin_num = pwm_gpio_to_slice_num(motor_pins.positive); + uint neg_pin_num = pwm_gpio_to_slice_num(motor_pins.negative); // Apply the new divider uint8_t div = div16 >> 4; uint8_t mod = div16 % 16; - pwm_set_clkdiv_int_frac(pin_num, div, mod); + pwm_set_clkdiv_int_frac(pos_pin_num, div, mod); + if(neg_pin_num != pos_pin_num) + pwm_set_clkdiv_int_frac(neg_pin_num, div, mod); // If the the period is larger, update the pwm before setting the new wraps if(state.is_enabled() && pre_update_pwm) { @@ -110,7 +125,9 @@ namespace motor { } // Set the new wrap (should be 1 less than the period to get full 0 to 100%) - pwm_set_wrap(pin_num, pwm_period - 1); + pwm_set_wrap(pos_pin_num, pwm_period - 1); + if(neg_pin_num != pos_pin_num) + pwm_set_wrap(neg_pin_num, pwm_period - 1); // If the the period is smaller, update the pwm after setting the new wraps if(state.is_enabled() && !pre_update_pwm) { @@ -140,28 +157,12 @@ namespace motor { disable(); } - float Motor2::min_speed() const { - return state.get_min_speed(); + void Motor2::to_full_negative() { + apply_duty(state.to_full_negative_with_return()); } - // float Motor2::mid_speed() const { - // return state.get_mid_speed(); - // } - - float Motor2::max_speed() const { - return state.get_max_speed(); - } - - void Motor2::to_min() { - apply_duty(state.to_min_with_return()); - } - - // void Motor2::to_mid() { - // apply_duty(state.to_mid_with_return()); - // } - - void Motor2::to_max() { - apply_duty(state.to_max_with_return()); + void Motor2::to_full_positive() { + apply_duty(state.to_full_positive_with_return()); } void Motor2::to_percent(float in, float in_min, float in_max) { @@ -172,38 +173,30 @@ namespace motor { apply_duty(state.to_percent_with_return(in, in_min, in_max, speed_min, speed_max)); } - // Calibration& Motor2::calibration() { - // return state.calibration(); - // } - - // const Calibration& Motor2::calibration() const { - // return state.calibration(); - // } - void Motor2::apply_duty(float duty) { int32_t signed_level = MotorState::duty_to_level(duty, pwm_period); switch(motor_decay_mode) { case MotorState::SLOW_DECAY: //aka 'Braking' if(signed_level >= 0) { - pwm_set_gpio_level(motor_pin_pos, pwm_period); - pwm_set_gpio_level(motor_pin_neg, pwm_period - signed_level); + 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_pin_pos, pwm_period + signed_level); - pwm_set_gpio_level(motor_pin_neg, pwm_period); + pwm_set_gpio_level(motor_pins.positive, pwm_period + signed_level); + pwm_set_gpio_level(motor_pins.negative, pwm_period); } break; case MotorState::FAST_DECAY: //aka 'Coasting' default: if(signed_level >= 0) { - pwm_set_gpio_level(motor_pin_pos, signed_level); - pwm_set_gpio_level(motor_pin_neg, 0); + pwm_set_gpio_level(motor_pins.positive, signed_level); + pwm_set_gpio_level(motor_pins.negative, 0); } else { - pwm_set_gpio_level(motor_pin_pos, 0); - pwm_set_gpio_level(motor_pin_neg, 0 - signed_level); + pwm_set_gpio_level(motor_pins.positive, 0); + pwm_set_gpio_level(motor_pins.negative, 0 - signed_level); } break; } diff --git a/drivers/motor/motor2.hpp b/drivers/motor/motor2.hpp index 43310c14..b4648e64 100644 --- a/drivers/motor/motor2.hpp +++ b/drivers/motor/motor2.hpp @@ -11,8 +11,7 @@ namespace motor { // Variables //-------------------------------------------------- private: - uint motor_pin_pos; - uint motor_pin_neg; + MotorPins motor_pins; MotorState state; pwm_config pwm_cfg; uint16_t pwm_period; @@ -24,8 +23,7 @@ namespace motor { // Constructors/Destructor //-------------------------------------------------- public: - Motor2(uint pin_pos, uint pin_neg, float freq = MotorState::DEFAULT_FREQUENCY, MotorState::DecayMode mode = MotorState::DEFAULT_DECAY_MODE); - //Motor2(uint pin, /*const Calibration& calibration,*/ float freq = MotorState::DEFAULT_FREQUENCY); + Motor2(const MotorPins &pins, float speed_scale = MotorState::DEFAULT_SPEED_SCALE, float freq = MotorState::DEFAULT_FREQUENCY, MotorState::DecayMode mode = MotorState::DEFAULT_DECAY_MODE); ~Motor2(); @@ -36,18 +34,24 @@ namespace motor { bool init(); // For print access in micropython - uint pin() const; + MotorPins pins() const; void enable(); void disable(); bool is_enabled() const; - //float duty() const; - //void duty(float duty); + float duty() const; + void duty(float duty); float speed() const; void speed(float speed); + float speed_scale() const; + void speed_scale(float speed_scale); + + void invert_direction(bool invert); + bool is_inverted() const; + float frequency() const; bool frequency(float freq); @@ -58,19 +62,11 @@ namespace motor { void coast(); // An alias for disable //-------------------------------------------------- - float min_speed() const; - //float mid_speed() const; - float max_speed() const; - - void to_min(); - //void to_mid(); - void to_max(); + void to_full_negative(); + void to_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); - //Calibration& calibration(); - //const Calibration& calibration() const; - //-------------------------------------------------- private: void apply_duty(float duty); diff --git a/drivers/motor/motor_cluster.cpp b/drivers/motor/motor_cluster.cpp index ad97751d..9760cc8e 100644 --- a/drivers/motor/motor_cluster.cpp +++ b/drivers/motor/motor_cluster.cpp @@ -3,44 +3,24 @@ #include namespace motor { - MotorCluster::MotorCluster(PIO pio, uint sm, uint pin_mask, CalibrationType default_type, float freq, bool auto_phase, PWMCluster::Sequence *seq_buffer, PWMCluster::TransitionData *dat_buffer) + MotorCluster::MotorCluster(PIO pio, uint sm, uint pin_mask, float speed_scale, bool inverted, float freq, bool auto_phase, PWMCluster::Sequence *seq_buffer, PWMCluster::TransitionData *dat_buffer) : pwms(pio, sm, pin_mask, seq_buffer, dat_buffer), pwm_frequency(freq) { - create_motor_states(default_type, auto_phase); + create_motor_states(speed_scale, inverted, auto_phase); } - MotorCluster::MotorCluster(PIO pio, uint sm, uint pin_base, uint pin_count, CalibrationType default_type, float freq, bool auto_phase, PWMCluster::Sequence *seq_buffer, PWMCluster::TransitionData *dat_buffer) + MotorCluster::MotorCluster(PIO pio, uint sm, uint pin_base, uint pin_count, float speed_scale, bool inverted, float freq, bool auto_phase, PWMCluster::Sequence *seq_buffer, PWMCluster::TransitionData *dat_buffer) : pwms(pio, sm, pin_base, pin_count, seq_buffer, dat_buffer), pwm_frequency(freq) { - create_motor_states(default_type, auto_phase); + create_motor_states(speed_scale, inverted, auto_phase); } - MotorCluster::MotorCluster(PIO pio, uint sm, const uint8_t *pins, uint32_t length, CalibrationType default_type, float freq, bool auto_phase, PWMCluster::Sequence *seq_buffer, PWMCluster::TransitionData *dat_buffer) + MotorCluster::MotorCluster(PIO pio, uint sm, const uint8_t *pins, uint32_t length, float speed_scale, bool inverted, float freq, bool auto_phase, PWMCluster::Sequence *seq_buffer, PWMCluster::TransitionData *dat_buffer) : pwms(pio, sm, pins, length, seq_buffer, dat_buffer), pwm_frequency(freq) { - create_motor_states(default_type, auto_phase); + create_motor_states(speed_scale, inverted, auto_phase); } - MotorCluster::MotorCluster(PIO pio, uint sm, std::initializer_list pins, CalibrationType default_type, float freq, bool auto_phase, PWMCluster::Sequence *seq_buffer, PWMCluster::TransitionData *dat_buffer) + MotorCluster::MotorCluster(PIO pio, uint sm, std::initializer_list pins, float speed_scale, bool inverted, float freq, bool auto_phase, PWMCluster::Sequence *seq_buffer, PWMCluster::TransitionData *dat_buffer) : pwms(pio, sm, pins, seq_buffer, dat_buffer), pwm_frequency(freq) { - create_motor_states(default_type, auto_phase); - } - - MotorCluster::MotorCluster(PIO pio, uint sm, uint pin_mask, const Calibration& calibration, float freq, bool auto_phase, PWMCluster::Sequence *seq_buffer, PWMCluster::TransitionData *dat_buffer) - : pwms(pio, sm, pin_mask, seq_buffer, dat_buffer), pwm_frequency(freq) { - create_motor_states(calibration, auto_phase); - } - - MotorCluster::MotorCluster(PIO pio, uint sm, uint pin_base, uint pin_count, const Calibration& calibration, float freq, bool auto_phase, PWMCluster::Sequence *seq_buffer, PWMCluster::TransitionData *dat_buffer) - : pwms(pio, sm, pin_base, pin_count, seq_buffer, dat_buffer), pwm_frequency(freq) { - create_motor_states(calibration, auto_phase); - } - - MotorCluster::MotorCluster(PIO pio, uint sm, const uint8_t *pins, uint32_t length, const Calibration& calibration, float freq, bool auto_phase, PWMCluster::Sequence *seq_buffer, PWMCluster::TransitionData *dat_buffer) - : pwms(pio, sm, pins, length, seq_buffer, dat_buffer), pwm_frequency(freq) { - create_motor_states(calibration, auto_phase); - } - - MotorCluster::MotorCluster(PIO pio, uint sm, std::initializer_list pins, const Calibration& calibration, float freq, bool auto_phase, PWMCluster::Sequence *seq_buffer, PWMCluster::TransitionData *dat_buffer) - : pwms(pio, sm, pins, seq_buffer, dat_buffer), pwm_frequency(freq) { - create_motor_states(calibration, auto_phase); + create_motor_states(speed_scale, inverted, auto_phase); } MotorCluster::~MotorCluster() { @@ -90,8 +70,8 @@ namespace motor { void MotorCluster::enable(uint8_t motor, bool load) { assert(motor < pwms.get_chan_count()); - float new_pulse = states[motor].enable(); - apply_pulse(motor, new_pulse, load); + float new_duty = states[motor].enable_with_return(); + apply_duty(motor, new_duty, load); } void MotorCluster::enable(const uint8_t *motors, uint8_t length, bool load) { @@ -122,8 +102,8 @@ namespace motor { void MotorCluster::disable(uint8_t motor, bool load) { assert(motor < pwms.get_chan_count()); - float new_pulse = states[motor].disable(); - apply_pulse(motor, new_pulse, load); + float new_duty = states[motor].disable_with_return(); + apply_duty(motor, new_duty, load); } void MotorCluster::disable(const uint8_t *motors, uint8_t length, bool load) { @@ -157,38 +137,38 @@ namespace motor { return states[motor].is_enabled(); } - float MotorCluster::pulse(uint8_t motor) const { + float MotorCluster::duty(uint8_t motor) const { assert(motor < pwms.get_chan_count()); - return states[motor].get_pulse(); + return states[motor].get_duty(); } - void MotorCluster::pulse(uint8_t motor, float pulse, bool load) { + void MotorCluster::duty(uint8_t motor, float duty, bool load) { assert(motor < pwms.get_chan_count()); - float new_pulse = states[motor].set_pulse_with_return(pulse); - apply_pulse(motor, new_pulse, load); + float new_duty = states[motor].set_duty_with_return(duty); + apply_duty(motor, new_duty, load); } - void MotorCluster::pulse(const uint8_t *motors, uint8_t length, float pulse, bool 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->pulse(motors[i], pulse, false); + this->duty(motors[i], duty, false); } if(load) pwms.load_pwm(); } - void MotorCluster::pulse(std::initializer_list motors, float pulse, bool load) { + void MotorCluster::duty(std::initializer_list motors, float duty, bool load) { for(auto motor : motors) { - this->pulse(motor, pulse, false); + this->duty(motor, duty, false); } if(load) pwms.load_pwm(); } - void MotorCluster::all_to_pulse(float pulse, bool load) { + void MotorCluster::all_to_duty(float duty, bool load) { uint8_t motor_count = pwms.get_chan_count(); for(uint8_t motor = 0; motor < motor_count; motor++) { - this->pulse(motor, pulse, false); + this->duty(motor, duty, false); } if(load) pwms.load_pwm(); @@ -201,8 +181,8 @@ namespace motor { void MotorCluster::speed(uint8_t motor, float speed, bool load) { assert(motor < pwms.get_chan_count()); - float new_pulse = states[motor].set_speed_with_return(speed); - apply_pulse(motor, new_pulse, load); + float new_duty = states[motor].set_speed_with_return(speed); + apply_duty(motor, new_duty, load); } void MotorCluster::speed(const uint8_t *motors, uint8_t length, float speed, bool load) { @@ -287,7 +267,7 @@ namespace motor { uint8_t motor_count = pwms.get_chan_count(); for(uint motor = 0; motor < motor_count; motor++) { if(states[motor].is_enabled()) { - apply_pulse(motor, states[motor].get_pulse(), false); + apply_duty(motor, states[motor].get_duty(), false); } pwms.set_chan_offset(motor, (uint32_t)(motor_phases[motor] * (float)pwm_period), false); } @@ -306,112 +286,70 @@ namespace motor { return success; } - float MotorCluster::min_speed(uint8_t motor) const { + float MotorCluster::speed_scale(uint8_t motor) const { assert(is_assigned(motor)); - return states[motor].get_min_speed(); + return states[motor].get_speed_scale(); } - float MotorCluster::mid_speed(uint8_t motor) const { + void MotorCluster::to_full_negative(uint8_t motor, bool load) { assert(is_assigned(motor)); - return states[motor].get_mid_speed(); + float new_duty = states[motor].to_full_negative_with_return(); + apply_duty(motor, new_duty, load); } - float MotorCluster::max_speed(uint8_t motor) const { - assert(is_assigned(motor)); - return states[motor].get_max_speed(); - } - - void MotorCluster::to_min(uint8_t motor, bool load) { - assert(is_assigned(motor)); - float new_pulse = states[motor].to_min_with_return(); - apply_pulse(motor, new_pulse, load); - } - - void MotorCluster::to_min(const uint8_t *motors, uint8_t length, bool load) { + void MotorCluster::to_full_negative(const uint8_t *motors, uint8_t length, bool load) { assert(motors != nullptr); for(uint8_t i = 0; i < length; i++) { - to_min(motors[i], false); + to_full_negative(motors[i], false); } if(load) pwms.load_pwm(); } - void MotorCluster::to_min(std::initializer_list motors, bool load) { + void MotorCluster::to_full_negative(std::initializer_list motors, bool load) { for(auto motor : motors) { - to_min(motor, false); + to_full_negative(motor, false); } if(load) pwms.load_pwm(); } - void MotorCluster::all_to_min(bool load) { + void MotorCluster::all_to_full_negative(bool load) { uint8_t motor_count = pwms.get_chan_count(); for(uint8_t motor = 0; motor < motor_count; motor++) { - to_min(motor, false); + to_full_negative(motor, false); } if(load) pwms.load_pwm(); } - void MotorCluster::to_mid(uint8_t motor, bool load) { + void MotorCluster::to_full_positive(uint8_t motor, bool load) { assert(is_assigned(motor)); - float new_pulse = states[motor].to_mid_with_return(); - apply_pulse(motor, new_pulse, load); + float new_duty = states[motor].to_full_positive_with_return(); + apply_duty(motor, new_duty, load); } - void MotorCluster::to_mid(const uint8_t *motors, uint8_t length, bool load) { + void MotorCluster::to_full_positive(const uint8_t *motors, uint8_t length, bool load) { assert(motors != nullptr); for(uint8_t i = 0; i < length; i++) { - to_mid(motors[i], false); + to_full_positive(motors[i], false); } if(load) pwms.load_pwm(); } - void MotorCluster::to_mid(std::initializer_list motors, bool load) { + void MotorCluster::to_full_positive(std::initializer_list motors, bool load) { for(auto motor : motors) { - to_mid(motor, false); + to_full_positive(motor, false); } if(load) pwms.load_pwm(); } - void MotorCluster::all_to_mid(bool load) { + void MotorCluster::all_to_full_positive(bool load) { uint8_t motor_count = pwms.get_chan_count(); for(uint8_t motor = 0; motor < motor_count; motor++) { - to_mid(motor, false); - } - if(load) - pwms.load_pwm(); - } - - void MotorCluster::to_max(uint8_t motor, bool load) { - assert(is_assigned(motor)); - float new_pulse = states[motor].to_max_with_return(); - apply_pulse(motor, new_pulse, load); - } - - void MotorCluster::to_max(const uint8_t *motors, uint8_t length, bool load) { - assert(motors != nullptr); - for(uint8_t i = 0; i < length; i++) { - to_max(motors[i], false); - } - if(load) - pwms.load_pwm(); - } - - void MotorCluster::to_max(std::initializer_list motors, bool load) { - for(auto motor : motors) { - to_max(motor, false); - } - if(load) - pwms.load_pwm(); - } - - void MotorCluster::all_to_max(bool load) { - uint8_t motor_count = pwms.get_chan_count(); - for(uint8_t motor = 0; motor < motor_count; motor++) { - to_max(motor, false); + to_full_positive(motor, false); } if(load) pwms.load_pwm(); @@ -419,8 +357,8 @@ namespace motor { void MotorCluster::to_percent(uint8_t motor, float in, float in_min, float in_max, bool load) { assert(is_assigned(motor)); - float new_pulse = states[motor].to_percent_with_return(in, in_min, in_max); - apply_pulse(motor, new_pulse, load); + float new_duty = states[motor].to_percent_with_return(in, in_min, in_max); + apply_duty(motor, new_duty, load); } void MotorCluster::to_percent(const uint8_t *motors, uint8_t length, float in, float in_min, float in_max, bool load) { @@ -451,8 +389,8 @@ namespace motor { void MotorCluster::to_percent(uint8_t motor, float in, float in_min, float in_max, float speed_min, float speed_max, bool load) { assert(is_assigned(motor)); - float new_pulse = states[motor].to_percent_with_return(in, in_min, in_max, speed_min, speed_max); - apply_pulse(motor, new_pulse, load); + float new_duty = states[motor].to_percent_with_return(in, in_min, in_max, speed_min, speed_max); + apply_duty(motor, new_duty, 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) { @@ -481,45 +419,22 @@ namespace motor { pwms.load_pwm(); } - Calibration& MotorCluster::calibration(uint8_t motor) { - assert(is_assigned(motor)); - return states[motor].calibration(); - } - - const Calibration& MotorCluster::calibration(uint8_t motor) const { - assert(is_assigned(motor)); - return states[motor].calibration(); - } - void MotorCluster::load() { pwms.load_pwm(); } - void MotorCluster::apply_pulse(uint8_t motor, float pulse, bool load) { - pwms.set_chan_level(motor, MotorState::pulse_to_level(pulse, pwm_period, pwm_frequency), load); + void MotorCluster::apply_duty(uint8_t motor, float duty, bool load) { + pwms.set_chan_level(motor, MotorState::duty_to_level(duty, pwm_period), load); } - void MotorCluster::create_motor_states(CalibrationType default_type, bool auto_phase) { + void MotorCluster::create_motor_states(float speed_scale, bool inverted, bool auto_phase) { uint8_t motor_count = pwms.get_chan_count(); if(motor_count > 0) { states = new MotorState[motor_count]; motor_phases = new float[motor_count]; for(uint motor = 0; motor < motor_count; motor++) { - states[motor] = MotorState(default_type); - motor_phases[motor] = (auto_phase) ? (float)motor / (float)motor_count : 0.0f; - } - } - } - - void MotorCluster::create_motor_states(const Calibration& calibration, bool auto_phase) { - uint8_t motor_count = pwms.get_chan_count(); - if(motor_count > 0) { - states = new MotorState[motor_count]; - motor_phases = new float[motor_count]; - - for(uint motor = 0; motor < motor_count; motor++) { - states[motor] = MotorState(calibration); + states[motor] = MotorState(speed_scale, inverted); motor_phases[motor] = (auto_phase) ? (float)motor / (float)motor_count : 0.0f; } } diff --git a/drivers/motor/motor_cluster.hpp b/drivers/motor/motor_cluster.hpp index 29c275b3..b4d66b30 100644 --- a/drivers/motor/motor_cluster.hpp +++ b/drivers/motor/motor_cluster.hpp @@ -24,15 +24,10 @@ namespace motor { // Constructors/Destructor //-------------------------------------------------- public: - MotorCluster(PIO pio, uint sm, uint pin_mask, CalibrationType default_type = ANGULAR, float freq = MotorState::DEFAULT_FREQUENCY, bool auto_phase = true, PWMCluster::Sequence *seq_buffer = nullptr, PWMCluster::TransitionData *dat_buffer = nullptr); - MotorCluster(PIO pio, uint sm, uint pin_base, uint pin_count, CalibrationType default_type = ANGULAR, float freq = MotorState::DEFAULT_FREQUENCY, bool auto_phase = true, PWMCluster::Sequence *seq_buffer = nullptr, PWMCluster::TransitionData *dat_buffer = nullptr); - MotorCluster(PIO pio, uint sm, const uint8_t *pins, uint32_t length, CalibrationType default_type = ANGULAR, float freq = MotorState::DEFAULT_FREQUENCY, bool auto_phase = true, PWMCluster::Sequence *seq_buffer = nullptr, PWMCluster::TransitionData *dat_buffer = nullptr); - MotorCluster(PIO pio, uint sm, std::initializer_list pins, CalibrationType default_type = ANGULAR, float freq = MotorState::DEFAULT_FREQUENCY, bool auto_phase = true, PWMCluster::Sequence *seq_buffer = nullptr, PWMCluster::TransitionData *dat_buffer = nullptr); - - MotorCluster(PIO pio, uint sm, uint pin_mask, const Calibration& calibration, float freq = MotorState::DEFAULT_FREQUENCY, bool auto_phase = true, PWMCluster::Sequence *seq_buffer = nullptr, PWMCluster::TransitionData *dat_buffer = nullptr); - MotorCluster(PIO pio, uint sm, uint pin_base, uint pin_count, const Calibration& calibration, float freq = MotorState::DEFAULT_FREQUENCY, bool auto_phase = true, PWMCluster::Sequence *seq_buffer = nullptr, PWMCluster::TransitionData *dat_buffer = nullptr); - MotorCluster(PIO pio, uint sm, const uint8_t *pins, uint32_t length, const Calibration& calibration, float freq = MotorState::DEFAULT_FREQUENCY, bool auto_phase = true, PWMCluster::Sequence *seq_buffer = nullptr, PWMCluster::TransitionData *dat_buffer = nullptr); - MotorCluster(PIO pio, uint sm, std::initializer_list pins, const Calibration& calibration, float freq = MotorState::DEFAULT_FREQUENCY, bool auto_phase = true, PWMCluster::Sequence *seq_buffer = nullptr, PWMCluster::TransitionData *dat_buffer = nullptr); + MotorCluster(PIO pio, uint sm, uint pin_mask, float speed_scale = MotorState::DEFAULT_SPEED_SCALE, bool inverted = false, float freq = MotorState::DEFAULT_FREQUENCY, bool auto_phase = true, PWMCluster::Sequence *seq_buffer = nullptr, PWMCluster::TransitionData *dat_buffer = nullptr); + MotorCluster(PIO pio, uint sm, uint pin_base, uint pin_count, float speed_scale = MotorState::DEFAULT_SPEED_SCALE, bool inverted = false, float freq = MotorState::DEFAULT_FREQUENCY, bool auto_phase = true, PWMCluster::Sequence *seq_buffer = nullptr, PWMCluster::TransitionData *dat_buffer = nullptr); + MotorCluster(PIO pio, uint sm, const uint8_t *pins, uint32_t length, float speed_scale = MotorState::DEFAULT_SPEED_SCALE, bool inverted = false, float freq = MotorState::DEFAULT_FREQUENCY, bool auto_phase = true, PWMCluster::Sequence *seq_buffer = nullptr, PWMCluster::TransitionData *dat_buffer = nullptr); + MotorCluster(PIO pio, uint sm, std::initializer_list pins, float speed_scale = MotorState::DEFAULT_SPEED_SCALE, bool inverted = false, float freq = MotorState::DEFAULT_FREQUENCY, bool auto_phase = true, PWMCluster::Sequence *seq_buffer = nullptr, PWMCluster::TransitionData *dat_buffer = nullptr); ~MotorCluster(); @@ -79,24 +74,17 @@ namespace motor { bool frequency(float freq); //-------------------------------------------------- - float min_speed(uint8_t motor) const; - float mid_speed(uint8_t motor) const; - float max_speed(uint8_t motor) const; + float speed_scale(uint8_t motor) const; - void to_min(uint8_t motor, bool load = true); - void to_min(const uint8_t *motors, uint8_t length, bool load = true); - void to_min(std::initializer_list motors, bool load = true); - void all_to_min(bool load = true); + void to_full_negative(uint8_t motor, bool load = true); + void to_full_negative(const uint8_t *motors, uint8_t length, bool load = true); + void to_full_negative(std::initializer_list motors, bool load = true); + void all_to_full_negative(bool load = true); - void to_mid(uint8_t motor, bool load = true); - void to_mid(const uint8_t *motors, uint8_t length, bool load = true); - void to_mid(std::initializer_list motors, bool load = true); - void all_to_mid(bool load = true); - - void to_max(uint8_t motor, bool load = true); - void to_max(const uint8_t *motors, uint8_t length, bool load = true); - void to_max(std::initializer_list motors, bool load = true); - void all_to_max(bool load = true); + void to_full_positive(uint8_t motor, bool load = true); + void to_full_positive(const uint8_t *motors, uint8_t length, bool load = true); + void to_full_positive(std::initializer_list motors, bool load = true); + void all_to_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); @@ -108,16 +96,12 @@ namespace motor { void to_percent(std::initializer_list 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); - Calibration& calibration(uint8_t motor); - const Calibration& calibration(uint8_t motor) const; - void load(); //-------------------------------------------------- private: void apply_duty(uint8_t motor, float duty, bool load); - void create_motor_states(CalibrationType default_type, bool auto_phase); - void create_motor_states(const Calibration& calibration, bool auto_phase); + void create_motor_states(float speed_scale, bool inverted, bool auto_phase); }; } \ No newline at end of file diff --git a/drivers/motor/motor_state.cpp b/drivers/motor/motor_state.cpp index 1a9f7d7e..079038e1 100644 --- a/drivers/motor/motor_state.cpp +++ b/drivers/motor/motor_state.cpp @@ -1,31 +1,33 @@ #include "motor_state.hpp" +#include "float.h" namespace motor { - MotorState::MotorState() { + MotorState::MotorState() + : motor_speed(0.0f), motor_scale(DEFAULT_SPEED_SCALE), inverted(false), last_enabled_duty(0.0f), enabled(false) { } - //MotorState::MotorState(CalibrationType default_type) - // : calib(default_type) { - //} - - //MotorState::MotorState(const Calibration& calibration) - // : calib(calibration) { - //} - - float MotorState::enable() { - return _enable(); + MotorState::MotorState(float speed_scale, bool inverted) + : motor_speed(0.0f), motor_scale(MAX(speed_scale, FLT_EPSILON)), inverted(inverted), last_enabled_duty(0.0f), enabled(false) { } - float MotorState::disable() { + float MotorState::enable_with_return() { + enabled = true; + + float duty = 0.0f; + if((duty <= 0.0f - deadzone_percent) || (duty >= deadzone_percent)) { + if(inverted) + duty = 0.0f - last_enabled_duty; + else + duty = last_enabled_duty; + } + return duty; + } + + float MotorState::disable_with_return() { enabled = false; return 0.0f; // A zero duty } - float MotorState::_enable() { - enabled = true; - return last_enabled_duty; - } - bool MotorState::is_enabled() const { return enabled; } @@ -35,14 +37,11 @@ namespace motor { } float MotorState::set_duty_with_return(float duty) { - //TODO - // float speed_out, duty_out; - // if(calib.duty_to_speed(duty, speed_out, duty_out)) { - // motor_speed = speed_out; - // last_enabled_duty = duty_out; - // return _enable(); - // } - return disable(); + // Clamp the duty between the hard limits + last_enabled_duty = MIN(MAX(duty, -1.0f), 1.0f); + motor_speed = last_enabled_duty * motor_scale; + + return enable_with_return(); } float MotorState::get_speed() const { @@ -50,58 +49,48 @@ namespace motor { } float MotorState::set_speed_with_return(float speed) { - //TODO - // float duty_out, speed_out; - // if(calib.speed_to_duty(speed, duty_out, speed_out)) { - // last_enabled_duty = duty_out; - // motor_speed = speed_out; - // return _enable(); - // } - return disable(); + // Clamp the speed between the hard limits + motor_speed = MIN(MAX(speed, -motor_scale), motor_scale); + last_enabled_duty = motor_speed / motor_scale; + + return enable_with_return(); } - float MotorState::get_min_speed() const { - float speed = 0.0f; - //TODO - //if(calib.size() > 0) { - // speed = calib.first().speed; - //} - return speed; + float MotorState::get_speed_scale() const { + return motor_scale; } - // float MotorState::get_mid_speed() const { - // float speed = 0.0f; - // if(calib.size() > 0) { - // const Calibration::Pair &first = calib.first(); - // const Calibration::Pair &last = calib.last(); - // speed = (first.speed + last.speed) / 2.0f; - // } - // return speed; - // } - - float MotorState::get_max_speed() const { - float speed = 0.0f; - //TODO - //if(calib.size() > 0) { - // speed = calib.last().speed; - //} - return speed; + void MotorState::set_speed_scale(float speed_scale) { + motor_scale = MAX(speed_scale, FLT_EPSILON); + motor_speed = last_enabled_duty * motor_scale; } - float MotorState::to_min_with_return() { - return set_speed_with_return(get_min_speed()); + void MotorState::invert_direction(bool invert) { + if(invert != inverted) { + inverted = invert; + motor_speed = 0.0f - motor_speed; + last_enabled_duty = 0.0f - last_enabled_duty; + } } - // float MotorState::to_mid_with_return() { - // return set_speed_with_return(get_mid_speed()); - // } + bool MotorState::is_inverted() const { + return inverted; + } - float MotorState::to_max_with_return() { - return set_speed_with_return(get_max_speed()); + float MotorState::stop_with_return() { + return set_duty_with_return(0.0f); + } + + float MotorState::to_full_negative_with_return() { + return set_duty_with_return(-1.0f); + } + + float MotorState::to_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, get_min_speed(), get_max_speed()); + float speed = MotorState::map_float(in, in_min, in_max, 0.0f - get_speed_scale(), get_speed_scale()); return set_speed_with_return(speed); } @@ -110,14 +99,6 @@ namespace motor { return set_speed_with_return(speed); } - // Calibration& MotorState::calibration() { - // return calib; - // } - - // const Calibration& MotorState::calibration() const { - // return calib; - // } - int32_t MotorState::duty_to_level(float duty, uint32_t resolution) { return (int32_t)(duty * (float)resolution); } diff --git a/drivers/motor/motor_state.hpp b/drivers/motor/motor_state.hpp index 31107fc8..d4b17f3b 100644 --- a/drivers/motor/motor_state.hpp +++ b/drivers/motor/motor_state.hpp @@ -1,10 +1,25 @@ #pragma once #include "pico/stdlib.h" -#include "calibration.hpp" namespace motor { + struct MotorPins { + uint positive; + uint negative; + + MotorPins() : positive(0), negative(0) {} + MotorPins(uint pos_pin, uint neg_pin) : positive(pos_pin), negative(neg_pin) {} + }; + + struct EncoderPins { + uint a; + uint b; + + EncoderPins() : a(0), b(0) {} + EncoderPins(uint a_pin, uint b_pin) : a(a_pin), b(b_pin) {} + }; + class MotorState { //-------------------------------------------------- // Enums @@ -22,23 +37,24 @@ namespace motor { public: static constexpr float DEFAULT_FREQUENCY = 25000.0f; // The standard motor update rate static const DecayMode DEFAULT_DECAY_MODE = SLOW_DECAY; + static constexpr float DEFAULT_SPEED_SCALE = 1.0f; // The standard motor update rate + static constexpr float MIN_FREQUENCY = 10.0f; // Lowest achievable with hardware PWM with good resolution static constexpr float MAX_FREQUENCY = 50000.0f; // Highest nice speed static constexpr float ZERO_PERCENT = 0.0f; static constexpr float ONEHUNDRED_PERCENT = 1.0f; - private: - static constexpr float MIN_VALID_DUTY = 1.0f; - //-------------------------------------------------- // Variables //-------------------------------------------------- private: - float motor_speed = 0.0f; - float last_enabled_duty = 0.0f; - bool enabled = false; - //Calibration calib; + float motor_speed; + float motor_scale; + bool inverted; + float last_enabled_duty; + bool enabled; + float deadzone_percent = 0.0f; //-------------------------------------------------- @@ -46,19 +62,16 @@ namespace motor { //-------------------------------------------------- public: MotorState(); - //MotorState(CalibrationType default_type); - //MotorState(const Calibration& calibration); + MotorState(float speed_scale, bool inverted); //-------------------------------------------------- // Methods //-------------------------------------------------- public: - float enable(); - float disable(); + float enable_with_return(); + float disable_with_return(); bool is_enabled() const; - private: - float _enable(); // Internal version of enable without convenient initialisation to the middle public: float get_duty() const; float set_duty_with_return(float duty); @@ -66,24 +79,23 @@ namespace motor { float get_speed() const; float set_speed_with_return(float speed); - public: - float get_min_speed() const; - //float get_mid_speed() const; - float get_max_speed() const; + float get_speed_scale() const; + void set_speed_scale(float speed_scale); - float to_min_with_return(); - //float to_mid_with_return(); - float to_max_with_return(); + void invert_direction(bool invert); + bool is_inverted() const; + + //-------------------------------------------------- + + float stop_with_return(); + float to_full_negative_with_return(); + float to_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); - //Calibration& calibration(); - //const Calibration& calibration() const; - //-------------------------------------------------- static int32_t duty_to_level(float duty, uint32_t resolution); - private: static float map_float(float in, float in_min, float in_max, float out_min, float out_max); }; diff --git a/examples/motor2040/motor2040_single_motor.cpp b/examples/motor2040/motor2040_single_motor.cpp index be32938e..e217c8fc 100644 --- a/examples/motor2040/motor2040_single_motor.cpp +++ b/examples/motor2040/motor2040_single_motor.cpp @@ -22,7 +22,7 @@ constexpr float SWEEP_EXTENT = 90.0f; // Create a motor on pin 0 and 1 -Motor2 m = Motor2(motor2040::SERVO_1, motor2040::SERVO_2); +Motor2 m = Motor2(motor2040::MOTOR_1); int main() { @@ -31,21 +31,21 @@ int main() { // Initialise the motor m.init(); - // Enable the motor (this puts it at the middle) + // Enable the motor m.enable(); sleep_ms(2000); - // Go to min - m.to_min(); + // Go at full neative + m.to_full_negative(); sleep_ms(2000); - // Go to max - m.to_max(); + // Go at full positive + m.to_full_positive(); sleep_ms(2000); - // Go back to mid - //m.to_mid(); - //sleep_ms(2000); + // Stop the motor + m.stop(); + sleep_ms(2000); // Do a sine sweep for(auto j = 0u; j < SWEEPS; j++) { diff --git a/examples/pico_motor_shim/balance/demo.cpp b/examples/pico_motor_shim/balance/demo.cpp index 8c964d72..95e98aa7 100644 --- a/examples/pico_motor_shim/balance/demo.cpp +++ b/examples/pico_motor_shim/balance/demo.cpp @@ -13,6 +13,7 @@ Press "A" to start and stop the balancer */ using namespace pimoroni; +using namespace motor; static constexpr float TOP_SPEED = 1.0f; //A value between 0 and 1 static constexpr float Z_BIAS_CORRECTION = 0.5f; //A magic number that seems to correct the MSA301's Z bias @@ -22,8 +23,8 @@ static constexpr float PROPORTIONAL = 0.03f; Button button_a(pico_motor_shim::BUTTON_A, Polarity::ACTIVE_LOW, 0); -Motor motor_1(pico_motor_shim::MOTOR_1_POS, pico_motor_shim::MOTOR_1_NEG);//, Motor::DEFAULT_PWM_FREQUENCY, Motor::DEFAULT_DECAY_MODE); -Motor motor_2(pico_motor_shim::MOTOR_2_POS, pico_motor_shim::MOTOR_2_NEG);//, Motor::DEFAULT_PWM_FREQUENCY, Motor::DEFAULT_DECAY_MODE); +Motor motor_1(pico_motor_shim::MOTOR_1);//, Motor::DEFAULT_PWM_FREQUENCY, Motor::DEFAULT_DECAY_MODE); +Motor motor_2(pico_motor_shim::MOTOR_2);//, Motor::DEFAULT_PWM_FREQUENCY, Motor::DEFAULT_DECAY_MODE); I2C i2c(BOARD::BREAKOUT_GARDEN); BreakoutMSA301 msa301(&i2c); diff --git a/examples/pico_motor_shim/sequence/demo.cpp b/examples/pico_motor_shim/sequence/demo.cpp index 0b9767a7..88a4a830 100644 --- a/examples/pico_motor_shim/sequence/demo.cpp +++ b/examples/pico_motor_shim/sequence/demo.cpp @@ -11,6 +11,7 @@ Press "A" to start and stop the movement sequence */ using namespace pimoroni; +using namespace motor; static constexpr float TOP_SPEED = 1.0f; //A value between 0 and 1 static const uint32_t ACCELERATE_TIME_MS = 2000; @@ -20,8 +21,8 @@ static const uint32_t STOP_TIME_MS = 1000; Button button_a(pico_motor_shim::BUTTON_A, Polarity::ACTIVE_LOW, 0); -Motor motor_1(pico_motor_shim::MOTOR_1_POS, pico_motor_shim::MOTOR_1_NEG);//, Motor::DEFAULT_PWM_FREQUENCY, Motor::DEFAULT_DECAY_MODE); -Motor motor_2(pico_motor_shim::MOTOR_2_POS, pico_motor_shim::MOTOR_2_NEG);//, Motor::DEFAULT_PWM_FREQUENCY, Motor::DEFAULT_DECAY_MODE); +Motor motor_1(pico_motor_shim::MOTOR_1);//, Motor::DEFAULT_PWM_FREQUENCY, Motor::DEFAULT_DECAY_MODE); +Motor motor_2(pico_motor_shim::MOTOR_2);//, Motor::DEFAULT_PWM_FREQUENCY, Motor::DEFAULT_DECAY_MODE); static bool button_toggle = false; diff --git a/examples/pico_motor_shim/song/demo.cpp b/examples/pico_motor_shim/song/demo.cpp index 75d920db..b6404863 100644 --- a/examples/pico_motor_shim/song/demo.cpp +++ b/examples/pico_motor_shim/song/demo.cpp @@ -11,6 +11,7 @@ Plug a motor into connector 1, and press "A" to start the song playing (does not */ 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 @@ -39,11 +40,11 @@ static const uint STATIONARY_TOGGLE_US = 2000; Button button_a(pico_motor_shim::BUTTON_A, Polarity::ACTIVE_LOW, 0); #ifdef USE_FAST_DECAY - Motor motor_1(pico_motor_shim::MOTOR_1_POS, pico_motor_shim::MOTOR_1_NEG, Motor::DEFAULT_PWM_FREQUENCY, Motor::FAST_DECAY); - Motor motor_2(pico_motor_shim::MOTOR_2_POS, pico_motor_shim::MOTOR_2_NEG, Motor::DEFAULT_PWM_FREQUENCY, Motor::FAST_DECAY); + Motor motor_1(pico_motor_shim::MOTOR_1, Motor::DEFAULT_PWM_FREQUENCY, Motor::FAST_DECAY); + Motor motor_2(pico_motor_shim::MOTOR_2, Motor::DEFAULT_PWM_FREQUENCY, Motor::FAST_DECAY); #else - Motor motor_1(pico_motor_shim::MOTOR_1_POS, pico_motor_shim::MOTOR_1_NEG, Motor::DEFAULT_PWM_FREQUENCY, Motor::SLOW_DECAY); - Motor motor_2(pico_motor_shim::MOTOR_2_POS, pico_motor_shim::MOTOR_2_NEG, Motor::DEFAULT_PWM_FREQUENCY, Motor::SLOW_DECAY); + Motor motor_1(pico_motor_shim::MOTOR_1, Motor::DEFAULT_PWM_FREQUENCY, Motor::SLOW_DECAY); + Motor motor_2(pico_motor_shim::MOTOR_2, Motor::DEFAULT_PWM_FREQUENCY, Motor::SLOW_DECAY); #endif static bool button_toggle = false; diff --git a/libraries/motor2040/motor2040.cmake b/libraries/motor2040/motor2040.cmake index 96aa50fa..60f67543 100644 --- a/libraries/motor2040/motor2040.cmake +++ b/libraries/motor2040/motor2040.cmake @@ -3,4 +3,4 @@ add_library(motor2040 INTERFACE) target_include_directories(motor2040 INTERFACE ${CMAKE_CURRENT_LIST_DIR}) # Pull in pico libraries that we need -target_link_libraries(motor2040 INTERFACE pico_stdlib plasma motor2) # motor_cluster) \ No newline at end of file +target_link_libraries(motor2040 INTERFACE pico_stdlib plasma motor2 motor_cluster) \ No newline at end of file diff --git a/libraries/motor2040/motor2040.hpp b/libraries/motor2040/motor2040.hpp index ee93be86..b362ec99 100644 --- a/libraries/motor2040/motor2040.hpp +++ b/libraries/motor2040/motor2040.hpp @@ -7,25 +7,38 @@ namespace motor { namespace motor2040 { - const uint SERVO_1 = 0; - const uint SERVO_2 = 1; - const uint SERVO_3 = 2; - const uint SERVO_4 = 3; - const uint SERVO_5 = 4; - const uint SERVO_6 = 5; - const uint SERVO_7 = 6; - const uint SERVO_8 = 7; - const uint SERVO_9 = 8; - const uint SERVO_10 = 9; - const uint SERVO_11 = 10; - const uint SERVO_12 = 11; - const uint SERVO_13 = 12; - const uint SERVO_14 = 13; - const uint SERVO_15 = 14; - const uint SERVO_16 = 15; - const uint SERVO_17 = 16; - const uint SERVO_18 = 17; - const uint NUM_SERVOS = 18; + const uint MOTOR_1P = 4; + const uint MOTOR_1N = 5; + const uint MOTOR_2P = 6; + const uint MOTOR_2N = 7; + const uint MOTOR_3P = 8; + const uint MOTOR_3N = 9; + const uint MOTOR_4P = 10; + const uint MOTOR_4N = 11; + + const MotorPins MOTOR_1(MOTOR_1P, MOTOR_1N); + const MotorPins MOTOR_2(MOTOR_2P, MOTOR_2N); + const MotorPins MOTOR_3(MOTOR_3P, MOTOR_3N); + const MotorPins MOTOR_4(MOTOR_4P, MOTOR_4N); + const uint NUM_MOTORS = 4; + + const uint ENCODER_1A = 0; + const uint ENCODER_1B = 1; + const uint ENCODER_2A = 2; + const uint ENCODER_2B = 3; + const uint ENCODER_3A = 12; + const uint ENCODER_3B = 13; + const uint ENCODER_4A = 14; + const uint ENCODER_4B = 15; + + const EncoderPins ENCODER_1(ENCODER_1A, ENCODER_1B); + const EncoderPins ENCODER_2(ENCODER_2A, ENCODER_2B); + const EncoderPins ENCODER_3(ENCODER_3A, ENCODER_3B); + const EncoderPins ENCODER_4(ENCODER_4A, ENCODER_4B); + 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; @@ -41,19 +54,19 @@ namespace motor { const uint ADC2 = 28; const uint SHARED_ADC = 29; // The pin used for the board's sensing features - const uint SENSOR_1_ADDR = 0b000; - const uint SENSOR_2_ADDR = 0b001; - const uint SENSOR_3_ADDR = 0b010; - const uint SENSOR_4_ADDR = 0b011; - const uint SENSOR_5_ADDR = 0b100; - const uint SENSOR_6_ADDR = 0b101; - const uint NUM_SENSORS = 6; + const uint CURRENT_SENSE_1_ADDR = 0b000; + const uint CURRENT_SENSE_2_ADDR = 0b001; + const uint CURRENT_SENSE_3_ADDR = 0b010; + const uint CURRENT_SENSE_4_ADDR = 0b011; + const uint VOLTAGE_SENSE_ADDR = 0b100; + const uint FAULT_SENSE_ADDR = 0b101; - const uint VOLTAGE_SENSE_ADDR = 0b110; - const uint CURRENT_SENSE_ADDR = 0b111; + const uint SENSOR_1_ADDR = 0b110; + const uint SENSOR_2_ADDR = 0b111; + const uint NUM_SENSORS = 2; - constexpr float SHUNT_RESISTOR = 0.003f; - constexpr float CURRENT_GAIN = 69; + constexpr float SHUNT_RESISTOR = 0.47f; + constexpr float CURRENT_GAIN = 5; constexpr float VOLTAGE_GAIN = 3.9f / 13.9f; constexpr float CURRENT_OFFSET = -0.02f; } diff --git a/libraries/pico_motor_shim/pico_motor_shim.cmake b/libraries/pico_motor_shim/pico_motor_shim.cmake index d28b6c2f..19b08935 100644 --- a/libraries/pico_motor_shim/pico_motor_shim.cmake +++ b/libraries/pico_motor_shim/pico_motor_shim.cmake @@ -3,4 +3,4 @@ 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) \ No newline at end of file +target_link_libraries(pico_motor_shim INTERFACE pico_stdlib motor) \ No newline at end of file diff --git a/libraries/pico_motor_shim/pico_motor_shim.hpp b/libraries/pico_motor_shim/pico_motor_shim.hpp index e93d6bf5..c0681138 100644 --- a/libraries/pico_motor_shim/pico_motor_shim.hpp +++ b/libraries/pico_motor_shim/pico_motor_shim.hpp @@ -1,12 +1,19 @@ #pragma once #include "pico/stdlib.h" -namespace pico_motor_shim { - const uint8_t BUTTON_A = 2; +#include "motor2.hpp" - const uint MOTOR_1_POS = 6; - const uint MOTOR_1_NEG = 7; +namespace motor { + namespace pico_motor_shim { + const uint BUTTON_A = 2; - const uint MOTOR_2_POS = 27; - const uint MOTOR_2_NEG = 26; + const uint MOTOR_1P = 6; + const uint MOTOR_1N = 7; + const uint MOTOR_2P = 27; + const uint MOTOR_2N = 26; + + const MotorPins MOTOR_1(MOTOR_1P, MOTOR_1N); + const MotorPins MOTOR_2(MOTOR_2P, MOTOR_2N); + const uint NUM_MOTORS = 2; + } } \ No newline at end of file From ac3edafbf4cd8e5b8fb58e53b87a7d6a0514d543 Mon Sep 17 00:00:00 2001 From: ZodiusInfuser Date: Tue, 5 Apr 2022 16:53:36 +0100 Subject: [PATCH 04/60] Contined motor dev --- common/pimoroni_common.hpp | 22 ++ drivers/motor/motor.cpp | 2 +- drivers/motor/motor.hpp | 7 +- drivers/motor/motor2.cpp | 76 ++-- drivers/motor/motor2.hpp | 41 +- drivers/motor/motor_cluster.cpp | 349 ++++++++++++++---- drivers/motor/motor_cluster.hpp | 73 +++- drivers/motor/motor_state.cpp | 82 ++-- drivers/motor/motor_state.hpp | 65 ++-- drivers/pwm/pwm_cluster.cpp | 63 ++++ drivers/pwm/pwm_cluster.hpp | 7 + examples/motor2040/motor2040_single_motor.cpp | 4 +- libraries/motor2040/motor2040.hpp | 16 +- libraries/pico_motor_shim/pico_motor_shim.hpp | 4 +- 14 files changed, 600 insertions(+), 211 deletions(-) diff --git a/common/pimoroni_common.hpp b/common/pimoroni_common.hpp index 6fc0a903..3a788732 100644 --- a/common/pimoroni_common.hpp +++ b/common/pimoroni_common.hpp @@ -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 @@ -74,4 +80,20 @@ 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; + }; + union { + uint8_t second; + uint8_t b; + uint8_t negative; + }; + + pin_pair() : first(0), second(0) {} + pin_pair(uint8_t first, uint8_t second) : first(first), second(second) {} + }; } \ No newline at end of file diff --git a/drivers/motor/motor.cpp b/drivers/motor/motor.cpp index d1db206e..73c812c4 100644 --- a/drivers/motor/motor.cpp +++ b/drivers/motor/motor.cpp @@ -2,7 +2,7 @@ #include "pwm.hpp" namespace motor { - Motor::Motor(const MotorPins &pins, float freq, DecayMode mode) + Motor::Motor(const pin_pair &pins, float freq, DecayMode mode) : pins(pins), pwm_frequency(freq), motor_decay_mode(mode) { } diff --git a/drivers/motor/motor.hpp b/drivers/motor/motor.hpp index a2e7bff3..921fbff5 100644 --- a/drivers/motor/motor.hpp +++ b/drivers/motor/motor.hpp @@ -2,8 +2,11 @@ #include "pico/stdlib.h" #include "hardware/pwm.h" +#include "common/pimoroni_common.hpp" #include "motor_state.hpp" +using namespace pimoroni; + namespace motor { class Motor { @@ -29,7 +32,7 @@ namespace motor { // Variables //-------------------------------------------------- private: - MotorPins pins; + pin_pair pins; pwm_config pwm_cfg; uint16_t pwm_period; float pwm_frequency = DEFAULT_PWM_FREQUENCY; @@ -42,7 +45,7 @@ namespace motor { // Constructors/Destructor //-------------------------------------------------- public: - Motor(const MotorPins &pins, float freq = DEFAULT_PWM_FREQUENCY, DecayMode mode = DEFAULT_DECAY_MODE); + Motor(const pin_pair &pins, float freq = DEFAULT_PWM_FREQUENCY, DecayMode mode = DEFAULT_DECAY_MODE); ~Motor(); diff --git a/drivers/motor/motor2.cpp b/drivers/motor/motor2.cpp index 2f584796..35ad7f32 100644 --- a/drivers/motor/motor2.cpp +++ b/drivers/motor/motor2.cpp @@ -3,8 +3,9 @@ #include "pwm.hpp" namespace motor { - Motor2::Motor2(const MotorPins &pins, float speed_scale, float freq, MotorState::DecayMode mode) - : motor_pins(pins), state(speed_scale, false), pwm_frequency(freq), motor_decay_mode(mode) { + Motor2::Motor2(const pin_pair &pins, MotorState::Direction direction, float speed_scale, + float deadzone_percent, float freq, MotorState::DecayMode mode) + : motor_pins(pins), state(direction, speed_scale, deadzone_percent), pwm_frequency(freq), motor_decay_mode(mode) { } Motor2::~Motor2() { @@ -41,7 +42,7 @@ namespace motor { return success; } - MotorPins Motor2::pins() const { + pin_pair Motor2::pins() const { return motor_pins; } @@ -73,22 +74,6 @@ namespace motor { apply_duty(state.set_speed_with_return(speed)); } - float Motor2::speed_scale() const { - return state.get_speed_scale(); - } - - void Motor2::speed_scale(float speed_scale) { - state.set_speed_scale(speed_scale); - } - - void Motor2::invert_direction(bool invert) { - state.invert_direction(invert); - } - - bool Motor2::is_inverted() const { - return state.is_inverted(); - } - float Motor2::frequency() const { return pwm_frequency; } @@ -140,29 +125,21 @@ namespace motor { return success; } - MotorState::DecayMode Motor2::decay_mode() { - return motor_decay_mode; - } - - void Motor2::decay_mode(MotorState::DecayMode mode) { - motor_decay_mode = mode; - apply_duty(state.get_duty()); - } - void Motor2::stop() { - apply_duty(state.set_speed_with_return(0.0f)); + apply_duty(state.stop_with_return()); } void Motor2::coast() { + state.set_duty_with_return(0.0f); disable(); } - void Motor2::to_full_negative() { - apply_duty(state.to_full_negative_with_return()); + void Motor2::full_negative() { + apply_duty(state.full_negative_with_return()); } - void Motor2::to_full_positive() { - apply_duty(state.to_full_positive_with_return()); + void Motor2::full_positive() { + apply_duty(state.full_positive_with_return()); } void Motor2::to_percent(float in, float in_min, float in_max) { @@ -173,6 +150,39 @@ namespace motor { apply_duty(state.to_percent_with_return(in, in_min, in_max, speed_min, speed_max)); } + MotorState::Direction Motor2::direction() const { + return state.get_direction(); + } + + void Motor2::direction(MotorState::Direction direction) { + state.set_direction(direction); + } + + float Motor2::speed_scale() const { + return state.get_speed_scale(); + } + + void Motor2::speed_scale(float speed_scale) { + state.set_speed_scale(speed_scale); + } + + float Motor2::deadzone_percent() const { + return state.get_deadzone_percent(); + } + + void Motor2::deadzone_percent(float speed_scale) { + apply_duty(state.set_deadzone_percent_with_return(speed_scale)); + } + + MotorState::DecayMode Motor2::decay_mode() { + return motor_decay_mode; + } + + void Motor2::decay_mode(MotorState::DecayMode mode) { + motor_decay_mode = mode; + apply_duty(state.get_duty()); + } + void Motor2::apply_duty(float duty) { int32_t signed_level = MotorState::duty_to_level(duty, pwm_period); diff --git a/drivers/motor/motor2.hpp b/drivers/motor/motor2.hpp index b4648e64..b81fc39b 100644 --- a/drivers/motor/motor2.hpp +++ b/drivers/motor/motor2.hpp @@ -2,8 +2,11 @@ #include "pico/stdlib.h" #include "hardware/pwm.h" +#include "common/pimoroni_common.hpp" #include "motor_state.hpp" +using namespace pimoroni; + namespace motor { class Motor2 { @@ -11,7 +14,7 @@ namespace motor { // Variables //-------------------------------------------------- private: - MotorPins motor_pins; + pin_pair motor_pins; MotorState state; pwm_config pwm_cfg; uint16_t pwm_period; @@ -23,7 +26,8 @@ namespace motor { // Constructors/Destructor //-------------------------------------------------- public: - Motor2(const MotorPins &pins, float speed_scale = MotorState::DEFAULT_SPEED_SCALE, float freq = MotorState::DEFAULT_FREQUENCY, MotorState::DecayMode mode = MotorState::DEFAULT_DECAY_MODE); + Motor2(const pin_pair &pins, MotorState::Direction direction = MotorState::DEFAULT_DIRECTION, float speed_scale = MotorState::DEFAULT_SPEED_SCALE, + float deadzone_percent = MotorState::DEFAULT_DEADZONE, float freq = MotorState::DEFAULT_FREQUENCY, MotorState::DecayMode mode = MotorState::DEFAULT_DECAY_MODE); ~Motor2(); @@ -34,7 +38,7 @@ namespace motor { bool init(); // For print access in micropython - MotorPins pins() const; + pin_pair pins() const; void enable(); void disable(); @@ -46,28 +50,33 @@ namespace motor { float speed() const; void speed(float speed); - float speed_scale() const; - void speed_scale(float speed_scale); - - void invert_direction(bool invert); - bool is_inverted() const; - float frequency() const; bool frequency(float freq); - MotorState::DecayMode decay_mode(); - void decay_mode(MotorState::DecayMode mode); + //-------------------------------------------------- void stop(); - void coast(); // An alias for disable - - //-------------------------------------------------- - void to_full_negative(); - void to_full_positive(); + void coast(); + 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); //-------------------------------------------------- + + MotorState::Direction direction() const; + void direction(MotorState::Direction direction); + + float speed_scale() const; + void speed_scale(float speed_scale); + + float deadzone_percent() const; + void deadzone_percent(float deadzone_percent); + + MotorState::DecayMode decay_mode(); + void decay_mode(MotorState::DecayMode mode); + + //-------------------------------------------------- private: void apply_duty(float duty); }; diff --git a/drivers/motor/motor_cluster.cpp b/drivers/motor/motor_cluster.cpp index 9760cc8e..eaba20b7 100644 --- a/drivers/motor/motor_cluster.cpp +++ b/drivers/motor/motor_cluster.cpp @@ -3,24 +3,25 @@ #include namespace motor { - MotorCluster::MotorCluster(PIO pio, uint sm, uint pin_mask, float speed_scale, bool inverted, float freq, bool auto_phase, PWMCluster::Sequence *seq_buffer, PWMCluster::TransitionData *dat_buffer) - : pwms(pio, sm, pin_mask, seq_buffer, dat_buffer), pwm_frequency(freq) { - create_motor_states(speed_scale, inverted, auto_phase); + MotorCluster::MotorCluster(PIO pio, uint sm, uint pin_base, uint pin_pair_count, MotorState::Direction direction, + float speed_scale, float deadzone_percent, float freq, MotorState::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), pwm_frequency(freq) { + create_motor_states(direction, speed_scale, deadzone_percent, mode, auto_phase); } - MotorCluster::MotorCluster(PIO pio, uint sm, uint pin_base, uint pin_count, float speed_scale, bool inverted, float freq, bool auto_phase, PWMCluster::Sequence *seq_buffer, PWMCluster::TransitionData *dat_buffer) - : pwms(pio, sm, pin_base, pin_count, seq_buffer, dat_buffer), pwm_frequency(freq) { - create_motor_states(speed_scale, inverted, auto_phase); + MotorCluster::MotorCluster(PIO pio, uint sm, const pin_pair *pin_pairs, uint32_t length, MotorState::Direction direction, + float speed_scale, float deadzone_percent, float freq, MotorState::DecayMode mode, + bool auto_phase, PWMCluster::Sequence *seq_buffer, PWMCluster::TransitionData *dat_buffer) + : pwms(pio, sm, pin_pairs, length, seq_buffer, dat_buffer), pwm_frequency(freq) { + create_motor_states(direction, speed_scale, deadzone_percent, mode, auto_phase); } - MotorCluster::MotorCluster(PIO pio, uint sm, const uint8_t *pins, uint32_t length, float speed_scale, bool inverted, float freq, bool auto_phase, PWMCluster::Sequence *seq_buffer, PWMCluster::TransitionData *dat_buffer) - : pwms(pio, sm, pins, length, seq_buffer, dat_buffer), pwm_frequency(freq) { - create_motor_states(speed_scale, inverted, auto_phase); - } - - MotorCluster::MotorCluster(PIO pio, uint sm, std::initializer_list pins, float speed_scale, bool inverted, float freq, bool auto_phase, PWMCluster::Sequence *seq_buffer, PWMCluster::TransitionData *dat_buffer) - : pwms(pio, sm, pins, seq_buffer, dat_buffer), pwm_frequency(freq) { - create_motor_states(speed_scale, inverted, auto_phase); + MotorCluster::MotorCluster(PIO pio, uint sm, std::initializer_list pin_pairs, MotorState::Direction direction, + float speed_scale, float deadzone_percent, float freq, MotorState::DecayMode mode, + bool auto_phase, PWMCluster::Sequence *seq_buffer, PWMCluster::TransitionData *dat_buffer) + : pwms(pio, sm, pin_pairs, seq_buffer, dat_buffer), pwm_frequency(freq) { + create_motor_states(direction, speed_scale, deadzone_percent, mode, auto_phase); } MotorCluster::~MotorCluster() { @@ -38,10 +39,12 @@ namespace motor { pwm_period = period; // Update the pwm before setting the new wrap - uint8_t motor_count = pwms.get_chan_count(); + uint8_t motor_count = pwms.get_chan_pair_count(); for(uint8_t motor = 0; motor < motor_count; motor++) { - pwms.set_chan_level(motor, 0, false); - pwms.set_chan_offset(motor, (uint32_t)(motor_phases[motor] * (float)pwm_period), false); + pwms.set_chan_level(motor_positive(motor), 0, false); + pwms.set_chan_level(motor_negative(motor), 0, false); + pwms.set_chan_offset(motor_positive(motor), (uint32_t)(motor_phases[motor] * (float)pwm_period), false); + pwms.set_chan_offset(motor_negative(motor), (uint32_t)(motor_phases[motor] * (float)pwm_period), false); } // Set the new wrap (should be 1 less than the period to get full 0 to 100%) @@ -61,15 +64,15 @@ namespace motor { } uint8_t MotorCluster::count() const { - return pwms.get_chan_count(); + return pwms.get_chan_pair_count(); } - uint8_t MotorCluster::pin(uint8_t motor) const { - return pwms.get_chan_pin(motor); + pin_pair MotorCluster::pins(uint8_t motor) const { + return pwms.get_chan_pin_pair(motor); } void MotorCluster::enable(uint8_t motor, bool load) { - assert(motor < pwms.get_chan_count()); + assert(motor < pwms.get_chan_pair_count()); float new_duty = states[motor].enable_with_return(); apply_duty(motor, new_duty, load); } @@ -92,7 +95,7 @@ namespace motor { } void MotorCluster::enable_all(bool load) { - uint8_t motor_count = pwms.get_chan_count(); + uint8_t motor_count = pwms.get_chan_pair_count(); for(uint8_t motor = 0; motor < motor_count; motor++) { enable(motor, false); } @@ -101,7 +104,7 @@ namespace motor { } void MotorCluster::disable(uint8_t motor, bool load) { - assert(motor < pwms.get_chan_count()); + assert(motor < pwms.get_chan_pair_count()); float new_duty = states[motor].disable_with_return(); apply_duty(motor, new_duty, load); } @@ -124,7 +127,7 @@ namespace motor { } void MotorCluster::disable_all(bool load) { - uint8_t motor_count = pwms.get_chan_count(); + uint8_t motor_count = pwms.get_chan_pair_count(); for(uint8_t motor = 0; motor < motor_count; motor++) { disable(motor, false); } @@ -133,17 +136,17 @@ namespace motor { } bool MotorCluster::is_enabled(uint8_t motor) const { - assert(motor < pwms.get_chan_count()); + assert(motor < pwms.get_chan_pair_count()); return states[motor].is_enabled(); } float MotorCluster::duty(uint8_t motor) const { - assert(motor < pwms.get_chan_count()); + 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_count()); + assert(motor < pwms.get_chan_pair_count()); float new_duty = states[motor].set_duty_with_return(duty); apply_duty(motor, new_duty, load); } @@ -166,7 +169,7 @@ namespace motor { } void MotorCluster::all_to_duty(float duty, bool load) { - uint8_t motor_count = pwms.get_chan_count(); + uint8_t motor_count = pwms.get_chan_pair_count(); for(uint8_t motor = 0; motor < motor_count; motor++) { this->duty(motor, duty, false); } @@ -175,12 +178,12 @@ namespace motor { } float MotorCluster::speed(uint8_t motor) const { - assert(motor < pwms.get_chan_count()); + 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_count()); + assert(motor < pwms.get_chan_pair_count()); float new_duty = states[motor].set_speed_with_return(speed); apply_duty(motor, new_duty, load); } @@ -203,7 +206,7 @@ namespace motor { } void MotorCluster::all_to_speed(float speed, bool load) { - uint8_t motor_count = pwms.get_chan_count(); + uint8_t motor_count = pwms.get_chan_pair_count(); for(uint8_t motor = 0; motor < motor_count; motor++) { this->speed(motor, speed, false); } @@ -212,12 +215,12 @@ namespace motor { } float MotorCluster::phase(uint8_t motor) const { - assert(motor < pwms.get_chan_count()); + assert(motor < pwms.get_chan_pair_count()); return motor_phases[motor]; } void MotorCluster::phase(uint8_t motor, float phase, bool load) { - assert(motor < pwms.get_chan_count()); + assert(motor < pwms.get_chan_pair_count()); motor_phases[motor] = MIN(MAX(phase, 0.0f), 1.0f); pwms.set_chan_offset(motor, (uint32_t)(motor_phases[motor] * (float)pwms.get_wrap()), load); } @@ -240,7 +243,7 @@ namespace motor { } void MotorCluster::all_to_phase(float phase, bool load) { - uint8_t motor_count = pwms.get_chan_count(); + uint8_t motor_count = pwms.get_chan_pair_count(); for(uint8_t motor = 0; motor < motor_count; motor++) { this->phase(motor, phase, false); } @@ -264,12 +267,13 @@ namespace motor { pwm_frequency = freq; // Update the pwm before setting the new wrap - uint8_t motor_count = pwms.get_chan_count(); + uint8_t motor_count = pwms.get_chan_pair_count(); for(uint motor = 0; motor < motor_count; motor++) { if(states[motor].is_enabled()) { apply_duty(motor, states[motor].get_duty(), false); } - pwms.set_chan_offset(motor, (uint32_t)(motor_phases[motor] * (float)pwm_period), false); + pwms.set_chan_offset(motor_positive(motor), (uint32_t)(motor_phases[motor] * (float)pwm_period), false); + pwms.set_chan_offset(motor_negative(motor), (uint32_t)(motor_phases[motor] * (float)pwm_period), false); } // Set the new wrap (should be 1 less than the period to get full 0 to 100%) @@ -286,77 +290,137 @@ namespace motor { return success; } - float MotorCluster::speed_scale(uint8_t motor) const { - assert(is_assigned(motor)); - return states[motor].get_speed_scale(); - } - - void MotorCluster::to_full_negative(uint8_t motor, bool load) { - assert(is_assigned(motor)); - float new_duty = states[motor].to_full_negative_with_return(); + 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, load); } - void MotorCluster::to_full_negative(const uint8_t *motors, uint8_t length, bool load) { + void MotorCluster::stop(const uint8_t *motors, uint8_t length, bool load) { assert(motors != nullptr); for(uint8_t i = 0; i < length; i++) { - to_full_negative(motors[i], false); + this->stop(motors[i], false); } if(load) pwms.load_pwm(); } - void MotorCluster::to_full_negative(std::initializer_list motors, bool load) { + void MotorCluster::stop(std::initializer_list motors, bool load) { for(auto motor : motors) { - to_full_negative(motor, false); + 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()); + states[motor].set_duty_with_return(0.0f); + float new_duty = states[motor].disable_with_return(); + apply_duty(motor, new_duty, 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 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::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, 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 motors, bool load) { + for(auto motor : motors) { + this->full_negative(motor, false); } if(load) pwms.load_pwm(); } void MotorCluster::all_to_full_negative(bool load) { - uint8_t motor_count = pwms.get_chan_count(); + uint8_t motor_count = pwms.get_chan_pair_count(); for(uint8_t motor = 0; motor < motor_count; motor++) { - to_full_negative(motor, false); + this->full_negative(motor, false); } if(load) pwms.load_pwm(); } - void MotorCluster::to_full_positive(uint8_t motor, bool load) { - assert(is_assigned(motor)); - float new_duty = states[motor].to_full_positive_with_return(); + 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, load); } - void MotorCluster::to_full_positive(const uint8_t *motors, uint8_t length, bool 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++) { - to_full_positive(motors[i], false); + this->full_positive(motors[i], false); } if(load) pwms.load_pwm(); } - void MotorCluster::to_full_positive(std::initializer_list motors, bool load) { + void MotorCluster::full_positive(std::initializer_list motors, bool load) { for(auto motor : motors) { - to_full_positive(motor, false); + this->full_positive(motor, false); } if(load) pwms.load_pwm(); } void MotorCluster::all_to_full_positive(bool load) { - uint8_t motor_count = pwms.get_chan_count(); + uint8_t motor_count = pwms.get_chan_pair_count(); for(uint8_t motor = 0; motor < motor_count; motor++) { - to_full_positive(motor, false); + 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(is_assigned(motor)); + 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, load); } @@ -379,7 +443,7 @@ namespace motor { } void MotorCluster::all_to_percent(float in, float in_min, float in_max, bool load) { - uint8_t motor_count = pwms.get_chan_count(); + 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); } @@ -388,7 +452,7 @@ namespace motor { } void MotorCluster::to_percent(uint8_t motor, float in, float in_min, float in_max, float speed_min, float speed_max, bool load) { - assert(is_assigned(motor)); + 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, load); } @@ -411,7 +475,7 @@ namespace motor { } 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_count(); + 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); } @@ -423,18 +487,167 @@ namespace motor { pwms.load_pwm(); } - void MotorCluster::apply_duty(uint8_t motor, float duty, bool load) { - pwms.set_chan_level(motor, MotorState::duty_to_level(duty, pwm_period), load); + MotorState::Direction MotorCluster::direction(uint8_t motor) const { + assert(motor < pwms.get_chan_pair_count()); + return states[motor].get_direction(); } - void MotorCluster::create_motor_states(float speed_scale, bool inverted, bool auto_phase) { - uint8_t motor_count = pwms.get_chan_count(); + void MotorCluster::direction(uint8_t motor, MotorState::Direction direction) { + assert(motor < pwms.get_chan_pair_count()); + states[motor].set_direction(direction); + } + + void MotorCluster::direction(const uint8_t *motors, uint8_t length, MotorState::Direction direction) { + assert(motors != nullptr); + for(uint8_t i = 0; i < length; i++) { + this->direction(motors[i], direction); + } + } + + void MotorCluster::direction(std::initializer_list motors, MotorState::Direction direction) { + for(auto motor : motors) { + this->direction(motor, direction); + } + } + + void MotorCluster::all_directions(MotorState::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 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::deadzone_percent(uint8_t motor) const { + assert(motor < pwms.get_chan_pair_count()); + return states[motor].get_deadzone_percent(); + } + + void MotorCluster::deadzone_percent(uint8_t motor, float deadzone_percent) { + assert(motor < pwms.get_chan_pair_count()); + states[motor].set_deadzone_percent_with_return(deadzone_percent); //TODO + } + + void MotorCluster::deadzone_percent(const uint8_t *motors, uint8_t length, float deadzone_percent) { + assert(motors != nullptr); + for(uint8_t i = 0; i < length; i++) { + this->deadzone_percent(motors[i], deadzone_percent); + } + } + + void MotorCluster::deadzone_percent(std::initializer_list motors, float deadzone_percent) { + for(auto motor : motors) { + this->deadzone_percent(motor, deadzone_percent); + } + } + + void MotorCluster::all_deadzone_percents(float deadzone_percent) { + uint8_t motor_count = pwms.get_chan_pair_count(); + for(uint8_t motor = 0; motor < motor_count; motor++) { + this->deadzone_percent(motor, deadzone_percent); + } + } + + MotorState::DecayMode MotorCluster::decay_mode(uint8_t motor) const { + assert(motor < pwms.get_chan_pair_count()); + return MotorState::SLOW_DECAY;//TODO states[motor].get_decay_mode(); + } + + void MotorCluster::decay_mode(uint8_t motor, MotorState::DecayMode mode) { + assert(motor < pwms.get_chan_pair_count()); + //TODO states[motor].set_decay_mode(mode); + } + + void MotorCluster::decay_mode(const uint8_t *motors, uint8_t length, MotorState::DecayMode mode) { + assert(motors != nullptr); + for(uint8_t i = 0; i < length; i++) { + this->decay_mode(motors[i], mode); + } + } + + void MotorCluster::decay_mode(std::initializer_list motors, MotorState::DecayMode mode) { + for(auto motor : motors) { + this->decay_mode(motor, mode); + } + } + + void MotorCluster::all_decay_modes(MotorState::DecayMode mode) { + 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, bool load) { + int32_t signed_level = MotorState::duty_to_level(duty, pwm_period); + + //TODO move this into motor state + MotorState::DecayMode decay_mode = MotorState::SLOW_DECAY; + switch(decay_mode) { + case MotorState::SLOW_DECAY: //aka 'Braking' + if(signed_level >= 0) { + pwms.set_chan_level(motor_positive(motor), pwm_period, false); + pwms.set_chan_level(motor_negative(motor), pwm_period - signed_level, load); + } + else { + pwms.set_chan_level(motor_positive(motor), pwm_period + signed_level, false); + pwms.set_chan_level(motor_negative(motor), pwm_period, load); + } + break; + + case MotorState::FAST_DECAY: //aka 'Coasting' + default: + if(signed_level >= 0) { + pwms.set_chan_level(motor_positive(motor), signed_level, false); + pwms.set_chan_level(motor_negative(motor), 0, load); + } + else { + pwms.set_chan_level(motor_positive(motor), 0, false); + pwms.set_chan_level(motor_negative(motor), 0 - signed_level, load); + } + break; + } + } + + void MotorCluster::create_motor_states(MotorState::Direction direction, float speed_scale, + float deadzone_percent, MotorState::DecayMode mode, bool auto_phase) { + uint8_t motor_count = pwms.get_chan_pair_count(); if(motor_count > 0) { states = new MotorState[motor_count]; motor_phases = new float[motor_count]; for(uint motor = 0; motor < motor_count; motor++) { - states[motor] = MotorState(speed_scale, inverted); + states[motor] = MotorState(direction, speed_scale, deadzone_percent); motor_phases[motor] = (auto_phase) ? (float)motor / (float)motor_count : 0.0f; } } diff --git a/drivers/motor/motor_cluster.hpp b/drivers/motor/motor_cluster.hpp index b4d66b30..96384149 100644 --- a/drivers/motor/motor_cluster.hpp +++ b/drivers/motor/motor_cluster.hpp @@ -24,10 +24,15 @@ namespace motor { // Constructors/Destructor //-------------------------------------------------- public: - MotorCluster(PIO pio, uint sm, uint pin_mask, float speed_scale = MotorState::DEFAULT_SPEED_SCALE, bool inverted = false, float freq = MotorState::DEFAULT_FREQUENCY, bool auto_phase = true, PWMCluster::Sequence *seq_buffer = nullptr, PWMCluster::TransitionData *dat_buffer = nullptr); - MotorCluster(PIO pio, uint sm, uint pin_base, uint pin_count, float speed_scale = MotorState::DEFAULT_SPEED_SCALE, bool inverted = false, float freq = MotorState::DEFAULT_FREQUENCY, bool auto_phase = true, PWMCluster::Sequence *seq_buffer = nullptr, PWMCluster::TransitionData *dat_buffer = nullptr); - MotorCluster(PIO pio, uint sm, const uint8_t *pins, uint32_t length, float speed_scale = MotorState::DEFAULT_SPEED_SCALE, bool inverted = false, float freq = MotorState::DEFAULT_FREQUENCY, bool auto_phase = true, PWMCluster::Sequence *seq_buffer = nullptr, PWMCluster::TransitionData *dat_buffer = nullptr); - MotorCluster(PIO pio, uint sm, std::initializer_list pins, float speed_scale = MotorState::DEFAULT_SPEED_SCALE, bool inverted = false, float freq = MotorState::DEFAULT_FREQUENCY, bool auto_phase = true, PWMCluster::Sequence *seq_buffer = nullptr, PWMCluster::TransitionData *dat_buffer = nullptr); + MotorCluster(PIO pio, uint sm, uint pin_base, uint pin_pair_count, MotorState::Direction direction = MotorState::DEFAULT_DIRECTION, float speed_scale = MotorState::DEFAULT_SPEED_SCALE, + float deadzone_percent = MotorState::DEFAULT_DEADZONE, float freq = MotorState::DEFAULT_FREQUENCY, MotorState::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, MotorState::Direction direction = MotorState::DEFAULT_DIRECTION, float speed_scale = MotorState::DEFAULT_SPEED_SCALE, + float deadzone_percent = MotorState::DEFAULT_DEADZONE, float freq = MotorState::DEFAULT_FREQUENCY, MotorState::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_pairs, MotorState::Direction direction = MotorState::DEFAULT_DIRECTION, float speed_scale = MotorState::DEFAULT_SPEED_SCALE, + float deadzone_percent = MotorState::DEFAULT_DEADZONE, float freq = MotorState::DEFAULT_FREQUENCY, MotorState::DecayMode mode = MotorState::DEFAULT_DECAY_MODE, + bool auto_phase = true, PWMCluster::Sequence *seq_buffer = nullptr, PWMCluster::TransitionData *dat_buffer = nullptr); ~MotorCluster(); @@ -38,7 +43,7 @@ namespace motor { bool init(); uint8_t count() const; - uint8_t pin(uint8_t motor) 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); @@ -74,16 +79,24 @@ namespace motor { bool frequency(float freq); //-------------------------------------------------- - float speed_scale(uint8_t motor) const; + 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 motors, bool load = true); + void stop_all(bool load = true); - void to_full_negative(uint8_t motor, bool load = true); - void to_full_negative(const uint8_t *motors, uint8_t length, bool load = true); - void to_full_negative(std::initializer_list motors, 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 motors, bool load = true); + void coast_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 motors, bool load = true); void all_to_full_negative(bool load = true); - void to_full_positive(uint8_t motor, bool load = true); - void to_full_positive(const uint8_t *motors, uint8_t length, bool load = true); - void to_full_positive(std::initializer_list motors, 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 motors, bool load = true); void all_to_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); @@ -99,9 +112,43 @@ namespace motor { void load(); //-------------------------------------------------- + + MotorState::Direction direction(uint8_t motor) const; + void direction(uint8_t motor, MotorState::Direction direction); + void direction(const uint8_t *motors, uint8_t length, MotorState::Direction direction); + void direction(std::initializer_list motors, MotorState::Direction direction); + void all_directions(MotorState::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 motors, float speed_scale); + void all_speed_scales(float speed_scale); + + float deadzone_percent(uint8_t motor) const; + void deadzone_percent(uint8_t motor, float deadzone_percent); + void deadzone_percent(const uint8_t *motors, uint8_t length, float deadzone_percent); + void deadzone_percent(std::initializer_list motors, float deadzone_percent); + void all_deadzone_percents(float deadzone_percent); + + MotorState::DecayMode decay_mode(uint8_t motor) const; + void decay_mode(uint8_t motor, MotorState::DecayMode mode); + void decay_mode(const uint8_t *motors, uint8_t length, MotorState::DecayMode mode); + void decay_mode(std::initializer_list motors, MotorState::DecayMode mode); + void all_decay_modes(MotorState::DecayMode mode); + + //-------------------------------------------------- private: void apply_duty(uint8_t motor, float duty, bool load); - void create_motor_states(float speed_scale, bool inverted, bool auto_phase); + void create_motor_states(MotorState::Direction direction, float speed_scale, + float deadzone_percent, MotorState::DecayMode mode, bool auto_phase); + + static uint8_t motor_positive(uint8_t motor) { + return PWMCluster::channel_from_pair(motor); + } + static uint8_t motor_negative(uint8_t motor) { + return PWMCluster::channel_from_pair(motor) + 1; + } }; } \ No newline at end of file diff --git a/drivers/motor/motor_state.cpp b/drivers/motor/motor_state.cpp index 079038e1..9e9da46b 100644 --- a/drivers/motor/motor_state.cpp +++ b/drivers/motor/motor_state.cpp @@ -1,24 +1,27 @@ #include "motor_state.hpp" +#include "common/pimoroni_common.hpp" #include "float.h" namespace motor { - MotorState::MotorState() - : motor_speed(0.0f), motor_scale(DEFAULT_SPEED_SCALE), inverted(false), last_enabled_duty(0.0f), enabled(false) { + MotorState::MotorState() + : motor_speed(0.0f), last_enabled_duty(0.0f), enabled(false), + motor_direction(DEFAULT_DIRECTION), motor_scale(DEFAULT_SPEED_SCALE), motor_deadzone(DEFAULT_DEADZONE) { } - MotorState::MotorState(float speed_scale, bool inverted) - : motor_speed(0.0f), motor_scale(MAX(speed_scale, FLT_EPSILON)), inverted(inverted), last_enabled_duty(0.0f), enabled(false) { + MotorState::MotorState(Direction direction, float speed_scale, float deadzone_percent) + : motor_speed(0.0f), last_enabled_duty(0.0f), enabled(false), + motor_direction(direction), motor_scale(MAX(speed_scale, FLT_EPSILON)), motor_deadzone(CLAMP(deadzone_percent, 0.0f, 1.0f)) { } float MotorState::enable_with_return() { enabled = true; float duty = 0.0f; - if((duty <= 0.0f - deadzone_percent) || (duty >= deadzone_percent)) { - if(inverted) - duty = 0.0f - last_enabled_duty; - else + if((last_enabled_duty <= 0.0f - motor_deadzone) || (last_enabled_duty >= motor_deadzone)) { + if(motor_direction == NORMAL) duty = last_enabled_duty; + else + duty = 0.0f - last_enabled_duty; } return duty; } @@ -38,7 +41,7 @@ namespace motor { float MotorState::set_duty_with_return(float duty) { // Clamp the duty between the hard limits - last_enabled_duty = MIN(MAX(duty, -1.0f), 1.0f); + last_enabled_duty = CLAMP(duty, -1.0f, 1.0f); motor_speed = last_enabled_duty * motor_scale; return enable_with_return(); @@ -50,42 +53,21 @@ namespace motor { float MotorState::set_speed_with_return(float speed) { // Clamp the speed between the hard limits - motor_speed = MIN(MAX(speed, -motor_scale), motor_scale); + motor_speed = CLAMP(speed, -motor_scale, motor_scale); last_enabled_duty = motor_speed / motor_scale; return enable_with_return(); } - 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 = last_enabled_duty * motor_scale; - } - - void MotorState::invert_direction(bool invert) { - if(invert != inverted) { - inverted = invert; - motor_speed = 0.0f - motor_speed; - last_enabled_duty = 0.0f - last_enabled_duty; - } - } - - bool MotorState::is_inverted() const { - return inverted; - } - float MotorState::stop_with_return() { return set_duty_with_return(0.0f); } - float MotorState::to_full_negative_with_return() { + float MotorState::full_negative_with_return() { return set_duty_with_return(-1.0f); } - float MotorState::to_full_positive_with_return() { + float MotorState::full_positive_with_return() { return set_duty_with_return(1.0f); } @@ -99,6 +81,40 @@ namespace motor { return set_speed_with_return(speed); } + MotorState::Direction MotorState::get_direction() const { + return motor_direction; + } + + void MotorState::set_direction(Direction direction) { + if(direction != motor_direction) { + motor_direction = direction; + motor_speed = 0.0f - motor_speed; + last_enabled_duty = 0.0f - last_enabled_duty; + } + } + + 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 = last_enabled_duty * motor_scale; + } + + + float MotorState::get_deadzone_percent() const { + return motor_scale; + } + + float MotorState::set_deadzone_percent_with_return(float deadzone_percent) { + motor_deadzone = CLAMP(deadzone_percent, 0.0f, 1.0f); + if(enabled) + return enable_with_return(); + else + return disable_with_return(); + } + int32_t MotorState::duty_to_level(float duty, uint32_t resolution) { return (int32_t)(duty * (float)resolution); } diff --git a/drivers/motor/motor_state.hpp b/drivers/motor/motor_state.hpp index d4b17f3b..8bf94300 100644 --- a/drivers/motor/motor_state.hpp +++ b/drivers/motor/motor_state.hpp @@ -4,22 +4,6 @@ namespace motor { - struct MotorPins { - uint positive; - uint negative; - - MotorPins() : positive(0), negative(0) {} - MotorPins(uint pos_pin, uint neg_pin) : positive(pos_pin), negative(neg_pin) {} - }; - - struct EncoderPins { - uint a; - uint b; - - EncoderPins() : a(0), b(0) {} - EncoderPins(uint a_pin, uint b_pin) : a(a_pin), b(b_pin) {} - }; - class MotorState { //-------------------------------------------------- // Enums @@ -30,17 +14,25 @@ namespace motor { SLOW_DECAY = 1, //aka 'Braking' }; + enum Direction { + NORMAL = 0, + REVERSED = 1, + }; + //-------------------------------------------------- // Constants //-------------------------------------------------- public: + static const Direction DEFAULT_DIRECTION = NORMAL; // The standard motor direction + static constexpr float DEFAULT_SPEED_SCALE = 1.0f; // The standard motor speed scale + static constexpr float DEFAULT_DEADZONE = 0.1f; // 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 const DecayMode DEFAULT_DECAY_MODE = SLOW_DECAY; - static constexpr float DEFAULT_SPEED_SCALE = 1.0f; // The standard motor update rate + static constexpr float MIN_FREQUENCY = 10.0f; + static constexpr float MAX_FREQUENCY = 50000.0f; - static constexpr float MIN_FREQUENCY = 10.0f; // Lowest achievable with hardware PWM with good resolution - static constexpr float MAX_FREQUENCY = 50000.0f; // Highest nice speed static constexpr float ZERO_PERCENT = 0.0f; static constexpr float ONEHUNDRED_PERCENT = 1.0f; @@ -50,11 +42,13 @@ namespace motor { //-------------------------------------------------- private: float motor_speed; - float motor_scale; - bool inverted; float last_enabled_duty; bool enabled; - float deadzone_percent = 0.0f; + + // Customisation variables + Direction motor_direction; + float motor_scale; + float motor_deadzone; //-------------------------------------------------- @@ -62,7 +56,7 @@ namespace motor { //-------------------------------------------------- public: MotorState(); - MotorState(float speed_scale, bool inverted); + MotorState(Direction direction, float speed_scale, float deadzone_percent); //-------------------------------------------------- @@ -72,27 +66,32 @@ namespace motor { float enable_with_return(); float disable_with_return(); bool is_enabled() const; - public: + float get_duty() const; float set_duty_with_return(float duty); float get_speed() const; float set_speed_with_return(float speed); - float get_speed_scale() const; - void set_speed_scale(float speed_scale); - - void invert_direction(bool invert); - bool is_inverted() const; - //-------------------------------------------------- float stop_with_return(); - float to_full_negative_with_return(); - float to_full_positive_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_deadzone_percent() const; + float set_deadzone_percent_with_return(float deadzone_percent); + //-------------------------------------------------- static int32_t duty_to_level(float duty, uint32_t resolution); diff --git a/drivers/pwm/pwm_cluster.cpp b/drivers/pwm/pwm_cluster.cpp index 6197bbc9..c691d039 100644 --- a/drivers/pwm/pwm_cluster.cpp +++ b/drivers/pwm/pwm_cluster.cpp @@ -102,6 +102,55 @@ PWMCluster::PWMCluster(PIO pio, uint sm, std::initializer_list 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) +: pio(pio) +, sm(sm) +, pin_mask(0x00000000) +, channel_count(0) +, channels(nullptr) +, wrap_level(0) { + + // 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_pairs, Sequence *seq_buffer, TransitionData *dat_buffer) +: pio(pio) +, sm(sm) +, pin_mask(0x00000000) +, channel_count(0) +, channels(nullptr) +, wrap_level(0) { + + // 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 +381,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; diff --git a/drivers/pwm/pwm_cluster.hpp b/drivers/pwm/pwm_cluster.hpp index 147e8b4a..e60f2969 100644 --- a/drivers/pwm/pwm_cluster.hpp +++ b/drivers/pwm/pwm_cluster.hpp @@ -4,6 +4,7 @@ #include "hardware/pio.h" #include "hardware/dma.h" #include "hardware/irq.h" +#include "common/pimoroni_common.hpp" #include namespace pimoroni { @@ -137,6 +138,9 @@ namespace pimoroni { 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 pins, Sequence *seq_buffer = nullptr, TransitionData *dat_buffer = nullptr); + + PWMCluster(PIO pio, uint sm, const pin_pair *pin_pairs, uint32_t length, Sequence *seq_buffer = nullptr, TransitionData *dat_buffer = nullptr); + PWMCluster(PIO pio, uint sm, std::initializer_list pin_pairs, Sequence *seq_buffer = nullptr, TransitionData *dat_buffer = nullptr); ~PWMCluster(); private: @@ -150,7 +154,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); diff --git a/examples/motor2040/motor2040_single_motor.cpp b/examples/motor2040/motor2040_single_motor.cpp index e217c8fc..dae8f56a 100644 --- a/examples/motor2040/motor2040_single_motor.cpp +++ b/examples/motor2040/motor2040_single_motor.cpp @@ -36,11 +36,11 @@ int main() { sleep_ms(2000); // Go at full neative - m.to_full_negative(); + m.full_negative(); sleep_ms(2000); // Go at full positive - m.to_full_positive(); + m.full_positive(); sleep_ms(2000); // Stop the motor diff --git a/libraries/motor2040/motor2040.hpp b/libraries/motor2040/motor2040.hpp index b362ec99..1da18c91 100644 --- a/libraries/motor2040/motor2040.hpp +++ b/libraries/motor2040/motor2040.hpp @@ -16,10 +16,10 @@ namespace motor { const uint MOTOR_4P = 10; const uint MOTOR_4N = 11; - const MotorPins MOTOR_1(MOTOR_1P, MOTOR_1N); - const MotorPins MOTOR_2(MOTOR_2P, MOTOR_2N); - const MotorPins MOTOR_3(MOTOR_3P, MOTOR_3N); - const MotorPins MOTOR_4(MOTOR_4P, MOTOR_4N); + const pin_pair MOTOR_1(MOTOR_1P, MOTOR_1N); + const pin_pair MOTOR_2(MOTOR_2P, MOTOR_2N); + const pin_pair MOTOR_3(MOTOR_3P, MOTOR_3N); + const pin_pair MOTOR_4(MOTOR_4P, MOTOR_4N); const uint NUM_MOTORS = 4; const uint ENCODER_1A = 0; @@ -31,10 +31,10 @@ namespace motor { const uint ENCODER_4A = 14; const uint ENCODER_4B = 15; - const EncoderPins ENCODER_1(ENCODER_1A, ENCODER_1B); - const EncoderPins ENCODER_2(ENCODER_2A, ENCODER_2B); - const EncoderPins ENCODER_3(ENCODER_3A, ENCODER_3B); - const EncoderPins ENCODER_4(ENCODER_4A, ENCODER_4B); + const pin_pair ENCODER_1(ENCODER_1A, ENCODER_1B); + const pin_pair ENCODER_2(ENCODER_2A, ENCODER_2B); + const pin_pair ENCODER_3(ENCODER_3A, ENCODER_3B); + const pin_pair ENCODER_4(ENCODER_4A, ENCODER_4B); const uint NUM_ENCODERS = 4; const uint TX_TRIG = 16; diff --git a/libraries/pico_motor_shim/pico_motor_shim.hpp b/libraries/pico_motor_shim/pico_motor_shim.hpp index c0681138..ab456807 100644 --- a/libraries/pico_motor_shim/pico_motor_shim.hpp +++ b/libraries/pico_motor_shim/pico_motor_shim.hpp @@ -12,8 +12,8 @@ namespace motor { const uint MOTOR_2P = 27; const uint MOTOR_2N = 26; - const MotorPins MOTOR_1(MOTOR_1P, MOTOR_1N); - const MotorPins MOTOR_2(MOTOR_2P, MOTOR_2N); + const pin_pair MOTOR_1(MOTOR_1P, MOTOR_1N); + const pin_pair MOTOR_2(MOTOR_2P, MOTOR_2N); const uint NUM_MOTORS = 2; } } \ No newline at end of file From b36993f4927503284ab79468987fc5b713cf1509 Mon Sep 17 00:00:00 2001 From: ZodiusInfuser Date: Tue, 5 Apr 2022 20:44:03 +0100 Subject: [PATCH 05/60] First setup of motor micropython bindings --- micropython/modules/micropython.cmake | 2 + micropython/modules/motor/README.md | 702 +++++++ micropython/modules/motor/micropython.cmake | 27 + micropython/modules/motor/motor.c | 263 +++ micropython/modules/motor/motor.cpp | 1929 +++++++++++++++++++ micropython/modules/motor/motor.h | 72 + micropython/modules/pwm/micropython.cmake | 20 + micropython/modules/servo/micropython.cmake | 3 +- 8 files changed, 3017 insertions(+), 1 deletion(-) create mode 100644 micropython/modules/motor/README.md create mode 100644 micropython/modules/motor/micropython.cmake create mode 100644 micropython/modules/motor/motor.c create mode 100644 micropython/modules/motor/motor.cpp create mode 100644 micropython/modules/motor/motor.h create mode 100644 micropython/modules/pwm/micropython.cmake diff --git a/micropython/modules/micropython.cmake b/micropython/modules/micropython.cmake index f9e6314b..7c3560b9 100644 --- a/micropython/modules/micropython.cmake +++ b/micropython/modules/micropython.cmake @@ -41,7 +41,9 @@ include(pico_wireless/micropython) include(plasma/micropython) include(hub75/micropython) +include(pwm/micropython) include(servo/micropython) +include(motor/micropython) include(ulab/code/micropython) include(qrcode/micropython/micropython) diff --git a/micropython/modules/motor/README.md b/micropython/modules/motor/README.md new file mode 100644 index 00000000..7fe0890b --- /dev/null +++ b/micropython/modules/motor/README.md @@ -0,0 +1,702 @@ +# Servos and Servo 2040 + +The Servo library lets you drive 3-pin hobby servo motors from a Raspberry Pi Pico or any other RP2040-based board, such as the [Pimoroni Servo 2040](https://pimoroni.com/servo2040). + +This library offers two servo implementations: +* a `Servo` class that uses hardware PWM to drive a single servo, with support for up to 16 servos. +* a `ServoCluster` class that uses Programmable IO (PIO) hardware to drive up to 30 servos. + +There is also a `Calibration` class for performing advanced tweaking of each servo's movement behaviour. + + +## Table of Content +- [Servo 2040](#servo-2040) + - [Reading the User Button](#reading-the-user-button) + - [Reading the Sensors](#reading-the-sensors) + - [Configuring Pulls](#configuring-pulls) + - [Controlling the LED Bar](#controlling-the-led-bar) + - [Pin Constants](#pin-constants) + - [Servo Pins](#servo-pins) + - [LED Pin](#led-pin) + - [I2C Pins](#i2c-pins) + - [Button Pin](#button-pin) + - [Address Pins](#address-pins) + - [ADC Pins](#adc-pins) + - [Other Constants](#other-constants) + - [Counts](#counts) + - [Addresses](#addresses) + - [Sensing](#sensing) +- [Servo](#servo) + - [Getting Started](#getting-started) + - [Control by Value](#control-by-value) + - [Common Values](#common-values) + - [Control by Percent](#control-by-percent) + - [Control by Pulse Width](#control-by-pulse-width) + - [Frequency Control](#frequency-control) + - [Calibration](#calibration) + - [Function Reference](#function-reference) + - [PWM Limitations](#pwm-limitations) +- [ServoCluster](#servocluster) + - [Getting Started](#getting-started-1) + - [Control by Value](#control-by-value-1) + - [Common Values](#common-values-1) + - [Control by Percent](#control-by-percent-1) + - [Control by Pulse Width](#control-by-pulse-width-1) + - [Frequency Control](#frequency-control-1) + - [Phase Control](#phase-control) + - [Calibration](#calibration-1) + - [Delayed Loading](#delayed-loading) + - [Function Reference](#function-reference-1) + - [PIO Limitations](#pio-limitations) +- [Calibration](#calibration-2) + - [Common Types](#common-types) + - [Custom Calibration](#custom-calibration) + - [Modifying a Calibration](#modifying-a-calibration) + - [Movement Limits](#movement-limits) + - [Populating a Calibration](#populating-a-calibration) + - [Viewing the Mapping](#viewing-the-mapping) + - [Function Reference](#function-reference-2) + + +## Servo 2040 + +Servo 2040 is a **standalone servo controller** for making things with lots of moving parts. It has pre-soldered pin headers for plugging in up to 18 servos, with additional bells and whistles like sensor headers, current monitoring, RGB LEDs, and a user button that make it ideal for many robotics projects! + +For more information on this board, check out the store page: [pimoroni.com/servo2040](https://pimoroni.com/servo2040). + + +### Reading the User Button + +As is the case for many of Pimoroni's RP2040-based boards, the boot button on Servo 2040 that is used for programming also acts as a user button once a program is running. To simplify the use of this and other buttons, the `pimoroni` module contains a `Button` class that handles button debounce and auto-repeat. + +```python +Button(button, invert=True, repeat_time=200, hold_time=1000) +``` + +To set up the user button, first import the `Button` class from the `pimoroni` module and the pin constant for the button from `servo`: +```python +from pimoroni import Button +from servo import servo2040 +``` + +Then create an instance of `Button` for the user button: +```python +user_sw = Button(servo2040.USER_SW) +``` + +To get the button state, call `.read()`. If the button is held down, then this will return `True` at the interval specified by `repeat_time` until `hold_time` is reached, at which point it will return `True` every `repeat_time / 3` milliseconds. This is useful for rapidly increasing/decreasing values: + +```python +state = user_sw.read() +``` + +It is also possible to read the raw button state without the repeat feature, if you prefer: +```python +state = user_sw.raw() +``` + + +### Reading the Sensors + +On the right-hand edge of Servo 2040 are six analog inputs, with neighbouring 3.3V and GND. These let you connect up sensors to enable your mechanical creations to sense how they are interacting with the world. For example, a servo driven gripper with analog force sensors in its fingers could report how much pressure it is applying as it grabs an object, or the extra pin of an analog feedback servo could be wired up to report its actual angle. + +Servo 2040 also has two internal sensors: +* A voltage sensor, letting you measure the supply voltage to the servos. +* A low-side current sensor, letting you measure how much current a set of servos is drawing. +Both of these could be used just for monitoring, or as the trigger to turn off servos safely when voltage gets too low or current draw gets too high. + +To allow for all of these inputs, Servo 2040 has an onboard analog multiplexer that expands a single analog pin into eight, by use of three digital address pins that select a single input at a time. As such, the setup for these sensors is more involved than it would be to just read eight analog pins directly. + +To begin reading these inputs, first import the `Analog` and `AnalogMux` classes from `pimoroni` and the pin, address, and gain constants from `servo`: + +```python +from pimoroni import Analog +from servo import servo2040 +``` + +Then set up three instances of `Analog` for the sensor, voltage, and current sensing: + +```python +sen_adc = Analog(servo2040.SHARED_ADC) +vol_adc = Analog(servo2040.SHARED_ADC, servo2040.VOLTAGE_GAIN) +cur_adc = Analog(servo2040.SHARED_ADC, servo2040.CURRENT_GAIN, + servo2040.SHUNT_RESISTOR, servo2040.CURRENT_OFFSET) +``` + +You may notice, all three of these use the same `SHARED_ADC` pin. This is intentional as it is just a single pin that is being used for all three different functions, only the gains differ. + +The next step is to set up the analog multiplexer, by providing it with the three address pins: +```python +mux = AnalogMux(servo2040.ADC_ADDR_0, servo2040.ADC_ADDR_1, servo2040.ADC_ADDR_2) +``` +Note that the `AnalogMux` does not know about any of the `Analog` classes that were created before. + +With the multiplexer now configured, reading each sensor is simply a case of 'selecting' the sensor on the multiplexer then reading the value from one of the three `Analog` classes created at the start. + +To read the siz sensor headers: +```python +for addr in range(servo2040.NUM_SENSORS): + mux.select(addr) + print("Sensor", addr + 1, "=", sen_adc.read_voltage()) +``` + +To read the voltage sense: +```python +mux.select(servo2040.VOLTAGE_SENSE_ADDR) +print("Voltage =", vol_adc.read_voltage(), "V") +``` + +To read the current draw in amps (A): +```python +mux.select(servo2040.CURRENT_SENSE_ADDR) +print("Current =", cur_adc.read_current(), "A") +``` + + +#### Configuring Pulls + +For some sensors you may need to have the input be pulled high or low before taking a reading. To support this there is an optional `muxed_pin` parameter that can be passed into the `AnalogMux` when creating it, which gives the multiplexer access to the pin to control the pulls. + +```python +mux = AnalogMux(servo2040.ADC_ADDR_0, servo2040.ADC_ADDR_1, servo2040.ADC_ADDR_2, + muxed_pin=Pin(servo2040.SHARED_ADC)) +``` + +From there the pull state of each of the multiplexer's addresses can be configured independently by calling `.configure_pull()`, with the address and the pull state (either `Pin.PULL_UP`, `Pin.PULL_DOWN`, or `None`). + +The below example shows how to set all 6 sensor addresses to have pull-downs: +```python +sensor_addrs = list(range(servo2040.SENSOR_1_ADDR, servo2040.SENSOR_6_ADDR + 1)) +for addr in sensor_addrs: + mux.configure_pull(addr, Pin.PULL_DOWN) +``` + + +#### Controlling the LED Bar + +Alongside Servo 2040's six sensor headers are six addressable RGB LEDs. These work using the same chainable 1-wire signalling as WS2812 LED's, commonly known as Neopixels. As such, they can be controlled using the same Plasma Library used by the [Pimoroni Plasma 2040](https://shop.pimoroni.com/products/plasma-2040). + +To set up the LED bar, first import the `WS2812` class from the `plasma` module and the pin constants for the LEDs from `servo`: +```python +from plasma import WS2812 +from servo import servo2040 +``` + +Then construct a new `WS2812` instance, specifying the number of LEDs, PIO, PIO state-machine and GPIO pin. +```python +led_bar = WS2812(servo2040.NUM_LEDS, 1, 0, servo2040.LED_DATA) +``` + +Finally, start the LED bar by calling `start`: +```python +led_bar.start() +``` + +For more information on how to control the LEDs on the bar, please refer to the [Plasma Library](https://github.com/pimoroni/pimoroni-pico/tree/main/micropython/modules/plasma). + + +### Pin Constants + +The `servo` module contains a `servo2040` sub module with constants for the servo, LED, sensor and button pins. + + +#### Servo Pins + +* `SERVO_1` = `0` +* `SERVO_2` = `1` +* `SERVO_3` = `2` +* `SERVO_4` = `3` +* `SERVO_5` = `4` +* `SERVO_6` = `5` +* `SERVO_7` = `6` +* `SERVO_8` = `7` +* `SERVO_9` = `8` +* `SERVO_10` = `9` +* `SERVO_11` = `10` +* `SERVO_12` = `11` +* `SERVO_13` = `12` +* `SERVO_14` = `13` +* `SERVO_15` = `14` +* `SERVO_16` = `15` +* `SERVO_17` = `16` +* `SERVO_18` = `17` + + +#### LED Pin + +* `LED_DATA` = `18` + + +#### I2C Pins + +* `INT` = `19` +* `SDA` = `20` +* `SCL` = `21` + + +#### Button Pin + +* `USER_SW` = `23` + + +#### Address Pins + +* `ADC_ADDR_0` = `22` +* `ADC_ADDR_1` = `24` +* `ADC_ADDR_2` = `25` + + +#### ADC Pins + +* `ADC0` = `26` +* `ADC1` = `27` +* `ADC2` = `28` +* `SHARED_ADC` = `29` + + +### Other Constants + +The `servo2040` sub module also contains other constants to help with the control of Servo 2040's various features: + + +#### Counts + +* `NUM_SERVOS` = `18` +* `NUM_SENSORS` = `6` +* `NUM_LEDS` = `6` + + +#### Addresses + +* `SENSOR_1_ADDR` = `0b000` +* `SENSOR_2_ADDR` = `0b001` +* `SENSOR_3_ADDR` = `0b010` +* `SENSOR_4_ADDR` = `0b011` +* `SENSOR_5_ADDR` = `0b100` +* `SENSOR_6_ADDR` = `0b101` +* `VOLTAGE_SENSE_ADDR` = `0b110` +* `CURRENT_SENSE_ADDR` = `0b111` + + +#### Sensing + +* `VOLTAGE_GAIN` = `0.28058` +* `SHUNT_RESISTOR` = `0.003` +* `CURRENT_GAIN` = `69` +* `CURRENT_OFFSET` = `-0.02` + + +## Servo + +### Getting Started + +To start using servos with your Servo 2040, you will need to first import the `Servo` class. +```python +from servo import Servo, servo2040 +``` +If you are using another RP2040 based board, then `servo2040` can be omitted from the above line + +To create your servo, choose which GPIO pin it will be connected to, and pass that into `Servo`. For this example we will use one of the handy constants of the `servo2040`. +```python +s = Servo(servo2040.SERVO_1) +``` + +You now have a `Servo` class called `s` that will control the physical servo connected to `SERVO_1`. To start using this servo, simply enable it using: +```python +s.enable() +``` + +This activates the servo and moves it to it's last known position. Since this is the first time enabling the servo, there is no last known position, so instead it will move to the middle of its movement range instead. + +Once you have finished with the servo, it can be disabled by calling: +```python +s.disable() +``` + +From here the servo can be controlled in several ways. These are covered in more detail in the following sections. + + +### Control by Value + +The most intuitive way of controlling a servo is by value. Value can be any number that has a real-world meaning for that type of servo, for example its angle in degrees if it's a regular angular servo, or its speed as a percentage if it's a continuous rotation servo. See [Calibration](#calibration) for more details. + +The value of a servo can be set by calling `.value(value)`, which takes a float as its `value` input. If the servo is disabled, this will enable it. The resulting pulse width will also be stored. + +To read back the current value of the servo, call `.value()` without any input. If the servo is disabled, this will be the last value that was provided when enabled. + + +#### Common Values + +To simplify certain motion patterns, a servo can be commanded to three common values. These are, their minimum, middle, and maximum. These are performed by calling `.to_min()`, `.to_mid()`, and `.to_max()`, respectively. If the servo is disabled, these will enable it. + +It is also possible to read back the values each of those three commands is using internally, using `.min_value()`, `.mid_value()`, and `.max_value()`. These can be useful as inputs to equations that provide numbers directly to `.value(value)`, for example. + + +### Control by Percent + +Sometimes there are projects where a servo needs to move based on the reading from a sensor or another device, but the numbers given out are not easy to convert to values the servo accepts. To overcome this the library lets you move the servo to a percent between its minimum and maximum values, or two values provided, based on that input. + +With an input between `-1.0` and `1.0`, a servo can be moved to a percent between its minimum and maximum values using `.to_percent(in)`. + +With an input between a provided min and max, a servo can be moved to a percent between its minimum and maximum values using `.to_percent(in, in_min, in_max)`. + +With an input between a provided min and max, a servo can be moved to a percent between two provided values using `.to_percent(in, in_min, value_min, value_max)`. + +If the servo is disabled, these will enable it. + + +### Control by Pulse Width + +At a hardware level servos operate by receiving a digital signal with specific pulse widths. Typically pulses are between 500 microseconds and 2500 microseconds in length, and are usually repeated every 20 milliseconds (50Hz). These functions let you interact with pulse widths directly. + +The pulse width (in microseconds) of a servo can be set by calling `.pulse(pulse)`, which takes a float as its `pulse` input. If the servo is disabled this will enable it, except for the case of `0` where instead the servo will be disabled. This function will also recalculate the related value. + +To read back the current pulse width (in microseconds) of the servo, call `.pulse()` without any input. If the servo is disabled, this will be the last pulse that was provided when enabled. + + +### Frequency Control + +The vast majority of servos expect to receive pulses with a frequency of 50Hz, so this library uses that as its default. However, there may be cases where this value needs to be changed, such as when using servos that operate up to frequencies of 333Hz. + +The frequency (in Hz) of a servo can be set by calling `.frequency(freq)`, which takes a float as its `freq` input. The supported range of this input is `10` to `350` Hz. + +To read back the current frequency (in Hz) of the servo, call `.frequency()` without any input. + +Note that changing the frequency does not change the pulse widths sent to the servos, only how frequently they are sent. This is why `350` is the upper limit, as any higher and the maximum pulse would be longer than the time period. If you encounter any servos where this behaviour is not what they expect, please raise it as a Github issue. + + +### Calibration + +There are different types of servos, with `ANGULAR`, `LINEAR`, and `CONTINUOUS` being common. To support these different types, each `Servo` class contains a calibration object that stores the specific value to pulse mapping needed for its type. A copy of a servo's calibration can be accessed using `.calibration()`. It is also possible to provide a servo with a new calibration using `.calibration(calibration)`. + + +### Function Reference + +Here is the complete list of functions available on the `Servo` class: +```python +Servo(pin, calibration=ANGULAR, freq=50) # calibration can either be an integer or a Calibration class +pin() +enable() +disable() +is_enabled() +pulse() +pulse(pulse) +value() +value(value) +frequency() +frequency(freq) +min_value() +mid_value() +max_value() +to_min() +to_mid() +to_max() +to_percent(in) +to_percent(in, in_min, in_max) +to_percent(in, in_min, in_max, value_min, value_max) +calibration() +calibration(calibration) +``` + + +### PWM Limitations + +Although the RP2040 is capable of outputting up to 16 PWM signals, there are limitations of which pins can be used together: +* The PWMs output from pins 0 to 15 are repeated for pins 16 to 29, meaning that those pins share the same signals if PWM is enabled on both. For example if you used pin 3 for PWM and then tried to use pin 19, they would both output the same signal and it would not be possible to control them independently. +* The 16 PWM channels are grouped into 8 PWM slices, each containing A and B sub channels that are synchronised with each other. This means that parameters such as frequency are shared, which can cause issues if you want one servo to operate at a different frequency to it's pin neighbour or to drive an LED with PWM at a high frequency. + +The official [RP2040 datasheet](https://datasheets.raspberrypi.com/rp2040/rp2040-datasheet.pdf), includes the handy _Table 525_ that details the pwm channel for each GPIO pin. This is shown below for convenience: + +| GPIO | 0 | 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | +|-------------|----|----|----|----|----|----|----|----|----|----|----|----|----|----|----|----| +| PWM Channel | 0A | 0B | 1A | 1B | 2A | 2B | 3A | 3B | 4A | 4B | 5A | 5B | 6A | 6B | 7A | 7B | + +| GPIO | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | +|-------------|----|----|----|----|----|----|----|----|----|----|----|----|----|----| +| PWM Channel | 0A | 0B | 1A | 1B | 2A | 2B | 3A | 3B | 4A | 4B | 5A | 5B | 6A | 6B | + + +## ServoCluster + +### Getting Started + +An alternative way of controlling servos with your Servo 2040 is to use the `ServoCluster` class. This can be imported as follows: + +```python +from servo import ServoCluster, servo2040 +``` +(If you are using another RP2040 based board, then `servo2040` can be omitted from the above line) + +The next step is to choose which GPIO pins the cluster will be connected to and store them in a `list`. For a consecutive set of pins the easiest way to do this is to use a `range` to create each pin number, and convert that to a `list`. For example, using the handy constants of the `servo2040`, the below line creates the list `[0, 1, 2, 3]` +```python +pins = list(range(servo2040.SERVO_1, servo2040.SERVO_4 + 1)) +``` + +To create your servo cluster, specify the PIO, PIO state-machine and GPIO pins you chose a moment ago, and pass those into `ServoCluster`. + +```python +cluster = ServoCluster(0, 0, pins) +``` + +You now have a `ServoCluster` class called `cluster` that will control the physical servos connected to `SERVO_1`, `SERVO_2`, `SERVO_3`, and `SERVO_4`. To start using these servos, simply enable them using: +```python +cluster.enable_all() +``` +or +```python +cluster.enable(servo) +``` +where `servo` is the servo's number within the cluster from `0` to `cluster.count() - 1`. + +These functions activate the servos and move them to their last known positions. Since this is the first time enabling the servos, there are no last known positions, so instead they will move to the middle of their movement ranges instead. + +Once you have finished with the servos, they can be disabled by calling: +```python +cluster.disable_all() +``` +or +```python +cluster.disable(servo) +``` +where `servo` is the servo's number within the cluster from `0` to `cluster.count() - 1`. + +From here the servos can be controlled in several ways. These are covered in more detail in the following sections. + + +### Control by Value + +The most intuitive way of controlling the servos is by their value. Value can be any number that has a real-world meaning for that type of servo, for example its angle in degrees if it's a regular angular servo, or its speed as a percentage if it's a continuous rotation servo. See [Calibration](#calibration) for more details. + +The value of a servo on a cluster can be set calling `.value(servo, value)` or `.all_to_value(value)`, which take a float as their `value` input. If a servo is disabled, these will enable it. The resulting pulse width will also be stored. + +To read back the current value of a servo on the cluster, call `.value(servo)`. If the servo is disabled, this will be the last value that was provided when enabled. + + +#### Common Values + +To simplify certain motion patterns, servos on a cluster can be commanded to three common values. These are, their minimum, middle, and maximum. These are performed by calling `.to_min(servo)`, `.to_mid(servo)`, and `.to_max(servo)`, respectively. If the servo is disabled, these will enable it. There are also `.all_to_min()`, `.all_to_mid()`, and `.all_to_max()` for having all the servos on the cluster move at once. + +It is also possible to read back the values each of those three commands is using internally, using `.min_value(servo)`, `.mid_value(servo)`, and `.max_value(servo)`. These can be useful as inputs to equations that provide numbers directly to `.value(servo, value)`, for example. + + +### Control by Percent + +Sometimes there are projects where servos need to move based on the readings from sensors or another devices, but the numbers given out are not easy to convert to values the servos accept. To overcome this the library lets you move the servos on a cluster to a percent between their minimum and maximum values, or two values provided, based on that input. + +With an input between `-1.0` and `1.0`, a servo on a cluster can be moved to a percent between its minimum and maximum values using `.to_percent(servo, in)`, or all servos using `.all_to_percent(in)`. + +With an input between a provided min and max, a servo on a cluster can be moved to a percent between its minimum and maximum values using `.to_percent(servo, in, in_min, in_max)`, or all servos using `s.all_to_percent(in, in_min, in_max)`. + +With an input between a provided min and max, a servo on a cluster can be moved to a percent between two provided values using `.to_percent(servo, in, in_min, value_min, value_max)`, or all servos using `.all_to_percent(in, in_min, value_min, value_max)`. + +If the servo is disabled, these will enable it. + + +### Control by Pulse Width + +At a hardware level servos operate by receiving a digital signal with specific pulse widths. Typically pulses are between 500 microseconds and 2500 microseconds in length, and are usually repeated every 20 milliseconds (50Hz). These functions let you interact with pulse widths directly. + +The pulse width (in microseconds) of a servo on a cluster can be set by calling `.pulse(servo, pulse)` or `.all_to_pulse(pulse)`, which take a float as their `pulse` input. If a servo is disabled, these will enable it, except for the case of `0` where instead the servo will be disabled. These functions will also recalculate the related value. + +To read back the current pulse width (in microseconds) of a servo on a cluster, call `.pulse(servo)`. If the servo is disabled, this will be the last pulse that was provided when enabled. + + +### Frequency Control + +The vast majority of servos expect to receive pulses with a frequency of 50Hz, so this library uses that as its default. However, there may be cases where this value needs to be changed, such as when using servos that operate up to frequencies of 333Hz. Un + +The frequency (in Hz) of all the servos on a cluster can be set by calling `.frequency(freq)`, which takes a float as its `freq` input. The supported range of this input is `10` to `350` Hz. Due to how `ServoCluster` works, there is no way to set independent frequencies for each servo. + +To read back the current frequency (in Hz) all the servos on a cluster, call `.frequency()` without any input. + +Note that changing the frequency does not change the pulse widths sent to the servos, only how frequently they are sent. This is why `350` is the upper limit, as any higher and the maximum pulse would be longer than the time period. If you encounter any servos where this behaviour is not what they expect, please raise it as a Github issue. + +Also, be aware that currently the frequency changes immediately, even if part-way through outputting a pulse. It is therefore recommended to disable all servos first before changing the frequency. + + +### Phase Control + +When dealing with many servos, there can often be large current draw spikes caused by them all responding to pulses at the same time. To minimise this, the ServoCluster class allows for the start time of each servo's pulses to be delayed by a percentage of the available time period. This is called their phase. + +The phase of a servo on a cluster can be set by calling `.phase(servo, phase)` or `.all_to_phase(phase)`, which take a float between `0.0` and `1.0` as their `phase` input. + +To read back the current phase of a servo on a cluster, call `.phase(servo)`. + +By default all servos on a cluster will start with different phases unless `auto_phase=False` is provided when creating the `ServoCluster`. + + +### Calibration + +There are different types of servos, with `ANGULAR`, `LINEAR`, and `CONTINUOUS` being common. To support these different types, each `ServoCluster` class contains calibration objects for each of its servos that store the specific value to pulse mappings needed for their types. A copy of a servo's calibration on a cluster can be accessed using `.calibration(servo)`. It is also possible to provide a servo on a cluster with a new calibration using `.calibration(servo, calibration)`. + + +### Delayed Loading + +To match behaviour with the regular `Servo` class, `ServoCluster` automatically applies each change to its servo's states immediately. However, sometimes this may not be wanted, and instead you want all servos to receive updated pulses at the same time, regardless of how long the code ran that calculated the update. + +For this purpose, all the functions that modify a servo state on a cluster include an optional parameter `load`, which by default is `True`. To avoid this "loading" include `load=False` in the relevant function calls. Then either the last call can include `load=True`, or a specific call to `.load()` can be made. + + +### Function Reference + +Here is the complete list of functions available on the `ServoCluster` class: + +```python +ServoCluster(pio, sm, pins, calibration=ANGULAR, freq=50, auto_phase=True) # calibration can either be an integer or a Calibration class +count() +pin(servo) +enable(servo, load=True) +enable_all(load=True) +disable(servo, load=True) +disable_all(servo, load=True) +is_enabled(servo) +pulse(servo) +pulse(servo, pulse, load=True) +all_to_pulse(pulse, load=True) +value(servo) +value(servo, value, load=True) +all_to_value(value, load=True) +phase(servo) +phase(servo, phase, load=True) +all_to_phase(phase, load=True) +frequency() +frequency(freq) +min_value(servo) +mid_value(servo) +max_value(servo) +to_min(servo, load=True) +all_to_min(load=True) +to_mid(servo, load=True) +all_to_mid(load=True) +to_max(servo, load=True) +all_to_max(load=True) +to_percent(servo, in, load=True) +to_percent(servo, in, in_min, in_max, load=True) +to_percent(servo, in, in_min, in_max, value_min, value_max, load=True) +all_to_percent(in, load=True) +all_to_percent(in, in_min, in_max, load=True) +all_to_percent(in, in_min, in_max, value_min, value_max, load=True) +calibration(servo) +calibration(servo, calibration) +load() +``` + + +### PIO Limitations + +The RP2040 features two PIOs with four state machines each. This places a hard limit on how many ServoClusters can be created. As this class is capable of driving all 30 GPIO pins, the only time this limit will be of concern is when servos with different frequencies are wanted, as all the outputs a ServoCluster controls share the same frequency. Relating this to the hardware PWM, think of it as a single PWM slice with up to 30 sub channels, A, B, C, D etc. + +When creating a ServoCluster, in most cases you'll use `0` for PIO and `0` for PIO state-machine. You should change these though if you plan on running multiple clusters, or using a cluster alongside something else that uses PIO, such as our [Plasma library](https://github.com/pimoroni/pimoroni-pico/tree/main/micropython/modules/plasma). + + +## Calibration + +After using servos for a while, you may notice that some don't quite go to the positions you would expect. Or perhaps you are giving values to a continuous rotation servo in degrees when it would make more sense to use a speed or rpm. To compensate for these cases, each `Servo` class or servo within a `ServoCluster` has an individual `Calibration` class. This class contains a set of pulse-value pairs that are used by the `.value(value)` functions (and those similar) to convert real-world numbers into pulses each servo understand. + +### Common Types + +There are three common `type`s of servo's supported: +* `ANGULAR` = `0` - A servo with a value that ranges from -90 to +90 degrees. +* `LINEAR` = `1` - A servo with a value that ranges from 0 to +1.0. +* `CONTINUOUS` = `2` - A servo with a value that ranges from -1.0 to +1.0. + +By default all `Servo` classes or servo within a `ServoCluster` are `ANGULAR`. This can be changed by providing one of the other types as a parameter during their creation, as shown below: +```python +angular = Servo(servo2040.SERVO_1) # ANGULAR is the default so does not need to be specified here +linear = Servo(servo2040.SERVO_2, LINEAR) +continuous = Servo(servo2040.SERVO_3, CONTINUOUS) +``` + + +### Custom Calibration + +As well as the common types, a custom calibration can also be provided to one or more servos during creation. Below is an example that creates an angular servo that can only travel from -45 degrees to 45 degrees. + +```python +cal = Calibration() +cal.apply_two_pairs(1000, 2000, -45, 45) +s = Servo(servo2040.SERVO_1, cal) +``` + +This could be useful for example if the servo turning beyond those values would cause damage to whatever mechanism it is driving, since it would not be possible to go to angles beyond these unless limits were disabled (see [Limits](#limits)). Also it lets the exact pulse widths matching the angles be set (the `1000` and `2000` in the example). Discovering these values can take some trial and error, and will offen be different for each servo. + + + +# Modifying a Calibration + +It is also possible to access and modify the calibration of a `Servo` or a servo on a `ServoCluster` after their creation. This is done by first getting a copy of the servo's calibration using `.calibration()` or `.calibration(servo)`, modifying its pulses or values, then applying the modified calibration back onto to the servo. + +Below, an angular servo is modified to increase its reported rotation range from 180 degrees to 270 degrees. +```python +wide_angle = Servo(servo2040.SERVO_1) +cal = wide_angle.calibration() +cal.first_value(-135) +cal.last_value(+135) +wide_angle.calibration(cal) +``` + + +### Movement Limits + +As well as providing a mapping between pulses and values, the calibration class also limits a servo from going beyond its minimum and maximum values. In some cases this may not be wanted, so the state of the limits can be modified by calling `.limit_to_calibration(lower, upper)` where `lower` and `upper` are booleans. Additionally, the current state of these limits can be queried by calling `.has_lower_limit()` and `.has_upper_limit()`, respectively. + +A case where you may want to disable limits is if you want a servo to go to a value (e.g. 90 degrees), but are not physically able to get a pulse measurement for that but can do another value instead (e.g. 60 degrees). + +Note, even with limits disables, servos still have hard limits of `400` and `2600` microsecond pulse widths. These are intended to protect servos from receiving pulses that are too far beyond their expected range. These can vary from servo to servo though, with some hitting a physical end-stop before getting to the typical `500` and `2500` associated with -90 and +90 degrees. + + +### Populating a Calibration + +To aid in populating a `Calibration` class, there are five helper functions that fill the class with pulse-value pairs: +* `apply_blank_pairs(size)` - Fills the calibration with the specified number of zero pairs +* `apply_two_pairs(min_pulse, max_pulse, min_value, max_value)` - Fills the calibration with two pairs using the min and max numbers provided +* `apply_three_pairs(min_pulse, mid_pulse, max_pulse, min_value, mid_value, max_value)` - Fills the calibration with three pairs using the min, mid and max numbers provided +* `apply_uniform_pairs(size, min_pulse, max_pulse, min_value, max_value)` - Fills the calibration with the specified number of pairs, interpolated from the min to max numbers provided +* `apply_default_pairs(type)` - Fills the calibration with the pairs of one of the common types + +Once a `Calibration` class contains pairs (as checked `.size() > 0`), these can then be accessed by calling `.pair(index)` and can then be modified by calling `.pair(index, pair)`. The former function returns a list containing the pulse and value of the pair, and the latter accepts a list or tuple containing the pulse and value. For situations when only a single element of each pair is needed, `.pulse(index)` and `.value(index)` will return the current numbers, and `.pulse(index, pulse)` and `.value(index, value)` will override them. + +For further convenience there are functions for accessing and modifying the `.first()` and `.last()` pair/pulse/value of the calibration. + + +### Viewing the Mapping + +To aid in visualising a calibration's pulse-value mapping, the pulse for any given value can be queried by calling `.value_to_pulse(value)`. Similarly, the value for any given pulse can be queried by calling `.pulse_to_value(pulse)`. These are the same functions used by `Servo` and `ServoCluster` when controlling their servos. + + +### Function Reference + +Here is the complete list of functions available on the `Calibration` class: +```python +Calibration() +Calibration(type) +apply_blank_pairs(size) +apply_two_pairs(min_pulse, max_pulse, min_value, max_value) +apply_three_pairs(min_pulse, mid_pulse, max_pulse, min_value, mid_value, max_value) +apply_uniform_pairs(size, min_pulse, max_pulse, min_value, max_value) +apply_default_pairs(type) +size() +pair(index) +pair(index, pair) +pulse(index) +pulse(index, pulse) +value(index) +value(index, value) +first() +first(pair) +first_pulse() +first_pulse(pulse) +first_value() +first_value(value) +last() +last(pair) +last_pulse() +last_pulse(pulse) +last_value() +last_value(value) +has_lower_limit() +has_upper_limit() +limit_to_calibration(lower, upper) +value_to_pulse(value) +pulse_to_value(pulse) +``` diff --git a/micropython/modules/motor/micropython.cmake b/micropython/modules/motor/micropython.cmake new file mode 100644 index 00000000..80fb2aea --- /dev/null +++ b/micropython/modules/motor/micropython.cmake @@ -0,0 +1,27 @@ +set(MOD_NAME motor) +string(TOUPPER ${MOD_NAME} MOD_NAME_UPPER) +add_library(usermod_${MOD_NAME} INTERFACE) + +target_sources(usermod_${MOD_NAME} INTERFACE + ${CMAKE_CURRENT_LIST_DIR}/${MOD_NAME}.c + ${CMAKE_CURRENT_LIST_DIR}/${MOD_NAME}.cpp + ${CMAKE_CURRENT_LIST_DIR}/../../../drivers/pwm/pwm.cpp + ${CMAKE_CURRENT_LIST_DIR}/../../../drivers/pwm/pwm_cluster.cpp + ${CMAKE_CURRENT_LIST_DIR}/../../../drivers/motor/motor2.cpp + ${CMAKE_CURRENT_LIST_DIR}/../../../drivers/motor/motor_cluster.cpp + ${CMAKE_CURRENT_LIST_DIR}/../../../drivers/motor/motor_state.cpp +) +# pico_generate_pio_header(usermod_${MOD_NAME} ${CMAKE_CURRENT_LIST_DIR}/../../../drivers/pwm/pwm_cluster.pio) + +target_include_directories(usermod_${MOD_NAME} INTERFACE + ${CMAKE_CURRENT_LIST_DIR} + ${CMAKE_CURRENT_LIST_DIR}/../../../drivers/pwm/ + ${CMAKE_CURRENT_LIST_DIR}/../../../drivers/motor/ +) + +target_compile_definitions(usermod_${MOD_NAME} INTERFACE + MODULE_PWM_ENABLED=1 + MODULE_MOTOR_ENABLED=1 +) + +target_link_libraries(usermod INTERFACE usermod_${MOD_NAME}) \ No newline at end of file diff --git a/micropython/modules/motor/motor.c b/micropython/modules/motor/motor.c new file mode 100644 index 00000000..f5d34174 --- /dev/null +++ b/micropython/modules/motor/motor.c @@ -0,0 +1,263 @@ +#include "motor.h" + +/***** Methods *****/ +MP_DEFINE_CONST_FUN_OBJ_1(Motor___del___obj, Motor___del__); +MP_DEFINE_CONST_FUN_OBJ_1(Motor_pins_obj, Motor_pins); +MP_DEFINE_CONST_FUN_OBJ_1(Motor_enable_obj, Motor_enable); +MP_DEFINE_CONST_FUN_OBJ_1(Motor_disable_obj, Motor_disable); +MP_DEFINE_CONST_FUN_OBJ_1(Motor_is_enabled_obj, Motor_is_enabled); +MP_DEFINE_CONST_FUN_OBJ_KW(Motor_duty_obj, 1, Motor_duty); +MP_DEFINE_CONST_FUN_OBJ_KW(Motor_speed_obj, 1, Motor_speed); +MP_DEFINE_CONST_FUN_OBJ_KW(Motor_frequency_obj, 1, Motor_frequency); +MP_DEFINE_CONST_FUN_OBJ_1(Motor_stop_obj, Motor_stop); +MP_DEFINE_CONST_FUN_OBJ_1(Motor_coast_obj, Motor_coast); +MP_DEFINE_CONST_FUN_OBJ_1(Motor_full_negative_obj, Motor_full_negative); +MP_DEFINE_CONST_FUN_OBJ_1(Motor_full_positive_obj, Motor_full_positive); +MP_DEFINE_CONST_FUN_OBJ_KW(Motor_to_percent_obj, 2, Motor_to_percent); +MP_DEFINE_CONST_FUN_OBJ_KW(Motor_direction_obj, 1, Motor_direction); +MP_DEFINE_CONST_FUN_OBJ_KW(Motor_speed_scale_obj, 1, Motor_speed_scale); +MP_DEFINE_CONST_FUN_OBJ_KW(Motor_deadzone_percent_obj, 1, Motor_deadzone_percent); +MP_DEFINE_CONST_FUN_OBJ_KW(Motor_decay_mode_obj, 1, Motor_decay_mode); + +MP_DEFINE_CONST_FUN_OBJ_1(MotorCluster___del___obj, MotorCluster___del__); +MP_DEFINE_CONST_FUN_OBJ_1(MotorCluster_count_obj, MotorCluster_count); +MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_pins_obj, 2, MotorCluster_pins); +MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_enable_obj, 2, MotorCluster_enable); +MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_enable_all_obj, 1, MotorCluster_enable_all); +MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_disable_obj, 2, MotorCluster_disable); +MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_disable_all_obj, 1, MotorCluster_disable_all); +MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_is_enabled_obj, 2, MotorCluster_is_enabled); +MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_duty_obj, 2, MotorCluster_duty); +MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_all_to_duty_obj, 1, MotorCluster_all_to_duty); +MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_speed_obj, 2, MotorCluster_speed); +MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_all_to_speed_obj, 1, MotorCluster_all_to_speed); +MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_phase_obj, 2, MotorCluster_phase); +MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_all_to_phase_obj, 1, MotorCluster_all_to_phase); +MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_frequency_obj, 1, MotorCluster_frequency); +MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_stop_obj, 2, MotorCluster_stop); +MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_stop_all_obj, 1, MotorCluster_stop_all); +MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_coast_obj, 2, MotorCluster_coast); +MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_coast_all_obj, 1, MotorCluster_coast_all); +MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_full_negative_obj, 2, MotorCluster_full_negative); +MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_all_to_full_negative_obj, 1, MotorCluster_all_to_full_negative); +MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_full_positive_obj, 2, MotorCluster_full_positive); +MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_all_to_full_positive_obj, 1, MotorCluster_all_to_full_positive); +MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_to_percent_obj, 3, MotorCluster_to_percent); +MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_all_to_percent_obj, 2, MotorCluster_all_to_percent); +MP_DEFINE_CONST_FUN_OBJ_1(MotorCluster_load_obj, MotorCluster_load); +//MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_direction_obj, 2, MotorCluster_direction); +//MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_all_directions_obj, 1, MotorCluster_all_directions); +//MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_speed_scale_obj, 2, MotorCluster_speed_scale); +//MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_all_speed_scales_obj, 1, MotorCluster_all_speed_scales); +//MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_deadzone_percent_obj, 2, MotorCluster_deadzone_percent); +//MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_all_deadzone_percents_obj, 1, MotorCluster_all_deadzone_percents); +//MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_decay_mode_obj, 2, MotorCluster_decay_mode); +//MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_all_decay_modes_obj, 1, MotorCluster_all_decay_modes); + +/***** Binding of Methods *****/ +STATIC const mp_rom_map_elem_t Motor_locals_dict_table[] = { + { MP_ROM_QSTR(MP_QSTR___del__), MP_ROM_PTR(&Motor___del___obj) }, + { MP_ROM_QSTR(MP_QSTR_pins), MP_ROM_PTR(&Motor_pins_obj) }, + { MP_ROM_QSTR(MP_QSTR_enable), MP_ROM_PTR(&Motor_enable_obj) }, + { MP_ROM_QSTR(MP_QSTR_disable), MP_ROM_PTR(&Motor_disable_obj) }, + { MP_ROM_QSTR(MP_QSTR_is_enabled), MP_ROM_PTR(&Motor_is_enabled_obj) }, + { MP_ROM_QSTR(MP_QSTR_duty), MP_ROM_PTR(&Motor_duty_obj) }, + { MP_ROM_QSTR(MP_QSTR_speed), MP_ROM_PTR(&Motor_speed_obj) }, + { MP_ROM_QSTR(MP_QSTR_frequency), MP_ROM_PTR(&Motor_frequency_obj) }, + { MP_ROM_QSTR(MP_QSTR_stop), MP_ROM_PTR(&Motor_stop_obj) }, + { MP_ROM_QSTR(MP_QSTR_coast), MP_ROM_PTR(&Motor_coast_obj) }, + { MP_ROM_QSTR(MP_QSTR_full_negative), MP_ROM_PTR(&Motor_full_negative_obj) }, + { MP_ROM_QSTR(MP_QSTR_full_positive), MP_ROM_PTR(&Motor_full_positive_obj) }, + { MP_ROM_QSTR(MP_QSTR_to_percent), MP_ROM_PTR(&Motor_to_percent_obj) }, + { MP_ROM_QSTR(MP_QSTR_direction), MP_ROM_PTR(&Motor_direction_obj) }, + { MP_ROM_QSTR(MP_QSTR_speed_scale), MP_ROM_PTR(&Motor_speed_scale_obj) }, + { MP_ROM_QSTR(MP_QSTR_deadzone_percent), MP_ROM_PTR(&Motor_deadzone_percent_obj) }, + { MP_ROM_QSTR(MP_QSTR_decay_mode), MP_ROM_PTR(&Motor_decay_mode_obj) }, +}; + +STATIC const mp_rom_map_elem_t MotorCluster_locals_dict_table[] = { + { MP_ROM_QSTR(MP_QSTR___del__), MP_ROM_PTR(&MotorCluster___del___obj) }, + { MP_ROM_QSTR(MP_QSTR_count), MP_ROM_PTR(&MotorCluster_count_obj) }, + { MP_ROM_QSTR(MP_QSTR_pins), MP_ROM_PTR(&MotorCluster_pins_obj) }, + { MP_ROM_QSTR(MP_QSTR_enable), MP_ROM_PTR(&MotorCluster_enable_obj) }, + { MP_ROM_QSTR(MP_QSTR_enable_all), MP_ROM_PTR(&MotorCluster_enable_all_obj) }, + { MP_ROM_QSTR(MP_QSTR_disable), MP_ROM_PTR(&MotorCluster_disable_obj) }, + { MP_ROM_QSTR(MP_QSTR_disable_all), MP_ROM_PTR(&MotorCluster_disable_all_obj) }, + { MP_ROM_QSTR(MP_QSTR_is_enabled), MP_ROM_PTR(&MotorCluster_is_enabled_obj) }, + { MP_ROM_QSTR(MP_QSTR_duty), MP_ROM_PTR(&MotorCluster_duty_obj) }, + { MP_ROM_QSTR(MP_QSTR_all_to_duty), MP_ROM_PTR(&MotorCluster_all_to_duty_obj) }, + { MP_ROM_QSTR(MP_QSTR_speed), MP_ROM_PTR(&MotorCluster_speed_obj) }, + { MP_ROM_QSTR(MP_QSTR_all_to_speed), MP_ROM_PTR(&MotorCluster_all_to_speed_obj) }, + { MP_ROM_QSTR(MP_QSTR_phase), MP_ROM_PTR(&MotorCluster_phase_obj) }, + { MP_ROM_QSTR(MP_QSTR_all_to_phase), MP_ROM_PTR(&MotorCluster_all_to_phase_obj) }, + { MP_ROM_QSTR(MP_QSTR_frequency), MP_ROM_PTR(&MotorCluster_frequency_obj) }, + { MP_ROM_QSTR(MP_QSTR_stop), MP_ROM_PTR(&MotorCluster_stop_obj) }, + { MP_ROM_QSTR(MP_QSTR_stop_all), MP_ROM_PTR(&MotorCluster_stop_all_obj) }, + { MP_ROM_QSTR(MP_QSTR_coast), MP_ROM_PTR(&MotorCluster_coast_obj) }, + { MP_ROM_QSTR(MP_QSTR_coast_all), MP_ROM_PTR(&MotorCluster_coast_all_obj) }, + { MP_ROM_QSTR(MP_QSTR_full_negative), MP_ROM_PTR(&MotorCluster_full_negative_obj) }, + { MP_ROM_QSTR(MP_QSTR_all_to_full_negative), MP_ROM_PTR(&MotorCluster_all_to_full_negative_obj) }, + { MP_ROM_QSTR(MP_QSTR_full_positive), MP_ROM_PTR(&MotorCluster_full_positive_obj) }, + { MP_ROM_QSTR(MP_QSTR_all_to_full_positive), MP_ROM_PTR(&MotorCluster_all_to_full_positive_obj) }, + { MP_ROM_QSTR(MP_QSTR_to_percent), MP_ROM_PTR(&MotorCluster_to_percent_obj) }, + { MP_ROM_QSTR(MP_QSTR_all_to_percent), MP_ROM_PTR(&MotorCluster_all_to_percent_obj) }, + { MP_ROM_QSTR(MP_QSTR_load), MP_ROM_PTR(&MotorCluster_load_obj) }, + //{ MP_ROM_QSTR(MP_QSTR_direction), MP_ROM_PTR(&MotorCluster_direction_obj) }, + //{ MP_ROM_QSTR(MP_QSTR_all_directions), MP_ROM_PTR(&MotorCluster_direction_obj) }, + //{ MP_ROM_QSTR(MP_QSTR_speed_scale), MP_ROM_PTR(&MotorCluster_speed_scale_obj) }, + //{ MP_ROM_QSTR(MP_QSTR_all_speed_scales), MP_ROM_PTR(&MotorCluster_all_speed_scales_obj) }, + //{ MP_ROM_QSTR(MP_QSTR_deadzone_percent), MP_ROM_PTR(&MotorCluster_deadzone_percent_obj) }, + //{ MP_ROM_QSTR(MP_QSTR_all_deadzone_percents), MP_ROM_PTR(&MotorCluster_all_deadzone_percents_obj) }, + //{ MP_ROM_QSTR(MP_QSTR_decay_mode), MP_ROM_PTR(&MotorCluster_decay_mode_obj) }, + //{ MP_ROM_QSTR(MP_QSTR_all_decay_modes), MP_ROM_PTR(&MotorCluster_all_decay_modes_obj) }, +}; + +STATIC MP_DEFINE_CONST_DICT(Motor_locals_dict, Motor_locals_dict_table); +STATIC MP_DEFINE_CONST_DICT(MotorCluster_locals_dict, MotorCluster_locals_dict_table); + +/***** Class Definition *****/ +const mp_obj_type_t Motor_type = { + { &mp_type_type }, + .name = MP_QSTR_motor, + .print = Motor_print, + .make_new = Motor_make_new, + .locals_dict = (mp_obj_dict_t*)&Motor_locals_dict, +}; + +const mp_obj_type_t MotorCluster_type = { + { &mp_type_type }, + .name = MP_QSTR_motor_cluster, + .print = MotorCluster_print, + .make_new = MotorCluster_make_new, + .locals_dict = (mp_obj_dict_t*)&MotorCluster_locals_dict, +}; + + +/***** Module Constants *****/ +const mp_rom_obj_tuple_t pico_motor_shim_motor1_pins = { + {&mp_type_tuple}, 2, { MP_ROM_INT(6), MP_ROM_INT(7), }, +}; +const mp_rom_obj_tuple_t pico_motor_shim_motor2_pins = { + {&mp_type_tuple}, 2, { MP_ROM_INT(27), MP_ROM_INT(26), }, +}; + +const mp_rom_obj_tuple_t motor2040_motor1_pins = { + {&mp_type_tuple}, 2, { MP_ROM_INT(4), MP_ROM_INT(5) }, +}; +const mp_rom_obj_tuple_t motor2040_motor2_pins = { + {&mp_type_tuple}, 2, { MP_ROM_INT(6), MP_ROM_INT(7) }, +}; +const mp_rom_obj_tuple_t motor2040_motor3_pins = { + {&mp_type_tuple}, 2, { MP_ROM_INT(8), MP_ROM_INT(9) }, +}; +const mp_rom_obj_tuple_t motor2040_motor4_pins = { + {&mp_type_tuple}, 2, { MP_ROM_INT(10), MP_ROM_INT(11) }, +}; +const mp_rom_obj_tuple_t motor2040_encoder1_pins = { + {&mp_type_tuple}, 2, { MP_ROM_INT(0), MP_ROM_INT(1) }, +}; +const mp_rom_obj_tuple_t motor2040_encoder2_pins = { + {&mp_type_tuple}, 2, { MP_ROM_INT(2), MP_ROM_INT(3) }, +}; +const mp_rom_obj_tuple_t motor2040_encoder3_pins = { + {&mp_type_tuple}, 2, { MP_ROM_INT(12), MP_ROM_INT(13) }, +}; +const mp_rom_obj_tuple_t motor2040_encoder4_pins = { + {&mp_type_tuple}, 2, { MP_ROM_INT(14), MP_ROM_INT(15) }, +}; + +typedef struct _mp_obj_float_t { + mp_obj_base_t base; + mp_float_t value; +} mp_obj_float_t; +mp_obj_float_t motor2040_shunt_resistor = {{&mp_type_float}, 0.47f}; +mp_obj_float_t motor2040_voltage_gain = {{&mp_type_float}, 3.9f / 13.9f}; +mp_obj_float_t motor2040_current_offset = {{&mp_type_float}, -0.02f}; + + +/***** Globals Table *****/ +STATIC const mp_rom_map_elem_t pico_motor_shim_globals_table[] = { + { MP_OBJ_NEW_QSTR(MP_QSTR___name__), MP_OBJ_NEW_QSTR(MP_QSTR_pico_motor_shim) }, + { MP_ROM_QSTR(MP_QSTR_BUTTON_A), MP_ROM_INT(2) }, + { MP_ROM_QSTR(MP_QSTR_MOTOR_1), MP_ROM_PTR(&pico_motor_shim_motor1_pins) }, + { MP_ROM_QSTR(MP_QSTR_MOTOR_2), MP_ROM_PTR(&pico_motor_shim_motor2_pins) }, + { MP_ROM_QSTR(MP_QSTR_NUM_MOTORS), MP_ROM_INT(2) }, + { MP_ROM_QSTR(MP_QSTR_SDA), MP_ROM_INT(4) }, + { MP_ROM_QSTR(MP_QSTR_SCL), MP_ROM_INT(5) }, +}; + +STATIC const mp_rom_map_elem_t motor2040_globals_table[] = { + { MP_OBJ_NEW_QSTR(MP_QSTR___name__), MP_OBJ_NEW_QSTR(MP_QSTR_motor2040) }, + { MP_ROM_QSTR(MP_QSTR_MOTOR_1), MP_ROM_PTR(&motor2040_motor1_pins) }, + { MP_ROM_QSTR(MP_QSTR_MOTOR_2), MP_ROM_PTR(&motor2040_motor2_pins) }, + { MP_ROM_QSTR(MP_QSTR_MOTOR_3), MP_ROM_PTR(&motor2040_motor3_pins) }, + { MP_ROM_QSTR(MP_QSTR_MOTOR_4), MP_ROM_PTR(&motor2040_motor4_pins) }, + { MP_ROM_QSTR(MP_QSTR_NUM_MOTORS), MP_ROM_INT(4) }, + { MP_ROM_QSTR(MP_QSTR_ENCODER_1), MP_ROM_PTR(&motor2040_encoder1_pins) }, + { MP_ROM_QSTR(MP_QSTR_ENCODER_2), MP_ROM_PTR(&motor2040_encoder2_pins) }, + { MP_ROM_QSTR(MP_QSTR_ENCODER_3), MP_ROM_PTR(&motor2040_encoder3_pins) }, + { MP_ROM_QSTR(MP_QSTR_ENCODER_4), MP_ROM_PTR(&motor2040_encoder4_pins) }, + { MP_ROM_QSTR(MP_QSTR_NUM_ENCODERS), MP_ROM_INT(4) }, + { MP_ROM_QSTR(MP_QSTR_TX_TRIG), MP_ROM_INT(16) }, + { MP_ROM_QSTR(MP_QSTR_RX_ECHO), MP_ROM_INT(17) }, + { MP_ROM_QSTR(MP_QSTR_LED_DATA), MP_ROM_INT(18) }, + { MP_ROM_QSTR(MP_QSTR_NUM_LEDS), MP_ROM_INT(6) }, + { MP_ROM_QSTR(MP_QSTR_INT), MP_ROM_INT(19) }, + { MP_ROM_QSTR(MP_QSTR_SDA), MP_ROM_INT(20) }, + { MP_ROM_QSTR(MP_QSTR_SCL), MP_ROM_INT(21) }, + { MP_ROM_QSTR(MP_QSTR_USER_SW), MP_ROM_INT(23) }, + { MP_ROM_QSTR(MP_QSTR_ADC_ADDR_0), MP_ROM_INT(22) }, + { MP_ROM_QSTR(MP_QSTR_ADC_ADDR_1), MP_ROM_INT(24) }, + { MP_ROM_QSTR(MP_QSTR_ADC_ADDR_2), MP_ROM_INT(25) }, + { MP_ROM_QSTR(MP_QSTR_ADC0), MP_ROM_INT(26) }, + { MP_ROM_QSTR(MP_QSTR_ADC1), MP_ROM_INT(27) }, + { MP_ROM_QSTR(MP_QSTR_ADC2), MP_ROM_INT(28) }, + { MP_ROM_QSTR(MP_QSTR_SHARED_ADC), MP_ROM_INT(29) }, + { MP_ROM_QSTR(MP_QSTR_CURRENT_SENSE_1_ADDR), MP_ROM_INT(0b000) }, + { MP_ROM_QSTR(MP_QSTR_CURRENT_SENSE_2_ADDR), MP_ROM_INT(0b001) }, + { MP_ROM_QSTR(MP_QSTR_CURRENT_SENSE_3_ADDR), MP_ROM_INT(0b010) }, + { MP_ROM_QSTR(MP_QSTR_CURRENT_SENSE_4_ADDR), MP_ROM_INT(0b011) }, + { MP_ROM_QSTR(MP_QSTR_VOLTAGE_SENSE_ADDR), MP_ROM_INT(0b100) }, + { MP_ROM_QSTR(MP_QSTR_FAULT_SENSE_ADDR), MP_ROM_INT(0b101) }, + { MP_ROM_QSTR(MP_QSTR_SENSOR_1_ADDR), MP_ROM_INT(0b110) }, + { MP_ROM_QSTR(MP_QSTR_SENSOR_2_ADDR), MP_ROM_INT(0b111) }, + { MP_ROM_QSTR(MP_QSTR_NUM_SENSORS), MP_ROM_INT(2) }, + { MP_ROM_QSTR(MP_QSTR_SHUNT_RESISTOR), MP_ROM_PTR(&motor2040_shunt_resistor) }, + { MP_ROM_QSTR(MP_QSTR_CURRENT_GAIN), MP_ROM_INT(5) }, + { MP_ROM_QSTR(MP_QSTR_VOLTAGE_GAIN), MP_ROM_PTR(&motor2040_voltage_gain) }, + { MP_ROM_QSTR(MP_QSTR_CURRENT_OFFSET), MP_ROM_PTR(&motor2040_current_offset) }, +}; + +STATIC MP_DEFINE_CONST_DICT(mp_module_pico_motor_shim_globals, pico_motor_shim_globals_table); +STATIC MP_DEFINE_CONST_DICT(mp_module_motor2040_globals, motor2040_globals_table); + +const mp_obj_module_t pico_motor_shim_user_cmodule = { + .base = { &mp_type_module }, + .globals = (mp_obj_dict_t*)&mp_module_pico_motor_shim_globals, +}; + +const mp_obj_module_t motor2040_user_cmodule = { + .base = { &mp_type_module }, + .globals = (mp_obj_dict_t*)&mp_module_motor2040_globals, +}; + +STATIC const mp_map_elem_t motor_globals_table[] = { + { MP_OBJ_NEW_QSTR(MP_QSTR___name__), MP_OBJ_NEW_QSTR(MP_QSTR_motor) }, + { MP_OBJ_NEW_QSTR(MP_QSTR_Motor), (mp_obj_t)&Motor_type }, + { MP_OBJ_NEW_QSTR(MP_QSTR_MotorCluster), (mp_obj_t)&MotorCluster_type }, + { MP_OBJ_NEW_QSTR(MP_QSTR_pico_motor_shim), (mp_obj_t)&pico_motor_shim_user_cmodule }, + { MP_OBJ_NEW_QSTR(MP_QSTR_motor2040), (mp_obj_t)&motor2040_user_cmodule }, + + // TODO + //{ MP_ROM_QSTR(MP_QSTR_ANGULAR), MP_ROM_INT(0x00) }, + //{ MP_ROM_QSTR(MP_QSTR_LINEAR), MP_ROM_INT(0x01) }, + //{ MP_ROM_QSTR(MP_QSTR_CONTINUOUS), MP_ROM_INT(0x02) }, +}; +STATIC MP_DEFINE_CONST_DICT(mp_module_motor_globals, motor_globals_table); + +/***** Module Definition *****/ +const mp_obj_module_t motor_user_cmodule = { + .base = { &mp_type_module }, + .globals = (mp_obj_dict_t*)&mp_module_motor_globals, +}; +MP_REGISTER_MODULE(MP_QSTR_motor, motor_user_cmodule, MODULE_MOTOR_ENABLED); diff --git a/micropython/modules/motor/motor.cpp b/micropython/modules/motor/motor.cpp new file mode 100644 index 00000000..d7a7fd84 --- /dev/null +++ b/micropython/modules/motor/motor.cpp @@ -0,0 +1,1929 @@ +#include "drivers/motor/motor2.hpp" +#include "drivers/motor/motor_cluster.hpp" +#include "common/pimoroni_common.hpp" +#include + +#define MP_OBJ_TO_PTR2(o, t) ((t *)(uintptr_t)(o)) + +using namespace pimoroni; +using namespace motor; + +extern "C" { +#include "motor.h" +#include "py/builtin.h" + + +/********** Motor **********/ + +/***** Variables Struct *****/ +typedef struct _Motor_obj_t { + mp_obj_base_t base; + Motor2* motor; +} _Motor_obj_t; + + +/***** Print *****/ +void Motor_print(const mp_print_t *print, mp_obj_t self_in, mp_print_kind_t kind) { + (void)kind; //Unused input parameter + _Motor_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Motor_obj_t); + mp_print_str(print, "Motor("); + + mp_print_str(print, "pins = "); + mp_obj_print_helper(print, mp_obj_new_int(self->motor->pins().positive), PRINT_REPR); + mp_print_str(print, ", enabled = "); + mp_obj_print_helper(print, self->motor->is_enabled() ? mp_const_true : mp_const_false, PRINT_REPR); + mp_print_str(print, ", duty = "); + mp_obj_print_helper(print, mp_obj_new_float(self->motor->duty()), PRINT_REPR); + mp_print_str(print, ", speed = "); + mp_obj_print_helper(print, mp_obj_new_float(self->motor->speed()), PRINT_REPR); + mp_print_str(print, ", freq = "); + mp_obj_print_helper(print, mp_obj_new_float(self->motor->frequency()), PRINT_REPR); + + mp_print_str(print, ")"); +} + + +/***** Constructor *****/ +mp_obj_t Motor_make_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw, const mp_obj_t *all_args) { + _Motor_obj_t *self = nullptr; + + enum { ARG_pin, ARG_calibration, ARG_freq }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_pin, MP_ARG_REQUIRED | MP_ARG_INT }, + { MP_QSTR_calibration, MP_ARG_OBJ, {.u_obj = mp_const_none} }, + { MP_QSTR_freq, MP_ARG_OBJ, {.u_obj = mp_const_none} }, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all_kw_array(n_args, n_kw, all_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + int pin = args[ARG_pin].u_int; + + //motor::Calibration *calib = nullptr; + //motor::CalibrationType calibration_type = motor::CalibrationType::ANGULAR; + const mp_obj_t calib_object = args[ARG_calibration].u_obj; + if(calib_object != mp_const_none) { + /*if(mp_obj_is_int(calib_object)) { + int type = mp_obj_get_int(calib_object); + if(type < 0 || type >= 3) { + mp_raise_ValueError("type out of range. Expected ANGULAR (0), LINEAR (1) or CONTINUOUS (2)"); + } + calibration_type = (motor::CalibrationType)type; + } + else if(mp_obj_is_type(calib_object, &Calibration_type)) { + calib = (MP_OBJ_TO_PTR2(calib_object, _Calibration_obj_t)->calibration); + } + else { + mp_raise_TypeError("cannot convert object to an integer or a Calibration class instance"); + }*/ + } + + /*float freq = motor::MotorState::DEFAULT_FREQUENCY; + if(args[ARG_freq].u_obj != mp_const_none) { + freq = mp_obj_get_float(args[ARG_freq].u_obj); + }*/ + + self = m_new_obj_with_finaliser(_Motor_obj_t); + self->base.type = &Motor_type; + + //if(calib != nullptr) + // self->motor = new Motor(pin, *calib, freq); + //else + self->motor = new Motor2(pin_pair(pin, pin));//TODO, calibration_type, freq); + self->motor->init(); + + return MP_OBJ_FROM_PTR(self); +} + + +/***** Destructor ******/ +mp_obj_t Motor___del__(mp_obj_t self_in) { + _Motor_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Motor_obj_t); + delete self->motor; + return mp_const_none; +} + + +/***** Methods *****/ +extern mp_obj_t Motor_pins(mp_obj_t self_in) { + _Motor_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Motor_obj_t); + pin_pair pins = self->motor->pins(); + + mp_obj_t tuple[2]; + tuple[0] = mp_obj_new_int(pins.positive); + tuple[1] = mp_obj_new_int(pins.negative); + return mp_obj_new_tuple(2, tuple); +} + +extern mp_obj_t Motor_enable(mp_obj_t self_in) { + _Motor_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Motor_obj_t); + self->motor->enable(); + return mp_const_none; +} + +extern mp_obj_t Motor_disable(mp_obj_t self_in) { + _Motor_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Motor_obj_t); + self->motor->disable(); + return mp_const_none; +} + +extern mp_obj_t Motor_is_enabled(mp_obj_t self_in) { + _Motor_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Motor_obj_t); + return self->motor->is_enabled() ? mp_const_true : mp_const_false; +} + +extern mp_obj_t Motor_duty(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { + if(n_args <= 1) { + enum { ARG_self }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _Motor_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Motor_obj_t); + + return mp_obj_new_float(self->motor->duty()); + } + else { + enum { ARG_self, ARG_duty }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_duty, MP_ARG_REQUIRED | MP_ARG_OBJ }, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _Motor_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Motor_obj_t); + + float duty = mp_obj_get_float(args[ARG_duty].u_obj); + + self->motor->duty(duty); + return mp_const_none; + } +} + +extern mp_obj_t Motor_speed(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { + if(n_args <= 1) { + enum { ARG_self }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _Motor_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Motor_obj_t); + + return mp_obj_new_float(self->motor->speed()); + } + else { + enum { ARG_self, ARG_speed }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_speed, MP_ARG_REQUIRED | MP_ARG_OBJ }, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _Motor_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Motor_obj_t); + + float speed = mp_obj_get_float(args[ARG_speed].u_obj); + + self->motor->speed(speed); + return mp_const_none; + } +} + +extern mp_obj_t Motor_frequency(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { + if(n_args <= 1) { + enum { ARG_self }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _Motor_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Motor_obj_t); + + return mp_obj_new_float(self->motor->frequency()); + } + else { + enum { ARG_self, ARG_freq }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_freq, MP_ARG_REQUIRED | MP_ARG_OBJ }, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _Motor_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Motor_obj_t); + + float freq = mp_obj_get_float(args[ARG_freq].u_obj); + + if(!self->motor->frequency(freq)) { + mp_raise_ValueError("freq out of range. Expected 10Hz to 350Hz"); //TODO + } + return mp_const_none; + } +} + +extern mp_obj_t Motor_stop(mp_obj_t self_in) { + _Motor_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Motor_obj_t); + self->motor->stop(); + return mp_const_none; +} + +extern mp_obj_t Motor_coast(mp_obj_t self_in) { + _Motor_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Motor_obj_t); + self->motor->coast(); + return mp_const_none; +} + +extern mp_obj_t Motor_full_negative(mp_obj_t self_in) { + _Motor_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Motor_obj_t); + self->motor->full_negative(); + return mp_const_none; +} + +extern mp_obj_t Motor_full_positive(mp_obj_t self_in) { + _Motor_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Motor_obj_t); + self->motor->full_positive(); + return mp_const_none; +} + +extern mp_obj_t Motor_to_percent(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { + if(n_args <= 2) { + enum { ARG_self, ARG_in }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_in, MP_ARG_REQUIRED | MP_ARG_OBJ }, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _Motor_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Motor_obj_t); + + float in = mp_obj_get_float(args[ARG_in].u_obj); + + self->motor->to_percent(in); + } + else if(n_args <= 4) { + enum { ARG_self, ARG_in, ARG_in_min, ARG_in_max }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_in, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_in_min, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_in_max, MP_ARG_REQUIRED | MP_ARG_OBJ }, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _Motor_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Motor_obj_t); + + float in = mp_obj_get_float(args[ARG_in].u_obj); + float in_min = mp_obj_get_float(args[ARG_in_min].u_obj); + float in_max = mp_obj_get_float(args[ARG_in_max].u_obj); + + self->motor->to_percent(in, in_min, in_max); + } + else { + enum { ARG_self, ARG_in, ARG_in_min, ARG_in_max, ARG_speed_min, ARG_speed_max }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_in, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_in_min, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_in_max, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_speed_min, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_speed_max, MP_ARG_REQUIRED | MP_ARG_OBJ } + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _Motor_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Motor_obj_t); + + float in = mp_obj_get_float(args[ARG_in].u_obj); + float in_min = mp_obj_get_float(args[ARG_in_min].u_obj); + float in_max = mp_obj_get_float(args[ARG_in_max].u_obj); + float speed_min = mp_obj_get_float(args[ARG_speed_min].u_obj); + float speed_max = mp_obj_get_float(args[ARG_speed_max].u_obj); + + self->motor->to_percent(in, in_min, in_max, speed_min, speed_max); + } + + return mp_const_none; +} + +extern mp_obj_t Motor_direction(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { + if(n_args <= 1) { + enum { ARG_self }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _Motor_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Motor_obj_t); + + return mp_obj_new_int(self->motor->direction()); + } + else { + enum { ARG_self, ARG_direction }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_direction, MP_ARG_REQUIRED | MP_ARG_OBJ }, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _Motor_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Motor_obj_t); + + int direction = mp_obj_get_int(args[ARG_direction].u_obj); + + self->motor->direction((MotorState::Direction)direction); + return mp_const_none; + } +} + +extern mp_obj_t Motor_speed_scale(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { + if(n_args <= 1) { + enum { ARG_self }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _Motor_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Motor_obj_t); + + return mp_obj_new_float(self->motor->speed_scale()); + } + else { + enum { ARG_self, ARG_speed_scale }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_speed_scale, MP_ARG_REQUIRED | MP_ARG_OBJ }, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _Motor_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Motor_obj_t); + + float speed_scale = mp_obj_get_float(args[ARG_speed_scale].u_obj); + + self->motor->speed_scale(speed_scale); + return mp_const_none; + } +} + +extern mp_obj_t Motor_deadzone_percent(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { + if(n_args <= 1) { + enum { ARG_self }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _Motor_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Motor_obj_t); + + return mp_obj_new_float(self->motor->deadzone_percent()); + } + else { + enum { ARG_self, ARG_deadzone_percent }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_deadzone_percent, MP_ARG_REQUIRED | MP_ARG_OBJ }, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _Motor_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Motor_obj_t); + + float deadzone_percent = mp_obj_get_float(args[ARG_deadzone_percent].u_obj); + + self->motor->deadzone_percent(deadzone_percent); + return mp_const_none; + } +} + +extern mp_obj_t Motor_decay_mode(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { + if(n_args <= 1) { + enum { ARG_self }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _Motor_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Motor_obj_t); + + return mp_obj_new_int(self->motor->decay_mode()); + } + else { + enum { ARG_self, ARG_decay_mode }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_decay_mode, MP_ARG_REQUIRED | MP_ARG_OBJ }, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _Motor_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Motor_obj_t); + + int decay_mode = mp_obj_get_int(args[ARG_decay_mode].u_obj); + + self->motor->decay_mode((MotorState::DecayMode)decay_mode); + return mp_const_none; + } +} + + +/********** MotorCluster **********/ + +/***** Variables Struct *****/ +typedef struct _MotorCluster_obj_t { + mp_obj_base_t base; + MotorCluster* cluster; + PWMCluster::Sequence *seq_buf; + PWMCluster::TransitionData *dat_buf; +} _MotorCluster_obj_t; + + +/***** Print *****/ +void MotorCluster_print(const mp_print_t *print, mp_obj_t self_in, mp_print_kind_t kind) { + (void)kind; //Unused input parameter + _MotorCluster_obj_t *self = MP_OBJ_TO_PTR2(self_in, _MotorCluster_obj_t); + mp_print_str(print, "MotorCluster("); + + mp_print_str(print, "motors = {"); + + uint8_t motor_count = self->cluster->count(); + for(uint8_t motor = 0; motor < motor_count; motor++) { + mp_print_str(print, "\n\t{pins = "); + mp_obj_print_helper(print, mp_obj_new_int(self->cluster->pins(motor).positive), PRINT_REPR); + mp_print_str(print, ", enabled = "); + mp_obj_print_helper(print, self->cluster->is_enabled(motor) ? mp_const_true : mp_const_false, PRINT_REPR); + mp_print_str(print, ", duty = "); + mp_obj_print_helper(print, mp_obj_new_float(self->cluster->duty(motor)), PRINT_REPR); + mp_print_str(print, ", speed = "); + mp_obj_print_helper(print, mp_obj_new_float(self->cluster->speed(motor)), PRINT_REPR); + mp_print_str(print, ", phase = "); + mp_obj_print_helper(print, mp_obj_new_float(self->cluster->phase(motor)), PRINT_REPR); + mp_print_str(print, "}"); + if(motor < motor_count - 1) + mp_print_str(print, ", "); + } + if(motor_count > 0) { + mp_print_str(print, "\n"); + } + mp_print_str(print, "}, freq = "); + mp_obj_print_helper(print, mp_obj_new_float(self->cluster->frequency()), PRINT_REPR); + + mp_print_str(print, ")"); +} + + +/***** Constructor *****/ +mp_obj_t MotorCluster_make_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw, const mp_obj_t *all_args) { + _MotorCluster_obj_t *self = nullptr; + + enum { ARG_pio, ARG_sm, ARG_pins, ARG_calibration, ARG_freq, ARG_auto_phase }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_pio, MP_ARG_REQUIRED | MP_ARG_INT }, + { MP_QSTR_sm, MP_ARG_REQUIRED | MP_ARG_INT }, + { MP_QSTR_pins, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_calibration, MP_ARG_OBJ, {.u_obj = mp_const_none} }, + { MP_QSTR_freq, MP_ARG_OBJ, {.u_obj = mp_const_none} }, + { MP_QSTR_auto_phase, MP_ARG_BOOL, {.u_bool = true} }, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all_kw_array(n_args, n_kw, all_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + PIO pio = args[ARG_pio].u_int == 0 ? pio0 : pio1; + int sm = args[ARG_sm].u_int; + + //uint pin_mask = 0; + //bool mask_provided = true; + uint32_t pair_count = 0; + pin_pair* pins = nullptr; + + // Determine what pins this cluster will use + const mp_obj_t object = args[ARG_pins].u_obj; + if(mp_obj_is_int(object)) { + //pin_mask = (uint)mp_obj_get_int(object); + } + else { + mp_obj_t *items = nullptr; + if(mp_obj_is_type(object, &mp_type_list)) { + mp_obj_list_t *list = MP_OBJ_TO_PTR2(object, mp_obj_list_t); + pair_count = list->len; + items = list->items; + } + else if(mp_obj_is_type(object, &mp_type_tuple)) { + mp_obj_tuple_t *tuple = MP_OBJ_TO_PTR2(object, mp_obj_tuple_t); + pair_count = tuple->len; + items = tuple->items; + } + + if(items == nullptr) + mp_raise_TypeError("cannot convert object to a list or tuple of pins, or a pin mask integer"); + else if(pair_count == 0) + mp_raise_TypeError("list or tuple must contain at least one integer"); + else { + // Create and populate a local array of pins + pins = new pin_pair[pair_count]; + for(size_t i = 0; i < pair_count; i++) { + int pin = mp_obj_get_int(items[i]); + if(pin < 0 || pin >= (int)NUM_BANK0_GPIOS) { + delete[] pins; + mp_raise_ValueError("a pin in the list or tuple is out of range. Expected 0 to 29"); + } + else { + pins[i] = pin_pair((uint8_t)pin, (uint8_t)pin); //TODO + } + } + //mask_provided = false; + } + } + + //motor::Calibration *calib = nullptr; + //motor::CalibrationType calibration_type = motor::CalibrationType::ANGULAR; + const mp_obj_t calib_object = args[ARG_calibration].u_obj; + if(calib_object != mp_const_none) { + /*if(mp_obj_is_int(calib_object)) { + int type = mp_obj_get_int(calib_object); + if(type < 0 || type >= 3) { + mp_raise_ValueError("type out of range. Expected ANGULAR (0), LINEAR (1) or CONTINUOUS (2)"); + } + calibration_type = (motor::CalibrationType)mp_obj_get_int(calib_object); + } + else if(mp_obj_is_type(calib_object, &Calibration_type)) { + calib = (MP_OBJ_TO_PTR2(calib_object, _Calibration_obj_t)->calibration); + } + else { + mp_raise_TypeError("cannot convert object to an integer or a Calibration class instance"); + }*/ + } + + float freq = motor::MotorState::DEFAULT_FREQUENCY; + if(args[ARG_freq].u_obj != mp_const_none) { + freq = mp_obj_get_float(args[ARG_freq].u_obj); + } + + bool auto_phase = args[ARG_auto_phase].u_bool; + + MotorCluster *cluster; + PWMCluster::Sequence *seq_buffer = m_new(PWMCluster::Sequence, PWMCluster::NUM_BUFFERS * 2); + PWMCluster::TransitionData *dat_buffer = m_new(PWMCluster::TransitionData, PWMCluster::BUFFER_SIZE * 2); + //if(calib != nullptr) + //cluster = new MotorCluster(pio, sm, pins, pair_count, *calib, freq, auto_phase, seq_buffer, dat_buffer); + //else + cluster = new MotorCluster(pio, sm, pins, pair_count, MotorState::NORMAL, 1.0f, 0.0f, freq, MotorState::SLOW_DECAY, auto_phase, seq_buffer, dat_buffer); + + // Cleanup the pins array + if(pins != nullptr) + delete[] pins; + + if(!cluster->init()) { + delete cluster; + m_del(PWMCluster::Sequence, seq_buffer, PWMCluster::NUM_BUFFERS * 2); + m_del(PWMCluster::TransitionData, dat_buffer, PWMCluster::BUFFER_SIZE * 2); + mp_raise_msg(&mp_type_RuntimeError, "unable to allocate the hardware resources needed to initialise this MotorCluster. Try running `import gc` followed by `gc.collect()` before creating it"); + } + + self = m_new_obj_with_finaliser(_MotorCluster_obj_t); + self->base.type = &MotorCluster_type; + self->cluster = cluster; + self->seq_buf = seq_buffer; + self->dat_buf = dat_buffer; + + return MP_OBJ_FROM_PTR(self); +} + + +/***** Destructor ******/ +mp_obj_t MotorCluster___del__(mp_obj_t self_in) { + _MotorCluster_obj_t *self = MP_OBJ_TO_PTR2(self_in, _MotorCluster_obj_t); + delete self->cluster; + return mp_const_none; +} + + +/***** Methods *****/ +extern mp_obj_t MotorCluster_count(mp_obj_t self_in) { + _MotorCluster_obj_t *self = MP_OBJ_TO_PTR2(self_in, _MotorCluster_obj_t); + return mp_obj_new_int(self->cluster->count()); +} + +extern mp_obj_t MotorCluster_pins(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { + enum { ARG_self, ARG_motor }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_motor, MP_ARG_REQUIRED | MP_ARG_INT }, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _MotorCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _MotorCluster_obj_t); + + int motor = args[ARG_motor].u_int; + int motor_count = (int)self->cluster->count(); + if(motor_count == 0) + mp_raise_ValueError("this cluster does not have any motors"); + else if(motor < 0 || motor >= motor_count) + mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("motor out of range. Expected 0 to %d"), motor_count - 1); + else + return mp_obj_new_int(self->cluster->pins((uint)motor).positive); + + return mp_const_none; +} + +extern mp_obj_t MotorCluster_enable(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { + enum { ARG_self, ARG_motors, ARG_load }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_motors, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_load, MP_ARG_BOOL, { .u_bool = true }}, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _MotorCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _MotorCluster_obj_t); + + int motor_count = (int)self->cluster->count(); + if(motor_count == 0) + mp_raise_ValueError("this cluster does not have any motors"); + else { + // Determine what motor(s) to enable + const mp_obj_t object = args[ARG_motors].u_obj; + if(mp_obj_is_int(object)) { + int motor = mp_obj_get_int(object); + if(motor < 0 || motor >= motor_count) + mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("motor out of range. Expected 0 to %d"), motor_count - 1); + else + self->cluster->enable((uint)motor, args[ARG_load].u_bool); + } + else { + size_t length = 0; + mp_obj_t *items = nullptr; + if(mp_obj_is_type(object, &mp_type_list)) { + mp_obj_list_t *list = MP_OBJ_TO_PTR2(object, mp_obj_list_t); + length = list->len; + items = list->items; + } + else if(mp_obj_is_type(object, &mp_type_tuple)) { + mp_obj_tuple_t *tuple = MP_OBJ_TO_PTR2(object, mp_obj_tuple_t); + length = tuple->len; + items = tuple->items; + } + + if(items == nullptr) + mp_raise_TypeError("cannot convert object to a list or tuple of integers, or a single integer"); + else if(length == 0) + mp_raise_TypeError("list or tuple must contain at least one integer"); + else { + // Create and populate a local array of motor indices + uint8_t *motors = new uint8_t[length]; + for(size_t i = 0; i < length; i++) { + int motor = mp_obj_get_int(items[i]); + if(motor < 0 || motor >= motor_count) { + delete[] motors; + mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("a motor in the list or tuple is out of range. Expected 0 to %d"), motor_count - 1); + } + else { + motors[i] = (uint8_t)motor; + } + } + self->cluster->enable(motors, length, args[ARG_load].u_bool); + delete[] motors; + } + } + } + + return mp_const_none; +} + +extern mp_obj_t MotorCluster_enable_all(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { + enum { ARG_self, ARG_load }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_load, MP_ARG_BOOL, { .u_bool = true }}, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _MotorCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _MotorCluster_obj_t); + self->cluster->enable_all(args[ARG_load].u_bool); + + return mp_const_none; +} + +extern mp_obj_t MotorCluster_disable(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { + enum { ARG_self, ARG_motors, ARG_load }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_motors, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_load, MP_ARG_BOOL, { .u_bool = true }}, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _MotorCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _MotorCluster_obj_t); + + int motor_count = (int)self->cluster->count(); + if(motor_count == 0) + mp_raise_ValueError("this cluster does not have any motors"); + else { + // Determine what motor(s) to disable + const mp_obj_t object = args[ARG_motors].u_obj; + if(mp_obj_is_int(object)) { + int motor = mp_obj_get_int(object); + if(motor < 0 || motor >= motor_count) + mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("motor out of range. Expected 0 to %d"), motor_count - 1); + else + self->cluster->disable((uint)motor, args[ARG_load].u_bool); + } + else { + size_t length = 0; + mp_obj_t *items = nullptr; + if(mp_obj_is_type(object, &mp_type_list)) { + mp_obj_list_t *list = MP_OBJ_TO_PTR2(object, mp_obj_list_t); + length = list->len; + items = list->items; + } + else if(mp_obj_is_type(object, &mp_type_tuple)) { + mp_obj_tuple_t *tuple = MP_OBJ_TO_PTR2(object, mp_obj_tuple_t); + length = tuple->len; + items = tuple->items; + } + + if(items == nullptr) + mp_raise_TypeError("cannot convert object to a list or tuple of integers, or a single integer"); + else if(length == 0) + mp_raise_TypeError("list or tuple must contain at least one integer"); + else { + // Create and populate a local array of motor indices + uint8_t *motors = new uint8_t[length]; + for(size_t i = 0; i < length; i++) { + int motor = mp_obj_get_int(items[i]); + if(motor < 0 || motor >= motor_count) { + delete[] motors; + mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("a motor in the list or tuple is out of range. Expected 0 to %d"), motor_count - 1); + } + else { + motors[i] = (uint8_t)motor; + } + } + self->cluster->disable(motors, length, args[ARG_load].u_bool); + delete[] motors; + } + } + } + + return mp_const_none; +} + +extern mp_obj_t MotorCluster_disable_all(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { + enum { ARG_self, ARG_load }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_load, MP_ARG_BOOL, { .u_bool = true }}, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _MotorCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _MotorCluster_obj_t); + self->cluster->disable_all(args[ARG_load].u_bool); + + return mp_const_none; +} + +extern mp_obj_t MotorCluster_is_enabled(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { + enum { ARG_self, ARG_motor }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_motor, MP_ARG_REQUIRED | MP_ARG_INT }, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _MotorCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _MotorCluster_obj_t); + + int motor = args[ARG_motor].u_int; + int motor_count = (int)self->cluster->count(); + if(motor_count == 0) + mp_raise_ValueError("this cluster does not have any motors"); + else if(motor < 0 || motor >= motor_count) + mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("motor out of range. Expected 0 to %d"), motor_count - 1); + else + return self->cluster->is_enabled((uint)motor) ? mp_const_true : mp_const_false; + + return mp_const_none; +} + +extern mp_obj_t MotorCluster_duty(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { + if(n_args <= 2) { + enum { ARG_self, ARG_motor }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_motor, MP_ARG_REQUIRED | MP_ARG_INT }, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _MotorCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _MotorCluster_obj_t); + + int motor = args[ARG_motor].u_int; + int motor_count = (int)self->cluster->count(); + if(motor_count == 0) + mp_raise_ValueError("this cluster does not have any motors"); + else if(motor < 0 || motor >= motor_count) + mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("motor out of range. Expected 0 to %d"), motor_count - 1); + else + return mp_obj_new_float(self->cluster->duty((uint)motor)); + } + else { + enum { ARG_self, ARG_motors, ARG_duty, ARG_load }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_motors, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_duty, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_load, MP_ARG_BOOL, { .u_bool = true }}, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _MotorCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _MotorCluster_obj_t); + + int motor_count = (int)self->cluster->count(); + if(motor_count == 0) + mp_raise_ValueError("this cluster does not have any motors"); + else { + // Determine what motor(s) to enable + const mp_obj_t object = args[ARG_motors].u_obj; + if(mp_obj_is_int(object)) { + int motor = mp_obj_get_int(object); + if(motor < 0 || motor >= motor_count) + mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("motor out of range. Expected 0 to %d"), motor_count - 1); + else { + float duty = mp_obj_get_float(args[ARG_duty].u_obj); + self->cluster->duty((uint)motor, duty, args[ARG_load].u_bool); + } + } + else { + size_t length = 0; + mp_obj_t *items = nullptr; + if(mp_obj_is_type(object, &mp_type_list)) { + mp_obj_list_t *list = MP_OBJ_TO_PTR2(object, mp_obj_list_t); + length = list->len; + items = list->items; + } + else if(mp_obj_is_type(object, &mp_type_tuple)) { + mp_obj_tuple_t *tuple = MP_OBJ_TO_PTR2(object, mp_obj_tuple_t); + length = tuple->len; + items = tuple->items; + } + + if(items == nullptr) + mp_raise_TypeError("cannot convert object to a list or tuple of integers, or a single integer"); + else if(length == 0) + mp_raise_TypeError("list or tuple must contain at least one integer"); + else { + // Create and populate a local array of motor indices + uint8_t *motors = new uint8_t[length]; + for(size_t i = 0; i < length; i++) { + int motor = mp_obj_get_int(items[i]); + if(motor < 0 || motor >= motor_count) { + delete[] motors; + mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("a motor in the list or tuple is out of range. Expected 0 to %d"), motor_count - 1); + } + else { + motors[i] = (uint8_t)motor; + } + } + float duty = mp_obj_get_float(args[ARG_duty].u_obj); + self->cluster->duty(motors, length, duty, args[ARG_load].u_bool); + delete[] motors; + } + } + } + } + return mp_const_none; +} + +extern mp_obj_t MotorCluster_all_to_duty(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { + enum { ARG_self, ARG_duty, ARG_load }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_duty, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_load, MP_ARG_BOOL, { .u_bool = true }}, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _MotorCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _MotorCluster_obj_t); + + int motor_count = (int)self->cluster->count(); + if(motor_count == 0) + mp_raise_ValueError("this cluster does not have any motors"); + else { + float duty = mp_obj_get_float(args[ARG_duty].u_obj); + self->cluster->all_to_duty(duty, args[ARG_load].u_bool); + } + return mp_const_none; +} + +extern mp_obj_t MotorCluster_speed(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { + if(n_args <= 2) { + enum { ARG_self, ARG_motor }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_motor, MP_ARG_REQUIRED | MP_ARG_INT }, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _MotorCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _MotorCluster_obj_t); + + int motor = args[ARG_motor].u_int; + int motor_count = (int)self->cluster->count(); + if(motor_count == 0) + mp_raise_ValueError("this cluster does not have any motors"); + else if(motor < 0 || motor >= motor_count) + mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("motor out of range. Expected 0 to %d"), motor_count - 1); + else + return mp_obj_new_float(self->cluster->speed((uint)motor)); + } + else { + enum { ARG_self, ARG_motors, ARG_speed, ARG_load }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_motors, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_speed, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_load, MP_ARG_BOOL, { .u_bool = true }}, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _MotorCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _MotorCluster_obj_t); + + int motor_count = (int)self->cluster->count(); + if(motor_count == 0) + mp_raise_ValueError("this cluster does not have any motors"); + else { + // Determine what motor(s) to enable + const mp_obj_t object = args[ARG_motors].u_obj; + if(mp_obj_is_int(object)) { + int motor = mp_obj_get_int(object); + if(motor < 0 || motor >= motor_count) + mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("motor out of range. Expected 0 to %d"), motor_count - 1); + else { + float speed = mp_obj_get_float(args[ARG_speed].u_obj); + self->cluster->speed((uint)motor, speed, args[ARG_load].u_bool); + } + } + else { + size_t length = 0; + mp_obj_t *items = nullptr; + if(mp_obj_is_type(object, &mp_type_list)) { + mp_obj_list_t *list = MP_OBJ_TO_PTR2(object, mp_obj_list_t); + length = list->len; + items = list->items; + } + else if(mp_obj_is_type(object, &mp_type_tuple)) { + mp_obj_tuple_t *tuple = MP_OBJ_TO_PTR2(object, mp_obj_tuple_t); + length = tuple->len; + items = tuple->items; + } + + if(items == nullptr) + mp_raise_TypeError("cannot convert object to a list or tuple of integers, or a single integer"); + else if(length == 0) + mp_raise_TypeError("list or tuple must contain at least one integer"); + else { + // Create and populate a local array of motor indices + uint8_t *motors = new uint8_t[length]; + for(size_t i = 0; i < length; i++) { + int motor = mp_obj_get_int(items[i]); + if(motor < 0 || motor >= motor_count) { + delete[] motors; + mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("a motor in the list or tuple is out of range. Expected 0 to %d"), motor_count - 1); + } + else { + motors[i] = (uint8_t)motor; + } + } + float speed = mp_obj_get_float(args[ARG_speed].u_obj); + self->cluster->speed(motors, length, speed, args[ARG_load].u_bool); + delete[] motors; + } + } + } + } + return mp_const_none; +} + +extern mp_obj_t MotorCluster_all_to_speed(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { + enum { ARG_self, ARG_speed, ARG_load }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_speed, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_load, MP_ARG_BOOL, { .u_bool = true }}, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _MotorCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _MotorCluster_obj_t); + + int motor_count = (int)self->cluster->count(); + if(motor_count == 0) + mp_raise_ValueError("this cluster does not have any motors"); + else { + float speed = mp_obj_get_float(args[ARG_speed].u_obj); + self->cluster->all_to_speed(speed, args[ARG_load].u_bool); + } + return mp_const_none; +} + +extern mp_obj_t MotorCluster_phase(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { + if(n_args <= 2) { + enum { ARG_self, ARG_motor }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_motor, MP_ARG_REQUIRED | MP_ARG_INT }, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _MotorCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _MotorCluster_obj_t); + + int motor = args[ARG_motor].u_int; + int motor_count = (int)self->cluster->count(); + if(motor_count == 0) + mp_raise_ValueError("this cluster does not have any motors"); + else if(motor < 0 || motor >= motor_count) + mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("motor out of range. Expected 0 to %d"), motor_count - 1); + else + return mp_obj_new_float(self->cluster->phase((uint)motor)); + } + else { + enum { ARG_self, ARG_motors, ARG_phase, ARG_load }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_motors, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_phase, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_load, MP_ARG_BOOL, { .u_bool = true }}, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _MotorCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _MotorCluster_obj_t); + + int motor_count = (int)self->cluster->count(); + if(motor_count == 0) + mp_raise_ValueError("this cluster does not have any motors"); + else { + // Determine what motor(s) to enable + const mp_obj_t object = args[ARG_motors].u_obj; + if(mp_obj_is_int(object)) { + int motor = mp_obj_get_int(object); + if(motor < 0 || motor >= motor_count) + mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("motor out of range. Expected 0 to %d"), motor_count - 1); + else { + float phase = mp_obj_get_float(args[ARG_phase].u_obj); + self->cluster->phase((uint)motor, phase, args[ARG_load].u_bool); + } + } + else { + size_t length = 0; + mp_obj_t *items = nullptr; + if(mp_obj_is_type(object, &mp_type_list)) { + mp_obj_list_t *list = MP_OBJ_TO_PTR2(object, mp_obj_list_t); + length = list->len; + items = list->items; + } + else if(mp_obj_is_type(object, &mp_type_tuple)) { + mp_obj_tuple_t *tuple = MP_OBJ_TO_PTR2(object, mp_obj_tuple_t); + length = tuple->len; + items = tuple->items; + } + + if(items == nullptr) + mp_raise_TypeError("cannot convert object to a list or tuple of integers, or a single integer"); + else if(length == 0) + mp_raise_TypeError("list or tuple must contain at least one integer"); + else { + // Create and populate a local array of motor indices + uint8_t *motors = new uint8_t[length]; + for(size_t i = 0; i < length; i++) { + int motor = mp_obj_get_int(items[i]); + if(motor < 0 || motor >= motor_count) { + delete[] motors; + mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("a motor in the list or tuple is out of range. Expected 0 to %d"), motor_count - 1); + } + else { + motors[i] = (uint8_t)motor; + } + } + float phase = mp_obj_get_float(args[ARG_phase].u_obj); + self->cluster->phase(motors, length, phase, args[ARG_load].u_bool); + delete[] motors; + } + } + } + } + return mp_const_none; +} + +extern mp_obj_t MotorCluster_all_to_phase(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { + enum { ARG_self, ARG_phase, ARG_load }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_phase, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_load, MP_ARG_BOOL, { .u_bool = true }}, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _MotorCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _MotorCluster_obj_t); + + int motor_count = (int)self->cluster->count(); + if(motor_count == 0) + mp_raise_ValueError("this cluster does not have any motors"); + else { + float phase = mp_obj_get_float(args[ARG_phase].u_obj); + self->cluster->all_to_phase(phase, args[ARG_load].u_bool); + } + return mp_const_none; +} + +extern mp_obj_t MotorCluster_frequency(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { + if(n_args <= 1) { + enum { ARG_self }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _MotorCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _MotorCluster_obj_t); + + return mp_obj_new_float(self->cluster->frequency()); + } + else { + enum { ARG_self, ARG_freq }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_freq, MP_ARG_REQUIRED | MP_ARG_OBJ }, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _MotorCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _MotorCluster_obj_t); + + float freq = mp_obj_get_float(args[ARG_freq].u_obj); + + if(!self->cluster->frequency(freq)) + mp_raise_ValueError("freq out of range. Expected 10Hz to 350Hz"); + else + return mp_const_none; + } +} + +extern mp_obj_t MotorCluster_stop(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { + enum { ARG_self, ARG_motors, ARG_load }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_motors, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_load, MP_ARG_BOOL, { .u_bool = true }}, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _MotorCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _MotorCluster_obj_t); + + int motor_count = (int)self->cluster->count(); + if(motor_count == 0) + mp_raise_ValueError("this cluster does not have any motors"); + else { + // Determine what motor(s) to enable + const mp_obj_t object = args[ARG_motors].u_obj; + if(mp_obj_is_int(object)) { + int motor = mp_obj_get_int(object); + if(motor < 0 || motor >= motor_count) + mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("motor out of range. Expected 0 to %d"), motor_count - 1); + else + self->cluster->stop((uint)motor, args[ARG_load].u_bool); + } + else { + size_t length = 0; + mp_obj_t *items = nullptr; + if(mp_obj_is_type(object, &mp_type_list)) { + mp_obj_list_t *list = MP_OBJ_TO_PTR2(object, mp_obj_list_t); + length = list->len; + items = list->items; + } + else if(mp_obj_is_type(object, &mp_type_tuple)) { + mp_obj_tuple_t *tuple = MP_OBJ_TO_PTR2(object, mp_obj_tuple_t); + length = tuple->len; + items = tuple->items; + } + + if(items == nullptr) + mp_raise_TypeError("cannot convert object to a list or tuple of integers, or a single integer"); + else if(length == 0) + mp_raise_TypeError("list or tuple must contain at least one integer"); + else { + // Create and populate a local array of motor indices + uint8_t *motors = new uint8_t[length]; + for(size_t i = 0; i < length; i++) { + int motor = mp_obj_get_int(items[i]); + if(motor < 0 || motor >= motor_count) { + delete[] motors; + mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("a motor in the list or tuple is out of range. Expected 0 to %d"), motor_count - 1); + } + else { + motors[i] = (uint8_t)motor; + } + } + self->cluster->stop(motors, length, args[ARG_load].u_bool); + delete[] motors; + } + } + } + return mp_const_none; +} + +extern mp_obj_t MotorCluster_stop_all(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { + enum { ARG_self, ARG_load }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_load, MP_ARG_BOOL, { .u_bool = true }}, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _MotorCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _MotorCluster_obj_t); + + int motor_count = (int)self->cluster->count(); + if(motor_count == 0) + mp_raise_ValueError("this cluster does not have any motors"); + else { + self->cluster->stop_all(args[ARG_load].u_bool); + } + return mp_const_none; +} + +extern mp_obj_t MotorCluster_coast(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { + enum { ARG_self, ARG_motors, ARG_load }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_motors, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_load, MP_ARG_BOOL, { .u_bool = true }}, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _MotorCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _MotorCluster_obj_t); + + int motor_count = (int)self->cluster->count(); + if(motor_count == 0) + mp_raise_ValueError("this cluster does not have any motors"); + else { + // Determine what motor(s) to enable + const mp_obj_t object = args[ARG_motors].u_obj; + if(mp_obj_is_int(object)) { + int motor = mp_obj_get_int(object); + if(motor < 0 || motor >= motor_count) + mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("motor out of range. Expected 0 to %d"), motor_count - 1); + else + self->cluster->coast((uint)motor, args[ARG_load].u_bool); + } + else { + size_t length = 0; + mp_obj_t *items = nullptr; + if(mp_obj_is_type(object, &mp_type_list)) { + mp_obj_list_t *list = MP_OBJ_TO_PTR2(object, mp_obj_list_t); + length = list->len; + items = list->items; + } + else if(mp_obj_is_type(object, &mp_type_tuple)) { + mp_obj_tuple_t *tuple = MP_OBJ_TO_PTR2(object, mp_obj_tuple_t); + length = tuple->len; + items = tuple->items; + } + + if(items == nullptr) + mp_raise_TypeError("cannot convert object to a list or tuple of integers, or a single integer"); + else if(length == 0) + mp_raise_TypeError("list or tuple must contain at least one integer"); + else { + // Create and populate a local array of motor indices + uint8_t *motors = new uint8_t[length]; + for(size_t i = 0; i < length; i++) { + int motor = mp_obj_get_int(items[i]); + if(motor < 0 || motor >= motor_count) { + delete[] motors; + mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("a motor in the list or tuple is out of range. Expected 0 to %d"), motor_count - 1); + } + else { + motors[i] = (uint8_t)motor; + } + } + self->cluster->coast(motors, length, args[ARG_load].u_bool); + delete[] motors; + } + } + } + return mp_const_none; +} + +extern mp_obj_t MotorCluster_coast_all(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { + enum { ARG_self, ARG_load }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_load, MP_ARG_BOOL, { .u_bool = true }}, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _MotorCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _MotorCluster_obj_t); + + int motor_count = (int)self->cluster->count(); + if(motor_count == 0) + mp_raise_ValueError("this cluster does not have any motors"); + else { + self->cluster->coast_all(args[ARG_load].u_bool); + } + return mp_const_none; +} + +extern mp_obj_t MotorCluster_full_negative(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { + enum { ARG_self, ARG_motors, ARG_load }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_motors, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_load, MP_ARG_BOOL, { .u_bool = true }}, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _MotorCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _MotorCluster_obj_t); + + int motor_count = (int)self->cluster->count(); + if(motor_count == 0) + mp_raise_ValueError("this cluster does not have any motors"); + else { + // Determine what motor(s) to enable + const mp_obj_t object = args[ARG_motors].u_obj; + if(mp_obj_is_int(object)) { + int motor = mp_obj_get_int(object); + if(motor < 0 || motor >= motor_count) + mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("motor out of range. Expected 0 to %d"), motor_count - 1); + else + self->cluster->full_negative((uint)motor, args[ARG_load].u_bool); + } + else { + size_t length = 0; + mp_obj_t *items = nullptr; + if(mp_obj_is_type(object, &mp_type_list)) { + mp_obj_list_t *list = MP_OBJ_TO_PTR2(object, mp_obj_list_t); + length = list->len; + items = list->items; + } + else if(mp_obj_is_type(object, &mp_type_tuple)) { + mp_obj_tuple_t *tuple = MP_OBJ_TO_PTR2(object, mp_obj_tuple_t); + length = tuple->len; + items = tuple->items; + } + + if(items == nullptr) + mp_raise_TypeError("cannot convert object to a list or tuple of integers, or a single integer"); + else if(length == 0) + mp_raise_TypeError("list or tuple must contain at least one integer"); + else { + // Create and populate a local array of motor indices + uint8_t *motors = new uint8_t[length]; + for(size_t i = 0; i < length; i++) { + int motor = mp_obj_get_int(items[i]); + if(motor < 0 || motor >= motor_count) { + delete[] motors; + mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("a motor in the list or tuple is out of range. Expected 0 to %d"), motor_count - 1); + } + else { + motors[i] = (uint8_t)motor; + } + } + self->cluster->full_negative(motors, length, args[ARG_load].u_bool); + delete[] motors; + } + } + } + return mp_const_none; +} + +extern mp_obj_t MotorCluster_all_to_full_negative(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { + enum { ARG_self, ARG_load }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_load, MP_ARG_BOOL, { .u_bool = true }}, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _MotorCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _MotorCluster_obj_t); + + int motor_count = (int)self->cluster->count(); + if(motor_count == 0) + mp_raise_ValueError("this cluster does not have any motors"); + else { + self->cluster->all_to_full_negative(args[ARG_load].u_bool); + } + return mp_const_none; +} + +extern mp_obj_t MotorCluster_full_positive(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { + enum { ARG_self, ARG_motors, ARG_load }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_motors, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_load, MP_ARG_BOOL, { .u_bool = true }}, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _MotorCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _MotorCluster_obj_t); + + int motor_count = (int)self->cluster->count(); + if(motor_count == 0) + mp_raise_ValueError("this cluster does not have any motors"); + else { + // Determine what motor(s) to enable + const mp_obj_t object = args[ARG_motors].u_obj; + if(mp_obj_is_int(object)) { + int motor = mp_obj_get_int(object); + if(motor < 0 || motor >= motor_count) + mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("motor out of range. Expected 0 to %d"), motor_count - 1); + else + self->cluster->full_positive((uint)motor, args[ARG_load].u_bool); + } + else { + size_t length = 0; + mp_obj_t *items = nullptr; + if(mp_obj_is_type(object, &mp_type_list)) { + mp_obj_list_t *list = MP_OBJ_TO_PTR2(object, mp_obj_list_t); + length = list->len; + items = list->items; + } + else if(mp_obj_is_type(object, &mp_type_tuple)) { + mp_obj_tuple_t *tuple = MP_OBJ_TO_PTR2(object, mp_obj_tuple_t); + length = tuple->len; + items = tuple->items; + } + + if(items == nullptr) + mp_raise_TypeError("cannot convert object to a list or tuple of integers, or a single integer"); + else if(length == 0) + mp_raise_TypeError("list or tuple must contain at least one integer"); + else { + // Create and populate a local array of motor indices + uint8_t *motors = new uint8_t[length]; + for(size_t i = 0; i < length; i++) { + int motor = mp_obj_get_int(items[i]); + if(motor < 0 || motor >= motor_count) { + delete[] motors; + mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("a motor in the list or tuple is out of range. Expected 0 to %d"), motor_count - 1); + } + else { + motors[i] = (uint8_t)motor; + } + } + self->cluster->full_positive(motors, length, args[ARG_load].u_bool); + delete[] motors; + } + } + } + return mp_const_none; +} + +extern mp_obj_t MotorCluster_all_to_full_positive(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { + enum { ARG_self, ARG_load }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_load, MP_ARG_BOOL, { .u_bool = true }}, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _MotorCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _MotorCluster_obj_t); + + int motor_count = (int)self->cluster->count(); + if(motor_count == 0) + mp_raise_ValueError("this cluster does not have any motors"); + else { + self->cluster->all_to_full_positive(args[ARG_load].u_bool); + } + return mp_const_none; +} + +extern mp_obj_t MotorCluster_to_percent(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { + if(n_args <= 4) { + enum { ARG_self, ARG_motors, ARG_in, ARG_load }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_motors, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_in, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_load, MP_ARG_BOOL, { .u_bool = true }}, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _MotorCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _MotorCluster_obj_t); + + int motor_count = (int)self->cluster->count(); + if(motor_count == 0) + mp_raise_ValueError("this cluster does not have any motors"); + else { + // Determine what motor(s) to enable + const mp_obj_t object = args[ARG_motors].u_obj; + if(mp_obj_is_int(object)) { + int motor = mp_obj_get_int(object); + if(motor < 0 || motor >= motor_count) + mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("motor out of range. Expected 0 to %d"), motor_count - 1); + else { + float in = mp_obj_get_float(args[ARG_in].u_obj); + self->cluster->to_percent((uint)motor, in, args[ARG_load].u_bool); + } + } + else { + size_t length = 0; + mp_obj_t *items = nullptr; + if(mp_obj_is_type(object, &mp_type_list)) { + mp_obj_list_t *list = MP_OBJ_TO_PTR2(object, mp_obj_list_t); + length = list->len; + items = list->items; + } + else if(mp_obj_is_type(object, &mp_type_tuple)) { + mp_obj_tuple_t *tuple = MP_OBJ_TO_PTR2(object, mp_obj_tuple_t); + length = tuple->len; + items = tuple->items; + } + + if(items == nullptr) + mp_raise_TypeError("cannot convert object to a list or tuple of integers, or a single integer"); + else if(length == 0) + mp_raise_TypeError("list or tuple must contain at least one integer"); + else { + // Create and populate a local array of motor indices + uint8_t *motors = new uint8_t[length]; + for(size_t i = 0; i < length; i++) { + int motor = mp_obj_get_int(items[i]); + if(motor < 0 || motor >= motor_count) { + delete[] motors; + mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("a motor in the list or tuple is out of range. Expected 0 to %d"), motor_count - 1); + } + else { + motors[i] = (uint8_t)motor; + } + } + float in = mp_obj_get_float(args[ARG_in].u_obj); + self->cluster->to_percent(motors, length, in, args[ARG_load].u_bool); + delete[] motors; + } + } + } + } + else if(n_args <= 6) { + enum { ARG_self, ARG_motors, ARG_in, ARG_in_min, ARG_in_max, ARG_load }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_motors, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_in, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_in_min, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_in_max, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_load, MP_ARG_BOOL, { .u_bool = true }}, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _MotorCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _MotorCluster_obj_t); + + int motor_count = (int)self->cluster->count(); + if(motor_count == 0) + mp_raise_ValueError("this cluster does not have any motors"); + else { + // Determine what motor(s) to enable + const mp_obj_t object = args[ARG_motors].u_obj; + if(mp_obj_is_int(object)) { + int motor = mp_obj_get_int(object); + if(motor < 0 || motor >= motor_count) + mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("motor out of range. Expected 0 to %d"), motor_count - 1); + else { + float in = mp_obj_get_float(args[ARG_in].u_obj); + float in_min = mp_obj_get_float(args[ARG_in_min].u_obj); + float in_max = mp_obj_get_float(args[ARG_in_max].u_obj); + self->cluster->to_percent((uint)motor, in, in_min, in_max, args[ARG_load].u_bool); + } + } + else { + size_t length = 0; + mp_obj_t *items = nullptr; + if(mp_obj_is_type(object, &mp_type_list)) { + mp_obj_list_t *list = MP_OBJ_TO_PTR2(object, mp_obj_list_t); + length = list->len; + items = list->items; + } + else if(mp_obj_is_type(object, &mp_type_tuple)) { + mp_obj_tuple_t *tuple = MP_OBJ_TO_PTR2(object, mp_obj_tuple_t); + length = tuple->len; + items = tuple->items; + } + + if(items == nullptr) + mp_raise_TypeError("cannot convert object to a list or tuple of integers, or a single integer"); + else if(length == 0) + mp_raise_TypeError("list or tuple must contain at least one integer"); + else { + // Create and populate a local array of motor indices + uint8_t *motors = new uint8_t[length]; + for(size_t i = 0; i < length; i++) { + int motor = mp_obj_get_int(items[i]); + if(motor < 0 || motor >= motor_count) { + delete[] motors; + mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("a motor in the list or tuple is out of range. Expected 0 to %d"), motor_count - 1); + } + else { + motors[i] = (uint8_t)motor; + } + } + float in = mp_obj_get_float(args[ARG_in].u_obj); + float in_min = mp_obj_get_float(args[ARG_in_min].u_obj); + float in_max = mp_obj_get_float(args[ARG_in_max].u_obj); + self->cluster->to_percent(motors, length, in, in_min, in_max, args[ARG_load].u_bool); + delete[] motors; + } + } + } + } + else { + enum { ARG_self, ARG_motors, ARG_in, ARG_in_min, ARG_in_max, ARG_speed_min, ARG_speed_max, ARG_load }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_motors, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_in, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_in_min, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_in_max, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_speed_min, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_speed_max, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_load, MP_ARG_BOOL, { .u_bool = true }}, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _MotorCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _MotorCluster_obj_t); + + int motor_count = (int)self->cluster->count(); + if(motor_count == 0) + mp_raise_ValueError("this cluster does not have any motors"); + else { + // Determine what motor(s) to enable + const mp_obj_t object = args[ARG_motors].u_obj; + if(mp_obj_is_int(object)) { + int motor = mp_obj_get_int(object); + if(motor < 0 || motor >= motor_count) + mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("motor out of range. Expected 0 to %d"), motor_count - 1); + else { + float in = mp_obj_get_float(args[ARG_in].u_obj); + float in_min = mp_obj_get_float(args[ARG_in_min].u_obj); + float in_max = mp_obj_get_float(args[ARG_in_max].u_obj); + float speed_min = mp_obj_get_float(args[ARG_speed_min].u_obj); + float speed_max = mp_obj_get_float(args[ARG_speed_max].u_obj); + self->cluster->to_percent((uint)motor, in, in_min, in_max, speed_min, speed_max, args[ARG_load].u_bool); + } + } + else { + size_t length = 0; + mp_obj_t *items = nullptr; + if(mp_obj_is_type(object, &mp_type_list)) { + mp_obj_list_t *list = MP_OBJ_TO_PTR2(object, mp_obj_list_t); + length = list->len; + items = list->items; + } + else if(mp_obj_is_type(object, &mp_type_tuple)) { + mp_obj_tuple_t *tuple = MP_OBJ_TO_PTR2(object, mp_obj_tuple_t); + length = tuple->len; + items = tuple->items; + } + + if(items == nullptr) + mp_raise_TypeError("cannot convert object to a list or tuple of integers, or a single integer"); + else if(length == 0) + mp_raise_TypeError("list or tuple must contain at least one integer"); + else { + // Create and populate a local array of motor indices + uint8_t *motors = new uint8_t[length]; + for(size_t i = 0; i < length; i++) { + int motor = mp_obj_get_int(items[i]); + if(motor < 0 || motor >= motor_count) { + delete[] motors; + mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("a motor in the list or tuple is out of range. Expected 0 to %d"), motor_count - 1); + } + else { + motors[i] = (uint8_t)motor; + } + } + float in = mp_obj_get_float(args[ARG_in].u_obj); + float in_min = mp_obj_get_float(args[ARG_in_min].u_obj); + float in_max = mp_obj_get_float(args[ARG_in_max].u_obj); + float speed_min = mp_obj_get_float(args[ARG_speed_min].u_obj); + float speed_max = mp_obj_get_float(args[ARG_speed_max].u_obj); + self->cluster->to_percent(motors, length, in, in_min, in_max, speed_min, speed_max, args[ARG_load].u_bool); + delete[] motors; + } + } + } + } + return mp_const_none; +} + +extern mp_obj_t MotorCluster_all_to_percent(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { + if(n_args <= 3) { + enum { ARG_self, ARG_in, ARG_load }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_in, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_load, MP_ARG_BOOL, { .u_bool = true }}, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _MotorCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _MotorCluster_obj_t); + + int motor_count = (int)self->cluster->count(); + if(motor_count == 0) + mp_raise_ValueError("this cluster does not have any motors"); + else { + float in = mp_obj_get_float(args[ARG_in].u_obj); + self->cluster->all_to_percent(in, args[ARG_load].u_bool); + } + } + else if(n_args <= 5) { + enum { ARG_self, ARG_in, ARG_in_min, ARG_in_max, ARG_load }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_in, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_in_min, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_in_max, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_load, MP_ARG_BOOL, { .u_bool = true }}, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _MotorCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _MotorCluster_obj_t); + + int motor_count = (int)self->cluster->count(); + if(motor_count == 0) + mp_raise_ValueError("this cluster does not have any motors"); + else { + float in = mp_obj_get_float(args[ARG_in].u_obj); + float in_min = mp_obj_get_float(args[ARG_in_min].u_obj); + float in_max = mp_obj_get_float(args[ARG_in_max].u_obj); + self->cluster->all_to_percent(in, in_min, in_max, args[ARG_load].u_bool); + } + } + else { + enum { ARG_self, ARG_in, ARG_in_min, ARG_in_max, ARG_speed_min, ARG_speed_max, ARG_load }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_in, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_in_min, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_in_max, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_speed_min, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_speed_max, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_load, MP_ARG_BOOL, { .u_bool = true }}, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _MotorCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _MotorCluster_obj_t); + + int motor_count = (int)self->cluster->count(); + if(motor_count == 0) + mp_raise_ValueError("this cluster does not have any motors"); + else { + float in = mp_obj_get_float(args[ARG_in].u_obj); + float in_min = mp_obj_get_float(args[ARG_in_min].u_obj); + float in_max = mp_obj_get_float(args[ARG_in_max].u_obj); + float speed_min = mp_obj_get_float(args[ARG_speed_min].u_obj); + float speed_max = mp_obj_get_float(args[ARG_speed_max].u_obj); + self->cluster->all_to_percent(in, in_min, in_max, speed_min, speed_max, args[ARG_load].u_bool); + } + } + return mp_const_none; +} + +extern mp_obj_t MotorCluster_load(mp_obj_t self_in) { + _MotorCluster_obj_t *self = MP_OBJ_TO_PTR2(self_in, _MotorCluster_obj_t); + self->cluster->load(); + return mp_const_none; +} +} diff --git a/micropython/modules/motor/motor.h b/micropython/modules/motor/motor.h new file mode 100644 index 00000000..08b499c8 --- /dev/null +++ b/micropython/modules/motor/motor.h @@ -0,0 +1,72 @@ +// Include MicroPython API. +#include "py/runtime.h" + +enum { + PICO_MOTOR_SHIM_BUTTON_A = 2, + PICO_MOTOR_SHIM_MOTOR_1P = 6, + PICO_MOTOR_SHIM_MOTOR_1N = 7, + PICO_MOTOR_SHIM_MOTOR_2P = 27, + PICO_MOTOR_SHIM_MOTOR_2N = 26, +}; + +/***** Extern of Class Definition *****/ +extern const mp_obj_type_t Motor_type; +extern const mp_obj_type_t MotorCluster_type; + +/***** Extern of Class Methods *****/ +extern void Motor_print(const mp_print_t *print, mp_obj_t self_in, mp_print_kind_t kind); +extern mp_obj_t Motor_make_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw, const mp_obj_t *all_args); +extern mp_obj_t Motor___del__(mp_obj_t self_in); +extern mp_obj_t Motor_pins(mp_obj_t self_in); +extern mp_obj_t Motor_enable(mp_obj_t self_in); +extern mp_obj_t Motor_disable(mp_obj_t self_in); +extern mp_obj_t Motor_is_enabled(mp_obj_t self_in); +extern mp_obj_t Motor_duty(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); +extern mp_obj_t Motor_speed(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); +extern mp_obj_t Motor_frequency(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); +extern mp_obj_t Motor_stop(mp_obj_t self_in); +extern mp_obj_t Motor_coast(mp_obj_t self_in); +extern mp_obj_t Motor_full_negative(mp_obj_t self_in); +extern mp_obj_t Motor_full_positive(mp_obj_t self_in); +extern mp_obj_t Motor_to_percent(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); +extern mp_obj_t Motor_direction(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); +extern mp_obj_t Motor_speed_scale(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); +extern mp_obj_t Motor_deadzone_percent(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); +extern mp_obj_t Motor_decay_mode(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); + +extern void MotorCluster_print(const mp_print_t *print, mp_obj_t self_in, mp_print_kind_t kind); +extern mp_obj_t MotorCluster_make_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw, const mp_obj_t *all_args); +extern mp_obj_t MotorCluster___del__(mp_obj_t self_in); +extern mp_obj_t MotorCluster_count(mp_obj_t self_in); +extern mp_obj_t MotorCluster_pins(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); +extern mp_obj_t MotorCluster_enable(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); +extern mp_obj_t MotorCluster_enable_all(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); +extern mp_obj_t MotorCluster_disable(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); +extern mp_obj_t MotorCluster_disable_all(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); +extern mp_obj_t MotorCluster_is_enabled(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); +extern mp_obj_t MotorCluster_duty(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); +extern mp_obj_t MotorCluster_all_to_duty(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); +extern mp_obj_t MotorCluster_speed(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); +extern mp_obj_t MotorCluster_all_to_speed(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); +extern mp_obj_t MotorCluster_phase(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); +extern mp_obj_t MotorCluster_all_to_phase(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); +extern mp_obj_t MotorCluster_frequency(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); +extern mp_obj_t MotorCluster_stop(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); +extern mp_obj_t MotorCluster_stop_all(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); +extern mp_obj_t MotorCluster_coast(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); +extern mp_obj_t MotorCluster_coast_all(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); +extern mp_obj_t MotorCluster_full_negative(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); +extern mp_obj_t MotorCluster_all_to_full_negative(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); +extern mp_obj_t MotorCluster_full_positive(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); +extern mp_obj_t MotorCluster_all_to_full_positive(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); +extern mp_obj_t MotorCluster_to_percent(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); +extern mp_obj_t MotorCluster_all_to_percent(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); +extern mp_obj_t MotorCluster_load(mp_obj_t self_in); +//extern mp_obj_t MotorCluster_direction(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); +//extern mp_obj_t MotorCluster_all_directions(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); +//extern mp_obj_t MotorCluster_speed_scale(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); +//extern mp_obj_t MotorCluster_all_speed_scales(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); +//extern mp_obj_t MotorCluster_deadzone_percent(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); +//extern mp_obj_t MotorCluster_all_deadzone_percents(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); +//extern mp_obj_t MotorCluster_decay_mode(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); +//extern mp_obj_t MotorCluster_all_decay_modes(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); diff --git a/micropython/modules/pwm/micropython.cmake b/micropython/modules/pwm/micropython.cmake new file mode 100644 index 00000000..6bfba90c --- /dev/null +++ b/micropython/modules/pwm/micropython.cmake @@ -0,0 +1,20 @@ +set(MOD_NAME pwm) +string(TOUPPER ${MOD_NAME} MOD_NAME_UPPER) +add_library(usermod_${MOD_NAME} INTERFACE) + +target_sources(usermod_${MOD_NAME} INTERFACE + ${CMAKE_CURRENT_LIST_DIR}/../../../drivers/pwm/pwm.cpp + ${CMAKE_CURRENT_LIST_DIR}/../../../drivers/pwm/pwm_cluster.cpp +) +pico_generate_pio_header(usermod_${MOD_NAME} ${CMAKE_CURRENT_LIST_DIR}/../../../drivers/pwm/pwm_cluster.pio) + +target_include_directories(usermod_${MOD_NAME} INTERFACE + ${CMAKE_CURRENT_LIST_DIR} + ${CMAKE_CURRENT_LIST_DIR}/../../../drivers/pwm/ +) + +target_compile_definitions(usermod_${MOD_NAME} INTERFACE + MODULE_PWM_ENABLED=1 +) + +target_link_libraries(usermod INTERFACE usermod_${MOD_NAME}) \ No newline at end of file diff --git a/micropython/modules/servo/micropython.cmake b/micropython/modules/servo/micropython.cmake index e626ddc5..9e51dfbe 100644 --- a/micropython/modules/servo/micropython.cmake +++ b/micropython/modules/servo/micropython.cmake @@ -12,7 +12,7 @@ target_sources(usermod_${MOD_NAME} INTERFACE ${CMAKE_CURRENT_LIST_DIR}/../../../drivers/servo/servo_state.cpp ${CMAKE_CURRENT_LIST_DIR}/../../../drivers/servo/calibration.cpp ) -pico_generate_pio_header(usermod_${MOD_NAME} ${CMAKE_CURRENT_LIST_DIR}/../../../drivers/pwm/pwm_cluster.pio) +# pico_generate_pio_header(usermod_${MOD_NAME} ${CMAKE_CURRENT_LIST_DIR}/../../../drivers/pwm/pwm_cluster.pio) target_include_directories(usermod_${MOD_NAME} INTERFACE ${CMAKE_CURRENT_LIST_DIR} @@ -21,6 +21,7 @@ target_include_directories(usermod_${MOD_NAME} INTERFACE ) target_compile_definitions(usermod_${MOD_NAME} INTERFACE + MODULE_PWM_ENABLED=1 MODULE_SERVO_ENABLED=1 ) From 7e9860e780a3c0a6626375e878a431648fa96b10 Mon Sep 17 00:00:00 2001 From: ZodiusInfuser Date: Thu, 7 Apr 2022 17:57:38 +0100 Subject: [PATCH 06/60] More mp binding and example work --- micropython/examples/motor2040/README.md | 85 +++++++++ .../examples/motor2040/current_meter.py | 90 ++++++++++ micropython/examples/motor2040/led_rainbow.py | 43 +++++ .../examples/motor2040/motor_cluster.py | 60 +++++++ micropython/examples/motor2040/motor_wave.py | 64 +++++++ .../examples/motor2040/multiple_motors.py | 59 +++++++ .../examples/motor2040/read_sensors.py | 48 ++++++ .../examples/motor2040/sensor_feedback.py | 58 +++++++ .../examples/motor2040/simple_easing.py | 64 +++++++ .../examples/motor2040/single_motor.py | 49 ++++++ .../examples/motor2040/turn_off_led.py | 16 ++ .../examples/motor2040/turn_off_motors.py | 13 ++ micropython/modules/motor/motor.c | 8 +- micropython/modules/motor/motor.cpp | 161 +++++++++++++----- micropython/modules/motor/motor.h | 8 - 15 files changed, 776 insertions(+), 50 deletions(-) create mode 100644 micropython/examples/motor2040/README.md create mode 100644 micropython/examples/motor2040/current_meter.py create mode 100644 micropython/examples/motor2040/led_rainbow.py create mode 100644 micropython/examples/motor2040/motor_cluster.py create mode 100644 micropython/examples/motor2040/motor_wave.py create mode 100644 micropython/examples/motor2040/multiple_motors.py create mode 100644 micropython/examples/motor2040/read_sensors.py create mode 100644 micropython/examples/motor2040/sensor_feedback.py create mode 100644 micropython/examples/motor2040/simple_easing.py create mode 100644 micropython/examples/motor2040/single_motor.py create mode 100644 micropython/examples/motor2040/turn_off_led.py create mode 100644 micropython/examples/motor2040/turn_off_motors.py diff --git a/micropython/examples/motor2040/README.md b/micropython/examples/motor2040/README.md new file mode 100644 index 00000000..4bb84e0a --- /dev/null +++ b/micropython/examples/motor2040/README.md @@ -0,0 +1,85 @@ +# Motor 2040 Micropython Examples + +- [Motor Examples](#motor-examples) + - [Single Motor](#single-motor) + - [Multiple Motors](#multiple-motors) + - [Motor Cluster](#motor-cluster) + - [Simple Easing](#simple-easing) + - [Motor Wave](#motor-wave) + - [Calibration](#calibration) +- [Function Examples](#function-examples) + - [Read Sensors](#read-sensors) + - [Sensor Feedback](#sensor-feedback) + - [Current Meter](#current-meter) + - [LED Rainbow](#led-rainbow) + - [Turn Off LEDs](#turn-off-leds) + + +## Motor Examples + +### Single Motor +[single_motor.py](single_motor.py) + +Demonstrates how to create a Motor object and control it. + + +### Multiple Motors +[multiple_motors.py](multiple_motors.py) + +Demonstrates how to create multiple Motor objects and control them together. + + +### Motor Cluster +[motor_cluster.py](motor_cluster.py) + +Demonstrates how to create a MotorCluster object to control multiple motors at once. + + +### Simple Easing +[simple_easing.py](simple_easing.py) + +An example of how to move a motor smoothly between random positions. + + +### Motor Wave +[motor_wave.py](motor_wave.py) + +An example of applying a wave pattern to a group of motors and the LEDs. + + +### Calibration +[calibration.py](calibration.py) + +Shows how to create motors with different common calibrations, modify a motor's existing calibration, and create a motor with a custom calibration. + + +## Function Examples + +### Read Sensors +[read_sensors.py](read_sensors.py) + +Shows how to initialise and read the 6 external and 2 internal sensors of Motor 2040. + + +### Sensor Feedback +[sensor_feedback.py](sensor_feedback.py) + +Show how to read the 6 external sensors and display their values on the neighbouring LEDs. + + +### Current Meter +[current_meter.py](current_meter.py) + +An example of how to use Motor 2040's current measuring ability and display the value on the onboard LED bar. + + +### LED Rainbow +[led_rainbow.py](led_rainbow.py) + +Displays a rotating rainbow pattern on the Motor 2040's onboard LED. + + +### Turn Off LED +[turn_off_led.py](turn_off_led.py) + +A simple program that turns off the onboard LED. diff --git a/micropython/examples/motor2040/current_meter.py b/micropython/examples/motor2040/current_meter.py new file mode 100644 index 00000000..5e4f4220 --- /dev/null +++ b/micropython/examples/motor2040/current_meter.py @@ -0,0 +1,90 @@ +import gc +import time +from machine import Pin +from pimoroni import Analog, AnalogMux, Button +from plasma import WS2812 +from servo import ServoCluster, servo2040 + +""" +An example of how to use Servo 2040's current measuring +ability and display the value on the onboard LED bar. + +Press "Boot" to exit the program. + +NOTE: ServoCluster and Plasma WS2812 use the RP2040's PIO system, +and as such may have problems when running code multiple times. +If you encounter issues, try resetting your board. +""" + +BRIGHTNESS = 0.4 # The brightness of the LEDs +UPDATES = 50 # How many times to update LEDs and Servos per second +MAX_CURRENT = 3.0 # The maximum current, in amps, to show on the meter +SAMPLES = 50 # The number of current measurements to take per reading +TIME_BETWEEN = 0.001 # The time between each current measurement + +# Free up hardware resources ahead of creating a new ServoCluster +gc.collect() + +# Create a servo cluster for pins 0 to 7, using PIO 0 and State Machine 0 +START_PIN = servo2040.SERVO_1 +END_PIN = servo2040.SERVO_8 +servos = ServoCluster(pio=0, sm=0, pins=list(range(START_PIN, END_PIN + 1))) + +# Set up the shared analog inputs +cur_adc = Analog(servo2040.SHARED_ADC, servo2040.CURRENT_GAIN, + servo2040.SHUNT_RESISTOR, servo2040.CURRENT_OFFSET) + +# Set up the analog multiplexer, including the pin for controlling pull-up/pull-down +mux = AnalogMux(servo2040.ADC_ADDR_0, servo2040.ADC_ADDR_1, servo2040.ADC_ADDR_2, + muxed_pin=Pin(servo2040.SHARED_ADC)) + +# Create the LED bar, using PIO 1 and State Machine 0 +led_bar = WS2812(servo2040.NUM_LEDS, 1, 0, servo2040.LED_DATA) + +# Create the user button +user_sw = Button(servo2040.USER_SW) + +# Start updating the LED bar +led_bar.start() + +# Enable all servos (this puts them at the middle). +# The servos are not going to be moved, but are activated to give a current draw +servos.enable_all() + +# Read sensors until the user button is pressed +while user_sw.raw() is not True: + + # Select the current sense + mux.select(servo2040.CURRENT_SENSE_ADDR) + + # Read the current sense several times and average the result + current = 0 + for i in range(SAMPLES): + current += cur_adc.read_current() + time.sleep(TIME_BETWEEN) + current /= SAMPLES + + # Print out the current sense value + print("Current =", round(current, 4)) + + # Convert the current to a percentage of the maximum we want to show + percent = (current / MAX_CURRENT) + + # Update all the LEDs + for i in range(servo2040.NUM_LEDS): + # Calculate the LED's hue, with Red for high currents and Green for low + hue = (1.0 - i / (servo2040.NUM_LEDS - 1)) * 0.333 + + # Calculate the current level the LED represents + level = (i + 0.5) / servo2040.NUM_LEDS + # If the percent is above the level, light the LED, otherwise turn it off + if percent >= level: + led_bar.set_hsv(i, hue, 1.0, BRIGHTNESS) + else: + led_bar.set_hsv(i, hue, 1.0, 0.0) + +# Disable the servos +servos.disable_all() + +# Turn off the LED bar +led_bar.clear() diff --git a/micropython/examples/motor2040/led_rainbow.py b/micropython/examples/motor2040/led_rainbow.py new file mode 100644 index 00000000..bb3db6b6 --- /dev/null +++ b/micropython/examples/motor2040/led_rainbow.py @@ -0,0 +1,43 @@ +import time +from pimoroni import Button +from plasma import WS2812 +from motor import motor2040 + +""" +Displays a rotating rainbow pattern on the Motor 2040's onboard LED. + +Press "Boot" to exit the program. + +NOTE: Plasma WS2812 uses the RP2040's PIO system, and as +such may have problems when running code multiple times. +If you encounter issues, try resetting your board. +""" + +SPEED = 5 # The speed that the LEDs will cycle at +BRIGHTNESS = 0.4 # The brightness of the LEDs +UPDATES = 50 # How many times the LEDs will be updated per second + +# Create the LED, using PIO 1 and State Machine 0 +led = WS2812(motor2040.NUM_LEDS, 1, 0, motor2040.LED_DATA) + +# Create the user button +user_sw = Button(motor2040.USER_SW) + +# Start updating the LED +led.start() + + +hue = 0.0 + +# Make rainbows until the user button is pressed +while user_sw.raw() is not True: + + hue += SPEED / 1000.0 + + # Update the LED + led.set_hsv(0, hue, 1.0, BRIGHTNESS) + + time.sleep(1.0 / UPDATES) + +# Turn off the LED +led.clear() diff --git a/micropython/examples/motor2040/motor_cluster.py b/micropython/examples/motor2040/motor_cluster.py new file mode 100644 index 00000000..400e364d --- /dev/null +++ b/micropython/examples/motor2040/motor_cluster.py @@ -0,0 +1,60 @@ +import gc +import time +import math +from servo import ServoCluster, servo2040 + +""" +Demonstrates how to create a ServoCluster object to control multiple servos at once. + +NOTE: ServoCluster uses the RP2040's PIO system, and as +such may have problems when running code multiple times. +If you encounter issues, try resetting your board. +""" + +# Free up hardware resources ahead of creating a new ServoCluster +gc.collect() + +# Create a servo cluster for pins 0 to 3, using PIO 0 and State Machine 0 +START_PIN = servo2040.SERVO_1 +END_PIN = servo2040.SERVO_4 +servos = ServoCluster(pio=0, sm=0, pins=list(range(START_PIN, END_PIN + 1))) + +# Enable all servos (this puts them at the middle) +servos.enable_all() +time.sleep(2) + +# Go to min +servos.all_to_min() +time.sleep(2) + +# Go to max +servos.all_to_max() +time.sleep(2) + +# Go back to mid +servos.all_to_mid() +time.sleep(2) + +SWEEPS = 3 # How many sweeps of the servo to perform +STEPS = 10 # The number of discrete sweep steps +STEPS_INTERVAL = 0.5 # The time in seconds between each step of the sequence +SWEEP_EXTENT = 90.0 # How far from zero to move the servos when sweeping + +# Do a sine sweep +for j in range(SWEEPS): + for i in range(360): + value = math.sin(math.radians(i)) * SWEEP_EXTENT + servos.all_to_value(value) + time.sleep(0.02) + +# Do a stepped sweep +for j in range(SWEEPS): + for i in range(0, STEPS): + servos.all_to_percent(i, 0, STEPS, 0.0 - SWEEP_EXTENT, SWEEP_EXTENT) + time.sleep(STEPS_INTERVAL) + for i in range(0, STEPS): + servos.all_to_percent(i, STEPS, 0, 0.0 - SWEEP_EXTENT, SWEEP_EXTENT) + time.sleep(STEPS_INTERVAL) + +# Disable the servos +servos.disable_all() diff --git a/micropython/examples/motor2040/motor_wave.py b/micropython/examples/motor2040/motor_wave.py new file mode 100644 index 00000000..7d5c228e --- /dev/null +++ b/micropython/examples/motor2040/motor_wave.py @@ -0,0 +1,64 @@ +import gc +import time +import math +from pimoroni import Button +from plasma import WS2812 +from motor import Motor, MotorCluster, motor2040 + +""" +An example of applying a wave pattern to a group of motors and the LED. + +Press "Boot" to exit the program. + +NOTE: MotorCluster and Plasma WS2812 use the RP2040's PIO system, +and as such may have problems when running code multiple times. +If you encounter issues, try resetting your board. +""" + +SPEED = 5 # The speed that the LEDs will cycle at +BRIGHTNESS = 0.4 # The brightness of the LEDs +UPDATES = 50 # How many times to update LEDs and Motors per second +MOTOR_EXTENT = 1.0 # How far from zero to move the motors + +# Free up hardware resources ahead of creating a new MotorCluster +gc.collect() + +# Create a motor cluster for pins 0 to 7, using PIO 0 and State Machine 0 +# motors = MotorCluster(pio=0, sm=0, pins=list(range(START_PIN, END_PIN + 1))) +MOTOR_PINS = [ motor2040.MOTOR_1, motor2040.MOTOR_2, motor2040.MOTOR_3, motor2040.MOTOR_4] +motors = [Motor(pins) for pins in MOTOR_PINS] + +# Create the LED, using PIO 1 and State Machine 0 +led = WS2812(motor2040.NUM_LEDS, 1, 0, motor2040.LED_DATA) + +# Create the user button +user_sw = Button(motor2040.USER_SW) + +# Start updating the LED +led.start() + + +offset = 0.0 + +# Make waves until the user button is pressed +while user_sw.raw() is not True: + + offset += SPEED / 1000.0 + + # Update the LED + led.set_hsv(0, offset / 2, 1.0, BRIGHTNESS) + + # Update all the MOTORs + #for i in range(motors.count()): + for i in range(len(motors)): + angle = ((i / len(motors)) + offset) * math.pi + motors[i].speed(math.sin(angle) * MOTOR_EXTENT) + + time.sleep(1.0 / UPDATES) + +# Stop all the motors +for m in motors: + m.disable() + +# Turn off the LED bar +led.clear() diff --git a/micropython/examples/motor2040/multiple_motors.py b/micropython/examples/motor2040/multiple_motors.py new file mode 100644 index 00000000..32696607 --- /dev/null +++ b/micropython/examples/motor2040/multiple_motors.py @@ -0,0 +1,59 @@ +import time +import math +from motor import Motor, motor2040 + +""" +Demonstrates how to create multiple Motor objects and control them together. +""" + +# Create a list of motors +MOTOR_PINS = [ motor2040.MOTOR_1, motor2040.MOTOR_2, motor2040.MOTOR_3, motor2040.MOTOR_4] +motors = [Motor(pins) for pins in MOTOR_PINS] + +# Enable all motors (this puts them at the middle) +for m in motors: + m.enable() +time.sleep(2) + +# Go to min +for m in motors: + m.full_positive() +time.sleep(2) + +# Go to max +for m in motors: + m.full_negative() +time.sleep(2) + +# Go back to mid +for m in motors: + m.stop() +time.sleep(2) + +SWEEPS = 3 # How many sweeps of the motor to perform +STEPS = 10 # The number of discrete sweep steps +STEPS_INTERVAL = 0.5 # The time in seconds between each step of the sequence +SPEED_EXTENT = 1.0 # How far from zero to move the motor when sweeping + +# Do a sine sweep +for j in range(SWEEPS): + for i in range(360): + speed = math.sin(math.radians(i)) * SPEED_EXTENT + for s in motors: + s.speed(speed) + time.sleep(0.02) + +# Do a stepped sweep +for j in range(SWEEPS): + for i in range(0, STEPS): + for m in motors: + m.to_percent(i, 0, STEPS, 0.0 - SPEED_EXTENT, SPEED_EXTENT) + time.sleep(STEPS_INTERVAL) + for i in range(0, STEPS): + for m in motors: + m.to_percent(i, STEPS, 0, 0.0 - SPEED_EXTENT, SPEED_EXTENT) + time.sleep(STEPS_INTERVAL) + +# Disable the motors +for m in motors: + m.disable() diff --git a/micropython/examples/motor2040/read_sensors.py b/micropython/examples/motor2040/read_sensors.py new file mode 100644 index 00000000..e6c64c66 --- /dev/null +++ b/micropython/examples/motor2040/read_sensors.py @@ -0,0 +1,48 @@ +import time +from machine import Pin +from pimoroni import Analog, AnalogMux, Button +from servo import servo2040 + +""" +Shows how to initialise and read the 6 external +and 2 internal sensors of Servo 2040. + +Press "Boot" to exit the program. +""" + +# Set up the shared analog inputs +sen_adc = Analog(servo2040.SHARED_ADC) +vol_adc = Analog(servo2040.SHARED_ADC, servo2040.VOLTAGE_GAIN) +cur_adc = Analog(servo2040.SHARED_ADC, servo2040.CURRENT_GAIN, + servo2040.SHUNT_RESISTOR, servo2040.CURRENT_OFFSET) + +# Set up the analog multiplexer, including the pin for controlling pull-up/pull-down +mux = AnalogMux(servo2040.ADC_ADDR_0, servo2040.ADC_ADDR_1, servo2040.ADC_ADDR_2, + muxed_pin=Pin(servo2040.SHARED_ADC)) + +# Set up the sensor addresses and have them pulled down by default +sensor_addrs = list(range(servo2040.SENSOR_1_ADDR, servo2040.SENSOR_6_ADDR + 1)) +for addr in sensor_addrs: + mux.configure_pull(addr, Pin.PULL_DOWN) + +# Create the user button +user_sw = Button(servo2040.USER_SW) + + +# Read sensors until the user button is pressed +while user_sw.raw() is not True: + + # Read each sensor in turn and print its voltage + for i in range(len(sensor_addrs)): + mux.select(sensor_addrs[i]) + print("S", i + 1, " = ", round(sen_adc.read_voltage(), 3), sep="", end=", ") + + # Read the voltage sense and print the value + mux.select(servo2040.VOLTAGE_SENSE_ADDR) + print("Voltage =", round(vol_adc.read_voltage(), 4), end=", ") + + # Read the current sense and print the value + mux.select(servo2040.CURRENT_SENSE_ADDR) + print("Current =", round(cur_adc.read_current(), 4)) + + time.sleep(0.5) diff --git a/micropython/examples/motor2040/sensor_feedback.py b/micropython/examples/motor2040/sensor_feedback.py new file mode 100644 index 00000000..c781cc92 --- /dev/null +++ b/micropython/examples/motor2040/sensor_feedback.py @@ -0,0 +1,58 @@ +import time +from machine import Pin +from pimoroni import Analog, AnalogMux, Button +from plasma import WS2812 +from servo import servo2040 + +""" +Show how to read the 6 external sensors and +display their values on the neighbouring LEDs. + +Press "Boot" to exit the program. + +NOTE: Plasma WS2812 uses the RP2040's PIO system, and as +such may have problems when running code multiple times. +If you encounter issues, try resetting your board. +""" + +BRIGHTNESS = 0.4 # The brightness of the LEDs +UPDATES = 50 # How many times to update LEDs and Servos per second + +# Set up the shared analog inputs +sen_adc = Analog(servo2040.SHARED_ADC) + +# Set up the analog multiplexer, including the pin for controlling pull-up/pull-down +mux = AnalogMux(servo2040.ADC_ADDR_0, servo2040.ADC_ADDR_1, servo2040.ADC_ADDR_2, + muxed_pin=Pin(servo2040.SHARED_ADC)) + +# Set up the sensor addresses and have them pulled down by default +sensor_addrs = list(range(servo2040.SENSOR_1_ADDR, servo2040.SENSOR_6_ADDR + 1)) +for addr in sensor_addrs: + mux.configure_pull(addr, Pin.PULL_DOWN) + +# Create the LED bar, using PIO 1 and State Machine 0 +led_bar = WS2812(servo2040.NUM_LEDS, 1, 0, servo2040.LED_DATA) + +# Create the user button +user_sw = Button(servo2040.USER_SW) + +# Start updating the LED bar +led_bar.start() + + +# Read sensors until the user button is pressed +while user_sw.raw() is not True: + + # Read each sensor in turn and print its voltage + for i in range(len(sensor_addrs)): + mux.select(sensor_addrs[i]) + sensor_voltage = sen_adc.read_voltage() + + # Calculate the LED's hue, with Green for high voltages and Blue for low + hue = (2.0 - (sensor_voltage / 3.3)) * 0.333 + led_bar.set_hsv(i, hue, 1.0, BRIGHTNESS) + + print("S", i + 1, " = ", round(sensor_voltage, 3), sep="", end=", ") + print() + + time.sleep(1.0 / UPDATES) diff --git a/micropython/examples/motor2040/simple_easing.py b/micropython/examples/motor2040/simple_easing.py new file mode 100644 index 00000000..54647ebc --- /dev/null +++ b/micropython/examples/motor2040/simple_easing.py @@ -0,0 +1,64 @@ +import time +import math +import random +from pimoroni import Button +from servo import Servo, servo2040 + +""" +An example of how to move a servo smoothly between random positions. + +Press "Boot" to exit the program. +""" + +UPDATES = 50 # How many times to update Servos per second +TIME_FOR_EACH_MOVE = 2 # The time to travel between each random value +UPDATES_PER_MOVE = TIME_FOR_EACH_MOVE * UPDATES + +SERVO_EXTENT = 80 # How far from zero to move the servo +USE_COSINE = True # Whether or not to use a cosine path between values + +# Create a servo on pin 0 +s = Servo(servo2040.SERVO_1) + +# Get the initial value and create a random end value between the extents +start_value = s.mid_value() +end_value = random.uniform(-SERVO_EXTENT, SERVO_EXTENT) + +# Create the user button +user_sw = Button(servo2040.USER_SW) + + +update = 0 + +# Continually move the servo until the user button is pressed +while user_sw.raw() is not True: + + # Calculate how far along this movement to be + percent_along = update / UPDATES_PER_MOVE + + if USE_COSINE: + # Move the servo between values using cosine + s.to_percent(math.cos(percent_along * math.pi), 1.0, -1.0, start_value, end_value) + else: + # Move the servo linearly between values + s.to_percent(percent_along, 0.0, 1.0, start_value, end_value) + + # Print out the value the servo is now at + print("Value = ", round(s.value(), 3), sep="") + + # Move along in time + update += 1 + + # Have we reached the end of this movement? + if update >= UPDATES_PER_MOVE: + # Reset the counter + update = 0 + + # Set the start as the last end and create a new random end value + start_value = end_value + end_value = random.uniform(-SERVO_EXTENT, SERVO_EXTENT) + + time.sleep(1.0 / UPDATES) + +# Disable the servo +s.disable() diff --git a/micropython/examples/motor2040/single_motor.py b/micropython/examples/motor2040/single_motor.py new file mode 100644 index 00000000..3c7e81d4 --- /dev/null +++ b/micropython/examples/motor2040/single_motor.py @@ -0,0 +1,49 @@ +import time +import math +from motor import Motor, motor2040 + +""" +Demonstrates how to create a Motor object and control it. +""" + +# Create a motor on pins 4 and 5 +m = Motor(motor2040.MOTOR_1) + +# Enable the motor (this puts it at the middle) +m.enable() +time.sleep(2) + +# Go to full positive +m.full_positive() +time.sleep(2) + +# Go to full negative +m.full_negative() +time.sleep(2) + +# Stop moving +m.stop() +time.sleep(2) + + +SWEEPS = 3 # How many sweeps of the motor to perform +STEPS = 10 # The number of discrete sweep steps +STEPS_INTERVAL = 0.5 # The time in seconds between each step of the sequence + +# Do a sine sweep +for j in range(SWEEPS): + for i in range(360): + m.speed(math.sin(math.radians(i))) + time.sleep(0.02) + +# Do a stepped sweep +for j in range(SWEEPS): + for i in range(0, STEPS): + m.to_percent(i, 0, STEPS) + time.sleep(STEPS_INTERVAL) + for i in range(0, STEPS): + m.to_percent(i, STEPS, 0) + time.sleep(STEPS_INTERVAL) + +# Disable the motor +m.disable() diff --git a/micropython/examples/motor2040/turn_off_led.py b/micropython/examples/motor2040/turn_off_led.py new file mode 100644 index 00000000..a52af0d1 --- /dev/null +++ b/micropython/examples/motor2040/turn_off_led.py @@ -0,0 +1,16 @@ +from plasma import WS2812 +from motor import motor2040 + +""" +A simple program that turns off the onboard LED. + +NOTE: Plasma WS2812 uses the RP2040's PIO system, and as +such may have problems when running code multiple times. +If you encounter issues, try resetting your board. +""" + +# Create the LED, using PIO 1 and State Machine 0 +led = WS2812(motor2040.NUM_LEDS, 1, 0, motor2040.LED_DATA) + +# Start updating the LED +led.start() diff --git a/micropython/examples/motor2040/turn_off_motors.py b/micropython/examples/motor2040/turn_off_motors.py new file mode 100644 index 00000000..1a33dd9c --- /dev/null +++ b/micropython/examples/motor2040/turn_off_motors.py @@ -0,0 +1,13 @@ +from motor import Motor, motor2040 + +""" +A simple program that turns off the motors. +""" + +# Create four motor objects. +# This will initialise the pins, stopping any +# previous movement that may be stuck on +m1 = Motor(motor2040.MOTOR_1) +m2 = Motor(motor2040.MOTOR_2) +m3 = Motor(motor2040.MOTOR_3) +m4 = Motor(motor2040.MOTOR_4) diff --git a/micropython/modules/motor/motor.c b/micropython/modules/motor/motor.c index f5d34174..2a0acc40 100644 --- a/micropython/modules/motor/motor.c +++ b/micropython/modules/motor/motor.c @@ -248,10 +248,10 @@ STATIC const mp_map_elem_t motor_globals_table[] = { { MP_OBJ_NEW_QSTR(MP_QSTR_pico_motor_shim), (mp_obj_t)&pico_motor_shim_user_cmodule }, { MP_OBJ_NEW_QSTR(MP_QSTR_motor2040), (mp_obj_t)&motor2040_user_cmodule }, - // TODO - //{ MP_ROM_QSTR(MP_QSTR_ANGULAR), MP_ROM_INT(0x00) }, - //{ MP_ROM_QSTR(MP_QSTR_LINEAR), MP_ROM_INT(0x01) }, - //{ MP_ROM_QSTR(MP_QSTR_CONTINUOUS), MP_ROM_INT(0x02) }, + { MP_ROM_QSTR(MP_QSTR_NORMAL), MP_ROM_INT(0x00) }, + { MP_ROM_QSTR(MP_QSTR_REVERSED), MP_ROM_INT(0x01) }, + { MP_ROM_QSTR(MP_QSTR_FAST_DECAY), MP_ROM_INT(0x00) }, + { MP_ROM_QSTR(MP_QSTR_SLOW_DECAY), MP_ROM_INT(0x01) }, }; STATIC MP_DEFINE_CONST_DICT(mp_module_motor_globals, motor_globals_table); diff --git a/micropython/modules/motor/motor.cpp b/micropython/modules/motor/motor.cpp index d7a7fd84..88bceecd 100644 --- a/micropython/modules/motor/motor.cpp +++ b/micropython/modules/motor/motor.cpp @@ -28,9 +28,12 @@ void Motor_print(const mp_print_t *print, mp_obj_t self_in, mp_print_kind_t kind _Motor_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Motor_obj_t); mp_print_str(print, "Motor("); - mp_print_str(print, "pins = "); - mp_obj_print_helper(print, mp_obj_new_int(self->motor->pins().positive), PRINT_REPR); - mp_print_str(print, ", enabled = "); + mp_print_str(print, "pins = ("); + pin_pair pins = self->motor->pins(); + mp_obj_print_helper(print, mp_obj_new_int(pins.positive), PRINT_REPR); + mp_print_str(print, ", "); + mp_obj_print_helper(print, mp_obj_new_int(pins.negative), PRINT_REPR); + mp_print_str(print, "), enabled = "); mp_obj_print_helper(print, self->motor->is_enabled() ? mp_const_true : mp_const_false, PRINT_REPR); mp_print_str(print, ", duty = "); mp_obj_print_helper(print, mp_obj_new_float(self->motor->duty()), PRINT_REPR); @@ -38,6 +41,18 @@ void Motor_print(const mp_print_t *print, mp_obj_t self_in, mp_print_kind_t kind mp_obj_print_helper(print, mp_obj_new_float(self->motor->speed()), PRINT_REPR); mp_print_str(print, ", freq = "); mp_obj_print_helper(print, mp_obj_new_float(self->motor->frequency()), PRINT_REPR); + if(self->motor->direction() == MotorState::NORMAL) + mp_print_str(print, ", direction = NORMAL"); + else + mp_print_str(print, ", direction = REVERSED"); + mp_print_str(print, ", speed_scale = "); + mp_obj_print_helper(print, mp_obj_new_float(self->motor->speed_scale()), PRINT_REPR); + mp_print_str(print, ", deadzone_percent = "); + mp_obj_print_helper(print, mp_obj_new_float(self->motor->deadzone_percent()), PRINT_REPR); + if(self->motor->decay_mode() == MotorState::SLOW_DECAY) + mp_print_str(print, ", decay_mode = SLOW_DECAY"); + else + mp_print_str(print, ", decay_mode = FAST_DECAY"); mp_print_str(print, ")"); } @@ -47,9 +62,9 @@ void Motor_print(const mp_print_t *print, mp_obj_t self_in, mp_print_kind_t kind mp_obj_t Motor_make_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw, const mp_obj_t *all_args) { _Motor_obj_t *self = nullptr; - enum { ARG_pin, ARG_calibration, ARG_freq }; + enum { ARG_pins, ARG_calibration, ARG_freq }; static const mp_arg_t allowed_args[] = { - { MP_QSTR_pin, MP_ARG_REQUIRED | MP_ARG_INT }, + { MP_QSTR_pins, MP_ARG_REQUIRED | MP_ARG_OBJ }, { MP_QSTR_calibration, MP_ARG_OBJ, {.u_obj = mp_const_none} }, { MP_QSTR_freq, MP_ARG_OBJ, {.u_obj = mp_const_none} }, }; @@ -58,7 +73,41 @@ mp_obj_t Motor_make_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw, c mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; mp_arg_parse_all_kw_array(n_args, n_kw, all_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); - int pin = args[ARG_pin].u_int; + size_t pin_count = 0; + pin_pair pins; + + // Determine what pair of pins this motor will use + const mp_obj_t object = args[ARG_pins].u_obj; + mp_obj_t *items = nullptr; + if(mp_obj_is_type(object, &mp_type_list)) { + mp_obj_list_t *list = MP_OBJ_TO_PTR2(object, mp_obj_list_t); + pin_count = list->len; + items = list->items; + } + else if(mp_obj_is_type(object, &mp_type_tuple)) { + mp_obj_tuple_t *tuple = MP_OBJ_TO_PTR2(object, mp_obj_tuple_t); + pin_count = tuple->len; + items = tuple->items; + } + + if(items == nullptr) + mp_raise_TypeError("cannot convert object to a list or tuple of pins"); + else if(pin_count != 2) + mp_raise_TypeError("list or tuple must only contain two integers"); + else { + int pos = mp_obj_get_int(items[0]); + int neg = mp_obj_get_int(items[1]); + if((pos < 0 || pos >= (int)NUM_BANK0_GPIOS) || + (neg < 0 || neg >= (int)NUM_BANK0_GPIOS)) { + mp_raise_ValueError("a pin in the list or tuple is out of range. Expected 0 to 29"); + } + else if(pos == neg) { + mp_raise_ValueError("cannot use the same pin for motor positive and negative"); + } + + pins.positive = (uint8_t)pos; + pins.negative = (uint8_t)neg; + } //motor::Calibration *calib = nullptr; //motor::CalibrationType calibration_type = motor::CalibrationType::ANGULAR; @@ -90,7 +139,7 @@ mp_obj_t Motor_make_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw, c //if(calib != nullptr) // self->motor = new Motor(pin, *calib, freq); //else - self->motor = new Motor2(pin_pair(pin, pin));//TODO, calibration_type, freq); + self->motor = new Motor2(pins);//TODO, calibration_type, freq); self->motor->init(); return MP_OBJ_FROM_PTR(self); @@ -233,6 +282,7 @@ extern mp_obj_t Motor_frequency(size_t n_args, const mp_obj_t *pos_args, mp_map_ float freq = mp_obj_get_float(args[ARG_freq].u_obj); + // TODO confirm frequency range if(!self->motor->frequency(freq)) { mp_raise_ValueError("freq out of range. Expected 10Hz to 350Hz"); //TODO } @@ -494,9 +544,12 @@ void MotorCluster_print(const mp_print_t *print, mp_obj_t self_in, mp_print_kind uint8_t motor_count = self->cluster->count(); for(uint8_t motor = 0; motor < motor_count; motor++) { - mp_print_str(print, "\n\t{pins = "); - mp_obj_print_helper(print, mp_obj_new_int(self->cluster->pins(motor).positive), PRINT_REPR); - mp_print_str(print, ", enabled = "); + mp_print_str(print, "\n\t{pins = ("); + pin_pair pins = self->cluster->pins(motor); + mp_obj_print_helper(print, mp_obj_new_int(pins.positive), PRINT_REPR); + mp_print_str(print, ", "); + mp_obj_print_helper(print, mp_obj_new_int(pins.negative), PRINT_REPR); + mp_print_str(print, "), enabled = "); mp_obj_print_helper(print, self->cluster->is_enabled(motor) ? mp_const_true : mp_const_false, PRINT_REPR); mp_print_str(print, ", duty = "); mp_obj_print_helper(print, mp_obj_new_float(self->cluster->duty(motor)), PRINT_REPR); @@ -539,47 +592,79 @@ mp_obj_t MotorCluster_make_new(const mp_obj_type_t *type, size_t n_args, size_t PIO pio = args[ARG_pio].u_int == 0 ? pio0 : pio1; int sm = args[ARG_sm].u_int; - //uint pin_mask = 0; - //bool mask_provided = true; - uint32_t pair_count = 0; - pin_pair* pins = nullptr; + size_t pair_count = 0; + pin_pair *pins = nullptr; - // Determine what pins this cluster will use + // Determine what pair of pins this motor will use const mp_obj_t object = args[ARG_pins].u_obj; - if(mp_obj_is_int(object)) { - //pin_mask = (uint)mp_obj_get_int(object); + mp_obj_t *items = nullptr; + if(mp_obj_is_type(object, &mp_type_list)) { + mp_obj_list_t *list = MP_OBJ_TO_PTR2(object, mp_obj_list_t); + pair_count = list->len; + items = list->items; + } + else if(mp_obj_is_type(object, &mp_type_tuple)) { + mp_obj_tuple_t *tuple = MP_OBJ_TO_PTR2(object, mp_obj_tuple_t); + pair_count = tuple->len; + items = tuple->items; } - else { - mp_obj_t *items = nullptr; - if(mp_obj_is_type(object, &mp_type_list)) { - mp_obj_list_t *list = MP_OBJ_TO_PTR2(object, mp_obj_list_t); - pair_count = list->len; - items = list->items; - } - else if(mp_obj_is_type(object, &mp_type_tuple)) { - mp_obj_tuple_t *tuple = MP_OBJ_TO_PTR2(object, mp_obj_tuple_t); - pair_count = tuple->len; - items = tuple->items; - } - if(items == nullptr) - mp_raise_TypeError("cannot convert object to a list or tuple of pins, or a pin mask integer"); - else if(pair_count == 0) - mp_raise_TypeError("list or tuple must contain at least one integer"); + if(items == nullptr) + mp_raise_TypeError("cannot convert object to a list or tuple of pins"); + else if(pair_count == 0) + mp_raise_TypeError("list or tuple must contain at least one pair tuple"); + else { + // Specific check for is a single 2 pin list/tuple was provided + if(pair_count == 2 && mp_obj_is_int(items[0]) && mp_obj_is_int(items[1])) { + pins = new pin_pair[1]; + pair_count = 1; + + int pos = mp_obj_get_int(items[0]); + int neg = mp_obj_get_int(items[1]); + if((pos < 0 || pos >= (int)NUM_BANK0_GPIOS) || + (neg < 0 || neg >= (int)NUM_BANK0_GPIOS)) { + delete[] pins; + mp_raise_ValueError("a pin in the list or tuple is out of range. Expected 0 to 29"); + } + else if(pos == neg) { + delete[] pins; + mp_raise_ValueError("cannot use the same pin for motor positive and negative"); + } + + pins[0].positive = (uint8_t)pos; + pins[0].negative = (uint8_t)neg; + } else { // Create and populate a local array of pins pins = new pin_pair[pair_count]; for(size_t i = 0; i < pair_count; i++) { - int pin = mp_obj_get_int(items[i]); - if(pin < 0 || pin >= (int)NUM_BANK0_GPIOS) { + mp_obj_t obj = items[i]; + if(!mp_obj_is_type(obj, &mp_type_tuple)) { delete[] pins; - mp_raise_ValueError("a pin in the list or tuple is out of range. Expected 0 to 29"); + mp_raise_ValueError("cannot convert item to a pair tuple"); } else { - pins[i] = pin_pair((uint8_t)pin, (uint8_t)pin); //TODO + mp_obj_tuple_t *tuple = MP_OBJ_TO_PTR2(obj, mp_obj_tuple_t); + if(tuple->len != 2) { + delete[] pins; + mp_raise_ValueError("pair tuple must only contain two integers"); + } + int pos = mp_obj_get_int(tuple->items[0]); + int neg = mp_obj_get_int(tuple->items[1]); + if((pos < 0 || pos >= (int)NUM_BANK0_GPIOS) || + (neg < 0 || neg >= (int)NUM_BANK0_GPIOS)) { + delete[] pins; + mp_raise_ValueError("a pin in the pair tuple is out of range. Expected 0 to 29"); + } + else if(pos == neg) { + delete[] pins; + mp_raise_ValueError("cannot use the same pin for motor positive and negative"); + } + + pins[i].positive = (uint8_t)pos; + pins[i].negative = (uint8_t)neg; } } - //mask_provided = false; } } diff --git a/micropython/modules/motor/motor.h b/micropython/modules/motor/motor.h index 08b499c8..45dd0987 100644 --- a/micropython/modules/motor/motor.h +++ b/micropython/modules/motor/motor.h @@ -1,14 +1,6 @@ // Include MicroPython API. #include "py/runtime.h" -enum { - PICO_MOTOR_SHIM_BUTTON_A = 2, - PICO_MOTOR_SHIM_MOTOR_1P = 6, - PICO_MOTOR_SHIM_MOTOR_1N = 7, - PICO_MOTOR_SHIM_MOTOR_2P = 27, - PICO_MOTOR_SHIM_MOTOR_2N = 26, -}; - /***** Extern of Class Definition *****/ extern const mp_obj_type_t Motor_type; extern const mp_obj_type_t MotorCluster_type; From f3c0a305f2770fbb298e9cc923dce70d8eb225a4 Mon Sep 17 00:00:00 2001 From: ZodiusInfuser Date: Sat, 9 Apr 2022 01:41:42 +0100 Subject: [PATCH 07/60] Improvements to MotorState --- drivers/motor/motor2.cpp | 119 ++++++++++++---------- drivers/motor/motor2.hpp | 24 ++--- drivers/motor/motor_cluster.cpp | 150 +++++++++++++++------------- drivers/motor/motor_cluster.hpp | 54 +++++----- drivers/motor/motor_state.cpp | 53 +++++----- drivers/motor/motor_state.hpp | 35 +++---- micropython/modules/motor/motor.cpp | 14 +-- 7 files changed, 232 insertions(+), 217 deletions(-) diff --git a/drivers/motor/motor2.cpp b/drivers/motor/motor2.cpp index 35ad7f32..1d7a86d3 100644 --- a/drivers/motor/motor2.cpp +++ b/drivers/motor/motor2.cpp @@ -1,11 +1,11 @@ #include "motor2.hpp" #include "hardware/clocks.h" #include "pwm.hpp" +#include "math.h" namespace motor { - Motor2::Motor2(const pin_pair &pins, MotorState::Direction direction, float speed_scale, - float deadzone_percent, float freq, MotorState::DecayMode mode) - : motor_pins(pins), state(direction, speed_scale, deadzone_percent), pwm_frequency(freq), motor_decay_mode(mode) { + Motor2::Motor2(const pin_pair &pins, Direction direction, float speed_scale, float deadzone, float freq, DecayMode mode) + : motor_pins(pins), state(direction, speed_scale, deadzone), pwm_frequency(freq), motor_decay_mode(mode) { } Motor2::~Motor2() { @@ -47,11 +47,11 @@ namespace motor { } void Motor2::enable() { - apply_duty(state.enable_with_return()); + apply_duty(state.enable_with_return(), motor_decay_mode); } void Motor2::disable() { - apply_duty(state.disable_with_return()); + apply_duty(state.disable_with_return(), motor_decay_mode); } bool Motor2::is_enabled() const { @@ -63,7 +63,7 @@ namespace motor { } void Motor2::duty(float duty) { - apply_duty(state.set_duty_with_return(duty)); + apply_duty(state.set_duty_with_return(duty), motor_decay_mode); } float Motor2::speed() const { @@ -71,7 +71,7 @@ namespace motor { } void Motor2::speed(float speed) { - apply_duty(state.set_speed_with_return(speed)); + apply_duty(state.set_speed_with_return(speed), motor_decay_mode); } float Motor2::frequency() const { @@ -106,7 +106,7 @@ namespace motor { // If the the period is larger, update the pwm before setting the new wraps if(state.is_enabled() && pre_update_pwm) { - apply_duty(state.get_duty()); + apply_duty(state.get_deadzoned_duty(), motor_decay_mode); } // Set the new wrap (should be 1 less than the period to get full 0 to 100%) @@ -116,7 +116,7 @@ namespace motor { // If the the period is smaller, update the pwm after setting the new wraps if(state.is_enabled() && !pre_update_pwm) { - apply_duty(state.get_duty()); + apply_duty(state.get_deadzoned_duty(), motor_decay_mode); } success = true; @@ -125,36 +125,50 @@ namespace motor { return success; } + DecayMode Motor2::decay_mode() { + return motor_decay_mode; + } + + void Motor2::decay_mode(DecayMode mode) { + motor_decay_mode = mode; + if(state.is_enabled()) { + apply_duty(state.get_deadzoned_duty(), motor_decay_mode); + } + } + void Motor2::stop() { - apply_duty(state.stop_with_return()); + apply_duty(state.stop_with_return(), motor_decay_mode); } void Motor2::coast() { - state.set_duty_with_return(0.0f); - disable(); + apply_duty(state.stop_with_return(), FAST_DECAY); + } + + void Motor2::brake() { + apply_duty(state.stop_with_return(), SLOW_DECAY); } void Motor2::full_negative() { - apply_duty(state.full_negative_with_return()); + apply_duty(state.full_negative_with_return(), motor_decay_mode); } void Motor2::full_positive() { - apply_duty(state.full_positive_with_return()); + apply_duty(state.full_positive_with_return(), motor_decay_mode); } void Motor2::to_percent(float in, float in_min, float in_max) { - apply_duty(state.to_percent_with_return(in, in_min, in_max)); + apply_duty(state.to_percent_with_return(in, in_min, in_max), motor_decay_mode); } void Motor2::to_percent(float in, float in_min, float in_max, float speed_min, float speed_max) { - apply_duty(state.to_percent_with_return(in, in_min, in_max, speed_min, speed_max)); + apply_duty(state.to_percent_with_return(in, in_min, in_max, speed_min, speed_max), motor_decay_mode); } - MotorState::Direction Motor2::direction() const { + Direction Motor2::direction() const { return state.get_direction(); } - void Motor2::direction(MotorState::Direction direction) { + void Motor2::direction(Direction direction) { state.set_direction(direction); } @@ -166,49 +180,46 @@ namespace motor { state.set_speed_scale(speed_scale); } - float Motor2::deadzone_percent() const { - return state.get_deadzone_percent(); + float Motor2::deadzone() const { + return state.get_deadzone(); } - void Motor2::deadzone_percent(float speed_scale) { - apply_duty(state.set_deadzone_percent_with_return(speed_scale)); + void Motor2::deadzone(float deadzone) { + apply_duty(state.set_deadzone_with_return(deadzone), motor_decay_mode); } - MotorState::DecayMode Motor2::decay_mode() { - return motor_decay_mode; - } + void Motor2::apply_duty(float duty, DecayMode mode) { + if(isfinite(duty)) { + int32_t signed_level = MotorState::duty_to_level(duty, pwm_period); - void Motor2::decay_mode(MotorState::DecayMode mode) { - motor_decay_mode = mode; - apply_duty(state.get_duty()); - } + 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; - void Motor2::apply_duty(float duty) { - int32_t signed_level = MotorState::duty_to_level(duty, pwm_period); - - switch(motor_decay_mode) { - case MotorState::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); + 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, pwm_period + signed_level); - pwm_set_gpio_level(motor_pins.negative, pwm_period); - } - break; - - case MotorState::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); } } }; \ No newline at end of file diff --git a/drivers/motor/motor2.hpp b/drivers/motor/motor2.hpp index b81fc39b..e3a21122 100644 --- a/drivers/motor/motor2.hpp +++ b/drivers/motor/motor2.hpp @@ -19,15 +19,14 @@ namespace motor { pwm_config pwm_cfg; uint16_t pwm_period; float pwm_frequency; - MotorState::DecayMode motor_decay_mode; - + DecayMode motor_decay_mode; //-------------------------------------------------- // Constructors/Destructor //-------------------------------------------------- public: - Motor2(const pin_pair &pins, MotorState::Direction direction = MotorState::DEFAULT_DIRECTION, float speed_scale = MotorState::DEFAULT_SPEED_SCALE, - float deadzone_percent = MotorState::DEFAULT_DEADZONE, float freq = MotorState::DEFAULT_FREQUENCY, MotorState::DecayMode mode = MotorState::DEFAULT_DECAY_MODE); + Motor2(const pin_pair &pins, Direction direction = NORMAL, float speed_scale = MotorState::DEFAULT_SPEED_SCALE, + float deadzone = MotorState::DEFAULT_DEADZONE, float freq = MotorState::DEFAULT_FREQUENCY, DecayMode mode = MotorState::DEFAULT_DECAY_MODE); ~Motor2(); @@ -53,10 +52,14 @@ namespace motor { float frequency() const; bool frequency(float freq); + DecayMode decay_mode(); + void decay_mode(DecayMode mode); + //-------------------------------------------------- void stop(); void coast(); + void brake(); //TODO void full_negative(); void full_positive(); void to_percent(float in, float in_min = MotorState::ZERO_PERCENT, float in_max = MotorState::ONEHUNDRED_PERCENT); @@ -64,21 +67,18 @@ namespace motor { //-------------------------------------------------- - MotorState::Direction direction() const; - void direction(MotorState::Direction direction); + Direction direction() const; + void direction(Direction direction); float speed_scale() const; void speed_scale(float speed_scale); - float deadzone_percent() const; - void deadzone_percent(float deadzone_percent); - - MotorState::DecayMode decay_mode(); - void decay_mode(MotorState::DecayMode mode); + float deadzone() const; + void deadzone(float deadzone); //-------------------------------------------------- private: - void apply_duty(float duty); + void apply_duty(float duty, DecayMode mode); }; } \ No newline at end of file diff --git a/drivers/motor/motor_cluster.cpp b/drivers/motor/motor_cluster.cpp index eaba20b7..0e820d25 100644 --- a/drivers/motor/motor_cluster.cpp +++ b/drivers/motor/motor_cluster.cpp @@ -1,32 +1,34 @@ #include "motor_cluster.hpp" #include "pwm.hpp" #include +#include "math.h" namespace motor { - MotorCluster::MotorCluster(PIO pio, uint sm, uint pin_base, uint pin_pair_count, MotorState::Direction direction, - float speed_scale, float deadzone_percent, float freq, MotorState::DecayMode mode, + MotorCluster::MotorCluster(PIO pio, uint sm, uint pin_base, uint pin_pair_count, Direction direction, + float speed_scale, 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), pwm_frequency(freq) { - create_motor_states(direction, speed_scale, deadzone_percent, mode, auto_phase); + create_motor_states(direction, speed_scale, deadzone, mode, auto_phase); } - MotorCluster::MotorCluster(PIO pio, uint sm, const pin_pair *pin_pairs, uint32_t length, MotorState::Direction direction, - float speed_scale, float deadzone_percent, float freq, MotorState::DecayMode mode, + MotorCluster::MotorCluster(PIO pio, uint sm, const pin_pair *pin_pairs, uint32_t length, Direction direction, + float speed_scale, 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), pwm_frequency(freq) { - create_motor_states(direction, speed_scale, deadzone_percent, mode, auto_phase); + create_motor_states(direction, speed_scale, deadzone, mode, auto_phase); } - MotorCluster::MotorCluster(PIO pio, uint sm, std::initializer_list pin_pairs, MotorState::Direction direction, - float speed_scale, float deadzone_percent, float freq, MotorState::DecayMode mode, + MotorCluster::MotorCluster(PIO pio, uint sm, std::initializer_list pin_pairs, Direction direction, + float speed_scale, 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), pwm_frequency(freq) { - create_motor_states(direction, speed_scale, deadzone_percent, mode, auto_phase); + create_motor_states(direction, speed_scale, deadzone, mode, auto_phase); } MotorCluster::~MotorCluster() { delete[] states; delete[] motor_phases; + delete[] motor_modes; } bool MotorCluster::init() { @@ -74,7 +76,7 @@ namespace 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, load); + apply_duty(motor, new_duty, motor_modes[motor], load); } void MotorCluster::enable(const uint8_t *motors, uint8_t length, bool load) { @@ -106,7 +108,7 @@ namespace motor { 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, load); + apply_duty(motor, new_duty, motor_modes[motor], load); } void MotorCluster::disable(const uint8_t *motors, uint8_t length, bool load) { @@ -148,7 +150,7 @@ namespace motor { 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, load); + apply_duty(motor, new_duty, motor_modes[motor], load); } void MotorCluster::duty(const uint8_t *motors, uint8_t length, float duty, bool load) { @@ -185,7 +187,7 @@ namespace motor { 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, load); + apply_duty(motor, new_duty, motor_modes[motor], load); } void MotorCluster::speed(const uint8_t *motors, uint8_t length, float speed, bool load) { @@ -270,7 +272,7 @@ namespace motor { uint8_t motor_count = pwms.get_chan_pair_count(); for(uint motor = 0; motor < motor_count; motor++) { if(states[motor].is_enabled()) { - apply_duty(motor, states[motor].get_duty(), false); + apply_duty(motor, states[motor].get_deadzoned_duty(), motor_modes[motor], false); } pwms.set_chan_offset(motor_positive(motor), (uint32_t)(motor_phases[motor] * (float)pwm_period), false); pwms.set_chan_offset(motor_negative(motor), (uint32_t)(motor_phases[motor] * (float)pwm_period), false); @@ -293,7 +295,7 @@ namespace motor { 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, load); + apply_duty(motor, new_duty, motor_modes[motor], load); } void MotorCluster::stop(const uint8_t *motors, uint8_t length, bool load) { @@ -326,7 +328,7 @@ namespace motor { assert(motor < pwms.get_chan_pair_count()); states[motor].set_duty_with_return(0.0f); float new_duty = states[motor].disable_with_return(); - apply_duty(motor, new_duty, load); + apply_duty(motor, new_duty, motor_modes[motor], load); } void MotorCluster::coast(const uint8_t *motors, uint8_t length, bool load) { @@ -358,7 +360,7 @@ namespace motor { 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, load); + apply_duty(motor, new_duty, motor_modes[motor], load); } void MotorCluster::full_negative(const uint8_t *motors, uint8_t length, bool load) { @@ -378,7 +380,7 @@ namespace motor { pwms.load_pwm(); } - void MotorCluster::all_to_full_negative(bool load) { + 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); @@ -390,7 +392,7 @@ namespace motor { 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, load); + apply_duty(motor, new_duty, motor_modes[motor], load); } void MotorCluster::full_positive(const uint8_t *motors, uint8_t length, bool load) { @@ -410,7 +412,7 @@ namespace motor { pwms.load_pwm(); } - void MotorCluster::all_to_full_positive(bool load) { + 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); @@ -422,7 +424,7 @@ namespace motor { 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, load); + apply_duty(motor, new_duty, motor_modes[motor], load); } void MotorCluster::to_percent(const uint8_t *motors, uint8_t length, float in, float in_min, float in_max, bool load) { @@ -454,7 +456,7 @@ namespace motor { 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, load); + apply_duty(motor, new_duty, motor_modes[motor], 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) { @@ -487,30 +489,30 @@ namespace motor { pwms.load_pwm(); } - MotorState::Direction MotorCluster::direction(uint8_t motor) const { + 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, MotorState::Direction 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, MotorState::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 motors, MotorState::Direction direction) { + void MotorCluster::direction(std::initializer_list motors, Direction direction) { for(auto motor : motors) { this->direction(motor, direction); } } - void MotorCluster::all_directions(MotorState::Direction 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); @@ -547,60 +549,60 @@ namespace motor { } } - float MotorCluster::deadzone_percent(uint8_t motor) const { + float MotorCluster::deadzone(uint8_t motor) const { assert(motor < pwms.get_chan_pair_count()); - return states[motor].get_deadzone_percent(); + return states[motor].get_deadzone(); } - void MotorCluster::deadzone_percent(uint8_t motor, float deadzone_percent) { + void MotorCluster::deadzone(uint8_t motor, float deadzone) { assert(motor < pwms.get_chan_pair_count()); - states[motor].set_deadzone_percent_with_return(deadzone_percent); //TODO + states[motor].set_deadzone_with_return(deadzone); //TODO } - void MotorCluster::deadzone_percent(const uint8_t *motors, uint8_t length, float deadzone_percent) { + void MotorCluster::deadzone(const uint8_t *motors, uint8_t length, float deadzone) { assert(motors != nullptr); for(uint8_t i = 0; i < length; i++) { - this->deadzone_percent(motors[i], deadzone_percent); + this->deadzone(motors[i], deadzone); } } - void MotorCluster::deadzone_percent(std::initializer_list motors, float deadzone_percent) { + void MotorCluster::deadzone(std::initializer_list motors, float deadzone) { for(auto motor : motors) { - this->deadzone_percent(motor, deadzone_percent); + this->deadzone(motor, deadzone); } } - void MotorCluster::all_deadzone_percents(float deadzone_percent) { + void MotorCluster::all_deadzones(float deadzone) { uint8_t motor_count = pwms.get_chan_pair_count(); for(uint8_t motor = 0; motor < motor_count; motor++) { - this->deadzone_percent(motor, deadzone_percent); + this->deadzone(motor, deadzone); } } - MotorState::DecayMode MotorCluster::decay_mode(uint8_t motor) const { + DecayMode MotorCluster::decay_mode(uint8_t motor) const { assert(motor < pwms.get_chan_pair_count()); - return MotorState::SLOW_DECAY;//TODO states[motor].get_decay_mode(); + return SLOW_DECAY;//TODO states[motor].get_decay_mode(); } - void MotorCluster::decay_mode(uint8_t motor, MotorState::DecayMode mode) { + void MotorCluster::decay_mode(uint8_t motor, DecayMode mode) { assert(motor < pwms.get_chan_pair_count()); //TODO states[motor].set_decay_mode(mode); } - void MotorCluster::decay_mode(const uint8_t *motors, uint8_t length, MotorState::DecayMode mode) { + void MotorCluster::decay_mode(const uint8_t *motors, uint8_t length, DecayMode mode) { assert(motors != nullptr); for(uint8_t i = 0; i < length; i++) { this->decay_mode(motors[i], mode); } } - void MotorCluster::decay_mode(std::initializer_list motors, MotorState::DecayMode mode) { + void MotorCluster::decay_mode(std::initializer_list motors, DecayMode mode) { for(auto motor : motors) { this->decay_mode(motor, mode); } } - void MotorCluster::all_decay_modes(MotorState::DecayMode mode) { + void MotorCluster::all_to_decay_mode(DecayMode mode) { uint8_t motor_count = pwms.get_chan_pair_count(); for(uint8_t motor = 0; motor < motor_count; motor++) { this->decay_mode(motor, mode); @@ -608,47 +610,53 @@ namespace motor { } - void MotorCluster::apply_duty(uint8_t motor, float duty, bool load) { - int32_t signed_level = MotorState::duty_to_level(duty, pwm_period); + 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); - //TODO move this into motor state - MotorState::DecayMode decay_mode = MotorState::SLOW_DECAY; - switch(decay_mode) { - case MotorState::SLOW_DECAY: //aka 'Braking' - if(signed_level >= 0) { - pwms.set_chan_level(motor_positive(motor), pwm_period, false); - pwms.set_chan_level(motor_negative(motor), pwm_period - signed_level, load); - } - else { - pwms.set_chan_level(motor_positive(motor), pwm_period + signed_level, false); - pwms.set_chan_level(motor_negative(motor), pwm_period, load); - } - break; + switch(mode) { + case SLOW_DECAY: //aka 'Braking' + if(signed_level >= 0) { + pwms.set_chan_level(motor_positive(motor), pwm_period, false); + pwms.set_chan_level(motor_negative(motor), pwm_period - signed_level, load); + } + else { + pwms.set_chan_level(motor_positive(motor), pwm_period + signed_level, false); + pwms.set_chan_level(motor_negative(motor), pwm_period, load); + } + break; - case MotorState::FAST_DECAY: //aka 'Coasting' - default: - if(signed_level >= 0) { - pwms.set_chan_level(motor_positive(motor), signed_level, false); - pwms.set_chan_level(motor_negative(motor), 0, load); + case FAST_DECAY: //aka 'Coasting' + default: + if(signed_level >= 0) { + pwms.set_chan_level(motor_positive(motor), signed_level, false); + pwms.set_chan_level(motor_negative(motor), 0, load); + } + else { + pwms.set_chan_level(motor_positive(motor), 0, false); + pwms.set_chan_level(motor_negative(motor), 0 - signed_level, load); + } + break; } - else { - pwms.set_chan_level(motor_positive(motor), 0, false); - pwms.set_chan_level(motor_negative(motor), 0 - signed_level, load); - } - break; + } + else { + pwms.set_chan_level(motor_positive(motor), 0, false); + pwms.set_chan_level(motor_negative(motor), 0, load); } } - void MotorCluster::create_motor_states(MotorState::Direction direction, float speed_scale, - float deadzone_percent, MotorState::DecayMode mode, bool auto_phase) { + void MotorCluster::create_motor_states(Direction direction, float speed_scale, + 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]; motor_phases = new float[motor_count]; + motor_modes = new DecayMode[motor_count]; for(uint motor = 0; motor < motor_count; motor++) { - states[motor] = MotorState(direction, speed_scale, deadzone_percent); + states[motor] = MotorState(direction, speed_scale, deadzone); motor_phases[motor] = (auto_phase) ? (float)motor / (float)motor_count : 0.0f; + motor_modes[motor] = mode; } } } diff --git a/drivers/motor/motor_cluster.hpp b/drivers/motor/motor_cluster.hpp index 96384149..435bb683 100644 --- a/drivers/motor/motor_cluster.hpp +++ b/drivers/motor/motor_cluster.hpp @@ -18,20 +18,21 @@ namespace motor { float pwm_frequency; MotorState* states; float* motor_phases; + DecayMode* motor_modes; //-------------------------------------------------- // Constructors/Destructor //-------------------------------------------------- public: - MotorCluster(PIO pio, uint sm, uint pin_base, uint pin_pair_count, MotorState::Direction direction = MotorState::DEFAULT_DIRECTION, float speed_scale = MotorState::DEFAULT_SPEED_SCALE, - float deadzone_percent = MotorState::DEFAULT_DEADZONE, float freq = MotorState::DEFAULT_FREQUENCY, MotorState::DecayMode mode = MotorState::DEFAULT_DECAY_MODE, + MotorCluster(PIO pio, uint sm, uint pin_base, uint pin_pair_count, Direction direction = NORMAL, float speed_scale = MotorState::DEFAULT_SPEED_SCALE, + 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, MotorState::Direction direction = MotorState::DEFAULT_DIRECTION, float speed_scale = MotorState::DEFAULT_SPEED_SCALE, - float deadzone_percent = MotorState::DEFAULT_DEADZONE, float freq = MotorState::DEFAULT_FREQUENCY, MotorState::DecayMode mode = MotorState::DEFAULT_DECAY_MODE, + MotorCluster(PIO pio, uint sm, const pin_pair *pin_pairs, uint32_t length, Direction direction = NORMAL, float speed_scale = MotorState::DEFAULT_SPEED_SCALE, + 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_pairs, MotorState::Direction direction = MotorState::DEFAULT_DIRECTION, float speed_scale = MotorState::DEFAULT_SPEED_SCALE, - float deadzone_percent = MotorState::DEFAULT_DEADZONE, float freq = MotorState::DEFAULT_FREQUENCY, MotorState::DecayMode mode = MotorState::DEFAULT_DECAY_MODE, + MotorCluster(PIO pio, uint sm, std::initializer_list pin_pairs, Direction direction = NORMAL, float speed_scale = MotorState::DEFAULT_SPEED_SCALE, + 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(); @@ -78,6 +79,12 @@ namespace motor { float frequency() const; bool frequency(float freq); + DecayMode decay_mode(uint8_t motor) const; + void decay_mode(uint8_t motor, DecayMode mode); + void decay_mode(const uint8_t *motors, uint8_t length, DecayMode mode); + void decay_mode(std::initializer_list motors, DecayMode mode); + void all_to_decay_mode(DecayMode mode); + //-------------------------------------------------- void stop(uint8_t motor, bool load = true); void stop(const uint8_t *motors, uint8_t length, bool load = true); @@ -92,12 +99,12 @@ namespace motor { 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 motors, bool load = true); - void all_to_full_negative(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 motors, bool load = true); - void all_to_full_positive(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); @@ -113,11 +120,11 @@ namespace motor { //-------------------------------------------------- - MotorState::Direction direction(uint8_t motor) const; - void direction(uint8_t motor, MotorState::Direction direction); - void direction(const uint8_t *motors, uint8_t length, MotorState::Direction direction); - void direction(std::initializer_list motors, MotorState::Direction direction); - void all_directions(MotorState::Direction direction); + 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 motors, Direction direction); + void all_directions(Direction direction); float speed_scale(uint8_t motor) const; void speed_scale(uint8_t motor, float speed_scale); @@ -125,23 +132,16 @@ namespace motor { void speed_scale(std::initializer_list motors, float speed_scale); void all_speed_scales(float speed_scale); - float deadzone_percent(uint8_t motor) const; - void deadzone_percent(uint8_t motor, float deadzone_percent); - void deadzone_percent(const uint8_t *motors, uint8_t length, float deadzone_percent); - void deadzone_percent(std::initializer_list motors, float deadzone_percent); - void all_deadzone_percents(float deadzone_percent); - - MotorState::DecayMode decay_mode(uint8_t motor) const; - void decay_mode(uint8_t motor, MotorState::DecayMode mode); - void decay_mode(const uint8_t *motors, uint8_t length, MotorState::DecayMode mode); - void decay_mode(std::initializer_list motors, MotorState::DecayMode mode); - void all_decay_modes(MotorState::DecayMode mode); + float deadzone(uint8_t motor) const; + void deadzone(uint8_t motor, float deadzone); + void deadzone(const uint8_t *motors, uint8_t length, float deadzone); + void deadzone(std::initializer_list motors, float deadzone); + void all_deadzones(float deadzone); //-------------------------------------------------- private: - void apply_duty(uint8_t motor, float duty, bool load); - void create_motor_states(MotorState::Direction direction, float speed_scale, - float deadzone_percent, MotorState::DecayMode mode, bool auto_phase); + void apply_duty(uint8_t motor, float duty, DecayMode mode, bool load); + void create_motor_states(Direction direction, float speed_scale, float deadzone, DecayMode mode, bool auto_phase); static uint8_t motor_positive(uint8_t motor) { return PWMCluster::channel_from_pair(motor); diff --git a/drivers/motor/motor_state.cpp b/drivers/motor/motor_state.cpp index 9e9da46b..00e381b5 100644 --- a/drivers/motor/motor_state.cpp +++ b/drivers/motor/motor_state.cpp @@ -1,34 +1,27 @@ #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(DEFAULT_DIRECTION), motor_scale(DEFAULT_SPEED_SCALE), motor_deadzone(DEFAULT_DEADZONE) { + motor_direction(NORMAL), motor_scale(DEFAULT_SPEED_SCALE), motor_deadzone(DEFAULT_DEADZONE){ } - MotorState::MotorState(Direction direction, float speed_scale, float deadzone_percent) + MotorState::MotorState(Direction direction, float speed_scale, float deadzone) : motor_speed(0.0f), last_enabled_duty(0.0f), enabled(false), - motor_direction(direction), motor_scale(MAX(speed_scale, FLT_EPSILON)), motor_deadzone(CLAMP(deadzone_percent, 0.0f, 1.0f)) { + motor_direction(direction), motor_scale(MAX(speed_scale, FLT_EPSILON)), motor_deadzone(CLAMP(deadzone, 0.0f, 1.0f)) { } float MotorState::enable_with_return() { enabled = true; - - float duty = 0.0f; - if((last_enabled_duty <= 0.0f - motor_deadzone) || (last_enabled_duty >= motor_deadzone)) { - if(motor_direction == NORMAL) - duty = last_enabled_duty; - else - duty = 0.0f - last_enabled_duty; - } - return duty; + return get_deadzoned_duty(); } float MotorState::disable_with_return() { enabled = false; - return 0.0f; // A zero duty + return NAN; } bool MotorState::is_enabled() const { @@ -39,6 +32,14 @@ namespace motor { return 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; + } + return duty; + } + float MotorState::set_duty_with_return(float duty) { // Clamp the duty between the hard limits last_enabled_duty = CLAMP(duty, -1.0f, 1.0f); @@ -48,10 +49,14 @@ namespace motor { } float MotorState::get_speed() const { - return motor_speed; + return (motor_direction == NORMAL) ? 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) + speed = 0.0f - speed; + // Clamp the speed between the hard limits motor_speed = CLAMP(speed, -motor_scale, motor_scale); last_enabled_duty = motor_speed / motor_scale; @@ -81,16 +86,12 @@ namespace motor { return set_speed_with_return(speed); } - MotorState::Direction MotorState::get_direction() const { + Direction MotorState::get_direction() const { return motor_direction; } void MotorState::set_direction(Direction direction) { - if(direction != motor_direction) { - motor_direction = direction; - motor_speed = 0.0f - motor_speed; - last_enabled_duty = 0.0f - last_enabled_duty; - } + motor_direction = direction; } float MotorState::get_speed_scale() const { @@ -103,16 +104,16 @@ namespace motor { } - float MotorState::get_deadzone_percent() const { - return motor_scale; + float MotorState::get_deadzone() const { + return motor_deadzone; } - float MotorState::set_deadzone_percent_with_return(float deadzone_percent) { - motor_deadzone = CLAMP(deadzone_percent, 0.0f, 1.0f); + float MotorState::set_deadzone_with_return(float deadzone) { + motor_deadzone = CLAMP(deadzone, 0.0f, 1.0f); if(enabled) - return enable_with_return(); + return get_deadzoned_duty(); else - return disable_with_return(); + return NAN; } int32_t MotorState::duty_to_level(float duty, uint32_t resolution) { diff --git a/drivers/motor/motor_state.hpp b/drivers/motor/motor_state.hpp index 8bf94300..990ecbd3 100644 --- a/drivers/motor/motor_state.hpp +++ b/drivers/motor/motor_state.hpp @@ -4,30 +4,24 @@ namespace motor { + enum Direction { + NORMAL = 0, + REVERSED = 1, + }; + + enum DecayMode { + FAST_DECAY = 0, //aka 'Coasting' + SLOW_DECAY = 1, //aka 'Braking' + }; + class MotorState { - //-------------------------------------------------- - // Enums - //-------------------------------------------------- - public: - enum DecayMode { - FAST_DECAY = 0, //aka 'Coasting' - SLOW_DECAY = 1, //aka 'Braking' - }; - - enum Direction { - NORMAL = 0, - REVERSED = 1, - }; - - //-------------------------------------------------- // Constants //-------------------------------------------------- public: - static const Direction DEFAULT_DIRECTION = NORMAL; // The standard motor direction static constexpr float DEFAULT_SPEED_SCALE = 1.0f; // The standard motor speed scale static constexpr float DEFAULT_DEADZONE = 0.1f; // 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; @@ -56,7 +50,7 @@ namespace motor { //-------------------------------------------------- public: MotorState(); - MotorState(Direction direction, float speed_scale, float deadzone_percent); + MotorState(Direction direction, float speed_scale, float deadzone); //-------------------------------------------------- @@ -68,6 +62,7 @@ namespace motor { bool is_enabled() const; float get_duty() const; + float get_deadzoned_duty() const; float set_duty_with_return(float duty); float get_speed() const; @@ -89,8 +84,8 @@ namespace motor { float get_speed_scale() const; void set_speed_scale(float speed_scale); - float get_deadzone_percent() const; - float set_deadzone_percent_with_return(float deadzone_percent); + float get_deadzone() const; + float set_deadzone_with_return(float deadzone); //-------------------------------------------------- static int32_t duty_to_level(float duty, uint32_t resolution); diff --git a/micropython/modules/motor/motor.cpp b/micropython/modules/motor/motor.cpp index 88bceecd..1351f793 100644 --- a/micropython/modules/motor/motor.cpp +++ b/micropython/modules/motor/motor.cpp @@ -47,8 +47,8 @@ void Motor_print(const mp_print_t *print, mp_obj_t self_in, mp_print_kind_t kind mp_print_str(print, ", direction = REVERSED"); mp_print_str(print, ", speed_scale = "); mp_obj_print_helper(print, mp_obj_new_float(self->motor->speed_scale()), PRINT_REPR); - mp_print_str(print, ", deadzone_percent = "); - mp_obj_print_helper(print, mp_obj_new_float(self->motor->deadzone_percent()), PRINT_REPR); + mp_print_str(print, ", deadzone = "); + mp_obj_print_helper(print, mp_obj_new_float(self->motor->deadzone()), PRINT_REPR); if(self->motor->decay_mode() == MotorState::SLOW_DECAY) mp_print_str(print, ", decay_mode = SLOW_DECAY"); else @@ -465,7 +465,7 @@ extern mp_obj_t Motor_deadzone_percent(size_t n_args, const mp_obj_t *pos_args, _Motor_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Motor_obj_t); - return mp_obj_new_float(self->motor->deadzone_percent()); + return mp_obj_new_float(self->motor->deadzone()); } else { enum { ARG_self, ARG_deadzone_percent }; @@ -480,9 +480,9 @@ extern mp_obj_t Motor_deadzone_percent(size_t n_args, const mp_obj_t *pos_args, _Motor_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Motor_obj_t); - float deadzone_percent = mp_obj_get_float(args[ARG_deadzone_percent].u_obj); + float deadzone = mp_obj_get_float(args[ARG_deadzone_percent].u_obj); - self->motor->deadzone_percent(deadzone_percent); + self->motor->deadzone(deadzone); return mp_const_none; } } @@ -1605,7 +1605,7 @@ extern mp_obj_t MotorCluster_all_to_full_negative(size_t n_args, const mp_obj_t if(motor_count == 0) mp_raise_ValueError("this cluster does not have any motors"); else { - self->cluster->all_to_full_negative(args[ARG_load].u_bool); + self->cluster->all_full_negative(args[ARG_load].u_bool); } return mp_const_none; } @@ -1693,7 +1693,7 @@ extern mp_obj_t MotorCluster_all_to_full_positive(size_t n_args, const mp_obj_t if(motor_count == 0) mp_raise_ValueError("this cluster does not have any motors"); else { - self->cluster->all_to_full_positive(args[ARG_load].u_bool); + self->cluster->all_full_positive(args[ARG_load].u_bool); } return mp_const_none; } From 5f6e4a3096a7408f963f82aa60afebebc0813873 Mon Sep 17 00:00:00 2001 From: ZodiusInfuser Date: Mon, 11 Apr 2022 13:35:53 +0100 Subject: [PATCH 08/60] Finalised C++ motor implementation --- drivers/motor/motor2.cpp | 58 +++++++------ drivers/motor/motor2.hpp | 10 +-- drivers/motor/motor_cluster.cpp | 139 ++++++++++++++++++++------------ drivers/motor/motor_cluster.hpp | 49 ++++++----- drivers/motor/motor_state.cpp | 13 +-- 5 files changed, 154 insertions(+), 115 deletions(-) diff --git a/drivers/motor/motor2.cpp b/drivers/motor/motor2.cpp index 1d7a86d3..71c0c4fa 100644 --- a/drivers/motor/motor2.cpp +++ b/drivers/motor/motor2.cpp @@ -5,7 +5,7 @@ namespace motor { Motor2::Motor2(const pin_pair &pins, Direction direction, float speed_scale, float deadzone, float freq, DecayMode mode) - : motor_pins(pins), state(direction, speed_scale, deadzone), pwm_frequency(freq), motor_decay_mode(mode) { + : motor_pins(pins), state(direction, speed_scale, deadzone), pwm_frequency(freq), motor_mode(mode) { } Motor2::~Motor2() { @@ -29,9 +29,9 @@ namespace motor { pwm_config_set_clkdiv(&pwm_cfg, (float)div16 / 16.0f); // There's no 'pwm_config_set_clkdiv_int_frac' for some reason... pwm_init(pwm_gpio_to_slice_num(motor_pins.positive), &pwm_cfg, true); - gpio_set_function(motor_pins.positive, GPIO_FUNC_PWM); - 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); @@ -47,11 +47,11 @@ namespace motor { } void Motor2::enable() { - apply_duty(state.enable_with_return(), motor_decay_mode); + apply_duty(state.enable_with_return(), motor_mode); } void Motor2::disable() { - apply_duty(state.disable_with_return(), motor_decay_mode); + apply_duty(state.disable_with_return(), motor_mode); } bool Motor2::is_enabled() const { @@ -63,7 +63,7 @@ namespace motor { } void Motor2::duty(float duty) { - apply_duty(state.set_duty_with_return(duty), motor_decay_mode); + apply_duty(state.set_duty_with_return(duty), motor_mode); } float Motor2::speed() const { @@ -71,7 +71,7 @@ namespace motor { } void Motor2::speed(float speed) { - apply_duty(state.set_speed_with_return(speed), motor_decay_mode); + apply_duty(state.set_speed_with_return(speed), motor_mode); } float Motor2::frequency() const { @@ -104,9 +104,9 @@ namespace motor { if(neg_pin_num != pos_pin_num) pwm_set_clkdiv_int_frac(neg_pin_num, div, mod); - // If the the period is larger, update the pwm before setting the new wraps - if(state.is_enabled() && pre_update_pwm) { - apply_duty(state.get_deadzoned_duty(), motor_decay_mode); + // If the period is larger, update the pwm before setting the new wraps + if(pre_update_pwm) { + apply_duty(state.get_deadzoned_duty(), motor_mode); } // Set the new wrap (should be 1 less than the period to get full 0 to 100%) @@ -114,9 +114,9 @@ namespace motor { if(neg_pin_num != pos_pin_num) pwm_set_wrap(neg_pin_num, pwm_period - 1); - // If the the period is smaller, update the pwm after setting the new wraps - if(state.is_enabled() && !pre_update_pwm) { - apply_duty(state.get_deadzoned_duty(), motor_decay_mode); + // If the period is smaller, update the pwm after setting the new wraps + if(!pre_update_pwm) { + apply_duty(state.get_deadzoned_duty(), motor_mode); } success = true; @@ -125,19 +125,8 @@ namespace motor { return success; } - DecayMode Motor2::decay_mode() { - return motor_decay_mode; - } - - void Motor2::decay_mode(DecayMode mode) { - motor_decay_mode = mode; - if(state.is_enabled()) { - apply_duty(state.get_deadzoned_duty(), motor_decay_mode); - } - } - void Motor2::stop() { - apply_duty(state.stop_with_return(), motor_decay_mode); + apply_duty(state.stop_with_return(), motor_mode); } void Motor2::coast() { @@ -149,19 +138,19 @@ namespace motor { } void Motor2::full_negative() { - apply_duty(state.full_negative_with_return(), motor_decay_mode); + apply_duty(state.full_negative_with_return(), motor_mode); } void Motor2::full_positive() { - apply_duty(state.full_positive_with_return(), motor_decay_mode); + apply_duty(state.full_positive_with_return(), motor_mode); } void Motor2::to_percent(float in, float in_min, float in_max) { - apply_duty(state.to_percent_with_return(in, in_min, in_max), motor_decay_mode); + apply_duty(state.to_percent_with_return(in, in_min, in_max), motor_mode); } void Motor2::to_percent(float in, float in_min, float in_max, float speed_min, float speed_max) { - apply_duty(state.to_percent_with_return(in, in_min, in_max, speed_min, speed_max), motor_decay_mode); + apply_duty(state.to_percent_with_return(in, in_min, in_max, speed_min, speed_max), motor_mode); } Direction Motor2::direction() const { @@ -185,7 +174,16 @@ namespace motor { } void Motor2::deadzone(float deadzone) { - apply_duty(state.set_deadzone_with_return(deadzone), motor_decay_mode); + apply_duty(state.set_deadzone_with_return(deadzone), motor_mode); + } + + DecayMode Motor2::decay_mode() { + return motor_mode; + } + + void Motor2::decay_mode(DecayMode mode) { + motor_mode = mode; + apply_duty(state.get_deadzoned_duty(), motor_mode); } void Motor2::apply_duty(float duty, DecayMode mode) { diff --git a/drivers/motor/motor2.hpp b/drivers/motor/motor2.hpp index e3a21122..ee1a095c 100644 --- a/drivers/motor/motor2.hpp +++ b/drivers/motor/motor2.hpp @@ -19,7 +19,7 @@ namespace motor { pwm_config pwm_cfg; uint16_t pwm_period; float pwm_frequency; - DecayMode motor_decay_mode; + DecayMode motor_mode; //-------------------------------------------------- // Constructors/Destructor @@ -52,14 +52,11 @@ namespace motor { float frequency() const; bool frequency(float freq); - DecayMode decay_mode(); - void decay_mode(DecayMode mode); - //-------------------------------------------------- void stop(); void coast(); - void brake(); //TODO + 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); @@ -76,6 +73,9 @@ namespace motor { float deadzone() const; void deadzone(float deadzone); + DecayMode decay_mode(); + void decay_mode(DecayMode mode); + //-------------------------------------------------- private: void apply_duty(float duty, DecayMode mode); diff --git a/drivers/motor/motor_cluster.cpp b/drivers/motor/motor_cluster.cpp index 0e820d25..4b77f48c 100644 --- a/drivers/motor/motor_cluster.cpp +++ b/drivers/motor/motor_cluster.cpp @@ -3,6 +3,9 @@ #include #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 deadzone, float freq, DecayMode mode, @@ -27,8 +30,7 @@ namespace motor { MotorCluster::~MotorCluster() { delete[] states; - delete[] motor_phases; - delete[] motor_modes; + delete[] configs; } bool MotorCluster::init() { @@ -43,10 +45,10 @@ namespace motor { // 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(motor_positive(motor), 0, false); - pwms.set_chan_level(motor_negative(motor), 0, false); - pwms.set_chan_offset(motor_positive(motor), (uint32_t)(motor_phases[motor] * (float)pwm_period), false); - pwms.set_chan_offset(motor_negative(motor), (uint32_t)(motor_phases[motor] * (float)pwm_period), false); + 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%) @@ -70,13 +72,14 @@ namespace motor { } 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, motor_modes[motor], load); + apply_duty(motor, new_duty, configs[motor].mode, load); } void MotorCluster::enable(const uint8_t *motors, uint8_t length, bool load) { @@ -108,7 +111,7 @@ namespace motor { 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, motor_modes[motor], load); + apply_duty(motor, new_duty, configs[motor].mode, load); } void MotorCluster::disable(const uint8_t *motors, uint8_t length, bool load) { @@ -150,7 +153,7 @@ namespace motor { 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, motor_modes[motor], load); + apply_duty(motor, new_duty, configs[motor].mode, load); } void MotorCluster::duty(const uint8_t *motors, uint8_t length, float duty, bool load) { @@ -187,7 +190,7 @@ namespace motor { 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, motor_modes[motor], load); + apply_duty(motor, new_duty, configs[motor].mode, load); } void MotorCluster::speed(const uint8_t *motors, uint8_t length, float speed, bool load) { @@ -218,13 +221,13 @@ namespace motor { float MotorCluster::phase(uint8_t motor) const { assert(motor < pwms.get_chan_pair_count()); - return motor_phases[motor]; + return configs[motor].phase; } void MotorCluster::phase(uint8_t motor, float phase, bool load) { assert(motor < pwms.get_chan_pair_count()); - motor_phases[motor] = MIN(MAX(phase, 0.0f), 1.0f); - pwms.set_chan_offset(motor, (uint32_t)(motor_phases[motor] * (float)pwms.get_wrap()), load); + 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) { @@ -271,11 +274,9 @@ namespace motor { // 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++) { - if(states[motor].is_enabled()) { - apply_duty(motor, states[motor].get_deadzoned_duty(), motor_modes[motor], false); - } - pwms.set_chan_offset(motor_positive(motor), (uint32_t)(motor_phases[motor] * (float)pwm_period), false); - pwms.set_chan_offset(motor_negative(motor), (uint32_t)(motor_phases[motor] * (float)pwm_period), false); + 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%) @@ -295,7 +296,7 @@ namespace motor { 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, motor_modes[motor], load); + apply_duty(motor, new_duty, configs[motor].mode, load); } void MotorCluster::stop(const uint8_t *motors, uint8_t length, bool load) { @@ -326,9 +327,8 @@ namespace motor { void MotorCluster::coast(uint8_t motor, bool load) { assert(motor < pwms.get_chan_pair_count()); - states[motor].set_duty_with_return(0.0f); - float new_duty = states[motor].disable_with_return(); - apply_duty(motor, new_duty, motor_modes[motor], load); + 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) { @@ -357,10 +357,42 @@ namespace motor { 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 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, motor_modes[motor], load); + apply_duty(motor, new_duty, configs[motor].mode, load); } void MotorCluster::full_negative(const uint8_t *motors, uint8_t length, bool load) { @@ -392,7 +424,7 @@ namespace motor { 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, motor_modes[motor], load); + apply_duty(motor, new_duty, configs[motor].mode, load); } void MotorCluster::full_positive(const uint8_t *motors, uint8_t length, bool load) { @@ -424,7 +456,7 @@ namespace motor { 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, motor_modes[motor], load); + 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) { @@ -456,7 +488,7 @@ namespace motor { 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, motor_modes[motor], load); + 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) { @@ -512,7 +544,7 @@ namespace motor { } } - void MotorCluster::all_directions(Direction direction) { + void MotorCluster::all_to_direction(Direction direction) { uint8_t motor_count = pwms.get_chan_pair_count(); for(uint8_t motor = 0; motor < motor_count; motor++) { this->direction(motor, direction); @@ -542,7 +574,7 @@ namespace motor { } } - void MotorCluster::all_speed_scales(float speed_scale) { + void MotorCluster::all_to_speed_scale(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); @@ -554,25 +586,26 @@ namespace motor { return states[motor].get_deadzone(); } - void MotorCluster::deadzone(uint8_t motor, float deadzone) { + void MotorCluster::deadzone(uint8_t motor, float deadzone, bool load) { assert(motor < pwms.get_chan_pair_count()); - states[motor].set_deadzone_with_return(deadzone); //TODO + 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) { + 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 motors, float deadzone) { + void MotorCluster::deadzone(std::initializer_list motors, float deadzone, bool load) { for(auto motor : motors) { this->deadzone(motor, deadzone); } } - void MotorCluster::all_deadzones(float deadzone) { + void MotorCluster::all_to_deadzone(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); @@ -581,28 +614,29 @@ namespace motor { DecayMode MotorCluster::decay_mode(uint8_t motor) const { assert(motor < pwms.get_chan_pair_count()); - return SLOW_DECAY;//TODO states[motor].get_decay_mode(); + return configs[motor].mode; } - void MotorCluster::decay_mode(uint8_t motor, DecayMode mode) { + void MotorCluster::decay_mode(uint8_t motor, DecayMode mode, bool load) { assert(motor < pwms.get_chan_pair_count()); - //TODO states[motor].set_decay_mode(mode); + 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) { + 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 motors, DecayMode mode) { + void MotorCluster::decay_mode(std::initializer_list motors, DecayMode mode, bool load) { for(auto motor : motors) { this->decay_mode(motor, mode); } } - void MotorCluster::all_to_decay_mode(DecayMode mode) { + void MotorCluster::all_to_decay_mode(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); @@ -617,31 +651,31 @@ namespace motor { switch(mode) { case SLOW_DECAY: //aka 'Braking' if(signed_level >= 0) { - pwms.set_chan_level(motor_positive(motor), pwm_period, false); - pwms.set_chan_level(motor_negative(motor), pwm_period - signed_level, load); + 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(motor_positive(motor), pwm_period + signed_level, false); - pwms.set_chan_level(motor_negative(motor), pwm_period, load); + 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(motor_positive(motor), signed_level, false); - pwms.set_chan_level(motor_negative(motor), 0, load); + pwms.set_chan_level(POS_MOTOR(motor), signed_level, false); + pwms.set_chan_level(NEG_MOTOR(motor), 0, load); } else { - pwms.set_chan_level(motor_positive(motor), 0, false); - pwms.set_chan_level(motor_negative(motor), 0 - signed_level, load); + 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(motor_positive(motor), 0, false); - pwms.set_chan_level(motor_negative(motor), 0, load); + pwms.set_chan_level(POS_MOTOR(motor), 0, false); + pwms.set_chan_level(NEG_MOTOR(motor), 0, load); } } @@ -650,13 +684,12 @@ namespace motor { uint8_t motor_count = pwms.get_chan_pair_count(); if(motor_count > 0) { states = new MotorState[motor_count]; - motor_phases = new float[motor_count]; - motor_modes = new DecayMode[motor_count]; + configs = new motor_config[motor_count]; for(uint motor = 0; motor < motor_count; motor++) { states[motor] = MotorState(direction, speed_scale, deadzone); - motor_phases[motor] = (auto_phase) ? (float)motor / (float)motor_count : 0.0f; - motor_modes[motor] = mode; + configs[motor].phase = (auto_phase) ? (float)motor / (float)motor_count : 0.0f; + configs[motor].mode = mode; } } } diff --git a/drivers/motor/motor_cluster.hpp b/drivers/motor/motor_cluster.hpp index 435bb683..d2468b21 100644 --- a/drivers/motor/motor_cluster.hpp +++ b/drivers/motor/motor_cluster.hpp @@ -9,6 +9,16 @@ using namespace pimoroni; namespace motor { class MotorCluster { + //-------------------------------------------------- + // Substructures + //-------------------------------------------------- + private: + struct motor_config { + float phase; + DecayMode mode; + }; + + //-------------------------------------------------- // Variables //-------------------------------------------------- @@ -17,8 +27,7 @@ namespace motor { uint32_t pwm_period; float pwm_frequency; MotorState* states; - float* motor_phases; - DecayMode* motor_modes; + motor_config* configs; //-------------------------------------------------- @@ -79,12 +88,6 @@ namespace motor { float frequency() const; bool frequency(float freq); - DecayMode decay_mode(uint8_t motor) const; - void decay_mode(uint8_t motor, DecayMode mode); - void decay_mode(const uint8_t *motors, uint8_t length, DecayMode mode); - void decay_mode(std::initializer_list motors, DecayMode mode); - void all_to_decay_mode(DecayMode mode); - //-------------------------------------------------- void stop(uint8_t motor, bool load = true); void stop(const uint8_t *motors, uint8_t length, bool load = true); @@ -96,6 +99,11 @@ namespace motor { void coast(std::initializer_list 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 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 motors, bool load = true); @@ -124,31 +132,30 @@ namespace motor { void direction(uint8_t motor, Direction direction); void direction(const uint8_t *motors, uint8_t length, Direction direction); void direction(std::initializer_list motors, Direction direction); - void all_directions(Direction direction); + void all_to_direction(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 motors, float speed_scale); - void all_speed_scales(float speed_scale); + void all_to_speed_scale(float speed_scale); float deadzone(uint8_t motor) const; - void deadzone(uint8_t motor, float deadzone); - void deadzone(const uint8_t *motors, uint8_t length, float deadzone); - void deadzone(std::initializer_list motors, float deadzone); - void all_deadzones(float deadzone); + 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 motors, float deadzone, bool load = true); + void all_to_deadzone(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 motors, DecayMode mode, bool load = true); + void all_to_decay_mode(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 deadzone, DecayMode mode, bool auto_phase); - - static uint8_t motor_positive(uint8_t motor) { - return PWMCluster::channel_from_pair(motor); - } - static uint8_t motor_negative(uint8_t motor) { - return PWMCluster::channel_from_pair(motor) + 1; - } }; } \ No newline at end of file diff --git a/drivers/motor/motor_state.cpp b/drivers/motor/motor_state.cpp index 00e381b5..6ae19373 100644 --- a/drivers/motor/motor_state.cpp +++ b/drivers/motor/motor_state.cpp @@ -37,7 +37,11 @@ namespace motor { if((last_enabled_duty <= 0.0f - motor_deadzone) || (last_enabled_duty >= motor_deadzone)) { duty = last_enabled_duty; } - return duty; + + if(enabled) + return duty; + else + return NAN; } float MotorState::set_duty_with_return(float duty) { @@ -77,7 +81,7 @@ namespace motor { } 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 - get_speed_scale(), get_speed_scale()); + float speed = MotorState::map_float(in, in_min, in_max, 0.0f - motor_speed, motor_speed); return set_speed_with_return(speed); } @@ -110,10 +114,7 @@ namespace motor { float MotorState::set_deadzone_with_return(float deadzone) { motor_deadzone = CLAMP(deadzone, 0.0f, 1.0f); - if(enabled) - return get_deadzoned_duty(); - else - return NAN; + return get_deadzoned_duty(); } int32_t MotorState::duty_to_level(float duty, uint32_t resolution) { From 0facfaa540c81203503aa7183e6e7de1b020f85c Mon Sep 17 00:00:00 2001 From: ZodiusInfuser Date: Mon, 11 Apr 2022 17:59:16 +0100 Subject: [PATCH 09/60] Updated MP bindings to newest C++ --- micropython/modules/motor/motor.c | 51 ++- micropython/modules/motor/motor.cpp | 678 ++++++++++++++++++++++++++-- micropython/modules/motor/motor.h | 25 +- 3 files changed, 692 insertions(+), 62 deletions(-) diff --git a/micropython/modules/motor/motor.c b/micropython/modules/motor/motor.c index 2a0acc40..d3ed6fa6 100644 --- a/micropython/modules/motor/motor.c +++ b/micropython/modules/motor/motor.c @@ -11,12 +11,13 @@ MP_DEFINE_CONST_FUN_OBJ_KW(Motor_speed_obj, 1, Motor_speed); MP_DEFINE_CONST_FUN_OBJ_KW(Motor_frequency_obj, 1, Motor_frequency); MP_DEFINE_CONST_FUN_OBJ_1(Motor_stop_obj, Motor_stop); MP_DEFINE_CONST_FUN_OBJ_1(Motor_coast_obj, Motor_coast); +MP_DEFINE_CONST_FUN_OBJ_1(Motor_brake_obj, Motor_brake); MP_DEFINE_CONST_FUN_OBJ_1(Motor_full_negative_obj, Motor_full_negative); MP_DEFINE_CONST_FUN_OBJ_1(Motor_full_positive_obj, Motor_full_positive); MP_DEFINE_CONST_FUN_OBJ_KW(Motor_to_percent_obj, 2, Motor_to_percent); MP_DEFINE_CONST_FUN_OBJ_KW(Motor_direction_obj, 1, Motor_direction); MP_DEFINE_CONST_FUN_OBJ_KW(Motor_speed_scale_obj, 1, Motor_speed_scale); -MP_DEFINE_CONST_FUN_OBJ_KW(Motor_deadzone_percent_obj, 1, Motor_deadzone_percent); +MP_DEFINE_CONST_FUN_OBJ_KW(Motor_deadzone_obj, 1, Motor_deadzone); MP_DEFINE_CONST_FUN_OBJ_KW(Motor_decay_mode_obj, 1, Motor_decay_mode); MP_DEFINE_CONST_FUN_OBJ_1(MotorCluster___del___obj, MotorCluster___del__); @@ -38,21 +39,23 @@ MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_stop_obj, 2, MotorCluster_stop); MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_stop_all_obj, 1, MotorCluster_stop_all); MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_coast_obj, 2, MotorCluster_coast); MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_coast_all_obj, 1, MotorCluster_coast_all); +MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_brake_obj, 2, MotorCluster_brake); +MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_brake_all_obj, 1, MotorCluster_brake_all); MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_full_negative_obj, 2, MotorCluster_full_negative); -MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_all_to_full_negative_obj, 1, MotorCluster_all_to_full_negative); +MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_all_full_negative_obj, 1, MotorCluster_all_full_negative); MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_full_positive_obj, 2, MotorCluster_full_positive); -MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_all_to_full_positive_obj, 1, MotorCluster_all_to_full_positive); +MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_all_full_positive_obj, 1, MotorCluster_all_full_positive); MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_to_percent_obj, 3, MotorCluster_to_percent); MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_all_to_percent_obj, 2, MotorCluster_all_to_percent); MP_DEFINE_CONST_FUN_OBJ_1(MotorCluster_load_obj, MotorCluster_load); -//MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_direction_obj, 2, MotorCluster_direction); -//MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_all_directions_obj, 1, MotorCluster_all_directions); -//MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_speed_scale_obj, 2, MotorCluster_speed_scale); -//MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_all_speed_scales_obj, 1, MotorCluster_all_speed_scales); -//MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_deadzone_percent_obj, 2, MotorCluster_deadzone_percent); -//MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_all_deadzone_percents_obj, 1, MotorCluster_all_deadzone_percents); -//MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_decay_mode_obj, 2, MotorCluster_decay_mode); -//MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_all_decay_modes_obj, 1, MotorCluster_all_decay_modes); +MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_direction_obj, 2, MotorCluster_direction); +MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_all_to_direction_obj, 1, MotorCluster_all_to_direction); +MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_speed_scale_obj, 2, MotorCluster_speed_scale); +MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_all_to_speed_scale_obj, 1, MotorCluster_all_to_speed_scale); +MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_deadzone_obj, 2, MotorCluster_deadzone); +MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_all_to_deadzone_obj, 1, MotorCluster_all_to_deadzone); +MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_decay_mode_obj, 2, MotorCluster_decay_mode); +MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_all_to_decay_mode_obj, 1, MotorCluster_all_to_decay_mode); /***** Binding of Methods *****/ STATIC const mp_rom_map_elem_t Motor_locals_dict_table[] = { @@ -66,12 +69,13 @@ STATIC const mp_rom_map_elem_t Motor_locals_dict_table[] = { { MP_ROM_QSTR(MP_QSTR_frequency), MP_ROM_PTR(&Motor_frequency_obj) }, { MP_ROM_QSTR(MP_QSTR_stop), MP_ROM_PTR(&Motor_stop_obj) }, { MP_ROM_QSTR(MP_QSTR_coast), MP_ROM_PTR(&Motor_coast_obj) }, + { MP_ROM_QSTR(MP_QSTR_brake), MP_ROM_PTR(&Motor_brake_obj) }, { MP_ROM_QSTR(MP_QSTR_full_negative), MP_ROM_PTR(&Motor_full_negative_obj) }, { MP_ROM_QSTR(MP_QSTR_full_positive), MP_ROM_PTR(&Motor_full_positive_obj) }, { MP_ROM_QSTR(MP_QSTR_to_percent), MP_ROM_PTR(&Motor_to_percent_obj) }, { MP_ROM_QSTR(MP_QSTR_direction), MP_ROM_PTR(&Motor_direction_obj) }, { MP_ROM_QSTR(MP_QSTR_speed_scale), MP_ROM_PTR(&Motor_speed_scale_obj) }, - { MP_ROM_QSTR(MP_QSTR_deadzone_percent), MP_ROM_PTR(&Motor_deadzone_percent_obj) }, + { MP_ROM_QSTR(MP_QSTR_deadzone), MP_ROM_PTR(&Motor_deadzone_obj) }, { MP_ROM_QSTR(MP_QSTR_decay_mode), MP_ROM_PTR(&Motor_decay_mode_obj) }, }; @@ -95,21 +99,23 @@ STATIC const mp_rom_map_elem_t MotorCluster_locals_dict_table[] = { { MP_ROM_QSTR(MP_QSTR_stop_all), MP_ROM_PTR(&MotorCluster_stop_all_obj) }, { MP_ROM_QSTR(MP_QSTR_coast), MP_ROM_PTR(&MotorCluster_coast_obj) }, { MP_ROM_QSTR(MP_QSTR_coast_all), MP_ROM_PTR(&MotorCluster_coast_all_obj) }, + { MP_ROM_QSTR(MP_QSTR_brake), MP_ROM_PTR(&MotorCluster_brake_obj) }, + { MP_ROM_QSTR(MP_QSTR_brake_all), MP_ROM_PTR(&MotorCluster_brake_all_obj) }, { MP_ROM_QSTR(MP_QSTR_full_negative), MP_ROM_PTR(&MotorCluster_full_negative_obj) }, - { MP_ROM_QSTR(MP_QSTR_all_to_full_negative), MP_ROM_PTR(&MotorCluster_all_to_full_negative_obj) }, + { MP_ROM_QSTR(MP_QSTR_all_full_negative), MP_ROM_PTR(&MotorCluster_all_full_negative_obj) }, { MP_ROM_QSTR(MP_QSTR_full_positive), MP_ROM_PTR(&MotorCluster_full_positive_obj) }, - { MP_ROM_QSTR(MP_QSTR_all_to_full_positive), MP_ROM_PTR(&MotorCluster_all_to_full_positive_obj) }, + { MP_ROM_QSTR(MP_QSTR_all_full_positive), MP_ROM_PTR(&MotorCluster_all_full_positive_obj) }, { MP_ROM_QSTR(MP_QSTR_to_percent), MP_ROM_PTR(&MotorCluster_to_percent_obj) }, { MP_ROM_QSTR(MP_QSTR_all_to_percent), MP_ROM_PTR(&MotorCluster_all_to_percent_obj) }, { MP_ROM_QSTR(MP_QSTR_load), MP_ROM_PTR(&MotorCluster_load_obj) }, - //{ MP_ROM_QSTR(MP_QSTR_direction), MP_ROM_PTR(&MotorCluster_direction_obj) }, - //{ MP_ROM_QSTR(MP_QSTR_all_directions), MP_ROM_PTR(&MotorCluster_direction_obj) }, - //{ MP_ROM_QSTR(MP_QSTR_speed_scale), MP_ROM_PTR(&MotorCluster_speed_scale_obj) }, - //{ MP_ROM_QSTR(MP_QSTR_all_speed_scales), MP_ROM_PTR(&MotorCluster_all_speed_scales_obj) }, - //{ MP_ROM_QSTR(MP_QSTR_deadzone_percent), MP_ROM_PTR(&MotorCluster_deadzone_percent_obj) }, - //{ MP_ROM_QSTR(MP_QSTR_all_deadzone_percents), MP_ROM_PTR(&MotorCluster_all_deadzone_percents_obj) }, - //{ MP_ROM_QSTR(MP_QSTR_decay_mode), MP_ROM_PTR(&MotorCluster_decay_mode_obj) }, - //{ MP_ROM_QSTR(MP_QSTR_all_decay_modes), MP_ROM_PTR(&MotorCluster_all_decay_modes_obj) }, + { MP_ROM_QSTR(MP_QSTR_direction), MP_ROM_PTR(&MotorCluster_direction_obj) }, + { MP_ROM_QSTR(MP_QSTR_all_to_direction), MP_ROM_PTR(&MotorCluster_direction_obj) }, + { MP_ROM_QSTR(MP_QSTR_speed_scale), MP_ROM_PTR(&MotorCluster_speed_scale_obj) }, + { MP_ROM_QSTR(MP_QSTR_all_to_speed_scale), MP_ROM_PTR(&MotorCluster_all_to_speed_scale_obj) }, + { MP_ROM_QSTR(MP_QSTR_deadzone), MP_ROM_PTR(&MotorCluster_deadzone_obj) }, + { MP_ROM_QSTR(MP_QSTR_all_to_deadzone), MP_ROM_PTR(&MotorCluster_all_to_deadzone_obj) }, + { MP_ROM_QSTR(MP_QSTR_decay_mode), MP_ROM_PTR(&MotorCluster_decay_mode_obj) }, + { MP_ROM_QSTR(MP_QSTR_all_to_decay_mode), MP_ROM_PTR(&MotorCluster_all_to_decay_mode_obj) }, }; STATIC MP_DEFINE_CONST_DICT(Motor_locals_dict, Motor_locals_dict_table); @@ -170,6 +176,7 @@ typedef struct _mp_obj_float_t { mp_obj_base_t base; mp_float_t value; } mp_obj_float_t; +//TODO confirm below numbers are correct mp_obj_float_t motor2040_shunt_resistor = {{&mp_type_float}, 0.47f}; mp_obj_float_t motor2040_voltage_gain = {{&mp_type_float}, 3.9f / 13.9f}; mp_obj_float_t motor2040_current_offset = {{&mp_type_float}, -0.02f}; diff --git a/micropython/modules/motor/motor.cpp b/micropython/modules/motor/motor.cpp index 1351f793..507b99ed 100644 --- a/micropython/modules/motor/motor.cpp +++ b/micropython/modules/motor/motor.cpp @@ -11,6 +11,7 @@ using namespace motor; extern "C" { #include "motor.h" #include "py/builtin.h" +#include "float.h" /********** Motor **********/ @@ -41,7 +42,7 @@ void Motor_print(const mp_print_t *print, mp_obj_t self_in, mp_print_kind_t kind mp_obj_print_helper(print, mp_obj_new_float(self->motor->speed()), PRINT_REPR); mp_print_str(print, ", freq = "); mp_obj_print_helper(print, mp_obj_new_float(self->motor->frequency()), PRINT_REPR); - if(self->motor->direction() == MotorState::NORMAL) + if(self->motor->direction() == NORMAL) mp_print_str(print, ", direction = NORMAL"); else mp_print_str(print, ", direction = REVERSED"); @@ -49,7 +50,7 @@ void Motor_print(const mp_print_t *print, mp_obj_t self_in, mp_print_kind_t kind mp_obj_print_helper(print, mp_obj_new_float(self->motor->speed_scale()), PRINT_REPR); mp_print_str(print, ", deadzone = "); mp_obj_print_helper(print, mp_obj_new_float(self->motor->deadzone()), PRINT_REPR); - if(self->motor->decay_mode() == MotorState::SLOW_DECAY) + if(self->motor->decay_mode() == SLOW_DECAY) mp_print_str(print, ", decay_mode = SLOW_DECAY"); else mp_print_str(print, ", decay_mode = FAST_DECAY"); @@ -68,6 +69,7 @@ mp_obj_t Motor_make_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw, c { MP_QSTR_calibration, MP_ARG_OBJ, {.u_obj = mp_const_none} }, { MP_QSTR_freq, MP_ARG_OBJ, {.u_obj = mp_const_none} }, }; + //TODO ^ // Parse args. mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; @@ -302,6 +304,12 @@ extern mp_obj_t Motor_coast(mp_obj_t self_in) { return mp_const_none; } +extern mp_obj_t Motor_brake(mp_obj_t self_in) { + _Motor_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Motor_obj_t); + self->motor->brake(); + return mp_const_none; +} + extern mp_obj_t Motor_full_negative(mp_obj_t self_in) { _Motor_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Motor_obj_t); self->motor->full_negative(); @@ -401,7 +409,7 @@ extern mp_obj_t Motor_direction(size_t n_args, const mp_obj_t *pos_args, mp_map_ enum { ARG_self, ARG_direction }; static const mp_arg_t allowed_args[] = { { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, - { MP_QSTR_direction, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_direction, MP_ARG_REQUIRED | MP_ARG_INT }, }; // Parse args. @@ -410,9 +418,11 @@ extern mp_obj_t Motor_direction(size_t n_args, const mp_obj_t *pos_args, mp_map_ _Motor_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Motor_obj_t); - int direction = mp_obj_get_int(args[ARG_direction].u_obj); - - self->motor->direction((MotorState::Direction)direction); + int direction = args[ARG_direction].u_int; + if(direction < 0 || direction > 1) { + mp_raise_ValueError("direction out of range. Expected NORMAL (0) or REVERSED (1)"); + } + self->motor->direction((Direction)direction); return mp_const_none; } } @@ -446,13 +456,15 @@ extern mp_obj_t Motor_speed_scale(size_t n_args, const mp_obj_t *pos_args, mp_ma _Motor_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Motor_obj_t); float speed_scale = mp_obj_get_float(args[ARG_speed_scale].u_obj); - + if(speed_scale < FLT_EPSILON) { + mp_raise_ValueError("speed_scale out of range. Expected greater than 0.0"); + } self->motor->speed_scale(speed_scale); return mp_const_none; } } -extern mp_obj_t Motor_deadzone_percent(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { +extern mp_obj_t Motor_deadzone(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { if(n_args <= 1) { enum { ARG_self }; static const mp_arg_t allowed_args[] = { @@ -481,7 +493,9 @@ extern mp_obj_t Motor_deadzone_percent(size_t n_args, const mp_obj_t *pos_args, _Motor_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Motor_obj_t); float deadzone = mp_obj_get_float(args[ARG_deadzone_percent].u_obj); - + if(deadzone < 0.0f || deadzone > 1.0f) { + mp_raise_ValueError("deadzone out of range. Expected 0.0 to 1.0"); + } self->motor->deadzone(deadzone); return mp_const_none; } @@ -506,7 +520,7 @@ extern mp_obj_t Motor_decay_mode(size_t n_args, const mp_obj_t *pos_args, mp_map enum { ARG_self, ARG_decay_mode }; static const mp_arg_t allowed_args[] = { { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, - { MP_QSTR_decay_mode, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_decay_mode, MP_ARG_REQUIRED | MP_ARG_INT }, }; // Parse args. @@ -515,9 +529,11 @@ extern mp_obj_t Motor_decay_mode(size_t n_args, const mp_obj_t *pos_args, mp_map _Motor_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Motor_obj_t); - int decay_mode = mp_obj_get_int(args[ARG_decay_mode].u_obj); - - self->motor->decay_mode((MotorState::DecayMode)decay_mode); + int mode = args[ARG_decay_mode].u_int; + if(mode < 0 || mode > 1) { + mp_raise_ValueError("mode out of range. Expected FAST_DECAY (0) or SLOW_DECAY (1)"); + } + self->motor->decay_mode((DecayMode)mode); return mp_const_none; } } @@ -584,6 +600,7 @@ mp_obj_t MotorCluster_make_new(const mp_obj_type_t *type, size_t n_args, size_t { MP_QSTR_freq, MP_ARG_OBJ, {.u_obj = mp_const_none} }, { MP_QSTR_auto_phase, MP_ARG_BOOL, {.u_bool = true} }, }; + //TODO ^ // Parse args. mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; @@ -700,7 +717,7 @@ mp_obj_t MotorCluster_make_new(const mp_obj_type_t *type, size_t n_args, size_t //if(calib != nullptr) //cluster = new MotorCluster(pio, sm, pins, pair_count, *calib, freq, auto_phase, seq_buffer, dat_buffer); //else - cluster = new MotorCluster(pio, sm, pins, pair_count, MotorState::NORMAL, 1.0f, 0.0f, freq, MotorState::SLOW_DECAY, auto_phase, seq_buffer, dat_buffer); + cluster = new MotorCluster(pio, sm, pins, pair_count, NORMAL, 1.0f, 0.0f, freq, SLOW_DECAY, auto_phase, seq_buffer, dat_buffer); // Cleanup the pins array if(pins != nullptr) @@ -756,8 +773,14 @@ extern mp_obj_t MotorCluster_pins(size_t n_args, const mp_obj_t *pos_args, mp_ma mp_raise_ValueError("this cluster does not have any motors"); else if(motor < 0 || motor >= motor_count) mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("motor out of range. Expected 0 to %d"), motor_count - 1); - else - return mp_obj_new_int(self->cluster->pins((uint)motor).positive); + else { + pin_pair pins = self->cluster->pins((uint)motor); + + mp_obj_t tuple[2]; + tuple[0] = mp_obj_new_int(pins.positive); + tuple[1] = mp_obj_new_int(pins.negative); + return mp_obj_new_tuple(2, tuple); + } return mp_const_none; } @@ -875,7 +898,7 @@ extern mp_obj_t MotorCluster_disable(size_t n_args, const mp_obj_t *pos_args, mp } else { size_t length = 0; - mp_obj_t *items = nullptr; + mp_obj_t *items = nullptr; if(mp_obj_is_type(object, &mp_type_list)) { mp_obj_list_t *list = MP_OBJ_TO_PTR2(object, mp_obj_list_t); length = list->len; @@ -997,7 +1020,7 @@ extern mp_obj_t MotorCluster_duty(size_t n_args, const mp_obj_t *pos_args, mp_ma if(motor_count == 0) mp_raise_ValueError("this cluster does not have any motors"); else { - // Determine what motor(s) to enable + // Determine what motor(s) to modify const mp_obj_t object = args[ARG_motors].u_obj; if(mp_obj_is_int(object)) { int motor = mp_obj_get_int(object); @@ -1115,7 +1138,7 @@ extern mp_obj_t MotorCluster_speed(size_t n_args, const mp_obj_t *pos_args, mp_m if(motor_count == 0) mp_raise_ValueError("this cluster does not have any motors"); else { - // Determine what motor(s) to enable + // Determine what motor(s) to modify const mp_obj_t object = args[ARG_motors].u_obj; if(mp_obj_is_int(object)) { int motor = mp_obj_get_int(object); @@ -1233,7 +1256,7 @@ extern mp_obj_t MotorCluster_phase(size_t n_args, const mp_obj_t *pos_args, mp_m if(motor_count == 0) mp_raise_ValueError("this cluster does not have any motors"); else { - // Determine what motor(s) to enable + // Determine what motor(s) to modify const mp_obj_t object = args[ARG_motors].u_obj; if(mp_obj_is_int(object)) { int motor = mp_obj_get_int(object); @@ -1364,7 +1387,7 @@ extern mp_obj_t MotorCluster_stop(size_t n_args, const mp_obj_t *pos_args, mp_ma if(motor_count == 0) mp_raise_ValueError("this cluster does not have any motors"); else { - // Determine what motor(s) to enable + // Determine what motor(s) to stop const mp_obj_t object = args[ARG_motors].u_obj; if(mp_obj_is_int(object)) { int motor = mp_obj_get_int(object); @@ -1452,7 +1475,7 @@ extern mp_obj_t MotorCluster_coast(size_t n_args, const mp_obj_t *pos_args, mp_m if(motor_count == 0) mp_raise_ValueError("this cluster does not have any motors"); else { - // Determine what motor(s) to enable + // Determine what motor(s) to coast const mp_obj_t object = args[ARG_motors].u_obj; if(mp_obj_is_int(object)) { int motor = mp_obj_get_int(object); @@ -1522,6 +1545,94 @@ extern mp_obj_t MotorCluster_coast_all(size_t n_args, const mp_obj_t *pos_args, return mp_const_none; } +extern mp_obj_t MotorCluster_brake(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { + enum { ARG_self, ARG_motors, ARG_load }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_motors, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_load, MP_ARG_BOOL, { .u_bool = true }}, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _MotorCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _MotorCluster_obj_t); + + int motor_count = (int)self->cluster->count(); + if(motor_count == 0) + mp_raise_ValueError("this cluster does not have any motors"); + else { + // Determine what motor(s) to brake + const mp_obj_t object = args[ARG_motors].u_obj; + if(mp_obj_is_int(object)) { + int motor = mp_obj_get_int(object); + if(motor < 0 || motor >= motor_count) + mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("motor out of range. Expected 0 to %d"), motor_count - 1); + else + self->cluster->brake((uint)motor, args[ARG_load].u_bool); + } + else { + size_t length = 0; + mp_obj_t *items = nullptr; + if(mp_obj_is_type(object, &mp_type_list)) { + mp_obj_list_t *list = MP_OBJ_TO_PTR2(object, mp_obj_list_t); + length = list->len; + items = list->items; + } + else if(mp_obj_is_type(object, &mp_type_tuple)) { + mp_obj_tuple_t *tuple = MP_OBJ_TO_PTR2(object, mp_obj_tuple_t); + length = tuple->len; + items = tuple->items; + } + + if(items == nullptr) + mp_raise_TypeError("cannot convert object to a list or tuple of integers, or a single integer"); + else if(length == 0) + mp_raise_TypeError("list or tuple must contain at least one integer"); + else { + // Create and populate a local array of motor indices + uint8_t *motors = new uint8_t[length]; + for(size_t i = 0; i < length; i++) { + int motor = mp_obj_get_int(items[i]); + if(motor < 0 || motor >= motor_count) { + delete[] motors; + mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("a motor in the list or tuple is out of range. Expected 0 to %d"), motor_count - 1); + } + else { + motors[i] = (uint8_t)motor; + } + } + self->cluster->brake(motors, length, args[ARG_load].u_bool); + delete[] motors; + } + } + } + return mp_const_none; +} + +extern mp_obj_t MotorCluster_brake_all(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { + enum { ARG_self, ARG_load }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_load, MP_ARG_BOOL, { .u_bool = true }}, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _MotorCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _MotorCluster_obj_t); + + int motor_count = (int)self->cluster->count(); + if(motor_count == 0) + mp_raise_ValueError("this cluster does not have any motors"); + else { + self->cluster->brake_all(args[ARG_load].u_bool); + } + return mp_const_none; +} + extern mp_obj_t MotorCluster_full_negative(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { enum { ARG_self, ARG_motors, ARG_load }; static const mp_arg_t allowed_args[] = { @@ -1540,7 +1651,7 @@ extern mp_obj_t MotorCluster_full_negative(size_t n_args, const mp_obj_t *pos_ar if(motor_count == 0) mp_raise_ValueError("this cluster does not have any motors"); else { - // Determine what motor(s) to enable + // Determine what motor(s) to set to full negative const mp_obj_t object = args[ARG_motors].u_obj; if(mp_obj_is_int(object)) { int motor = mp_obj_get_int(object); @@ -1588,7 +1699,7 @@ extern mp_obj_t MotorCluster_full_negative(size_t n_args, const mp_obj_t *pos_ar return mp_const_none; } -extern mp_obj_t MotorCluster_all_to_full_negative(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { +extern mp_obj_t MotorCluster_all_full_negative(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { enum { ARG_self, ARG_load }; static const mp_arg_t allowed_args[] = { { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, @@ -1628,7 +1739,7 @@ extern mp_obj_t MotorCluster_full_positive(size_t n_args, const mp_obj_t *pos_ar if(motor_count == 0) mp_raise_ValueError("this cluster does not have any motors"); else { - // Determine what motor(s) to enable + // Determine what motor(s) to set to full positive const mp_obj_t object = args[ARG_motors].u_obj; if(mp_obj_is_int(object)) { int motor = mp_obj_get_int(object); @@ -1676,7 +1787,7 @@ extern mp_obj_t MotorCluster_full_positive(size_t n_args, const mp_obj_t *pos_ar return mp_const_none; } -extern mp_obj_t MotorCluster_all_to_full_positive(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { +extern mp_obj_t MotorCluster_all_full_positive(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { enum { ARG_self, ARG_load }; static const mp_arg_t allowed_args[] = { { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, @@ -1718,7 +1829,7 @@ extern mp_obj_t MotorCluster_to_percent(size_t n_args, const mp_obj_t *pos_args, if(motor_count == 0) mp_raise_ValueError("this cluster does not have any motors"); else { - // Determine what motor(s) to enable + // Determine what motor(s) to modify const mp_obj_t object = args[ARG_motors].u_obj; if(mp_obj_is_int(object)) { int motor = mp_obj_get_int(object); @@ -1788,7 +1899,7 @@ extern mp_obj_t MotorCluster_to_percent(size_t n_args, const mp_obj_t *pos_args, if(motor_count == 0) mp_raise_ValueError("this cluster does not have any motors"); else { - // Determine what motor(s) to enable + // Determine what motor(s) to modify const mp_obj_t object = args[ARG_motors].u_obj; if(mp_obj_is_int(object)) { int motor = mp_obj_get_int(object); @@ -1864,7 +1975,7 @@ extern mp_obj_t MotorCluster_to_percent(size_t n_args, const mp_obj_t *pos_args, if(motor_count == 0) mp_raise_ValueError("this cluster does not have any motors"); else { - // Determine what motor(s) to enable + // Determine what motor(s) to modify const mp_obj_t object = args[ARG_motors].u_obj; if(mp_obj_is_int(object)) { int motor = mp_obj_get_int(object); @@ -2011,4 +2122,513 @@ extern mp_obj_t MotorCluster_load(mp_obj_t self_in) { self->cluster->load(); return mp_const_none; } + +extern mp_obj_t MotorCluster_direction(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { + if(n_args <= 2) { + enum { ARG_self, ARG_motor }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_motor, MP_ARG_REQUIRED | MP_ARG_INT }, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _MotorCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _MotorCluster_obj_t); + + int motor = args[ARG_motor].u_int; + int motor_count = (int)self->cluster->count(); + if(motor_count == 0) + mp_raise_ValueError("this cluster does not have any motors"); + else if(motor < 0 || motor >= motor_count) + mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("motor out of range. Expected 0 to %d"), motor_count - 1); + else + return mp_obj_new_int((int)self->cluster->direction((uint)motor)); + } + else { + enum { ARG_self, ARG_motors, ARG_direction }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_motors, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_direction, MP_ARG_REQUIRED | MP_ARG_INT }, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _MotorCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _MotorCluster_obj_t); + + int motor_count = (int)self->cluster->count(); + if(motor_count == 0) + mp_raise_ValueError("this cluster does not have any motors"); + else { + // Determine what motor(s) to modify + const mp_obj_t object = args[ARG_motors].u_obj; + if(mp_obj_is_int(object)) { + int motor = mp_obj_get_int(object); + if(motor < 0 || motor >= motor_count) + mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("motor out of range. Expected 0 to %d"), motor_count - 1); + else { + int direction = args[ARG_direction].u_int; + if(direction < 0 || direction > 1) { + mp_raise_ValueError("direction out of range. Expected NORMAL (0) or REVERSED (1)"); + } + self->cluster->direction((uint)motor, (Direction)direction); + } + } + else { + size_t length = 0; + mp_obj_t *items = nullptr; + if(mp_obj_is_type(object, &mp_type_list)) { + mp_obj_list_t *list = MP_OBJ_TO_PTR2(object, mp_obj_list_t); + length = list->len; + items = list->items; + } + else if(mp_obj_is_type(object, &mp_type_tuple)) { + mp_obj_tuple_t *tuple = MP_OBJ_TO_PTR2(object, mp_obj_tuple_t); + length = tuple->len; + items = tuple->items; + } + + if(items == nullptr) + mp_raise_TypeError("cannot convert object to a list or tuple of integers, or a single integer"); + else if(length == 0) + mp_raise_TypeError("list or tuple must contain at least one integer"); + else { + // Create and populate a local array of motor indices + uint8_t *motors = new uint8_t[length]; + for(size_t i = 0; i < length; i++) { + int motor = mp_obj_get_int(items[i]); + if(motor < 0 || motor >= motor_count) { + delete[] motors; + mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("a motor in the list or tuple is out of range. Expected 0 to %d"), motor_count - 1); + } + else { + motors[i] = (uint8_t)motor; + } + } + int direction = args[ARG_direction].u_int; + if(direction < 0 || direction > 1) { + delete[] motors; + mp_raise_ValueError("direction out of range. Expected NORMAL (0) or REVERSED (1)"); + } + self->cluster->direction(motors, length, (Direction)direction); + delete[] motors; + } + } + } + } + return mp_const_none; +} + +extern mp_obj_t MotorCluster_all_to_direction(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { + enum { ARG_self, ARG_direction }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_direction, MP_ARG_REQUIRED | MP_ARG_INT }, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _MotorCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _MotorCluster_obj_t); + + int motor_count = (int)self->cluster->count(); + if(motor_count == 0) + mp_raise_ValueError("this cluster does not have any motors"); + else { + int direction = args[ARG_direction].u_int; + if(direction < 0 || direction > 1) { + mp_raise_ValueError("direction out of range. Expected NORMAL (0) or REVERSED (1)"); + } + self->cluster->all_to_direction((Direction)direction); + } + return mp_const_none; +} + +extern mp_obj_t MotorCluster_speed_scale(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { + if(n_args <= 2) { + enum { ARG_self, ARG_motor }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_motor, MP_ARG_REQUIRED | MP_ARG_INT }, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _MotorCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _MotorCluster_obj_t); + + int motor = args[ARG_motor].u_int; + int motor_count = (int)self->cluster->count(); + if(motor_count == 0) + mp_raise_ValueError("this cluster does not have any motors"); + else if(motor < 0 || motor >= motor_count) + mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("motor out of range. Expected 0 to %d"), motor_count - 1); + else + return mp_obj_new_float(self->cluster->speed_scale((uint)motor)); + } + else { + enum { ARG_self, ARG_motors, ARG_speed_scale }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_motors, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_speed_scale, MP_ARG_REQUIRED | MP_ARG_OBJ }, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _MotorCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _MotorCluster_obj_t); + + int motor_count = (int)self->cluster->count(); + if(motor_count == 0) + mp_raise_ValueError("this cluster does not have any motors"); + else { + // Determine what motor(s) to modify + const mp_obj_t object = args[ARG_motors].u_obj; + if(mp_obj_is_int(object)) { + int motor = mp_obj_get_int(object); + if(motor < 0 || motor >= motor_count) + mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("motor out of range. Expected 0 to %d"), motor_count - 1); + else { + float speed_scale = mp_obj_get_float(args[ARG_speed_scale].u_obj); + if(speed_scale < FLT_EPSILON) { + mp_raise_ValueError("speed_scale out of range. Expected greater than 0.0"); + } + self->cluster->speed_scale((uint)motor, speed_scale); + } + } + else { + size_t length = 0; + mp_obj_t *items = nullptr; + if(mp_obj_is_type(object, &mp_type_list)) { + mp_obj_list_t *list = MP_OBJ_TO_PTR2(object, mp_obj_list_t); + length = list->len; + items = list->items; + } + else if(mp_obj_is_type(object, &mp_type_tuple)) { + mp_obj_tuple_t *tuple = MP_OBJ_TO_PTR2(object, mp_obj_tuple_t); + length = tuple->len; + items = tuple->items; + } + + if(items == nullptr) + mp_raise_TypeError("cannot convert object to a list or tuple of integers, or a single integer"); + else if(length == 0) + mp_raise_TypeError("list or tuple must contain at least one integer"); + else { + // Create and populate a local array of motor indices + uint8_t *motors = new uint8_t[length]; + for(size_t i = 0; i < length; i++) { + int motor = mp_obj_get_int(items[i]); + if(motor < 0 || motor >= motor_count) { + delete[] motors; + mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("a motor in the list or tuple is out of range. Expected 0 to %d"), motor_count - 1); + } + else { + motors[i] = (uint8_t)motor; + } + } + float speed_scale = mp_obj_get_float(args[ARG_speed_scale].u_obj); + if(speed_scale < FLT_EPSILON) { + delete[] motors; + mp_raise_ValueError("speed_scale out of range. Expected greater than 0.0"); + } + + self->cluster->speed_scale(motors, length, speed_scale); + delete[] motors; + } + } + } + } + return mp_const_none; +} + +extern mp_obj_t MotorCluster_all_to_speed_scale(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { + enum { ARG_self, ARG_speed_scale }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_speed_scale, MP_ARG_REQUIRED | MP_ARG_OBJ }, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _MotorCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _MotorCluster_obj_t); + + int motor_count = (int)self->cluster->count(); + if(motor_count == 0) + mp_raise_ValueError("this cluster does not have any motors"); + else { + float speed_scale = mp_obj_get_float(args[ARG_speed_scale].u_obj); + if(speed_scale < FLT_EPSILON) { + mp_raise_ValueError("speed_scale out of range. Expected greater than 0.0"); + } + self->cluster->all_to_speed_scale(speed_scale); + } + return mp_const_none; +} + +extern mp_obj_t MotorCluster_deadzone(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { + if(n_args <= 2) { + enum { ARG_self, ARG_motor }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_motor, MP_ARG_REQUIRED | MP_ARG_INT }, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _MotorCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _MotorCluster_obj_t); + + int motor = args[ARG_motor].u_int; + int motor_count = (int)self->cluster->count(); + if(motor_count == 0) + mp_raise_ValueError("this cluster does not have any motors"); + else if(motor < 0 || motor >= motor_count) + mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("motor out of range. Expected 0 to %d"), motor_count - 1); + else + return mp_obj_new_float(self->cluster->deadzone((uint)motor)); + } + else { + enum { ARG_self, ARG_motors, ARG_deadzone, ARG_load }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_motors, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_deadzone, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_load, MP_ARG_BOOL, { .u_bool = true }}, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _MotorCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _MotorCluster_obj_t); + + int motor_count = (int)self->cluster->count(); + if(motor_count == 0) + mp_raise_ValueError("this cluster does not have any motors"); + else { + // Determine what motor(s) to modify + const mp_obj_t object = args[ARG_motors].u_obj; + if(mp_obj_is_int(object)) { + int motor = mp_obj_get_int(object); + if(motor < 0 || motor >= motor_count) + mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("motor out of range. Expected 0 to %d"), motor_count - 1); + else { + float deadzone = mp_obj_get_float(args[ARG_deadzone].u_obj); + if(deadzone < 0.0f || deadzone > 1.0f) { + mp_raise_ValueError("deadzone out of range. Expected 0.0 to 1.0"); + } + self->cluster->deadzone((uint)motor, deadzone, args[ARG_load].u_bool); + } + } + else { + size_t length = 0; + mp_obj_t *items = nullptr; + if(mp_obj_is_type(object, &mp_type_list)) { + mp_obj_list_t *list = MP_OBJ_TO_PTR2(object, mp_obj_list_t); + length = list->len; + items = list->items; + } + else if(mp_obj_is_type(object, &mp_type_tuple)) { + mp_obj_tuple_t *tuple = MP_OBJ_TO_PTR2(object, mp_obj_tuple_t); + length = tuple->len; + items = tuple->items; + } + + if(items == nullptr) + mp_raise_TypeError("cannot convert object to a list or tuple of integers, or a single integer"); + else if(length == 0) + mp_raise_TypeError("list or tuple must contain at least one integer"); + else { + // Create and populate a local array of motor indices + uint8_t *motors = new uint8_t[length]; + for(size_t i = 0; i < length; i++) { + int motor = mp_obj_get_int(items[i]); + if(motor < 0 || motor >= motor_count) { + delete[] motors; + mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("a motor in the list or tuple is out of range. Expected 0 to %d"), motor_count - 1); + } + else { + motors[i] = (uint8_t)motor; + } + } + float deadzone = mp_obj_get_float(args[ARG_deadzone].u_obj); + if(deadzone < 0.0f || deadzone > 1.0f) { + delete[] motors; + mp_raise_ValueError("deadzone out of range. Expected 0.0 to 1.0"); + } + self->cluster->deadzone(motors, length, deadzone, args[ARG_load].u_bool); + delete[] motors; + } + } + } + } + return mp_const_none; +} + +extern mp_obj_t MotorCluster_all_to_deadzone(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { +enum { ARG_self, ARG_deadzone, ARG_load }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_deadzone, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_load, MP_ARG_BOOL, { .u_bool = true }}, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _MotorCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _MotorCluster_obj_t); + + int motor_count = (int)self->cluster->count(); + if(motor_count == 0) + mp_raise_ValueError("this cluster does not have any motors"); + else { + float deadzone = mp_obj_get_float(args[ARG_deadzone].u_obj); + if(deadzone < 0.0f || deadzone > 1.0f) { + mp_raise_ValueError("deadzone out of range. Expected 0.0 to 1.0"); + } + self->cluster->all_to_deadzone(deadzone, args[ARG_load].u_bool); + } + return mp_const_none; +} + +extern mp_obj_t MotorCluster_decay_mode(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { + if(n_args <= 2) { + enum { ARG_self, ARG_motor }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_motor, MP_ARG_REQUIRED | MP_ARG_INT }, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _MotorCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _MotorCluster_obj_t); + + int motor = args[ARG_motor].u_int; + int motor_count = (int)self->cluster->count(); + if(motor_count == 0) + mp_raise_ValueError("this cluster does not have any motors"); + else if(motor < 0 || motor >= motor_count) + mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("motor out of range. Expected 0 to %d"), motor_count - 1); + else + return mp_obj_new_int((int)self->cluster->decay_mode((uint)motor)); + } + else { + enum { ARG_self, ARG_motors, ARG_mode, ARG_load }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_motors, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_mode, MP_ARG_REQUIRED | MP_ARG_INT }, + { MP_QSTR_load, MP_ARG_BOOL, { .u_bool = true }}, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _MotorCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _MotorCluster_obj_t); + + int motor_count = (int)self->cluster->count(); + if(motor_count == 0) + mp_raise_ValueError("this cluster does not have any motors"); + else { + // Determine what motor(s) to modify + const mp_obj_t object = args[ARG_motors].u_obj; + if(mp_obj_is_int(object)) { + int motor = mp_obj_get_int(object); + if(motor < 0 || motor >= motor_count) + mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("motor out of range. Expected 0 to %d"), motor_count - 1); + else { + int mode = args[ARG_mode].u_int; + if(mode < 0 || mode > 1) { + mp_raise_ValueError("mode out of range. Expected FAST_DECAY (0) or SLOW_DECAY (1)"); + } + self->cluster->decay_mode((uint)motor, (DecayMode)mode, args[ARG_load].u_bool); + } + } + else { + size_t length = 0; + mp_obj_t *items = nullptr; + if(mp_obj_is_type(object, &mp_type_list)) { + mp_obj_list_t *list = MP_OBJ_TO_PTR2(object, mp_obj_list_t); + length = list->len; + items = list->items; + } + else if(mp_obj_is_type(object, &mp_type_tuple)) { + mp_obj_tuple_t *tuple = MP_OBJ_TO_PTR2(object, mp_obj_tuple_t); + length = tuple->len; + items = tuple->items; + } + + if(items == nullptr) + mp_raise_TypeError("cannot convert object to a list or tuple of integers, or a single integer"); + else if(length == 0) + mp_raise_TypeError("list or tuple must contain at least one integer"); + else { + // Create and populate a local array of motor indices + uint8_t *motors = new uint8_t[length]; + for(size_t i = 0; i < length; i++) { + int motor = mp_obj_get_int(items[i]); + if(motor < 0 || motor >= motor_count) { + delete[] motors; + mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("a motor in the list or tuple is out of range. Expected 0 to %d"), motor_count - 1); + } + else { + motors[i] = (uint8_t)motor; + } + } + int mode = args[ARG_mode].u_int; + if(mode < 0 || mode > 1) { + delete[] motors; + mp_raise_ValueError("mode out of range. Expected FAST_DECAY (0) or SLOW_DECAY (1)"); + } + self->cluster->decay_mode(motors, length, (DecayMode)mode, args[ARG_load].u_bool); + delete[] motors; + } + } + } + } + return mp_const_none; +} + +extern mp_obj_t MotorCluster_all_to_decay_mode(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { + enum { ARG_self, ARG_mode, ARG_load }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_mode, MP_ARG_REQUIRED | MP_ARG_INT }, + { MP_QSTR_load, MP_ARG_BOOL, { .u_bool = true }}, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _MotorCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _MotorCluster_obj_t); + + int motor_count = (int)self->cluster->count(); + if(motor_count == 0) + mp_raise_ValueError("this cluster does not have any motors"); + else { + int mode = args[ARG_mode].u_int; + if(mode < 0 || mode > 1) { + mp_raise_ValueError("mode out of range. Expected FAST_DECAY (0) or SLOW_DECAY (1)"); + } + self->cluster->all_to_decay_mode((DecayMode)mode, args[ARG_load].u_bool); + } + return mp_const_none; +} } diff --git a/micropython/modules/motor/motor.h b/micropython/modules/motor/motor.h index 45dd0987..1dff6978 100644 --- a/micropython/modules/motor/motor.h +++ b/micropython/modules/motor/motor.h @@ -18,12 +18,13 @@ extern mp_obj_t Motor_speed(size_t n_args, const mp_obj_t *pos_args, mp_map_t *k extern mp_obj_t Motor_frequency(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); extern mp_obj_t Motor_stop(mp_obj_t self_in); extern mp_obj_t Motor_coast(mp_obj_t self_in); +extern mp_obj_t Motor_brake(mp_obj_t self_in); extern mp_obj_t Motor_full_negative(mp_obj_t self_in); extern mp_obj_t Motor_full_positive(mp_obj_t self_in); extern mp_obj_t Motor_to_percent(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); extern mp_obj_t Motor_direction(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); extern mp_obj_t Motor_speed_scale(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); -extern mp_obj_t Motor_deadzone_percent(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); +extern mp_obj_t Motor_deadzone(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); extern mp_obj_t Motor_decay_mode(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); extern void MotorCluster_print(const mp_print_t *print, mp_obj_t self_in, mp_print_kind_t kind); @@ -47,18 +48,20 @@ extern mp_obj_t MotorCluster_stop(size_t n_args, const mp_obj_t *pos_args, mp_ma extern mp_obj_t MotorCluster_stop_all(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); extern mp_obj_t MotorCluster_coast(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); extern mp_obj_t MotorCluster_coast_all(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); +extern mp_obj_t MotorCluster_brake(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); +extern mp_obj_t MotorCluster_brake_all(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); extern mp_obj_t MotorCluster_full_negative(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); -extern mp_obj_t MotorCluster_all_to_full_negative(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); +extern mp_obj_t MotorCluster_all_full_negative(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); extern mp_obj_t MotorCluster_full_positive(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); -extern mp_obj_t MotorCluster_all_to_full_positive(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); +extern mp_obj_t MotorCluster_all_full_positive(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); extern mp_obj_t MotorCluster_to_percent(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); extern mp_obj_t MotorCluster_all_to_percent(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); extern mp_obj_t MotorCluster_load(mp_obj_t self_in); -//extern mp_obj_t MotorCluster_direction(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); -//extern mp_obj_t MotorCluster_all_directions(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); -//extern mp_obj_t MotorCluster_speed_scale(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); -//extern mp_obj_t MotorCluster_all_speed_scales(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); -//extern mp_obj_t MotorCluster_deadzone_percent(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); -//extern mp_obj_t MotorCluster_all_deadzone_percents(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); -//extern mp_obj_t MotorCluster_decay_mode(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); -//extern mp_obj_t MotorCluster_all_decay_modes(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); +extern mp_obj_t MotorCluster_direction(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); +extern mp_obj_t MotorCluster_all_to_direction(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); +extern mp_obj_t MotorCluster_speed_scale(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); +extern mp_obj_t MotorCluster_all_to_speed_scale(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); +extern mp_obj_t MotorCluster_deadzone(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); +extern mp_obj_t MotorCluster_all_to_deadzone(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); +extern mp_obj_t MotorCluster_decay_mode(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); +extern mp_obj_t MotorCluster_all_to_decay_mode(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); From 96070f61af1c2960514de119e20447e90742adc0 Mon Sep 17 00:00:00 2001 From: ZodiusInfuser Date: Tue, 12 Apr 2022 17:00:39 +0100 Subject: [PATCH 10/60] Started on motor readme, and updated API elements accordingly --- examples/motor2040/motor2040_single_motor.cpp | 2 +- libraries/motor2040/motor2040.hpp | 56 +- libraries/pico_motor_shim/pico_motor_shim.hpp | 12 +- micropython/modules/motor/README.md | 483 +++++++++--------- micropython/modules/motor/motor.c | 42 +- micropython/modules/motor/motor.cpp | 12 +- 6 files changed, 316 insertions(+), 291 deletions(-) diff --git a/examples/motor2040/motor2040_single_motor.cpp b/examples/motor2040/motor2040_single_motor.cpp index dae8f56a..a151227c 100644 --- a/examples/motor2040/motor2040_single_motor.cpp +++ b/examples/motor2040/motor2040_single_motor.cpp @@ -22,7 +22,7 @@ constexpr float SWEEP_EXTENT = 90.0f; // Create a motor on pin 0 and 1 -Motor2 m = Motor2(motor2040::MOTOR_1); +Motor2 m = Motor2(motor2040::MOTOR_A); int main() { diff --git a/libraries/motor2040/motor2040.hpp b/libraries/motor2040/motor2040.hpp index 1da18c91..17dc22d4 100644 --- a/libraries/motor2040/motor2040.hpp +++ b/libraries/motor2040/motor2040.hpp @@ -7,34 +7,34 @@ namespace motor { namespace motor2040 { - const uint MOTOR_1P = 4; - const uint MOTOR_1N = 5; - const uint MOTOR_2P = 6; - const uint MOTOR_2N = 7; - const uint MOTOR_3P = 8; - const uint MOTOR_3N = 9; - const uint MOTOR_4P = 10; - const uint MOTOR_4N = 11; + 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_1(MOTOR_1P, MOTOR_1N); - const pin_pair MOTOR_2(MOTOR_2P, MOTOR_2N); - const pin_pair MOTOR_3(MOTOR_3P, MOTOR_3N); - const pin_pair MOTOR_4(MOTOR_4P, MOTOR_4N); + 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_1A = 0; - const uint ENCODER_1B = 1; - const uint ENCODER_2A = 2; - const uint ENCODER_2B = 3; - const uint ENCODER_3A = 12; - const uint ENCODER_3B = 13; - const uint ENCODER_4A = 14; - const uint ENCODER_4B = 15; + 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; - const pin_pair ENCODER_1(ENCODER_1A, ENCODER_1B); - const pin_pair ENCODER_2(ENCODER_2A, ENCODER_2B); - const pin_pair ENCODER_3(ENCODER_3A, ENCODER_3B); - const pin_pair ENCODER_4(ENCODER_4A, ENCODER_4B); + const pin_pair ENCODER_A(ENCODER_A_A, ENCODER_A_B); + const pin_pair ENCODER_B(ENCODER_B_A, ENCODER_B_B); + const pin_pair ENCODER_C(ENCODER_C_A, ENCODER_C_B); + const pin_pair ENCODER_D(ENCODER_D_A, ENCODER_D_B); const uint NUM_ENCODERS = 4; const uint TX_TRIG = 16; @@ -54,10 +54,10 @@ namespace motor { const uint ADC2 = 28; const uint SHARED_ADC = 29; // The pin used for the board's sensing features - const uint CURRENT_SENSE_1_ADDR = 0b000; - const uint CURRENT_SENSE_2_ADDR = 0b001; - const uint CURRENT_SENSE_3_ADDR = 0b010; - const uint CURRENT_SENSE_4_ADDR = 0b011; + 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; diff --git a/libraries/pico_motor_shim/pico_motor_shim.hpp b/libraries/pico_motor_shim/pico_motor_shim.hpp index ab456807..9cb90c20 100644 --- a/libraries/pico_motor_shim/pico_motor_shim.hpp +++ b/libraries/pico_motor_shim/pico_motor_shim.hpp @@ -7,13 +7,13 @@ namespace motor { namespace pico_motor_shim { const uint BUTTON_A = 2; - const uint MOTOR_1P = 6; - const uint MOTOR_1N = 7; - const uint MOTOR_2P = 27; - const uint MOTOR_2N = 26; + 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_1P, MOTOR_1N); - const pin_pair MOTOR_2(MOTOR_2P, MOTOR_2N); + 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; } } \ No newline at end of file diff --git a/micropython/modules/motor/README.md b/micropython/modules/motor/README.md index 7fe0890b..752babe1 100644 --- a/micropython/modules/motor/README.md +++ b/micropython/modules/motor/README.md @@ -1,22 +1,21 @@ -# Servos and Servo 2040 +# Motors and Motor 2040 -The Servo library lets you drive 3-pin hobby servo motors from a Raspberry Pi Pico or any other RP2040-based board, such as the [Pimoroni Servo 2040](https://pimoroni.com/servo2040). +The Motor library lets you drive DC motors from a Raspberry Pi Pico or any other RP2040-based board, such as the [Pimoroni Motor 2040](https://pimoroni.com/servo2040). -This library offers two servo implementations: -* a `Servo` class that uses hardware PWM to drive a single servo, with support for up to 16 servos. -* a `ServoCluster` class that uses Programmable IO (PIO) hardware to drive up to 30 servos. - -There is also a `Calibration` class for performing advanced tweaking of each servo's movement behaviour. +This library offers two motor implementations: +* a `Motor` class that uses hardware PWM to drive a single motor, with support for up to 8 motors. +* a `MotorCluster` class that uses Programmable IO (PIO) hardware to drive up to 15 servos. ## Table of Content -- [Servo 2040](#servo-2040) +- [Motor 2040](#motor-2040) - [Reading the User Button](#reading-the-user-button) - [Reading the Sensors](#reading-the-sensors) - - [Configuring Pulls](#configuring-pulls) - - [Controlling the LED Bar](#controlling-the-led-bar) + - [Fault Sensing and Configuring Pulls](#fault-sensing-and-configuring-pulls) + - [Controlling the LED](#controlling-the-led) - [Pin Constants](#pin-constants) - - [Servo Pins](#servo-pins) + - [Motor Pin Tuples](#motor-pin-tuples) + - [Encoder Pin Tuples](#encoder-pin-tuples) - [LED Pin](#led-pin) - [I2C Pins](#i2c-pins) - [Button Pin](#button-pin) @@ -26,62 +25,52 @@ There is also a `Calibration` class for performing advanced tweaking of each ser - [Counts](#counts) - [Addresses](#addresses) - [Sensing](#sensing) -- [Servo](#servo) +- [Motor](#motor) - [Getting Started](#getting-started) - - [Control by Value](#control-by-value) - - [Common Values](#common-values) + - [Control by Speed](#control-by-speed) - [Control by Percent](#control-by-percent) - - [Control by Pulse Width](#control-by-pulse-width) + - [Control by Duty Cycle](#control-by-duty-cycle) - [Frequency Control](#frequency-control) - - [Calibration](#calibration) + - [Configuration](#configuration) - [Function Reference](#function-reference) - [PWM Limitations](#pwm-limitations) -- [ServoCluster](#servocluster) +- [MotorCluster](#motorcluster) - [Getting Started](#getting-started-1) - - [Control by Value](#control-by-value-1) - - [Common Values](#common-values-1) + - [Control by Speed](#control-by-speed-1) - [Control by Percent](#control-by-percent-1) - - [Control by Pulse Width](#control-by-pulse-width-1) + - [Control by Duty Cycle](#control-by-duty-cycle-1) - [Frequency Control](#frequency-control-1) - [Phase Control](#phase-control) - - [Calibration](#calibration-1) + - [Configuration](#configuration-1) - [Delayed Loading](#delayed-loading) - [Function Reference](#function-reference-1) - [PIO Limitations](#pio-limitations) -- [Calibration](#calibration-2) - - [Common Types](#common-types) - - [Custom Calibration](#custom-calibration) - - [Modifying a Calibration](#modifying-a-calibration) - - [Movement Limits](#movement-limits) - - [Populating a Calibration](#populating-a-calibration) - - [Viewing the Mapping](#viewing-the-mapping) - - [Function Reference](#function-reference-2) -## Servo 2040 +## Motor 2040 -Servo 2040 is a **standalone servo controller** for making things with lots of moving parts. It has pre-soldered pin headers for plugging in up to 18 servos, with additional bells and whistles like sensor headers, current monitoring, RGB LEDs, and a user button that make it ideal for many robotics projects! +Motor 2040 is a **standalone motor controller** for driving DC motors with encoder feedback. It has JST-SH connectors for plugging in up to 4 motors, with additional bells and whistles like sensor headers, current monitoring, an RGB LED, and a user button that make it ideal for many robotics projects! -For more information on this board, check out the store page: [pimoroni.com/servo2040](https://pimoroni.com/servo2040). +For more information on this board, check out the store page: [pimoroni.com/motor2040](https://pimoroni.com/motor2040). ### Reading the User Button -As is the case for many of Pimoroni's RP2040-based boards, the boot button on Servo 2040 that is used for programming also acts as a user button once a program is running. To simplify the use of this and other buttons, the `pimoroni` module contains a `Button` class that handles button debounce and auto-repeat. +As is the case for many of Pimoroni's RP2040-based boards, the boot button on Motor 2040 that is used for programming also acts as a user button once a program is running. To simplify the use of this and other buttons, the `pimoroni` module contains a `Button` class that handles button debounce and auto-repeat. ```python Button(button, invert=True, repeat_time=200, hold_time=1000) ``` -To set up the user button, first import the `Button` class from the `pimoroni` module and the pin constant for the button from `servo`: +To set up the user button, first import the `Button` class from the `pimoroni` module and the pin constant for the button from `motor`: ```python from pimoroni import Button -from servo import servo2040 +from motor import motor2040 ``` Then create an instance of `Button` for the user button: ```python -user_sw = Button(servo2040.USER_SW) +user_sw = Button(motor2040.USER_SW) ``` To get the button state, call `.read()`. If the button is held down, then this will return `True` at the interval specified by `repeat_time` until `hold_time` is reached, at which point it will return `True` every `repeat_time / 3` milliseconds. This is useful for rapidly increasing/decreasing values: @@ -98,128 +87,151 @@ state = user_sw.raw() ### Reading the Sensors -On the right-hand edge of Servo 2040 are six analog inputs, with neighbouring 3.3V and GND. These let you connect up sensors to enable your mechanical creations to sense how they are interacting with the world. For example, a servo driven gripper with analog force sensors in its fingers could report how much pressure it is applying as it grabs an object, or the extra pin of an analog feedback servo could be wired up to report its actual angle. +On the right-hand edge of Motor 2040 are two analog inputs, with neighbouring 3.3V and GND. These let you connect up sensors to enable your mechanical creations to sense how they are interacting with the world. For example, a pair of analog proximity sensors could be hooked up for wall avoidance or line following, or they could have microswitches wired to report when a motor driven mechanism has reached an end-stop. -Servo 2040 also has two internal sensors: -* A voltage sensor, letting you measure the supply voltage to the servos. -* A low-side current sensor, letting you measure how much current a set of servos is drawing. -Both of these could be used just for monitoring, or as the trigger to turn off servos safely when voltage gets too low or current draw gets too high. +Motor 2040 also has six internal sensors: +* A voltage sensor, letting you measure the supply voltage to the motors. +* Four current sensors, letting you measure how much current each motor is drawing. +* A fault sensor, letting you know if there is an issue with one of more of your motors +These could be used just for monitoring, or as the trigger to turn off motors safely when voltage gets too low or current draw gets too high. -To allow for all of these inputs, Servo 2040 has an onboard analog multiplexer that expands a single analog pin into eight, by use of three digital address pins that select a single input at a time. As such, the setup for these sensors is more involved than it would be to just read eight analog pins directly. +To allow for all of these inputs, Motor 2040 has an onboard analog multiplexer that expands a single analog pin into eight, by use of three digital address pins that select a single input at a time. As such, the setup for these sensors is more involved than it would be to just read eight analog pins directly. To begin reading these inputs, first import the `Analog` and `AnalogMux` classes from `pimoroni` and the pin, address, and gain constants from `servo`: ```python from pimoroni import Analog -from servo import servo2040 +from motor import motor2040 ``` -Then set up three instances of `Analog` for the sensor, voltage, and current sensing: +Then set up three instances of `Analog` for the sensor and fault, voltage, and current sensing: ```python -sen_adc = Analog(servo2040.SHARED_ADC) -vol_adc = Analog(servo2040.SHARED_ADC, servo2040.VOLTAGE_GAIN) -cur_adc = Analog(servo2040.SHARED_ADC, servo2040.CURRENT_GAIN, - servo2040.SHUNT_RESISTOR, servo2040.CURRENT_OFFSET) +sen_fault_adc = Analog(motor2040.SHARED_ADC) +vol_adc = Analog(motor2040.SHARED_ADC, motor2040.VOLTAGE_GAIN) +cur_adc = Analog(motor2040.SHARED_ADC, motor2040.CURRENT_GAIN, + motor2040.SHUNT_RESISTOR, motor2040.CURRENT_OFFSET) ``` You may notice, all three of these use the same `SHARED_ADC` pin. This is intentional as it is just a single pin that is being used for all three different functions, only the gains differ. The next step is to set up the analog multiplexer, by providing it with the three address pins: ```python -mux = AnalogMux(servo2040.ADC_ADDR_0, servo2040.ADC_ADDR_1, servo2040.ADC_ADDR_2) +mux = AnalogMux(motor2040.ADC_ADDR_0, motor2040.ADC_ADDR_1, motor2040.ADC_ADDR_2) ``` Note that the `AnalogMux` does not know about any of the `Analog` classes that were created before. With the multiplexer now configured, reading each sensor is simply a case of 'selecting' the sensor on the multiplexer then reading the value from one of the three `Analog` classes created at the start. -To read the siz sensor headers: +To read the two sensor headers: ```python -for addr in range(servo2040.NUM_SENSORS): - mux.select(addr) - print("Sensor", addr + 1, "=", sen_adc.read_voltage()) +for addr in range(motor2040.NUM_SENSORS): + mux.select(addr + motor2040.SENSOR_1_ADDR) + print("Sensor", addr + 1, "=", sen_fault_adc.read_voltage()) ``` To read the voltage sense: ```python -mux.select(servo2040.VOLTAGE_SENSE_ADDR) +mux.select(motor2040.VOLTAGE_SENSE_ADDR) print("Voltage =", vol_adc.read_voltage(), "V") ``` To read the current draw in amps (A): ```python -mux.select(servo2040.CURRENT_SENSE_ADDR) -print("Current =", cur_adc.read_current(), "A") +for addr in range(motor2040.NUM_MOTORS): + mux.select(addr + motor2040.CURRENT_SENSE_A_ADDR) + print("Current", addr + 1, "=", cur_adc.read_current(), "A") ``` #### Configuring Pulls -For some sensors you may need to have the input be pulled high or low before taking a reading. To support this there is an optional `muxed_pin` parameter that can be passed into the `AnalogMux` when creating it, which gives the multiplexer access to the pin to control the pulls. +For some sensors, as well as the internal fault sensor, you may need to have the input be pulled high or low before taking a reading. To support this there is an optional `muxed_pin` parameter that can be passed into the `AnalogMux` when creating it, which gives the multiplexer access to the pin to control the pulls. ```python -mux = AnalogMux(servo2040.ADC_ADDR_0, servo2040.ADC_ADDR_1, servo2040.ADC_ADDR_2, +mux = AnalogMux(motor2040.ADC_ADDR_0, motor2040.ADC_ADDR_1, motor2040.ADC_ADDR_2, muxed_pin=Pin(servo2040.SHARED_ADC)) ``` From there the pull state of each of the multiplexer's addresses can be configured independently by calling `.configure_pull()`, with the address and the pull state (either `Pin.PULL_UP`, `Pin.PULL_DOWN`, or `None`). -The below example shows how to set all 6 sensor addresses to have pull-downs: +The below example shows how to set both sensor addresses to have pull-downs: ```python -sensor_addrs = list(range(servo2040.SENSOR_1_ADDR, servo2040.SENSOR_6_ADDR + 1)) +sensor_addrs = list(range(motor2040.SENSOR_1_ADDR, motor2040.SENSOR_2_ADDR + 1)) for addr in sensor_addrs: mux.configure_pull(addr, Pin.PULL_DOWN) ``` +#### Fault Sensing -#### Controlling the LED Bar +The drivers on Motor 2040 can detect when there is a fault with their connected motors, such as if a short occurs, and shut themselves off for safety. As part of this they also signal that a fault has occurred, which can be read. The way they signal this is by pulling the line to ground. This means that the line needs to be high, and so the 'AnalogMux' needs to be configured accordingly. -Alongside Servo 2040's six sensor headers are six addressable RGB LEDs. These work using the same chainable 1-wire signalling as WS2812 LED's, commonly known as Neopixels. As such, they can be controlled using the same Plasma Library used by the [Pimoroni Plasma 2040](https://shop.pimoroni.com/products/plasma-2040). +```python +adc_as_io = Pin(motor2040.SHARED_ADC) +mux = AnalogMux(motor2040.ADC_ADDR_0, motor2040.ADC_ADDR_1, motor2040.ADC_ADDR_2, + muxed_pin=adc_as_io) +mux.configure_pull(motor2040.FAULT_SENSE_ADDR, Pin.PULL_UP) +``` -To set up the LED bar, first import the `WS2812` class from the `plasma` module and the pin constants for the LEDs from `servo`: +Then in your main code: +```python +mux.select(motor2040.FAULT_SENSE_ADDR) +if not adc_as_io.value(): + print("Fault!") +else: + print("No Fault") +``` +//TODO make this neater + + +#### Controlling the LED + +Between Motor 2040's for motor connectors is a single addressable RGB LEDs. This works using the same chainable 1-wire signalling as WS2812 LED's, commonly known as Neopixels. As such, it can be controlled using the same Plasma Library used by the [Pimoroni Plasma 2040](https://shop.pimoroni.com/products/plasma-2040). + +To set up the LED, first import the `WS2812` class from the `plasma` module and the pin constants for the LED from `motor`: ```python from plasma import WS2812 -from servo import servo2040 +from motor import motor2040 ``` Then construct a new `WS2812` instance, specifying the number of LEDs, PIO, PIO state-machine and GPIO pin. ```python -led_bar = WS2812(servo2040.NUM_LEDS, 1, 0, servo2040.LED_DATA) +led = WS2812(motor2040.NUM_LEDS, 1, 0, motor2040.LED_DATA) ``` -Finally, start the LED bar by calling `start`: +Finally, start the LED by calling `start`: ```python -led_bar.start() +led.start() ``` -For more information on how to control the LEDs on the bar, please refer to the [Plasma Library](https://github.com/pimoroni/pimoroni-pico/tree/main/micropython/modules/plasma). +For more information on how to control the LED, please refer to the [Plasma Library](https://github.com/pimoroni/pimoroni-pico/tree/main/micropython/modules/plasma). ### Pin Constants -The `servo` module contains a `servo2040` sub module with constants for the servo, LED, sensor and button pins. +The `motor` module contains a `motor2040` sub module with constants for the motor, encoder, LED, sensor and button pins. -#### Servo Pins +#### Motor Pin Tuples -* `SERVO_1` = `0` -* `SERVO_2` = `1` -* `SERVO_3` = `2` -* `SERVO_4` = `3` -* `SERVO_5` = `4` -* `SERVO_6` = `5` -* `SERVO_7` = `6` -* `SERVO_8` = `7` -* `SERVO_9` = `8` -* `SERVO_10` = `9` -* `SERVO_11` = `10` -* `SERVO_12` = `11` -* `SERVO_13` = `12` -* `SERVO_14` = `13` -* `SERVO_15` = `14` -* `SERVO_16` = `15` -* `SERVO_17` = `16` -* `SERVO_18` = `17` +* `MOTOR_A` = `(4, 5)` +* `MOTOR_B` = `(6, 7)` +* `MOTOR_C` = `(8, 9)` +* `MOTOR_D` = `(10, 11)` + + +#### Encoder Pin Tuples + +* `ENCODER_A` = `(0, 1)` +* `ENCODER_B` = `(2, 3)` +* `ENCODER_C` = `(12, 13)` +* `ENCODER_D` = `(14, 15)` + + +#### UART/ULTRASOUND Pins + +* `TX_TRIG` = `16` +* `RX_ECHO` = `17` #### LED Pin @@ -256,146 +268,159 @@ The `servo` module contains a `servo2040` sub module with constants for the serv ### Other Constants -The `servo2040` sub module also contains other constants to help with the control of Servo 2040's various features: +The `motor2040` sub module also contains other constants to help with the control of Motor 2040's various features: #### Counts -* `NUM_SERVOS` = `18` -* `NUM_SENSORS` = `6` -* `NUM_LEDS` = `6` +* `NUM_MOTORS` = `4` +* `NUM_ENCODERS` = `4` +* `NUM_SENSORS` = `2` +* `NUM_LEDS` = `1` #### Addresses -* `SENSOR_1_ADDR` = `0b000` -* `SENSOR_2_ADDR` = `0b001` -* `SENSOR_3_ADDR` = `0b010` -* `SENSOR_4_ADDR` = `0b011` -* `SENSOR_5_ADDR` = `0b100` -* `SENSOR_6_ADDR` = `0b101` -* `VOLTAGE_SENSE_ADDR` = `0b110` -* `CURRENT_SENSE_ADDR` = `0b111` +* `CURRENT_SENSE_A_ADDR` = `0b000` +* `CURRENT_SENSE_B_ADDR` = `0b001` +* `CURRENT_SENSE_C_ADDR` = `0b010` +* `CURRENT_SENSE_D_ADDR` = `0b011` +* `VOLTAGE_SENSE_ADDR` = `0b100` +* `FAULT_SENSE_ADDR` = `0b101` +* `SENSOR_1_ADDR` = `0b110` +* `SENSOR_2_ADDR` = `0b111` #### Sensing * `VOLTAGE_GAIN` = `0.28058` -* `SHUNT_RESISTOR` = `0.003` -* `CURRENT_GAIN` = `69` +* `SHUNT_RESISTOR` = `0.47` +* `CURRENT_GAIN` = `5` * `CURRENT_OFFSET` = `-0.02` -## Servo +## Motor ### Getting Started -To start using servos with your Servo 2040, you will need to first import the `Servo` class. +To start using motors with your Motor 2040, you will need to first import the `Motor` class. ```python -from servo import Servo, servo2040 +from motor import Motor, motor2040 ``` -If you are using another RP2040 based board, then `servo2040` can be omitted from the above line +If you are using another RP2040 based board, then `motor2040` can be omitted from the above line -To create your servo, choose which GPIO pin it will be connected to, and pass that into `Servo`. For this example we will use one of the handy constants of the `servo2040`. +To create your motor, choose which GPIO pins it will be connected to, and pass that into `Motor`. For this example we will use one of the handy constants of the `motor2040`. ```python -s = Servo(servo2040.SERVO_1) +m = Motor(motor2040.MOTOR_A) ``` -You now have a `Servo` class called `s` that will control the physical servo connected to `SERVO_1`. To start using this servo, simply enable it using: +You now have a `Motor` class called `m` that will control the physical motor connected to `MOTOR_A`. To start using this motor, simply enable it using: ```python -s.enable() +m.enable() ``` -This activates the servo and moves it to it's last known position. Since this is the first time enabling the servo, there is no last known position, so instead it will move to the middle of its movement range instead. +This activates the motor and sets it to its last known speed. Since this is the first time enabling the motor, there is no last known speed, so instead it will be zero. -Once you have finished with the servo, it can be disabled by calling: +Once you have finished with the motor, it can be disabled by calling: ```python -s.disable() +m.disable() ``` -From here the servo can be controlled in several ways. These are covered in more detail in the following sections. +From here the motor can be controlled in several ways. These are covered in more detail in the following sections. -### Control by Value +### Control by Speed -The most intuitive way of controlling a servo is by value. Value can be any number that has a real-world meaning for that type of servo, for example its angle in degrees if it's a regular angular servo, or its speed as a percentage if it's a continuous rotation servo. See [Calibration](#calibration) for more details. +The most intuitive way of controlling a motor is by speed. Speed can be any number that has a real-world meaning for that type of motor, for example revolutions per minute, or the linear or angular speed of the mechanism it is driving. By default the speed is a value ranging from `-1.0` to `1.0` but this can be changed by setting a new `speed_scale`. See [Configuration](#configuration) for more details. -The value of a servo can be set by calling `.value(value)`, which takes a float as its `value` input. If the servo is disabled, this will enable it. The resulting pulse width will also be stored. +The speed of a motor can be set by calling `.speed(speed)`, which takes a float as its `speed` input. If the motor is disabled, this will enable it. The resulting duty cycle will also be stored. -To read back the current value of the servo, call `.value()` without any input. If the servo is disabled, this will be the last value that was provided when enabled. +To read back the current speed of the motor, call `.speed()` without any input. If the motor is disabled, this will be the last speed that was provided when enabled. + +At this time the speed of a motor is the same as `m.duty() * m.speed_scale()`, though there could be the option in the future to add a curve to a motor's speed that make the relationship between speed and duty cycle become non-linear. -#### Common Values +#### Common Speeds -To simplify certain motion patterns, a servo can be commanded to three common values. These are, their minimum, middle, and maximum. These are performed by calling `.to_min()`, `.to_mid()`, and `.to_max()`, respectively. If the servo is disabled, these will enable it. +To simplify certain motion patterns, a motor can be commanded to three common speeds. These are, full negative, full positive, and stopped. These are performed by calling `.full_negative()`, `.full_positive()`, and `.stop()`, respectively. If the motor is disabled, these will enable it. -It is also possible to read back the values each of those three commands is using internally, using `.min_value()`, `.mid_value()`, and `.max_value()`. These can be useful as inputs to equations that provide numbers directly to `.value(value)`, for example. +The full negative and full positive speed can be read back using `.speed_scale()`. This can be useful as an input to equations that provide numbers directly to `.speed(speed)`, for example. ### Control by Percent -Sometimes there are projects where a servo needs to move based on the reading from a sensor or another device, but the numbers given out are not easy to convert to values the servo accepts. To overcome this the library lets you move the servo to a percent between its minimum and maximum values, or two values provided, based on that input. +Sometimes there are projects where a motor needs to drive based on the reading from a sensor or another device, but the numbers given out are not easy to convert to speeds the motor accepts. To overcome this the library lets you drive the motor at a percent between its negative and positive speeds, or two speeds provided, based on that input. -With an input between `-1.0` and `1.0`, a servo can be moved to a percent between its minimum and maximum values using `.to_percent(in)`. +With an input between `-1.0` and `1.0`, a motor can be driven at a percent between its negative and positive speeds using `.at_percent(in)`. -With an input between a provided min and max, a servo can be moved to a percent between its minimum and maximum values using `.to_percent(in, in_min, in_max)`. +With an input between a provided min and max, a motor can be driven at a percent between its negative and positive speeds using `.at_percent(in, in_min, in_max)`. -With an input between a provided min and max, a servo can be moved to a percent between two provided values using `.to_percent(in, in_min, value_min, value_max)`. +With an input between a provided min and max, a motor can be driven at a percent between two provided speeds using `.at_percent(in, in_min, value_min, value_max)`. -If the servo is disabled, these will enable it. +If the motor is disabled, these will enable it. -### Control by Pulse Width +### Control by Duty Cycle -At a hardware level servos operate by receiving a digital signal with specific pulse widths. Typically pulses are between 500 microseconds and 2500 microseconds in length, and are usually repeated every 20 milliseconds (50Hz). These functions let you interact with pulse widths directly. +At a hardware level DC motors operate by receiving a voltage across their two terminals, with positive causing a motion in one direction and negative causing a motion in the other. To avoid needing a negative voltage supply, motor drivers employ a H-Bridge arrangement of transistors or mosfets to flip which side of the motor is connected to ground and which is connected to power. By rapidly turing these transistors or mosfets on and off both the speed and direction of the motor can be varied. The common way this is achieved is by using a pair of pulse width modulated signals, where the duty cycle of the active signal controls the speed, and which signal is active controls the direction. Braking can also be controlled (see //TODO) -The pulse width (in microseconds) of a servo can be set by calling `.pulse(pulse)`, which takes a float as its `pulse` input. If the servo is disabled this will enable it, except for the case of `0` where instead the servo will be disabled. This function will also recalculate the related value. +The duty cycle of a motor can be set by calling `.duty(duty)`, which takes a float from `-1.0` to `1.0` as its `duty` input. If the motor is disabled this will enable it. This function will also recalculate the related speed. -To read back the current pulse width (in microseconds) of the servo, call `.pulse()` without any input. If the servo is disabled, this will be the last pulse that was provided when enabled. +To read back the current duty cycle of the motor, call `.duty()` without any input. If the motor is disabled, this will be the last duty that was provided when enabled. + +At this time the duty cycle of a motor is the same as `m.speed() / m.speed_scale()`, though there could be the option in the future to add a curve to a motor's speed that make the relationship between speed and duty cycle become non-linear. ### Frequency Control -The vast majority of servos expect to receive pulses with a frequency of 50Hz, so this library uses that as its default. However, there may be cases where this value needs to be changed, such as when using servos that operate up to frequencies of 333Hz. +Motors can be driven at a variety of frequencies, with a common values being above the range of human hearing. As such this library uses 25KHz as its default, but this can easily be changed. -The frequency (in Hz) of a servo can be set by calling `.frequency(freq)`, which takes a float as its `freq` input. The supported range of this input is `10` to `350` Hz. +The frequency (in Hz) of a motor can be set by calling `.frequency(freq)`, which takes a float as its `freq` input. //TODO The supported range of this input is `10` to `350` Hz. -To read back the current frequency (in Hz) of the servo, call `.frequency()` without any input. +To read back the current frequency (in Hz) of the motor, call `.frequency()` without any input. -Note that changing the frequency does not change the pulse widths sent to the servos, only how frequently they are sent. This is why `350` is the upper limit, as any higher and the maximum pulse would be longer than the time period. If you encounter any servos where this behaviour is not what they expect, please raise it as a Github issue. +Note that changing the frequency does not change the duty cycle sent to the motors, only how frequently pulses are sent. -### Calibration +### Configuration -There are different types of servos, with `ANGULAR`, `LINEAR`, and `CONTINUOUS` being common. To support these different types, each `Servo` class contains a calibration object that stores the specific value to pulse mapping needed for its type. A copy of a servo's calibration can be accessed using `.calibration()`. It is also possible to provide a servo with a new calibration using `.calibration(calibration)`. +There are several options for configuring a motor. As mentioned in earlier sections, the first is to adjust their speed scale. There is also the option to change their direction, change their decay mode, and add in a dead-zone. This is a percentage of the duty cycle below which the motor will be stopped. This is included to avoid annoying situations where the duty cycle is too low for a motor to move, but enough for it to make an audable sound. + +Decay mode is //TODO `.brake()` `.coast()` ### Function Reference -Here is the complete list of functions available on the `Servo` class: +Here is the complete list of functions available on the `Motor` class: ```python -Servo(pin, calibration=ANGULAR, freq=50) # calibration can either be an integer or a Calibration class -pin() +//TODO +Motor(pins, calibration=ANGULAR, freq=50) # calibration can either be an integer or a Calibration class +pins() enable() disable() is_enabled() -pulse() -pulse(pulse) -value() -value(value) +duty() +duty(duty) +speed() +speed(speed) frequency() frequency(freq) -min_value() -mid_value() -max_value() -to_min() -to_mid() -to_max() -to_percent(in) -to_percent(in, in_min, in_max) -to_percent(in, in_min, in_max, value_min, value_max) -calibration() -calibration(calibration) +stop() +coast() +brake() +full_negative() +full_positive() +at_percent(in) +at_percent(in, in_min, in_max) +at_percent(in, in_min, in_max, speed_min, speed_max) +direction() +direction(direction) +speed_scale() +speed_scale(speed_scale) +deadzone() +deadzone(deadzone) +decay_mode() +decay_mode(mode) ``` @@ -403,7 +428,7 @@ calibration(calibration) Although the RP2040 is capable of outputting up to 16 PWM signals, there are limitations of which pins can be used together: * The PWMs output from pins 0 to 15 are repeated for pins 16 to 29, meaning that those pins share the same signals if PWM is enabled on both. For example if you used pin 3 for PWM and then tried to use pin 19, they would both output the same signal and it would not be possible to control them independently. -* The 16 PWM channels are grouped into 8 PWM slices, each containing A and B sub channels that are synchronised with each other. This means that parameters such as frequency are shared, which can cause issues if you want one servo to operate at a different frequency to it's pin neighbour or to drive an LED with PWM at a high frequency. +* The 16 PWM channels are grouped into 8 PWM slices, each containing A and B sub channels that are synchronised with each other. This means that parameters such as frequency are shared, which can cause issues if you want one motor to operate at a different frequency to it's pin neighbour or to drive an LED with PWM at a high frequency. The official [RP2040 datasheet](https://datasheets.raspberrypi.com/rp2040/rp2040-datasheet.pdf), includes the handy _Table 525_ that details the pwm channel for each GPIO pin. This is shown below for convenience: @@ -416,102 +441,102 @@ The official [RP2040 datasheet](https://datasheets.raspberrypi.com/rp2040/rp2040 | PWM Channel | 0A | 0B | 1A | 1B | 2A | 2B | 3A | 3B | 4A | 4B | 5A | 5B | 6A | 6B | -## ServoCluster +## MotorCluster ### Getting Started -An alternative way of controlling servos with your Servo 2040 is to use the `ServoCluster` class. This can be imported as follows: +An alternative way of controlling motors with your Motor 2040 is to use the `MotorCluster` class. This can be imported as follows: ```python -from servo import ServoCluster, servo2040 +from motor import MotorCluster, motor2040 ``` -(If you are using another RP2040 based board, then `servo2040` can be omitted from the above line) +(If you are using another RP2040 based board, then `motor2040` can be omitted from the above line) -The next step is to choose which GPIO pins the cluster will be connected to and store them in a `list`. For a consecutive set of pins the easiest way to do this is to use a `range` to create each pin number, and convert that to a `list`. For example, using the handy constants of the `servo2040`, the below line creates the list `[0, 1, 2, 3]` +The next step is to choose which GPIO pins the cluster will be connected to and store them in a `list`. For example, using the handy constants of the `motor2040`, the below line creates the list `[ (4, 5), (6, 7), (8, 9), (10, 11) ]` ```python -pins = list(range(servo2040.SERVO_1, servo2040.SERVO_4 + 1)) +pins = [ motor2040.MOTOR_A, motor2040.MOTOR_B, motor2040.MOTOR_C, motor2040.MOTOR_D ] ``` -To create your servo cluster, specify the PIO, PIO state-machine and GPIO pins you chose a moment ago, and pass those into `ServoCluster`. +To create your motor cluster, specify the PIO, PIO state-machine and GPIO pins you chose a moment ago, and pass those into `MotorCluster`. ```python -cluster = ServoCluster(0, 0, pins) +cluster = MotorCluster(0, 0, pins) ``` -You now have a `ServoCluster` class called `cluster` that will control the physical servos connected to `SERVO_1`, `SERVO_2`, `SERVO_3`, and `SERVO_4`. To start using these servos, simply enable them using: +You now have a `MotorCluster` class called `cluster` that will control the physical motors connected to `MOTOR_A`, `MOTOR_B`, `MOTOR_C`, and `MOTOR_D`. To start using these motors, simply enable them using: ```python cluster.enable_all() ``` or ```python -cluster.enable(servo) +cluster.enable(motor) ``` -where `servo` is the servo's number within the cluster from `0` to `cluster.count() - 1`. +where `motor` is the motor's number within the cluster from `0` to `cluster.count() - 1`. -These functions activate the servos and move them to their last known positions. Since this is the first time enabling the servos, there are no last known positions, so instead they will move to the middle of their movement ranges instead. +These functions activate the motors and drives them at their last known speed. Since this is the first time enabling the motors, there are no last known speeds, so instead they will be set to zero instead. -Once you have finished with the servos, they can be disabled by calling: +Once you have finished with the motors, they can be disabled by calling: ```python cluster.disable_all() ``` or ```python -cluster.disable(servo) +cluster.disable(motor) ``` -where `servo` is the servo's number within the cluster from `0` to `cluster.count() - 1`. +where `motor` is the motor's number within the cluster from `0` to `cluster.count() - 1`. -From here the servos can be controlled in several ways. These are covered in more detail in the following sections. +From here the motors can be controlled in several ways. These are covered in more detail in the following sections. -### Control by Value +### Control by Speed -The most intuitive way of controlling the servos is by their value. Value can be any number that has a real-world meaning for that type of servo, for example its angle in degrees if it's a regular angular servo, or its speed as a percentage if it's a continuous rotation servo. See [Calibration](#calibration) for more details. +The most intuitive way of controlling a motor is by speed. Speed can be any number that has a real-world meaning for that type of motor, for example revolutions per minute, or the linear or angular speed of the mechanism it is driving. By default the speed is a value ranging from `-1.0` to `1.0` but this can be changed by setting a new `speed_scale`. See [Configuration](#configuration-1) for more details. -The value of a servo on a cluster can be set calling `.value(servo, value)` or `.all_to_value(value)`, which take a float as their `value` input. If a servo is disabled, these will enable it. The resulting pulse width will also be stored. +The speed of a motor on a cluster can be set calling `.speed(motor, speed)` or `.all_at_speed(speed)`, which take a float as their `speed` input. If a motor is disabled, these will enable it. The resulting duty cycle will also be stored. -To read back the current value of a servo on the cluster, call `.value(servo)`. If the servo is disabled, this will be the last value that was provided when enabled. +To read back the current speed of a motor on the cluster, call `.speed(motor)`. If the motor is disabled, this will be the last speed that was provided when enabled. -#### Common Values +#### Common Speeds -To simplify certain motion patterns, servos on a cluster can be commanded to three common values. These are, their minimum, middle, and maximum. These are performed by calling `.to_min(servo)`, `.to_mid(servo)`, and `.to_max(servo)`, respectively. If the servo is disabled, these will enable it. There are also `.all_to_min()`, `.all_to_mid()`, and `.all_to_max()` for having all the servos on the cluster move at once. +To simplify certain motion patterns, motors on a cluster can be commanded to three common speeds. These are, full negative, full positive, and stopped. These are performed by calling `.full_negative(motor)`, `.full_positive(motor)`, and `.stop(servo)`, respectively. If the motor is disabled, these will enable it. There are also `.all_full_negative()`, `.all_full_positive()`, and `.stop_all()` for having all the motors on the cluster drive at once. -It is also possible to read back the values each of those three commands is using internally, using `.min_value(servo)`, `.mid_value(servo)`, and `.max_value(servo)`. These can be useful as inputs to equations that provide numbers directly to `.value(servo, value)`, for example. +The full negative and full positive speed of each motor on a cluster can be read back using `.speed_scale(motor)`. This can be useful as an input to equations that provide numbers directly to `.speed(motor, speed)`, for example. ### Control by Percent -Sometimes there are projects where servos need to move based on the readings from sensors or another devices, but the numbers given out are not easy to convert to values the servos accept. To overcome this the library lets you move the servos on a cluster to a percent between their minimum and maximum values, or two values provided, based on that input. +Sometimes there are projects where motors need to move based on the readings from sensors or another devices, but the numbers given out are not easy to convert to speeds the motors accept. To overcome this the library lets you drive the motors on a cluster at a percent between their negative and positive speeds, or two speeds provided, based on that input. -With an input between `-1.0` and `1.0`, a servo on a cluster can be moved to a percent between its minimum and maximum values using `.to_percent(servo, in)`, or all servos using `.all_to_percent(in)`. +With an input between `-1.0` and `1.0`, a motor on a cluster can be driven at a percent between its negative and positive speeds using `.at_percent(motor, in)`, or all motors using `.all_at_percent(in)`. -With an input between a provided min and max, a servo on a cluster can be moved to a percent between its minimum and maximum values using `.to_percent(servo, in, in_min, in_max)`, or all servos using `s.all_to_percent(in, in_min, in_max)`. +With an input between a provided min and max, a motor on a cluster can be driven at a percent between its negative and postive speeds using `.at_percent(motor, in, in_min, in_max)`, or all motors using `.all_at_percent(in, in_min, in_max)`. -With an input between a provided min and max, a servo on a cluster can be moved to a percent between two provided values using `.to_percent(servo, in, in_min, value_min, value_max)`, or all servos using `.all_to_percent(in, in_min, value_min, value_max)`. +With an input between a provided min and max, a motor on a cluster can be driven at a percent between two provided speeds using `.at_percent(motor, in, in_min, speed_min, speed_max)`, or all motors using `.all_at_percent(in, in_min, speed_min, speed_max)`. -If the servo is disabled, these will enable it. +If the motor is disabled, these will enable it. -### Control by Pulse Width +### Control by Duty Cycle -At a hardware level servos operate by receiving a digital signal with specific pulse widths. Typically pulses are between 500 microseconds and 2500 microseconds in length, and are usually repeated every 20 milliseconds (50Hz). These functions let you interact with pulse widths directly. +At a hardware level DC motors operate by receiving a voltage across their two terminals, with positive causing a motion in one direction and negative causing a motion in the other. To avoid needing a negative voltage supply, motor drivers employ a H-Bridge arrangement of transistors or mosfets to flip which side of the motor is connected to ground and which is connected to power. By rapidly turing these transistors or mosfets on and off both the speed and direction of the motor can be varied. The common way this is achieved is by using a pair of pulse width modulated signals, where the duty cycle of the active signal controls the speed, and which signal is active controls the direction. Braking can also be controlled (see //TODO) -The pulse width (in microseconds) of a servo on a cluster can be set by calling `.pulse(servo, pulse)` or `.all_to_pulse(pulse)`, which take a float as their `pulse` input. If a servo is disabled, these will enable it, except for the case of `0` where instead the servo will be disabled. These functions will also recalculate the related value. +The duty cycle of a motor on a cluster can be set by calling `.duty(motor, duty)` or `.all_at_duty(duty)`, which take a float as their `duty` input. If a motor is disabled, these will enable it. These functions will also recalculate the related speed. -To read back the current pulse width (in microseconds) of a servo on a cluster, call `.pulse(servo)`. If the servo is disabled, this will be the last pulse that was provided when enabled. +To read back the current duty cycle of a motor on a cluster, call `.duty(motor)`. If the motor is disabled, this will be the last duty that was provided when enabled. ### Frequency Control -The vast majority of servos expect to receive pulses with a frequency of 50Hz, so this library uses that as its default. However, there may be cases where this value needs to be changed, such as when using servos that operate up to frequencies of 333Hz. Un +Motors can be driven at a variety of frequencies, with a common values being above the range of human hearing. As such this library uses 25KHz as its default, but this can easily be changed. -The frequency (in Hz) of all the servos on a cluster can be set by calling `.frequency(freq)`, which takes a float as its `freq` input. The supported range of this input is `10` to `350` Hz. Due to how `ServoCluster` works, there is no way to set independent frequencies for each servo. +The frequency (in Hz) of all the motors on a cluster can be set by calling `.frequency(freq)`, which takes a float as its `freq` input. //TODO The supported range of this input is `10` to `350` Hz. Due to how `MotorCluster` works, there is no way to set independent frequencies for each motor. -To read back the current frequency (in Hz) all the servos on a cluster, call `.frequency()` without any input. +To read back the current frequency (in Hz) of all the motors on a cluster, call `.frequency()` without any input. -Note that changing the frequency does not change the pulse widths sent to the servos, only how frequently they are sent. This is why `350` is the upper limit, as any higher and the maximum pulse would be longer than the time period. If you encounter any servos where this behaviour is not what they expect, please raise it as a Github issue. +Note that changing the frequency does not change the duty cycle sent to the motors, only how frequently pulses are sent. -Also, be aware that currently the frequency changes immediately, even if part-way through outputting a pulse. It is therefore recommended to disable all servos first before changing the frequency. +Also, be aware that currently the frequency changes immediately, even if part-way through outputting a pulse. It is therefore recommended to disable all motors first before changing the frequency. ### Phase Control @@ -532,64 +557,64 @@ There are different types of servos, with `ANGULAR`, `LINEAR`, and `CONTINUOUS` ### Delayed Loading -To match behaviour with the regular `Servo` class, `ServoCluster` automatically applies each change to its servo's states immediately. However, sometimes this may not be wanted, and instead you want all servos to receive updated pulses at the same time, regardless of how long the code ran that calculated the update. +To match behaviour with the regular `Motor` class, `MotorCluster` automatically applies each change to its motor's states immediately. However, sometimes this may not be wanted, and instead you want all motors to receive updated duty cycles at the same time, regardless of how long the code ran that calculated the update. -For this purpose, all the functions that modify a servo state on a cluster include an optional parameter `load`, which by default is `True`. To avoid this "loading" include `load=False` in the relevant function calls. Then either the last call can include `load=True`, or a specific call to `.load()` can be made. +For this purpose, all the functions that modify a motor state on a cluster include an optional parameter `load`, which by default is `True`. To avoid this "loading" include `load=False` in the relevant function calls. Then either the last call can include `load=True`, or a specific call to `.load()` can be made. ### Function Reference -Here is the complete list of functions available on the `ServoCluster` class: +Here is the complete list of functions available on the `MotorCluster` class: ```python -ServoCluster(pio, sm, pins, calibration=ANGULAR, freq=50, auto_phase=True) # calibration can either be an integer or a Calibration class +//TODO +MotorCluster(pio, sm, pins, calibration=ANGULAR, freq=50, auto_phase=True) # calibration can either be an integer or a Calibration class count() -pin(servo) -enable(servo, load=True) +pins(motor) +enable(motor, load=True) enable_all(load=True) -disable(servo, load=True) -disable_all(servo, load=True) -is_enabled(servo) -pulse(servo) -pulse(servo, pulse, load=True) -all_to_pulse(pulse, load=True) -value(servo) -value(servo, value, load=True) -all_to_value(value, load=True) -phase(servo) -phase(servo, phase, load=True) +disable(motor, load=True) +disable_all(load=True) +is_enabled(motor) +duty(motor) +duty(motor, duty, load=True) +all_at_duty(motor, load=True) +speed(motor) +speed(motor, speed, load=True) +all_at_speed(speed, load=True) +phase(motor) +phase(motor, phase, load=True) all_to_phase(phase, load=True) frequency() frequency(freq) -min_value(servo) -mid_value(servo) -max_value(servo) -to_min(servo, load=True) -all_to_min(load=True) -to_mid(servo, load=True) -all_to_mid(load=True) -to_max(servo, load=True) -all_to_max(load=True) -to_percent(servo, in, load=True) -to_percent(servo, in, in_min, in_max, load=True) -to_percent(servo, in, in_min, in_max, value_min, value_max, load=True) -all_to_percent(in, load=True) -all_to_percent(in, in_min, in_max, load=True) -all_to_percent(in, in_min, in_max, value_min, value_max, load=True) -calibration(servo) -calibration(servo, calibration) +full_negative(motor, load=True) +all_full_negative(load=True) +full_positive(motor, load=True) +all_full_positive(load=True) +stop(motor, load=True) +stop_all(load=True) +coast(motor, load=True) +coast_all(load=True) +brake(motor, load=True) +brake_all(load=True) +at_percent(motor, in, load=True) +at_percent(motor, in, in_min, in_max, load=True) +at_percent(motor, in, in_min, in_max, value_min, value_max, load=True) +all_at_percent(in, load=True) +all_at_percent(in, in_min, in_max, load=True) +all_at_percent(in, in_min, in_max, value_min, value_max, load=True) load() ``` ### PIO Limitations -The RP2040 features two PIOs with four state machines each. This places a hard limit on how many ServoClusters can be created. As this class is capable of driving all 30 GPIO pins, the only time this limit will be of concern is when servos with different frequencies are wanted, as all the outputs a ServoCluster controls share the same frequency. Relating this to the hardware PWM, think of it as a single PWM slice with up to 30 sub channels, A, B, C, D etc. +The RP2040 features two PIOs with four state machines each. This places a hard limit on how many MotorClusters can be created. As this class is capable of driving all 30 GPIO pins, the only time this limit will be of concern is when motors with different frequencies are wanted, as all the outputs a MotorCluster controls share the same frequency. Relating this to the hardware PWM, think of it as a single PWM slice with up to 30 sub channels, A, B, C, D etc. -When creating a ServoCluster, in most cases you'll use `0` for PIO and `0` for PIO state-machine. You should change these though if you plan on running multiple clusters, or using a cluster alongside something else that uses PIO, such as our [Plasma library](https://github.com/pimoroni/pimoroni-pico/tree/main/micropython/modules/plasma). +When creating a MotorCluster, in most cases you'll use `0` for PIO and `0` for PIO state-machine. You should change these though if you plan on running multiple clusters, or using a cluster alongside something else that uses PIO, such as our [Plasma library](https://github.com/pimoroni/pimoroni-pico/tree/main/micropython/modules/plasma). -## Calibration +## Configuration After using servos for a while, you may notice that some don't quite go to the positions you would expect. Or perhaps you are giving values to a continuous rotation servo in degrees when it would make more sense to use a speed or rpm. To compensate for these cases, each `Servo` class or servo within a `ServoCluster` has an individual `Calibration` class. This class contains a set of pulse-value pairs that are used by the `.value(value)` functions (and those similar) to convert real-world numbers into pulses each servo understand. diff --git a/micropython/modules/motor/motor.c b/micropython/modules/motor/motor.c index d3ed6fa6..87f40ff7 100644 --- a/micropython/modules/motor/motor.c +++ b/micropython/modules/motor/motor.c @@ -147,28 +147,28 @@ const mp_rom_obj_tuple_t pico_motor_shim_motor2_pins = { {&mp_type_tuple}, 2, { MP_ROM_INT(27), MP_ROM_INT(26), }, }; -const mp_rom_obj_tuple_t motor2040_motor1_pins = { +const mp_rom_obj_tuple_t motor2040_motorA_pins = { {&mp_type_tuple}, 2, { MP_ROM_INT(4), MP_ROM_INT(5) }, }; -const mp_rom_obj_tuple_t motor2040_motor2_pins = { +const mp_rom_obj_tuple_t motor2040_motorB_pins = { {&mp_type_tuple}, 2, { MP_ROM_INT(6), MP_ROM_INT(7) }, }; -const mp_rom_obj_tuple_t motor2040_motor3_pins = { +const mp_rom_obj_tuple_t motor2040_motorC_pins = { {&mp_type_tuple}, 2, { MP_ROM_INT(8), MP_ROM_INT(9) }, }; -const mp_rom_obj_tuple_t motor2040_motor4_pins = { +const mp_rom_obj_tuple_t motor2040_motorD_pins = { {&mp_type_tuple}, 2, { MP_ROM_INT(10), MP_ROM_INT(11) }, }; -const mp_rom_obj_tuple_t motor2040_encoder1_pins = { +const mp_rom_obj_tuple_t motor2040_encoderA_pins = { {&mp_type_tuple}, 2, { MP_ROM_INT(0), MP_ROM_INT(1) }, }; -const mp_rom_obj_tuple_t motor2040_encoder2_pins = { +const mp_rom_obj_tuple_t motor2040_encoderB_pins = { {&mp_type_tuple}, 2, { MP_ROM_INT(2), MP_ROM_INT(3) }, }; -const mp_rom_obj_tuple_t motor2040_encoder3_pins = { +const mp_rom_obj_tuple_t motor2040_encoderC_pins = { {&mp_type_tuple}, 2, { MP_ROM_INT(12), MP_ROM_INT(13) }, }; -const mp_rom_obj_tuple_t motor2040_encoder4_pins = { +const mp_rom_obj_tuple_t motor2040_encoderD_pins = { {&mp_type_tuple}, 2, { MP_ROM_INT(14), MP_ROM_INT(15) }, }; @@ -195,20 +195,20 @@ STATIC const mp_rom_map_elem_t pico_motor_shim_globals_table[] = { STATIC const mp_rom_map_elem_t motor2040_globals_table[] = { { MP_OBJ_NEW_QSTR(MP_QSTR___name__), MP_OBJ_NEW_QSTR(MP_QSTR_motor2040) }, - { MP_ROM_QSTR(MP_QSTR_MOTOR_1), MP_ROM_PTR(&motor2040_motor1_pins) }, - { MP_ROM_QSTR(MP_QSTR_MOTOR_2), MP_ROM_PTR(&motor2040_motor2_pins) }, - { MP_ROM_QSTR(MP_QSTR_MOTOR_3), MP_ROM_PTR(&motor2040_motor3_pins) }, - { MP_ROM_QSTR(MP_QSTR_MOTOR_4), MP_ROM_PTR(&motor2040_motor4_pins) }, + { MP_ROM_QSTR(MP_QSTR_MOTOR_A), MP_ROM_PTR(&motor2040_motorA_pins) }, + { MP_ROM_QSTR(MP_QSTR_MOTOR_B), MP_ROM_PTR(&motor2040_motorB_pins) }, + { MP_ROM_QSTR(MP_QSTR_MOTOR_C), MP_ROM_PTR(&motor2040_motorC_pins) }, + { MP_ROM_QSTR(MP_QSTR_MOTOR_D), MP_ROM_PTR(&motor2040_motorD_pins) }, { MP_ROM_QSTR(MP_QSTR_NUM_MOTORS), MP_ROM_INT(4) }, - { MP_ROM_QSTR(MP_QSTR_ENCODER_1), MP_ROM_PTR(&motor2040_encoder1_pins) }, - { MP_ROM_QSTR(MP_QSTR_ENCODER_2), MP_ROM_PTR(&motor2040_encoder2_pins) }, - { MP_ROM_QSTR(MP_QSTR_ENCODER_3), MP_ROM_PTR(&motor2040_encoder3_pins) }, - { MP_ROM_QSTR(MP_QSTR_ENCODER_4), MP_ROM_PTR(&motor2040_encoder4_pins) }, + { MP_ROM_QSTR(MP_QSTR_ENCODER_A), MP_ROM_PTR(&motor2040_encoderA_pins) }, + { MP_ROM_QSTR(MP_QSTR_ENCODER_B), MP_ROM_PTR(&motor2040_encoderB_pins) }, + { MP_ROM_QSTR(MP_QSTR_ENCODER_C), MP_ROM_PTR(&motor2040_encoderC_pins) }, + { MP_ROM_QSTR(MP_QSTR_ENCODER_D), MP_ROM_PTR(&motor2040_encoderD_pins) }, { MP_ROM_QSTR(MP_QSTR_NUM_ENCODERS), MP_ROM_INT(4) }, { MP_ROM_QSTR(MP_QSTR_TX_TRIG), MP_ROM_INT(16) }, { MP_ROM_QSTR(MP_QSTR_RX_ECHO), MP_ROM_INT(17) }, { MP_ROM_QSTR(MP_QSTR_LED_DATA), MP_ROM_INT(18) }, - { MP_ROM_QSTR(MP_QSTR_NUM_LEDS), MP_ROM_INT(6) }, + { MP_ROM_QSTR(MP_QSTR_NUM_LEDS), MP_ROM_INT(1) }, { MP_ROM_QSTR(MP_QSTR_INT), MP_ROM_INT(19) }, { MP_ROM_QSTR(MP_QSTR_SDA), MP_ROM_INT(20) }, { MP_ROM_QSTR(MP_QSTR_SCL), MP_ROM_INT(21) }, @@ -220,10 +220,10 @@ STATIC const mp_rom_map_elem_t motor2040_globals_table[] = { { MP_ROM_QSTR(MP_QSTR_ADC1), MP_ROM_INT(27) }, { MP_ROM_QSTR(MP_QSTR_ADC2), MP_ROM_INT(28) }, { MP_ROM_QSTR(MP_QSTR_SHARED_ADC), MP_ROM_INT(29) }, - { MP_ROM_QSTR(MP_QSTR_CURRENT_SENSE_1_ADDR), MP_ROM_INT(0b000) }, - { MP_ROM_QSTR(MP_QSTR_CURRENT_SENSE_2_ADDR), MP_ROM_INT(0b001) }, - { MP_ROM_QSTR(MP_QSTR_CURRENT_SENSE_3_ADDR), MP_ROM_INT(0b010) }, - { MP_ROM_QSTR(MP_QSTR_CURRENT_SENSE_4_ADDR), MP_ROM_INT(0b011) }, + { MP_ROM_QSTR(MP_QSTR_CURRENT_SENSE_A_ADDR), MP_ROM_INT(0b000) }, + { MP_ROM_QSTR(MP_QSTR_CURRENT_SENSE_B_ADDR), MP_ROM_INT(0b001) }, + { MP_ROM_QSTR(MP_QSTR_CURRENT_SENSE_C_ADDR), MP_ROM_INT(0b010) }, + { MP_ROM_QSTR(MP_QSTR_CURRENT_SENSE_D_ADDR), MP_ROM_INT(0b011) }, { MP_ROM_QSTR(MP_QSTR_VOLTAGE_SENSE_ADDR), MP_ROM_INT(0b100) }, { MP_ROM_QSTR(MP_QSTR_FAULT_SENSE_ADDR), MP_ROM_INT(0b101) }, { MP_ROM_QSTR(MP_QSTR_SENSOR_1_ADDR), MP_ROM_INT(0b110) }, diff --git a/micropython/modules/motor/motor.cpp b/micropython/modules/motor/motor.cpp index 507b99ed..27f3f16e 100644 --- a/micropython/modules/motor/motor.cpp +++ b/micropython/modules/motor/motor.cpp @@ -480,10 +480,10 @@ extern mp_obj_t Motor_deadzone(size_t n_args, const mp_obj_t *pos_args, mp_map_t return mp_obj_new_float(self->motor->deadzone()); } else { - enum { ARG_self, ARG_deadzone_percent }; + enum { ARG_self, ARG_deadzone }; static const mp_arg_t allowed_args[] = { { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, - { MP_QSTR_deadzone_percent, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_deadzone, MP_ARG_REQUIRED | MP_ARG_OBJ }, }; // Parse args. @@ -492,7 +492,7 @@ extern mp_obj_t Motor_deadzone(size_t n_args, const mp_obj_t *pos_args, mp_map_t _Motor_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Motor_obj_t); - float deadzone = mp_obj_get_float(args[ARG_deadzone_percent].u_obj); + float deadzone = mp_obj_get_float(args[ARG_deadzone].u_obj); if(deadzone < 0.0f || deadzone > 1.0f) { mp_raise_ValueError("deadzone out of range. Expected 0.0 to 1.0"); } @@ -517,10 +517,10 @@ extern mp_obj_t Motor_decay_mode(size_t n_args, const mp_obj_t *pos_args, mp_map return mp_obj_new_int(self->motor->decay_mode()); } else { - enum { ARG_self, ARG_decay_mode }; + enum { ARG_self, ARG_mode }; static const mp_arg_t allowed_args[] = { { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, - { MP_QSTR_decay_mode, MP_ARG_REQUIRED | MP_ARG_INT }, + { MP_QSTR_mode, MP_ARG_REQUIRED | MP_ARG_INT }, }; // Parse args. @@ -529,7 +529,7 @@ extern mp_obj_t Motor_decay_mode(size_t n_args, const mp_obj_t *pos_args, mp_map _Motor_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Motor_obj_t); - int mode = args[ARG_decay_mode].u_int; + int mode = args[ARG_mode].u_int; if(mode < 0 || mode > 1) { mp_raise_ValueError("mode out of range. Expected FAST_DECAY (0) or SLOW_DECAY (1)"); } From 55ee058d3ee2c33034a456094d047067b8136866 Mon Sep 17 00:00:00 2001 From: ZodiusInfuser Date: Wed, 10 Feb 2021 16:25:25 +0000 Subject: [PATCH 11/60] Created a quadrature encoder reader using the Pico's PIO --- drivers/CMakeLists.txt | 1 + drivers/encoder-pio/CMakeLists.txt | 13 + drivers/encoder-pio/capture.cpp | 70 ++++++ drivers/encoder-pio/capture.hpp | 44 ++++ drivers/encoder-pio/encoder-pio.cmake | 12 + drivers/encoder-pio/encoder.cpp | 333 ++++++++++++++++++++++++++ drivers/encoder-pio/encoder.hpp | 126 ++++++++++ drivers/encoder-pio/encoder.pio | 119 +++++++++ 8 files changed, 718 insertions(+) create mode 100644 drivers/encoder-pio/CMakeLists.txt create mode 100644 drivers/encoder-pio/capture.cpp create mode 100644 drivers/encoder-pio/capture.hpp create mode 100644 drivers/encoder-pio/encoder-pio.cmake create mode 100644 drivers/encoder-pio/encoder.cpp create mode 100644 drivers/encoder-pio/encoder.hpp create mode 100644 drivers/encoder-pio/encoder.pio diff --git a/drivers/CMakeLists.txt b/drivers/CMakeLists.txt index 32c6169c..52d60ebe 100644 --- a/drivers/CMakeLists.txt +++ b/drivers/CMakeLists.txt @@ -29,3 +29,4 @@ add_subdirectory(hub75) add_subdirectory(uc8151) add_subdirectory(pwm) add_subdirectory(servo) +add_subdirectory(encoder-pio) diff --git a/drivers/encoder-pio/CMakeLists.txt b/drivers/encoder-pio/CMakeLists.txt new file mode 100644 index 00000000..3acea382 --- /dev/null +++ b/drivers/encoder-pio/CMakeLists.txt @@ -0,0 +1,13 @@ +add_library(encoder-pio INTERFACE) + +target_sources(encoder-pio INTERFACE + ${CMAKE_CURRENT_LIST_DIR}/encoder.cpp + ${CMAKE_CURRENT_LIST_DIR}/capture.cpp +) + +pico_generate_pio_header(encoder-pio ${CMAKE_CURRENT_LIST_DIR}/encoder.pio) + +target_include_directories(encoder-pio INTERFACE ${CMAKE_CURRENT_LIST_DIR}) + +# Pull in pico libraries that we need +target_link_libraries(encoder-pio INTERFACE pico_stdlib hardware_pio) \ No newline at end of file diff --git a/drivers/encoder-pio/capture.cpp b/drivers/encoder-pio/capture.cpp new file mode 100644 index 00000000..e0208f08 --- /dev/null +++ b/drivers/encoder-pio/capture.cpp @@ -0,0 +1,70 @@ +#include +#include +#include "capture.hpp" + +namespace pimoroni { + + //////////////////////////////////////////////////////////////////////////////////////////////////// + // CONSTRUCTORS + //////////////////////////////////////////////////////////////////////////////////////////////////// + Capture::Capture(int32_t captured_count, int32_t count_change, float average_frequency, float counts_per_revolution) : + captured_count(captured_count), count_change(count_change), average_frequency(average_frequency), + counts_per_revolution(std::max(counts_per_revolution, FLT_MIN)) { //Clamp counts_per_rev to avoid potential NaN + } + + + + //////////////////////////////////////////////////////////////////////////////////////////////////// + // METHODS + //////////////////////////////////////////////////////////////////////////////////////////////////// + int32_t Capture::get_count() const { + return captured_count; + } + + //////////////////////////////////////////////////////////////////////////////////////////////////// + float Capture::get_revolutions() const { + return (float)get_count() / counts_per_revolution; + } + + //////////////////////////////////////////////////////////////////////////////////////////////////// + float Capture::get_angle_degrees() const { + return get_revolutions() * 360.0f; + } + + //////////////////////////////////////////////////////////////////////////////////////////////////// + float Capture::get_angle_radians() const { + return get_revolutions() * M_TWOPI; + } + + //////////////////////////////////////////////////////////////////////////////////////////////////// + int32_t Capture::get_count_change() const { + return count_change; + } + + //////////////////////////////////////////////////////////////////////////////////////////////////// + float Capture::get_frequency() const { + return average_frequency; + } + + //////////////////////////////////////////////////////////////////////////////////////////////////// + float Capture::get_revolutions_per_second() const { + return get_frequency() / counts_per_revolution; + } + + //////////////////////////////////////////////////////////////////////////////////////////////////// + float Capture::get_revolutions_per_minute() const { + return get_revolutions_per_second() * 60.0f; + } + + //////////////////////////////////////////////////////////////////////////////////////////////////// + float Capture::get_degrees_per_second() const { + return get_revolutions_per_second() * 360.0f; + } + + //////////////////////////////////////////////////////////////////////////////////////////////////// + float Capture::get_radians_per_second() const { + return get_revolutions_per_second() * M_TWOPI; + } + //////////////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////////////// +} \ No newline at end of file diff --git a/drivers/encoder-pio/capture.hpp b/drivers/encoder-pio/capture.hpp new file mode 100644 index 00000000..2aa6edaf --- /dev/null +++ b/drivers/encoder-pio/capture.hpp @@ -0,0 +1,44 @@ +#pragma once + +#include "pico/stdlib.h" + +namespace pimoroni { + + class Capture { + //-------------------------------------------------- + // Variables + //-------------------------------------------------- + private: + const int32_t captured_count = 0; + const int32_t count_change = 0; + const float average_frequency = 0.0f; + const float counts_per_revolution = 1; + + + //-------------------------------------------------- + // Constructors + //-------------------------------------------------- + public: + Capture() {} + Capture(int32_t captured_count, int32_t count_change, float average_frequency, float counts_per_revolution); + + + //-------------------------------------------------- + // Methods + //-------------------------------------------------- + public: + int32_t get_count() const; + float get_revolutions() const; + float get_angle_degrees() const; + float get_angle_radians() const; + + int32_t get_count_change() const; + + float get_frequency() const; + float get_revolutions_per_second() const; + float get_revolutions_per_minute() const; + float get_degrees_per_second() const; + float get_radians_per_second() const; + }; + +} \ No newline at end of file diff --git a/drivers/encoder-pio/encoder-pio.cmake b/drivers/encoder-pio/encoder-pio.cmake new file mode 100644 index 00000000..37b3959a --- /dev/null +++ b/drivers/encoder-pio/encoder-pio.cmake @@ -0,0 +1,12 @@ +add_library(encoder-pio INTERFACE) + +target_sources(encoder-pio INTERFACE + ${CMAKE_CURRENT_LIST_DIR}/msa301.cpp +) + +pico_generate_pio_header(encoder-pio ${CMAKE_CURRENT_LIST_DIR}/encoder.pio) + +target_include_directories(encoder-pio INTERFACE ${CMAKE_CURRENT_LIST_DIR}) + +# Pull in pico libraries that we need +target_link_libraries(encoder-pio INTERFACE pico_stdlib hardware_i2c) \ No newline at end of file diff --git a/drivers/encoder-pio/encoder.cpp b/drivers/encoder-pio/encoder.cpp new file mode 100644 index 00000000..730b74df --- /dev/null +++ b/drivers/encoder-pio/encoder.cpp @@ -0,0 +1,333 @@ +#include +#include +#include "hardware/irq.h" +#include "encoder.hpp" +#include "encoder.pio.h" + +#define LAST_STATE(state) ((state) & 0b0011) +#define CURR_STATE(state) (((state) & 0b1100) >> 2) + +namespace pimoroni { + + //////////////////////////////////////////////////////////////////////////////////////////////////// + // STATICS + //////////////////////////////////////////////////////////////////////////////////////////////////// + Encoder* Encoder::pio_encoders[][NUM_PIO_STATE_MACHINES] = { { nullptr, nullptr, nullptr, nullptr }, { nullptr, nullptr, nullptr, nullptr } }; + uint8_t Encoder::pio_claimed_sms[] = { 0x0, 0x0 }; + + //////////////////////////////////////////////////////////////////////////////////////////////////// + void Encoder::pio0_interrupt_callback() { + //Go through each of encoders on this PIO to see which triggered this interrupt + for(uint8_t sm = 0; sm < NUM_PIO_STATE_MACHINES; sm++) { + if(pio_encoders[0][sm] != nullptr) { + pio_encoders[0][sm]->check_for_transition(); + } + } + } + + //////////////////////////////////////////////////////////////////////////////////////////////////// + void Encoder::pio1_interrupt_callback() { + //Go through each of encoders on this PIO to see which triggered this interrupt + for(uint8_t sm = 0; sm < NUM_PIO_STATE_MACHINES; sm++) { + if(pio_encoders[1][sm] != nullptr) { + pio_encoders[1][sm]->check_for_transition(); + } + } + } + + + + //////////////////////////////////////////////////////////////////////////////////////////////////// + // CONSTRUCTORS / DESTRUCTOR + //////////////////////////////////////////////////////////////////////////////////////////////////// + Encoder::Encoder(PIO pio, uint8_t pinA, uint8_t pinB, uint8_t pinC, + float counts_per_revolution, bool count_microsteps, + uint16_t freq_divider) : + enc_pio(pio), pinA(pinA), pinB(pinB), pinC(pinC), + counts_per_revolution(counts_per_revolution), count_microsteps(count_microsteps), + freq_divider(freq_divider), clocks_per_time((float)(clock_get_hz(clk_sys) / (ENC_LOOP_CYCLES * freq_divider))) { + } + + //////////////////////////////////////////////////////////////////////////////////////////////////// + Encoder::~Encoder() { + //Clean up our use of the SM associated with this encoder + encoder_program_release(enc_pio, enc_sm); + uint index = pio_get_index(enc_pio); + pio_encoders[index][enc_sm] = nullptr; + pio_claimed_sms[index] &= ~(1u << enc_sm); + + //If there are no more SMs using the encoder program, then we can remove it from the PIO + if(pio_claimed_sms[index] == 0) { + pio_remove_program(enc_pio, &encoder_program, enc_offset); + } + } + + + + //////////////////////////////////////////////////////////////////////////////////////////////////// + // METHODS + //////////////////////////////////////////////////////////////////////////////////////////////////// + bool Encoder::init() { + bool initialised = false; + + //Are the pins we want to use actually valid? + if((pinA < NUM_BANK0_GPIOS) && (pinB < NUM_BANK0_GPIOS)) { + + //If a Pin C was defined, and valid, set it as a GND to pull the other two pins down + if((pinC != PIN_UNUSED) && (pinC < NUM_BANK0_GPIOS)) { + gpio_init(pinC); + gpio_set_dir(pinC, GPIO_OUT); + gpio_put(pinC, false); + } + + enc_sm = pio_claim_unused_sm(enc_pio, true); + uint pio_idx = pio_get_index(enc_pio); + + //Is this the first time using an encoder on this PIO? + if(pio_claimed_sms[pio_idx] == 0) { + //Add the program to the PIO memory and enable the appropriate interrupt + enc_offset = pio_add_program(enc_pio, &encoder_program); + encoder_program_init(enc_pio, enc_sm, enc_offset, pinA, pinB, freq_divider); + hw_set_bits(&enc_pio->inte0, PIO_IRQ0_INTE_SM0_RXNEMPTY_BITS << enc_sm); + if(pio_idx == 0) { + irq_set_exclusive_handler(PIO0_IRQ_0, pio0_interrupt_callback); + irq_set_enabled(PIO0_IRQ_0, true); + } + else { + irq_set_exclusive_handler(PIO1_IRQ_0, pio1_interrupt_callback); + irq_set_enabled(PIO1_IRQ_0, true); + } + } + + //Keep a record of this encoder for the interrupt callback + pio_encoders[pio_idx][enc_sm] = this; + pio_claimed_sms[pio_idx] |= 1u << enc_sm; + + //Read the current state of the encoder pins and start the PIO program on the SM + stateA = gpio_get(pinA); + stateB = gpio_get(pinB); + encoder_program_start(enc_pio, enc_sm, stateA, stateB); + + initialised = true; + } + return initialised; + } + + //////////////////////////////////////////////////////////////////////////////////////////////////// + bool Encoder::get_state_a() const { + return stateA; + } + + //////////////////////////////////////////////////////////////////////////////////////////////////// + bool Encoder::get_state_b() const { + return stateB; + } + + //////////////////////////////////////////////////////////////////////////////////////////////////// + int32_t Encoder::get_count() const { + return count - count_offset; + } + + //////////////////////////////////////////////////////////////////////////////////////////////////// + float Encoder::get_revolutions() const { + return (float)get_count() / counts_per_revolution; + } + + //////////////////////////////////////////////////////////////////////////////////////////////////// + float Encoder::get_angle_degrees() const { + return get_revolutions() * 360.0f; + } + + //////////////////////////////////////////////////////////////////////////////////////////////////// + float Encoder::get_angle_radians() const { + return get_revolutions() * M_TWOPI; + } + + //////////////////////////////////////////////////////////////////////////////////////////////////// + float Encoder::get_frequency() const { + return clocks_per_time / (float)time_since; + } + + //////////////////////////////////////////////////////////////////////////////////////////////////// + float Encoder::get_revolutions_per_second() const { + return get_frequency() / counts_per_revolution; + } + + //////////////////////////////////////////////////////////////////////////////////////////////////// + float Encoder::get_revolutions_per_minute() const { + return get_revolutions_per_second() * 60.0f; + } + + //////////////////////////////////////////////////////////////////////////////////////////////////// + float Encoder::get_degrees_per_second() const { + return get_revolutions_per_second() * 360.0f; + } + + //////////////////////////////////////////////////////////////////////////////////////////////////// + float Encoder::get_radians_per_second() const { + return get_revolutions_per_second() * M_TWOPI; + } + + //////////////////////////////////////////////////////////////////////////////////////////////////// + void Encoder::zero_count() { + count_offset = count; + } + + //////////////////////////////////////////////////////////////////////////////////////////////////// + Capture Encoder::perform_capture() { + //Capture the current values + int32_t captured_count = count; + int32_t captured_cumulative_time = cumulative_time; + cumulative_time = 0; + + //Determine the change in counts since the last capture was performed + int32_t count_change = captured_count - last_captured_count; + last_captured_count = captured_count; + + //Calculate the average frequency of state transitions + float average_frequency = 0.0f; + if(count_change != 0 && captured_cumulative_time != INT_MAX) { + average_frequency = (clocks_per_time * (float)count_change) / (float)captured_cumulative_time; + } + + return Capture(captured_count, count_change, average_frequency, counts_per_revolution); + } + + //////////////////////////////////////////////////////////////////////////////////////////////////// + void Encoder::microstep_up(int32_t time) { + count++; + time_since = time; + microstep_time = 0; + + if(time + cumulative_time < time) //Check to avoid integer overflow + cumulative_time = INT_MAX; + else + cumulative_time += time; + } + + //////////////////////////////////////////////////////////////////////////////////////////////////// + void Encoder::microstep_down(int32_t time) { + count--; + time_since = 0 - time; + microstep_time = 0; + + if(time + cumulative_time < time) //Check to avoid integer overflow + cumulative_time = INT_MAX; + else + cumulative_time += time; + } + + //////////////////////////////////////////////////////////////////////////////////////////////////// + void Encoder::check_for_transition() { + while(enc_pio->ints0 & (PIO_IRQ0_INTS_SM0_RXNEMPTY_BITS << enc_sm)) { + uint32_t received = pio_sm_get(enc_pio, enc_sm); + + // Extract the current and last encoder states from the received value + stateA = (bool)(received & STATE_A_MASK); + stateB = (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 transition is cared about, causing an inaccurate time value + // To address this we accumulate the times received and zero it when a transition 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; + } + + // Determine what transition occurred + switch(LAST_STATE(states)) { + //-------------------------------------------------- + case MICROSTEP_0: + switch(CURR_STATE(states)) { + // A ____|‾‾‾‾ + // B _________ + case MICROSTEP_1: + if(count_microsteps) + microstep_up(time_received); + break; + + // A _________ + // B ____|‾‾‾‾ + case MICROSTEP_3: + if(count_microsteps) + microstep_down(time_received); + break; + } + break; + + //-------------------------------------------------- + case MICROSTEP_1: + switch(CURR_STATE(states)) { + // A ‾‾‾‾‾‾‾‾‾ + // B ____|‾‾‾‾ + case MICROSTEP_2: + if(count_microsteps || last_travel_dir == CLOCKWISE) + microstep_up(time_received); + + last_travel_dir = NO_DIR; //Finished turning clockwise + break; + + // A ‾‾‾‾|____ + // B _________ + case MICROSTEP_0: + if(count_microsteps) + microstep_down(time_received); + break; + } + break; + + //-------------------------------------------------- + case MICROSTEP_2: + switch(CURR_STATE(states)) { + // A ‾‾‾‾|____ + // B ‾‾‾‾‾‾‾‾‾ + case MICROSTEP_3: + if(count_microsteps) + microstep_up(time_received); + + last_travel_dir = CLOCKWISE; //Started turning clockwise + break; + + // A ‾‾‾‾‾‾‾‾‾ + // B ‾‾‾‾|____ + case MICROSTEP_1: + if(count_microsteps) + microstep_down(time_received); + + last_travel_dir = COUNTERCLOCK; //Started turning counter-clockwise + break; + } + break; + + //-------------------------------------------------- + case MICROSTEP_3: + switch(CURR_STATE(states)) { + // A _________ + // B ‾‾‾‾|____ + case MICROSTEP_0: + if(count_microsteps) + microstep_up(time_received); + break; + + // A ____|‾‾‾‾ + // B ‾‾‾‾‾‾‾‾‾ + case MICROSTEP_2: + if(count_microsteps || last_travel_dir == COUNTERCLOCK) + microstep_down(time_received); + + last_travel_dir = NO_DIR; //Finished turning counter-clockwise + break; + } + break; + } + } + } + //////////////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////////////// +} \ No newline at end of file diff --git a/drivers/encoder-pio/encoder.hpp b/drivers/encoder-pio/encoder.hpp new file mode 100644 index 00000000..a3f27df5 --- /dev/null +++ b/drivers/encoder-pio/encoder.hpp @@ -0,0 +1,126 @@ +#pragma once + +#include "hardware/pio.h" +#include "capture.hpp" + +namespace pimoroni { + + class Encoder { + //-------------------------------------------------- + // Constants + //-------------------------------------------------- + public: + static constexpr float DEFAULT_COUNTS_PER_REV = 24; + static const uint16_t DEFAULT_COUNT_MICROSTEPS = false; + static const uint16_t DEFAULT_FREQ_DIVIDER = 1; + static const uint8_t PIN_UNUSED = UINT8_MAX; + + 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; + + static const uint8_t MICROSTEP_0 = 0b00; + static const uint8_t MICROSTEP_1 = 0b10; + static const uint8_t MICROSTEP_2 = 0b11; + static const uint8_t MICROSTEP_3 = 0b01; + + + //-------------------------------------------------- + // Enums + //-------------------------------------------------- + private: + enum Direction { + NO_DIR = 0, + CLOCKWISE = 1, + COUNTERCLOCK = -1, + }; + + + //-------------------------------------------------- + // Variables + //-------------------------------------------------- + private: + const PIO enc_pio = pio0; + const uint8_t pinA = PIN_UNUSED; + const uint8_t pinB = PIN_UNUSED; + const uint8_t pinC = PIN_UNUSED; + + const float counts_per_revolution = DEFAULT_COUNTS_PER_REV; + const bool count_microsteps = DEFAULT_COUNT_MICROSTEPS; + const uint16_t freq_divider = DEFAULT_FREQ_DIVIDER; + const float clocks_per_time = 0; + + //-------------------------------------------------- + + uint enc_sm = 0; + uint enc_offset = 0; + + volatile bool stateA = false; + volatile bool stateB = false; + volatile int32_t count = 0; + volatile int32_t time_since = 0; + volatile Direction last_travel_dir = NO_DIR; + volatile int32_t microstep_time = 0; + volatile int32_t cumulative_time = 0; + + int32_t count_offset = 0; + int32_t last_captured_count = 0; + + + //-------------------------------------------------- + // Statics + //-------------------------------------------------- + public: + static Encoder* pio_encoders[NUM_PIOS][NUM_PIO_STATE_MACHINES]; + static uint8_t pio_claimed_sms[NUM_PIOS]; + static void pio0_interrupt_callback(); + static void pio1_interrupt_callback(); + + + //-------------------------------------------------- + // Constructors/Destructor + //-------------------------------------------------- + public: + Encoder() {} + Encoder(PIO pio, uint8_t pinA, uint8_t pinB, uint8_t pinC = PIN_UNUSED, + float counts_per_revolution = DEFAULT_COUNTS_PER_REV, bool count_microsteps = DEFAULT_COUNT_MICROSTEPS, + uint16_t freq_divider = DEFAULT_FREQ_DIVIDER); + ~Encoder(); + + + //-------------------------------------------------- + // Methods + //-------------------------------------------------- + public: + bool init(); + + bool get_state_a() const; + bool get_state_b() const; + int32_t get_count() const; + float get_revolutions() const; + float get_angle_degrees() const; + float get_angle_radians() const; + + float get_frequency() const; + float get_revolutions_per_second() const; + float get_revolutions_per_minute() const; + float get_degrees_per_second() const; + float get_radians_per_second() const; + + void zero_count(); + Capture perform_capture(); + + private: + void microstep_up(int32_t time_since); + void microstep_down(int32_t time_since); + void check_for_transition(); + }; + +} \ No newline at end of file diff --git a/drivers/encoder-pio/encoder.pio b/drivers/encoder-pio/encoder.pio new file mode 100644 index 00000000..26138986 --- /dev/null +++ b/drivers/encoder-pio/encoder.pio @@ -0,0 +1,119 @@ +; -------------------------------------------------- +; 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 encA_was_high + mov isr, null + jmp read_encB +encA_was_high: + set y, 1 + mov isr, y +read_encB: + 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 { +#include "hardware/clocks.h" + +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; + + +static inline void encoder_program_init(PIO pio, uint sm, uint offset, uint pinA, uint pinB, uint16_t divider) { + pio_sm_config c = encoder_program_get_default_config(offset); + + sm_config_set_jmp_pin(&c, pinA); + sm_config_set_in_pins(&c, pinB); + sm_config_set_in_shift(&c, false, false, 1); + sm_config_set_fifo_join(&c, PIO_FIFO_JOIN_RX); + pio_gpio_init(pio, pinA); + pio_gpio_init(pio, pinB); + gpio_pull_up(pinA); + gpio_pull_up(pinB); + pio_sm_set_consecutive_pindirs(pio, sm, pinA, 1, 0); + pio_sm_set_consecutive_pindirs(pio, sm, pinB, 1, 0); + sm_config_set_clkdiv_int_frac(&c, divider, 0); + pio_sm_init(pio, sm, offset, &c); +} + +static inline void encoder_program_start(PIO pio, uint sm, bool stateA, bool stateB) { + pio_sm_exec(pio, sm, pio_encode_set(pio_x, (uint)stateA << 1 | (uint)stateB)); + pio_sm_set_enabled(pio, sm, true); +} + +static inline void encoder_program_release(PIO pio, uint sm) { + pio_sm_set_enabled(pio, sm, false); + pio_sm_unclaim(pio, sm); +} +%} \ No newline at end of file From 8b4badb4b94e54191f2a1773aa4a62becdcb102c Mon Sep 17 00:00:00 2001 From: ZodiusInfuser Date: Wed, 10 Feb 2021 16:26:18 +0000 Subject: [PATCH 12/60] Added an example for using an encoder with the PicoExplorer --- examples/CMakeLists.txt | 1 + examples/pico_explorer_encoder/CMakeLists.txt | 12 + examples/pico_explorer_encoder/demo.cpp | 349 ++++++++++++++++++ .../pico_explorer_encoder/quadrature_out.pio | 44 +++ 4 files changed, 406 insertions(+) create mode 100644 examples/pico_explorer_encoder/CMakeLists.txt create mode 100644 examples/pico_explorer_encoder/demo.cpp create mode 100644 examples/pico_explorer_encoder/quadrature_out.pio diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index e3293cb9..60a24815 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -30,6 +30,7 @@ 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_rgb_keypad) add_subdirectory(pico_rtc_display) add_subdirectory(pico_tof_display) diff --git a/examples/pico_explorer_encoder/CMakeLists.txt b/examples/pico_explorer_encoder/CMakeLists.txt new file mode 100644 index 00000000..1d598480 --- /dev/null +++ b/examples/pico_explorer_encoder/CMakeLists.txt @@ -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-pio) + +# create map/bin/hex file etc. +pico_add_extra_outputs(explorerencoder) \ No newline at end of file diff --git a/examples/pico_explorer_encoder/demo.cpp b/examples/pico_explorer_encoder/demo.cpp new file mode 100644 index 00000000..6eb80683 --- /dev/null +++ b/examples/pico_explorer_encoder/demo.cpp @@ -0,0 +1,349 @@ +#include +#include +#include +#include +#include +#include +#include "pico_explorer.hpp" +#include "pico/stdlib.h" +#include "encoder.hpp" +#include "quadrature_out.pio.h" + +using namespace pimoroni; + + +//-------------------------------------------------- +// Constants +//-------------------------------------------------- +static const uint8_t ENCODER_PIN_A = 1; +static const uint8_t ENCODER_PIN_B = 0; +static const uint8_t ENCODER_PIN_C = Encoder::PIN_UNUSED; +static const uint8_t ENCODER_SWITCH_PIN = 4; + +static constexpr float COUNTS_PER_REVOLUTION = 24; //24 is for rotary encoders. For motor magnetic encoders uses + //12 times the gear ratio (e.g. 12 * 20 with a 20:1 ratio motor +static const bool COUNT_MICROSTEPS = false; //Set to true for motor magnetic encoders + +static const uint16_t FREQ_DIVIDER = 1; //Increase this to deal with switch bounce. 250 Gives a 1ms debounce + +static const int32_t TIME_BETWEEN_SAMPLES_US = 100; //Time between each sample, in microseconds +static const int32_t WINDOW_DURATION_US = 1000000; //The full time window that will be stored + +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 + +static const bool QUADRATURE_OUT_ENABLED = true; +static constexpr float QUADRATURE_OUT_FREQ = 800; //The frequency the quadrature output will run at (note that counting microsteps will show 4x this value) +static const float QUADRATURE_OUT_1ST_PIN = 6; //Which first pin to output the quadrature signal to (e.g. pins 6 and 7) + +static const uint64_t MAIN_LOOP_TIME_US = 50000; //How long there should be in microseconds between each screen refresh +static const uint16_t EDGE_ALIGN_ABOVE_ZOOM = 4; //The zoom level beyond which edge alignment will be enabled to ma + + + +//-------------------------------------------------- +// Enums +//-------------------------------------------------- +enum DrawState { + DRAW_LOW = 0, + DRAW_HIGH, + DRAW_TRANSITION, +}; + + + +//-------------------------------------------------- +// Variables +//-------------------------------------------------- + +uint16_t buffer[PicoExplorer::WIDTH * PicoExplorer::HEIGHT]; +PicoExplorer pico_explorer(buffer); + +Encoder encoder(pio0, ENCODER_PIN_A, ENCODER_PIN_B, ENCODER_PIN_C, COUNTS_PER_REVOLUTION, COUNT_MICROSTEPS, FREQ_DIVIDER); + +volatile bool encA_readings[READINGS_SIZE]; +volatile bool encB_readings[READINGS_SIZE]; +volatile bool encA_scratch[SCRATCH_SIZE]; +volatile bool encB_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 readingPos, bool edge_align) +{ + uint32_t reading_window = READINGS_SIZE / current_zoom_level; + uint32_t start_index_no_modulus = (readingPos + (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 - readingPos); + + //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) { + if(drawing_to_screen && next_scratch_index < SCRATCH_SIZE) { + encA_scratch[next_scratch_index] = encoder.get_state_a(); + encB_scratch[next_scratch_index] = encoder.get_state_b(); + next_scratch_index++; + } + else { + encA_readings[next_reading_index] = encoder.get_state_a(); + encB_readings[next_reading_index] = encoder.get_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 != Encoder::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(); + + encoder.init(); + + bool encA = encoder.get_state_a(); + bool encB = encoder.get_state_b(); + for(uint i = 0; i < READINGS_SIZE; i++) { + encA_readings[i] = encA; + encB_readings[i] = encB; + } + + 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); + } + + pico_explorer.set_backlight(255); +} + + + +//////////////////////////////////////////////////////////////////////////////////////////////////// +// 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 aPressedLatch = false; + bool xPressedLatch = 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 != Encoder::PIN_UNUSED && gpio_get(ENCODER_SWITCH_PIN)) { + encoder.zero_count(); + } + + //Take a capture, or snapshot of the current encoder state + Capture capture = encoder.perform_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(!aPressedLatch) { + aPressedLatch = true; + current_zoom_level = std::max(current_zoom_level / 2, 1); + } + } + else { + aPressedLatch = false; + } + + //If X has been pressed, zoom the view in to the max of x512 + if(pico_explorer.is_pressed(PicoExplorer::X)) { + if(!xPressedLatch) { + xPressedLatch = true; + current_zoom_level = std::min(current_zoom_level * 2, 512); + } + } + else { + xPressedLatch = 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 localPos = next_reading_index; + uint32_t alignment_offset = draw_plot(Point(0, 10), Point(PicoExplorer::WIDTH, 10 + 50), encA_readings, localPos, current_zoom_level > EDGE_ALIGN_ABOVE_ZOOM); + + pico_explorer.set_pen(0, 255, 255); + draw_plot(Point(0, 80), Point(PicoExplorer::WIDTH, 80 + 50), encB_readings, (localPos + (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++) { + encA_readings[next_reading_index] = encA_scratch[i]; + encB_readings[next_reading_index] = encB_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.get_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.get_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.get_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 + } + } +} \ No newline at end of file diff --git a/examples/pico_explorer_encoder/quadrature_out.pio b/examples/pico_explorer_encoder/quadrature_out.pio new file mode 100644 index 00000000..d43db647 --- /dev/null +++ b/examples/pico_explorer_encoder/quadrature_out.pio @@ -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); +} +%} \ No newline at end of file From c113f30da6791ce1e8f9b2ebe91d39ac3fe995c2 Mon Sep 17 00:00:00 2001 From: ZodiusInfuser Date: Wed, 10 Feb 2021 16:26:49 +0000 Subject: [PATCH 13/60] Changed the motor implementation on PicoExplorer to used breaking mode --- libraries/pico_explorer/pico_explorer.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/libraries/pico_explorer/pico_explorer.cpp b/libraries/pico_explorer/pico_explorer.cpp index f6fa5a5e..11daa15f 100644 --- a/libraries/pico_explorer/pico_explorer.cpp +++ b/libraries/pico_explorer/pico_explorer.cpp @@ -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 { } } -} \ No newline at end of file +} From 33c04535151352ab466a124ab40f7c193eca1619 Mon Sep 17 00:00:00 2001 From: ZodiusInfuser Date: Wed, 10 Feb 2021 16:31:51 +0000 Subject: [PATCH 14/60] Removed some unnecessary includes from the encoder example --- examples/pico_explorer_encoder/demo.cpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/examples/pico_explorer_encoder/demo.cpp b/examples/pico_explorer_encoder/demo.cpp index 6eb80683..1f434c94 100644 --- a/examples/pico_explorer_encoder/demo.cpp +++ b/examples/pico_explorer_encoder/demo.cpp @@ -1,9 +1,5 @@ -#include -#include #include #include -#include -#include #include "pico_explorer.hpp" #include "pico/stdlib.h" #include "encoder.hpp" From a5572e5e44cb4999a9b7bd0ade8c1b63f0f53d12 Mon Sep 17 00:00:00 2001 From: ZodiusInfuser Date: Wed, 13 Apr 2022 11:29:40 +0100 Subject: [PATCH 15/60] Fixes post rebase --- drivers/encoder-pio/CMakeLists.txt | 14 +- drivers/encoder-pio/capture.hpp | 4 +- drivers/encoder-pio/encoder-pio.cmake | 17 +- drivers/encoder-pio/encoder.cpp | 54 +-- drivers/encoder-pio/encoder.hpp | 6 +- examples/pico_explorer_encoder/demo.cpp | 468 ++++++++++++------------ 6 files changed, 277 insertions(+), 286 deletions(-) diff --git a/drivers/encoder-pio/CMakeLists.txt b/drivers/encoder-pio/CMakeLists.txt index 3acea382..9dce7452 100644 --- a/drivers/encoder-pio/CMakeLists.txt +++ b/drivers/encoder-pio/CMakeLists.txt @@ -1,13 +1 @@ -add_library(encoder-pio INTERFACE) - -target_sources(encoder-pio INTERFACE - ${CMAKE_CURRENT_LIST_DIR}/encoder.cpp - ${CMAKE_CURRENT_LIST_DIR}/capture.cpp -) - -pico_generate_pio_header(encoder-pio ${CMAKE_CURRENT_LIST_DIR}/encoder.pio) - -target_include_directories(encoder-pio INTERFACE ${CMAKE_CURRENT_LIST_DIR}) - -# Pull in pico libraries that we need -target_link_libraries(encoder-pio INTERFACE pico_stdlib hardware_pio) \ No newline at end of file +include(encoder-pio.cmake) \ No newline at end of file diff --git a/drivers/encoder-pio/capture.hpp b/drivers/encoder-pio/capture.hpp index 2aa6edaf..458daebe 100644 --- a/drivers/encoder-pio/capture.hpp +++ b/drivers/encoder-pio/capture.hpp @@ -10,7 +10,7 @@ namespace pimoroni { //-------------------------------------------------- private: const int32_t captured_count = 0; - const int32_t count_change = 0; + const int32_t count_change = 0; const float average_frequency = 0.0f; const float counts_per_revolution = 1; @@ -34,7 +34,7 @@ namespace pimoroni { int32_t get_count_change() const; - float get_frequency() const; + float get_frequency() const; float get_revolutions_per_second() const; float get_revolutions_per_minute() const; float get_degrees_per_second() const; diff --git a/drivers/encoder-pio/encoder-pio.cmake b/drivers/encoder-pio/encoder-pio.cmake index 37b3959a..74737526 100644 --- a/drivers/encoder-pio/encoder-pio.cmake +++ b/drivers/encoder-pio/encoder-pio.cmake @@ -1,12 +1,17 @@ -add_library(encoder-pio INTERFACE) +set(DRIVER_NAME encoder-pio) +add_library(${DRIVER_NAME} INTERFACE) -target_sources(encoder-pio INTERFACE - ${CMAKE_CURRENT_LIST_DIR}/msa301.cpp +target_sources(${DRIVER_NAME} INTERFACE + ${CMAKE_CURRENT_LIST_DIR}/encoder.cpp + ${CMAKE_CURRENT_LIST_DIR}/capture.cpp ) -pico_generate_pio_header(encoder-pio ${CMAKE_CURRENT_LIST_DIR}/encoder.pio) +pico_generate_pio_header(${DRIVER_NAME} ${CMAKE_CURRENT_LIST_DIR}/encoder.pio) -target_include_directories(encoder-pio INTERFACE ${CMAKE_CURRENT_LIST_DIR}) +target_include_directories(${DRIVER_NAME} INTERFACE ${CMAKE_CURRENT_LIST_DIR}) # Pull in pico libraries that we need -target_link_libraries(encoder-pio INTERFACE pico_stdlib hardware_i2c) \ No newline at end of file +target_link_libraries(${DRIVER_NAME} INTERFACE + pico_stdlib + hardware_pio + ) \ No newline at end of file diff --git a/drivers/encoder-pio/encoder.cpp b/drivers/encoder-pio/encoder.cpp index 730b74df..908472d7 100644 --- a/drivers/encoder-pio/encoder.cpp +++ b/drivers/encoder-pio/encoder.cpp @@ -17,7 +17,7 @@ namespace pimoroni { //////////////////////////////////////////////////////////////////////////////////////////////////// void Encoder::pio0_interrupt_callback() { - //Go through each of encoders on this PIO to see which triggered this interrupt + // Go through each of encoders on this PIO to see which triggered this interrupt for(uint8_t sm = 0; sm < NUM_PIO_STATE_MACHINES; sm++) { if(pio_encoders[0][sm] != nullptr) { pio_encoders[0][sm]->check_for_transition(); @@ -27,7 +27,7 @@ namespace pimoroni { //////////////////////////////////////////////////////////////////////////////////////////////////// void Encoder::pio1_interrupt_callback() { - //Go through each of encoders on this PIO to see which triggered this interrupt + // Go through each of encoders on this PIO to see which triggered this interrupt for(uint8_t sm = 0; sm < NUM_PIO_STATE_MACHINES; sm++) { if(pio_encoders[1][sm] != nullptr) { pio_encoders[1][sm]->check_for_transition(); @@ -41,7 +41,7 @@ namespace pimoroni { // CONSTRUCTORS / DESTRUCTOR //////////////////////////////////////////////////////////////////////////////////////////////////// Encoder::Encoder(PIO pio, uint8_t pinA, uint8_t pinB, uint8_t pinC, - float counts_per_revolution, bool count_microsteps, + float counts_per_revolution, bool count_microsteps, uint16_t freq_divider) : enc_pio(pio), pinA(pinA), pinB(pinB), pinC(pinC), counts_per_revolution(counts_per_revolution), count_microsteps(count_microsteps), @@ -50,13 +50,13 @@ namespace pimoroni { //////////////////////////////////////////////////////////////////////////////////////////////////// Encoder::~Encoder() { - //Clean up our use of the SM associated with this encoder + // Clean up our use of the SM associated with this encoder encoder_program_release(enc_pio, enc_sm); uint index = pio_get_index(enc_pio); pio_encoders[index][enc_sm] = nullptr; pio_claimed_sms[index] &= ~(1u << enc_sm); - //If there are no more SMs using the encoder program, then we can remove it from the PIO + // If there are no more SMs using the encoder program, then we can remove it from the PIO if(pio_claimed_sms[index] == 0) { pio_remove_program(enc_pio, &encoder_program, enc_offset); } @@ -70,10 +70,10 @@ namespace pimoroni { bool Encoder::init() { bool initialised = false; - //Are the pins we want to use actually valid? + // Are the pins we want to use actually valid? if((pinA < NUM_BANK0_GPIOS) && (pinB < NUM_BANK0_GPIOS)) { - //If a Pin C was defined, and valid, set it as a GND to pull the other two pins down + // If a Pin C was defined, and valid, set it as a GND to pull the other two pins down if((pinC != PIN_UNUSED) && (pinC < NUM_BANK0_GPIOS)) { gpio_init(pinC); gpio_set_dir(pinC, GPIO_OUT); @@ -83,9 +83,9 @@ namespace pimoroni { enc_sm = pio_claim_unused_sm(enc_pio, true); uint pio_idx = pio_get_index(enc_pio); - //Is this the first time using an encoder on this PIO? + // Is this the first time using an encoder on this PIO? if(pio_claimed_sms[pio_idx] == 0) { - //Add the program to the PIO memory and enable the appropriate interrupt + // Add the program to the PIO memory and enable the appropriate interrupt enc_offset = pio_add_program(enc_pio, &encoder_program); encoder_program_init(enc_pio, enc_sm, enc_offset, pinA, pinB, freq_divider); hw_set_bits(&enc_pio->inte0, PIO_IRQ0_INTE_SM0_RXNEMPTY_BITS << enc_sm); @@ -99,11 +99,11 @@ namespace pimoroni { } } - //Keep a record of this encoder for the interrupt callback + // Keep a record of this encoder for the interrupt callback pio_encoders[pio_idx][enc_sm] = this; pio_claimed_sms[pio_idx] |= 1u << enc_sm; - //Read the current state of the encoder pins and start the PIO program on the SM + // Read the current state of the encoder pins and start the PIO program on the SM stateA = gpio_get(pinA); stateB = gpio_get(pinB); encoder_program_start(enc_pio, enc_sm, stateA, stateB); @@ -175,18 +175,18 @@ namespace pimoroni { //////////////////////////////////////////////////////////////////////////////////////////////////// Capture Encoder::perform_capture() { - //Capture the current values - int32_t captured_count = count; - int32_t captured_cumulative_time = cumulative_time; - cumulative_time = 0; + // Capture the current values + int32_t captured_count = count; + int32_t captured_cumulative_time = cumulative_time; + cumulative_time = 0; - //Determine the change in counts since the last capture was performed - int32_t count_change = captured_count - last_captured_count; - last_captured_count = captured_count; + // Determine the change in counts since the last capture was performed + int32_t count_change = captured_count - last_captured_count; + last_captured_count = captured_count; - //Calculate the average frequency of state transitions + // Calculate the average frequency of state transitions float average_frequency = 0.0f; - if(count_change != 0 && captured_cumulative_time != INT_MAX) { + if(count_change != 0 && captured_cumulative_time != INT_MAX) { average_frequency = (clocks_per_time * (float)count_change) / (float)captured_cumulative_time; } @@ -233,7 +233,7 @@ namespace pimoroni { // For rotary encoders, only every fourth transition is cared about, causing an inaccurate time value // To address this we accumulate the times received and zero it when a transition is counted if(!count_microsteps) { - if(time_received + microstep_time < time_received) //Check to avoid integer overflow + if(time_received + microstep_time < time_received) // Check to avoid integer overflow time_received = INT32_MAX; else time_received += microstep_time; @@ -256,7 +256,7 @@ namespace pimoroni { // B ____|‾‾‾‾ case MICROSTEP_3: if(count_microsteps) - microstep_down(time_received); + microstep_down(time_received); break; } break; @@ -270,7 +270,7 @@ namespace pimoroni { if(count_microsteps || last_travel_dir == CLOCKWISE) microstep_up(time_received); - last_travel_dir = NO_DIR; //Finished turning clockwise + last_travel_dir = NO_DIR; // Finished turning clockwise break; // A ‾‾‾‾|____ @@ -291,7 +291,7 @@ namespace pimoroni { if(count_microsteps) microstep_up(time_received); - last_travel_dir = CLOCKWISE; //Started turning clockwise + last_travel_dir = CLOCKWISE; // Started turning clockwise break; // A ‾‾‾‾‾‾‾‾‾ @@ -300,7 +300,7 @@ namespace pimoroni { if(count_microsteps) microstep_down(time_received); - last_travel_dir = COUNTERCLOCK; //Started turning counter-clockwise + last_travel_dir = COUNTERCLOCK; // Started turning counter-clockwise break; } break; @@ -320,8 +320,8 @@ namespace pimoroni { case MICROSTEP_2: if(count_microsteps || last_travel_dir == COUNTERCLOCK) microstep_down(time_received); - - last_travel_dir = NO_DIR; //Finished turning counter-clockwise + + last_travel_dir = NO_DIR; // Finished turning counter-clockwise break; } break; diff --git a/drivers/encoder-pio/encoder.hpp b/drivers/encoder-pio/encoder.hpp index a3f27df5..535db71c 100644 --- a/drivers/encoder-pio/encoder.hpp +++ b/drivers/encoder-pio/encoder.hpp @@ -12,7 +12,7 @@ namespace pimoroni { public: static constexpr float DEFAULT_COUNTS_PER_REV = 24; static const uint16_t DEFAULT_COUNT_MICROSTEPS = false; - static const uint16_t DEFAULT_FREQ_DIVIDER = 1; + static const uint16_t DEFAULT_FREQ_DIVIDER = 1; static const uint8_t PIN_UNUSED = UINT8_MAX; private: @@ -58,7 +58,7 @@ namespace pimoroni { const float clocks_per_time = 0; //-------------------------------------------------- - + uint enc_sm = 0; uint enc_offset = 0; @@ -108,7 +108,7 @@ namespace pimoroni { float get_angle_degrees() const; float get_angle_radians() const; - float get_frequency() const; + float get_frequency() const; float get_revolutions_per_second() const; float get_revolutions_per_minute() const; float get_degrees_per_second() const; diff --git a/examples/pico_explorer_encoder/demo.cpp b/examples/pico_explorer_encoder/demo.cpp index 1f434c94..fbf17dff 100644 --- a/examples/pico_explorer_encoder/demo.cpp +++ b/examples/pico_explorer_encoder/demo.cpp @@ -16,24 +16,24 @@ static const uint8_t ENCODER_PIN_B = 0; static const uint8_t ENCODER_PIN_C = Encoder::PIN_UNUSED; static const uint8_t ENCODER_SWITCH_PIN = 4; -static constexpr float COUNTS_PER_REVOLUTION = 24; //24 is for rotary encoders. For motor magnetic encoders uses - //12 times the gear ratio (e.g. 12 * 20 with a 20:1 ratio motor -static const bool COUNT_MICROSTEPS = false; //Set to true for motor magnetic encoders +static constexpr float COUNTS_PER_REVOLUTION = 24; // 24 is for rotary encoders. For motor magnetic encoders uses + // 12 times the gear ratio (e.g. 12 * 20 with a 20:1 ratio motor +static const bool COUNT_MICROSTEPS = false; // Set to true for motor magnetic encoders -static const uint16_t FREQ_DIVIDER = 1; //Increase this to deal with switch bounce. 250 Gives a 1ms debounce +static const uint16_t FREQ_DIVIDER = 1; // Increase this to deal with switch bounce. 250 Gives a 1ms debounce -static const int32_t TIME_BETWEEN_SAMPLES_US = 100; //Time between each sample, in microseconds -static const int32_t WINDOW_DURATION_US = 1000000; //The full time window that will be stored +static const int32_t TIME_BETWEEN_SAMPLES_US = 100; // Time between each sample, in microseconds +static const int32_t WINDOW_DURATION_US = 1000000; // The full time window that will be stored 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 +static const int32_t SCRATCH_SIZE = READINGS_SIZE / 10; // A smaller value, for temporarily storing readings during screen drawing static const bool QUADRATURE_OUT_ENABLED = true; -static constexpr float QUADRATURE_OUT_FREQ = 800; //The frequency the quadrature output will run at (note that counting microsteps will show 4x this value) -static const float QUADRATURE_OUT_1ST_PIN = 6; //Which first pin to output the quadrature signal to (e.g. pins 6 and 7) +static constexpr float QUADRATURE_OUT_FREQ = 800; // The frequency the quadrature output will run at (note that counting microsteps will show 4x this value) +static const float QUADRATURE_OUT_1ST_PIN = 6; // Which first pin to output the quadrature signal to (e.g. pins 6 and 7) -static const uint64_t MAIN_LOOP_TIME_US = 50000; //How long there should be in microseconds between each screen refresh -static const uint16_t EDGE_ALIGN_ABOVE_ZOOM = 4; //The zoom level beyond which edge alignment will be enabled to ma +static const uint64_t MAIN_LOOP_TIME_US = 50000; // How long there should be in microseconds between each screen refresh +static const uint16_t EDGE_ALIGN_ABOVE_ZOOM = 4; // The zoom level beyond which edge alignment will be enabled to ma @@ -71,136 +71,131 @@ uint16_t current_zoom_level = 1; //////////////////////////////////////////////////////////////////////////////////////////////////// // FUNCTIONS //////////////////////////////////////////////////////////////////////////////////////////////////// -uint32_t draw_plot(Point p1, Point p2, volatile bool (&readings)[READINGS_SIZE], uint32_t readingPos, bool edge_align) -{ - uint32_t reading_window = READINGS_SIZE / current_zoom_level; - uint32_t start_index_no_modulus = (readingPos + (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; +uint32_t draw_plot(Point p1, Point p2, volatile bool (&readings)[READINGS_SIZE], uint32_t readingPos, bool edge_align) { + uint32_t reading_window = READINGS_SIZE / current_zoom_level; + uint32_t start_index_no_modulus = (readingPos + (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]; + 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 - readingPos); + 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 - readingPos); - //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]; + // 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]; + // 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; } - //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; + last_reading = readings[start_index % READINGS_SIZE]; + } - //Set the draw state to be whatever the last reading was - DrawState draw_state = last_reading ? DRAW_HIGH : DRAW_LOW; + // 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; - //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; + // 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; - //Return the alignment offset so subsequent encoder channel plots can share the alignment - return alignment_offset; + // 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) { - if(drawing_to_screen && next_scratch_index < SCRATCH_SIZE) { - encA_scratch[next_scratch_index] = encoder.get_state_a(); - encB_scratch[next_scratch_index] = encoder.get_state_b(); - next_scratch_index++; - } - else { - encA_readings[next_reading_index] = encoder.get_state_a(); - encB_readings[next_reading_index] = encoder.get_state_b(); + if(drawing_to_screen && next_scratch_index < SCRATCH_SIZE) { + encA_scratch[next_scratch_index] = encoder.get_state_a(); + encB_scratch[next_scratch_index] = encoder.get_state_b(); + next_scratch_index++; + } + else { + encA_readings[next_reading_index] = encoder.get_state_a(); + encB_readings[next_reading_index] = encoder.get_state_b(); - next_reading_index++; - if(next_reading_index >= READINGS_SIZE) - next_reading_index = 0; - } + next_reading_index++; + if(next_reading_index >= READINGS_SIZE) + next_reading_index = 0; + } - return true; + return true; } void setup() { - stdio_init_all(); + stdio_init_all(); - gpio_init(PICO_DEFAULT_LED_PIN); - gpio_set_dir(PICO_DEFAULT_LED_PIN, GPIO_OUT); + gpio_init(PICO_DEFAULT_LED_PIN); + gpio_set_dir(PICO_DEFAULT_LED_PIN, GPIO_OUT); - if(ENCODER_SWITCH_PIN != Encoder::PIN_UNUSED) { - gpio_init(ENCODER_SWITCH_PIN); - gpio_set_dir(ENCODER_SWITCH_PIN, GPIO_IN); - gpio_pull_down(ENCODER_SWITCH_PIN); - } + if(ENCODER_SWITCH_PIN != Encoder::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(); + pico_explorer.init(); + pico_explorer.set_pen(0); + pico_explorer.clear(); + pico_explorer.update(); - encoder.init(); + encoder.init(); - bool encA = encoder.get_state_a(); - bool encB = encoder.get_state_b(); - for(uint i = 0; i < READINGS_SIZE; i++) { - encA_readings[i] = encA; - encB_readings[i] = encB; - } + bool encA = encoder.get_state_a(); + bool encB = encoder.get_state_b(); + for(uint i = 0; i < READINGS_SIZE; i++) { + encA_readings[i] = encA; + encB_readings[i] = encB; + } - 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); - } - - pico_explorer.set_backlight(255); + 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); + } } @@ -210,136 +205,139 @@ void setup() { //////////////////////////////////////////////////////////////////////////////////////////////////// int main() { - //Perform the main setup for the demo - setup(); + // 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); + // 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 aPressedLatch = false; - bool xPressedLatch = false; - uint64_t last_time = time_us_64(); + bool aPressedLatch = false; + bool xPressedLatch = 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 + while(true) { - //If the user has wired up their encoder switch, and it is pressed, set the encoder count to zero - if(ENCODER_SWITCH_PIN != Encoder::PIN_UNUSED && gpio_get(ENCODER_SWITCH_PIN)) { - encoder.zero_count(); - } + // 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; - //Take a capture, or snapshot of the current encoder state - Capture capture = encoder.perform_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); - + gpio_put(PICO_DEFAULT_LED_PIN, true); // Show the screen refresh has stated - //If A has been pressed, zoom the view out to a min of x1 - if(pico_explorer.is_pressed(PicoExplorer::A)) { - if(!aPressedLatch) { - aPressedLatch = true; - current_zoom_level = std::max(current_zoom_level / 2, 1); - } - } - else { - aPressedLatch = false; - } + // If the user has wired up their encoder switch, and it is pressed, set the encoder count to zero + if(ENCODER_SWITCH_PIN != Encoder::PIN_UNUSED && gpio_get(ENCODER_SWITCH_PIN)) { + encoder.zero_count(); + } - //If X has been pressed, zoom the view in to the max of x512 - if(pico_explorer.is_pressed(PicoExplorer::X)) { - if(!xPressedLatch) { - xPressedLatch = true; - current_zoom_level = std::min(current_zoom_level * 2, 512); - } - } - else { - xPressedLatch = false; - } + // Take a capture, or snapshot of the current encoder state + Capture capture = encoder.perform_capture(); - //-------------------------------------------------- - // Draw the encoder readings to the screen as a signal plot + // 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); + } - pico_explorer.set_pen(0, 0, 0); - pico_explorer.clear(); - - drawing_to_screen = true; - - pico_explorer.set_pen(255, 255, 0); - uint32_t localPos = next_reading_index; - uint32_t alignment_offset = draw_plot(Point(0, 10), Point(PicoExplorer::WIDTH, 10 + 50), encA_readings, localPos, current_zoom_level > EDGE_ALIGN_ABOVE_ZOOM); - - pico_explorer.set_pen(0, 255, 255); - draw_plot(Point(0, 80), Point(PicoExplorer::WIDTH, 80 + 50), encB_readings, (localPos + (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++) { - encA_readings[next_reading_index] = encA_scratch[i]; - encB_readings[next_reading_index] = encB_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.get_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.get_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.get_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 + // If A has been pressed, zoom the view out to a min of x1 + if(pico_explorer.is_pressed(PicoExplorer::A)) { + if(!aPressedLatch) { + aPressedLatch = true; + current_zoom_level = std::max(current_zoom_level / 2, 1); } + } + else { + aPressedLatch = false; + } + + // If X has been pressed, zoom the view in to the max of x512 + if(pico_explorer.is_pressed(PicoExplorer::X)) { + if(!xPressedLatch) { + xPressedLatch = true; + current_zoom_level = std::min(current_zoom_level * 2, 512); + } + } + else { + xPressedLatch = 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 localPos = next_reading_index; + uint32_t alignment_offset = draw_plot(Point(0, 10), Point(PicoExplorer::WIDTH, 10 + 50), encA_readings, localPos, current_zoom_level > EDGE_ALIGN_ABOVE_ZOOM); + + pico_explorer.set_pen(0, 255, 255); + draw_plot(Point(0, 80), Point(PicoExplorer::WIDTH, 80 + 50), encB_readings, (localPos + (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++) { + encA_readings[next_reading_index] = encA_scratch[i]; + encB_readings[next_reading_index] = encB_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.get_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.get_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.get_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 } + } } \ No newline at end of file From 70b589d43173a4c83dc33d96a60f2d7950bb5cbf Mon Sep 17 00:00:00 2001 From: ZodiusInfuser Date: Wed, 13 Apr 2022 20:12:44 +0100 Subject: [PATCH 16/60] Fixed inability to use more than one encode, plus other things --- common/pimoroni_common.hpp | 16 + drivers/encoder-pio/capture.cpp | 40 +- drivers/encoder-pio/capture.hpp | 25 +- drivers/encoder-pio/encoder.cpp | 560 ++++++++++++------------ drivers/encoder-pio/encoder.hpp | 80 ++-- drivers/encoder-pio/encoder.pio | 77 ++-- examples/pico_explorer_encoder/demo.cpp | 31 +- 7 files changed, 433 insertions(+), 396 deletions(-) diff --git a/common/pimoroni_common.hpp b/common/pimoroni_common.hpp index 6fc0a903..990bc99f 100644 --- a/common/pimoroni_common.hpp +++ b/common/pimoroni_common.hpp @@ -74,4 +74,20 @@ 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; + }; + union { + uint8_t second; + uint8_t b; + uint8_t negative; + }; + + pin_pair() : first(0), second(0) {} + pin_pair(uint8_t first, uint8_t second) : first(first), second(second) {} + }; } \ No newline at end of file diff --git a/drivers/encoder-pio/capture.cpp b/drivers/encoder-pio/capture.cpp index e0208f08..9e1b428d 100644 --- a/drivers/encoder-pio/capture.cpp +++ b/drivers/encoder-pio/capture.cpp @@ -8,8 +8,8 @@ namespace pimoroni { // CONSTRUCTORS //////////////////////////////////////////////////////////////////////////////////////////////////// Capture::Capture(int32_t captured_count, int32_t count_change, float average_frequency, float counts_per_revolution) : - captured_count(captured_count), count_change(count_change), average_frequency(average_frequency), - counts_per_revolution(std::max(counts_per_revolution, FLT_MIN)) { //Clamp counts_per_rev to avoid potential NaN + captured_count(captured_count), capture_count_change(count_change), average_frequency(average_frequency), + counts_per_revolution(MAX(counts_per_revolution, FLT_MIN)) { //Clamp counts_per_rev to avoid potential NaN } @@ -17,53 +17,53 @@ namespace pimoroni { //////////////////////////////////////////////////////////////////////////////////////////////////// // METHODS //////////////////////////////////////////////////////////////////////////////////////////////////// - int32_t Capture::get_count() const { + int32_t Capture::count() const { return captured_count; } //////////////////////////////////////////////////////////////////////////////////////////////////// - float Capture::get_revolutions() const { - return (float)get_count() / counts_per_revolution; + float Capture::revolutions() const { + return (float)count() / counts_per_revolution; } //////////////////////////////////////////////////////////////////////////////////////////////////// - float Capture::get_angle_degrees() const { - return get_revolutions() * 360.0f; + float Capture::angle_degrees() const { + return revolutions() * 360.0f; } //////////////////////////////////////////////////////////////////////////////////////////////////// - float Capture::get_angle_radians() const { - return get_revolutions() * M_TWOPI; + float Capture::angle_radians() const { + return revolutions() * M_TWOPI; } //////////////////////////////////////////////////////////////////////////////////////////////////// - int32_t Capture::get_count_change() const { - return count_change; + int32_t Capture::count_change() const { + return capture_count_change; } //////////////////////////////////////////////////////////////////////////////////////////////////// - float Capture::get_frequency() const { + float Capture::frequency() const { return average_frequency; } //////////////////////////////////////////////////////////////////////////////////////////////////// - float Capture::get_revolutions_per_second() const { - return get_frequency() / counts_per_revolution; + float Capture::revolutions_per_second() const { + return frequency() / counts_per_revolution; } //////////////////////////////////////////////////////////////////////////////////////////////////// - float Capture::get_revolutions_per_minute() const { - return get_revolutions_per_second() * 60.0f; + float Capture::revolutions_per_minute() const { + return revolutions_per_second() * 60.0f; } //////////////////////////////////////////////////////////////////////////////////////////////////// - float Capture::get_degrees_per_second() const { - return get_revolutions_per_second() * 360.0f; + float Capture::degrees_per_second() const { + return revolutions_per_second() * 360.0f; } //////////////////////////////////////////////////////////////////////////////////////////////////// - float Capture::get_radians_per_second() const { - return get_revolutions_per_second() * M_TWOPI; + float Capture::radians_per_second() const { + return revolutions_per_second() * M_TWOPI; } //////////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////////// diff --git a/drivers/encoder-pio/capture.hpp b/drivers/encoder-pio/capture.hpp index 458daebe..d5e46168 100644 --- a/drivers/encoder-pio/capture.hpp +++ b/drivers/encoder-pio/capture.hpp @@ -10,7 +10,7 @@ namespace pimoroni { //-------------------------------------------------- private: const int32_t captured_count = 0; - const int32_t count_change = 0; + const int32_t capture_count_change = 0; const float average_frequency = 0.0f; const float counts_per_revolution = 1; @@ -20,25 +20,26 @@ namespace pimoroni { //-------------------------------------------------- public: Capture() {} - Capture(int32_t captured_count, int32_t count_change, float average_frequency, float counts_per_revolution); + Capture(int32_t captured_count, int32_t count_change, + float average_frequency, float counts_per_revolution); //-------------------------------------------------- // Methods //-------------------------------------------------- public: - int32_t get_count() const; - float get_revolutions() const; - float get_angle_degrees() const; - float get_angle_radians() const; + int32_t count() const; + float revolutions() const; + float angle_degrees() const; + float angle_radians() const; - int32_t get_count_change() const; + int32_t count_change() const; - float get_frequency() const; - float get_revolutions_per_second() const; - float get_revolutions_per_minute() const; - float get_degrees_per_second() const; - float get_radians_per_second() const; + float frequency() const; + float revolutions_per_second() const; + float revolutions_per_minute() const; + float degrees_per_second() const; + float radians_per_second() const; }; } \ No newline at end of file diff --git a/drivers/encoder-pio/encoder.cpp b/drivers/encoder-pio/encoder.cpp index 908472d7..873d7673 100644 --- a/drivers/encoder-pio/encoder.cpp +++ b/drivers/encoder-pio/encoder.cpp @@ -9,325 +9,347 @@ namespace pimoroni { - //////////////////////////////////////////////////////////////////////////////////////////////////// - // STATICS - //////////////////////////////////////////////////////////////////////////////////////////////////// - Encoder* Encoder::pio_encoders[][NUM_PIO_STATE_MACHINES] = { { nullptr, nullptr, nullptr, nullptr }, { nullptr, nullptr, nullptr, nullptr } }; - uint8_t Encoder::pio_claimed_sms[] = { 0x0, 0x0 }; +//////////////////////////////////////////////////////////////////////////////////////////////////// +// 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 }; - //////////////////////////////////////////////////////////////////////////////////////////////////// - void Encoder::pio0_interrupt_callback() { - // Go through each of encoders on this PIO to see which triggered this interrupt - for(uint8_t sm = 0; sm < NUM_PIO_STATE_MACHINES; sm++) { - if(pio_encoders[0][sm] != nullptr) { - pio_encoders[0][sm]->check_for_transition(); + +Encoder::Encoder(PIO pio, uint sm, const pin_pair &pins, uint pin_c, + float counts_per_revolution, bool count_microsteps, + uint16_t freq_divider) +: pio(pio) +, sm(sm) +, enc_pins(pins) +, pin_c(pin_c) +, counts_per_revolution(counts_per_revolution) +, 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); } } - } - //////////////////////////////////////////////////////////////////////////////////////////////////// - void Encoder::pio1_interrupt_callback() { - // Go through each of encoders on this PIO to see which triggered this interrupt - for(uint8_t sm = 0; sm < NUM_PIO_STATE_MACHINES; sm++) { - if(pio_encoders[1][sm] != nullptr) { - pio_encoders[1][sm]->check_for_transition(); - } + // 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(pin_c != PIN_UNUSED) { + gpio_set_function(pin_c, GPIO_FUNC_NULL); } } +} - - - //////////////////////////////////////////////////////////////////////////////////////////////////// - // CONSTRUCTORS / DESTRUCTOR - //////////////////////////////////////////////////////////////////////////////////////////////////// - Encoder::Encoder(PIO pio, uint8_t pinA, uint8_t pinB, uint8_t pinC, - float counts_per_revolution, bool count_microsteps, - uint16_t freq_divider) : - enc_pio(pio), pinA(pinA), pinB(pinB), pinC(pinC), - counts_per_revolution(counts_per_revolution), count_microsteps(count_microsteps), - freq_divider(freq_divider), clocks_per_time((float)(clock_get_hz(clk_sys) / (ENC_LOOP_CYCLES * freq_divider))) { - } - - //////////////////////////////////////////////////////////////////////////////////////////////////// - Encoder::~Encoder() { - // Clean up our use of the SM associated with this encoder - encoder_program_release(enc_pio, enc_sm); - uint index = pio_get_index(enc_pio); - pio_encoders[index][enc_sm] = nullptr; - pio_claimed_sms[index] &= ~(1u << enc_sm); - - // If there are no more SMs using the encoder program, then we can remove it from the PIO - if(pio_claimed_sms[index] == 0) { - pio_remove_program(enc_pio, &encoder_program, enc_offset); +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]->check_for_transition(); } } +} +void Encoder::pio0_interrupt_handler() { + pio_interrupt_handler(0); +} +void Encoder::pio1_interrupt_handler() { + pio_interrupt_handler(1); +} - //////////////////////////////////////////////////////////////////////////////////////////////////// - // METHODS - //////////////////////////////////////////////////////////////////////////////////////////////////// - bool Encoder::init() { - bool initialised = false; - +bool Encoder::init() { + if(!initialised && !pio_sm_is_claimed(pio, sm)) { // Are the pins we want to use actually valid? - if((pinA < NUM_BANK0_GPIOS) && (pinB < NUM_BANK0_GPIOS)) { + 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((pinC != PIN_UNUSED) && (pinC < NUM_BANK0_GPIOS)) { - gpio_init(pinC); - gpio_set_dir(pinC, GPIO_OUT); - gpio_put(pinC, false); + if((pin_c != PIN_UNUSED) && (pin_c < NUM_BANK0_GPIOS)) { + gpio_init(pin_c); + gpio_set_dir(pin_c, GPIO_OUT); + gpio_put(pin_c, false); } - enc_sm = pio_claim_unused_sm(enc_pio, true); - uint pio_idx = pio_get_index(enc_pio); + pio_sm_claim(pio, sm); + uint pio_idx = pio_get_index(pio); - // Is this the first time using an encoder on this PIO? - if(pio_claimed_sms[pio_idx] == 0) { - // Add the program to the PIO memory and enable the appropriate interrupt - enc_offset = pio_add_program(enc_pio, &encoder_program); - encoder_program_init(enc_pio, enc_sm, enc_offset, pinA, pinB, freq_divider); - hw_set_bits(&enc_pio->inte0, PIO_IRQ0_INTE_SM0_RXNEMPTY_BITS << enc_sm); + // 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); + //float div = clock_get_hz(clk_sys) / 500000; + //sm_config_set_clkdiv(&c, div); + + 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_set_exclusive_handler(PIO0_IRQ_0, pio0_interrupt_callback); - irq_set_enabled(PIO0_IRQ_0, true); + 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_set_exclusive_handler(PIO1_IRQ_0, pio1_interrupt_callback); - irq_set_enabled(PIO1_IRQ_0, true); + 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 - pio_encoders[pio_idx][enc_sm] = this; - pio_claimed_sms[pio_idx] |= 1u << enc_sm; + //Keep a record of this encoder for the interrupt callback + encoders[pio_idx][sm] = this; + claimed_sms[pio_idx] |= 1u << sm; - // Read the current state of the encoder pins and start the PIO program on the SM - stateA = gpio_get(pinA); - stateB = gpio_get(pinB); - encoder_program_start(enc_pio, enc_sm, stateA, stateB); + 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; } - //////////////////////////////////////////////////////////////////////////////////////////////////// - bool Encoder::get_state_a() const { - return stateA; + return initialised; +} + +pin_pair Encoder::pins() const { + return enc_pins; +} + +bool_pair Encoder::state() const { + return bool_pair(enc_state_a, enc_state_b); +} + +int32_t Encoder::count() const { + return enc_count - count_offset; +} + +float Encoder::revolutions() const { + return (float)count() / counts_per_revolution; +} + +float Encoder::angle_degrees() const { + return revolutions() * 360.0f; +} + +float Encoder::angle_radians() const { + return revolutions() * M_TWOPI; +} + +float Encoder::frequency() const { + return clocks_per_time / (float)time_since; +} + +float Encoder::revolutions_per_second() const { + return frequency() / counts_per_revolution; +} + +float Encoder::revolutions_per_minute() const { + return revolutions_per_second() * 60.0f; +} + +float Encoder::degrees_per_second() const { + return revolutions_per_second() * 360.0f; +} + +float Encoder::radians_per_second() const { + return revolutions_per_second() * M_TWOPI; +} + +void Encoder::zero_count() { + count_offset = enc_count; +} + +Capture Encoder::perform_capture() { + // Capture the current values + int32_t captured_count = enc_count; + int32_t captured_cumulative_time = cumulative_time; + cumulative_time = 0; + + // Determine the change in counts since the last capture was performed + int32_t count_change = captured_count - last_captured_count; + last_captured_count = captured_count; + + // Calculate the average frequency of state transitions + float average_frequency = 0.0f; + if(count_change != 0 && captured_cumulative_time != INT_MAX) { + average_frequency = (clocks_per_time * (float)count_change) / (float)captured_cumulative_time; } - //////////////////////////////////////////////////////////////////////////////////////////////////// - bool Encoder::get_state_b() const { - return stateB; - } + return Capture(captured_count, count_change, average_frequency, counts_per_revolution); +} - //////////////////////////////////////////////////////////////////////////////////////////////////// - int32_t Encoder::get_count() const { - return count - count_offset; - } +void Encoder::microstep_up(int32_t time) { + enc_count++; + time_since = time; + microstep_time = 0; - //////////////////////////////////////////////////////////////////////////////////////////////////// - float Encoder::get_revolutions() const { - return (float)get_count() / counts_per_revolution; - } + if(time + cumulative_time < time) //Check to avoid integer overflow + cumulative_time = INT_MAX; + else + cumulative_time += time; +} - //////////////////////////////////////////////////////////////////////////////////////////////////// - float Encoder::get_angle_degrees() const { - return get_revolutions() * 360.0f; - } +void Encoder::microstep_down(int32_t time) { + enc_count--; + time_since = 0 - time; + microstep_time = 0; - //////////////////////////////////////////////////////////////////////////////////////////////////// - float Encoder::get_angle_radians() const { - return get_revolutions() * M_TWOPI; - } + if(time + cumulative_time < time) //Check to avoid integer overflow + cumulative_time = INT_MAX; + else + cumulative_time += time; +} - //////////////////////////////////////////////////////////////////////////////////////////////////// - float Encoder::get_frequency() const { - return clocks_per_time / (float)time_since; - } +void Encoder::check_for_transition() { + while(pio->ints1 & (PIO_IRQ1_INTS_SM0_RXNEMPTY_BITS << sm)) { + uint32_t received = pio_sm_get(pio, sm); - //////////////////////////////////////////////////////////////////////////////////////////////////// - float Encoder::get_revolutions_per_second() const { - return get_frequency() / counts_per_revolution; - } + // 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; - //////////////////////////////////////////////////////////////////////////////////////////////////// - float Encoder::get_revolutions_per_minute() const { - return get_revolutions_per_second() * 60.0f; - } + // Extract the time (in cycles) it has been since the last received + int32_t time_received = (received & TIME_MASK) + ENC_DEBOUNCE_TIME; - //////////////////////////////////////////////////////////////////////////////////////////////////// - float Encoder::get_degrees_per_second() const { - return get_revolutions_per_second() * 360.0f; - } - - //////////////////////////////////////////////////////////////////////////////////////////////////// - float Encoder::get_radians_per_second() const { - return get_revolutions_per_second() * M_TWOPI; - } - - //////////////////////////////////////////////////////////////////////////////////////////////////// - void Encoder::zero_count() { - count_offset = count; - } - - //////////////////////////////////////////////////////////////////////////////////////////////////// - Capture Encoder::perform_capture() { - // Capture the current values - int32_t captured_count = count; - int32_t captured_cumulative_time = cumulative_time; - cumulative_time = 0; - - // Determine the change in counts since the last capture was performed - int32_t count_change = captured_count - last_captured_count; - last_captured_count = captured_count; - - // Calculate the average frequency of state transitions - float average_frequency = 0.0f; - if(count_change != 0 && captured_cumulative_time != INT_MAX) { - average_frequency = (clocks_per_time * (float)count_change) / (float)captured_cumulative_time; + // For rotary encoders, only every fourth transition is cared about, causing an inaccurate time value + // To address this we accumulate the times received and zero it when a transition 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; } - return Capture(captured_count, count_change, average_frequency, counts_per_revolution); - } + // Determine what transition occurred + switch(LAST_STATE(states)) { + //-------------------------------------------------- + case MICROSTEP_0: + switch(CURR_STATE(states)) { + // A ____|‾‾‾‾ + // B _________ + case MICROSTEP_1: + if(count_microsteps) + microstep_up(time_received); + break; - //////////////////////////////////////////////////////////////////////////////////////////////////// - void Encoder::microstep_up(int32_t time) { - count++; - time_since = time; - microstep_time = 0; + // A _________ + // B ____|‾‾‾‾ + case MICROSTEP_3: + if(count_microsteps) + microstep_down(time_received); + break; + } + break; - if(time + cumulative_time < time) //Check to avoid integer overflow - cumulative_time = INT_MAX; - else - cumulative_time += time; - } + //-------------------------------------------------- + case MICROSTEP_1: + switch(CURR_STATE(states)) { + // A ‾‾‾‾‾‾‾‾‾ + // B ____|‾‾‾‾ + case MICROSTEP_2: + if(count_microsteps || last_travel_dir == CLOCKWISE) + microstep_up(time_received); - //////////////////////////////////////////////////////////////////////////////////////////////////// - void Encoder::microstep_down(int32_t time) { - count--; - time_since = 0 - time; - microstep_time = 0; + last_travel_dir = NO_DIR; // Finished turning clockwise + break; - if(time + cumulative_time < time) //Check to avoid integer overflow - cumulative_time = INT_MAX; - else - cumulative_time += time; - } + // A ‾‾‾‾|____ + // B _________ + case MICROSTEP_0: + if(count_microsteps) + microstep_down(time_received); + break; + } + break; + + //-------------------------------------------------- + case MICROSTEP_2: + switch(CURR_STATE(states)) { + // A ‾‾‾‾|____ + // B ‾‾‾‾‾‾‾‾‾ + case MICROSTEP_3: + if(count_microsteps) + microstep_up(time_received); + + last_travel_dir = CLOCKWISE; // Started turning clockwise + break; - //////////////////////////////////////////////////////////////////////////////////////////////////// - void Encoder::check_for_transition() { - while(enc_pio->ints0 & (PIO_IRQ0_INTS_SM0_RXNEMPTY_BITS << enc_sm)) { - uint32_t received = pio_sm_get(enc_pio, enc_sm); + // A ‾‾‾‾‾‾‾‾‾ + // B ‾‾‾‾|____ + case MICROSTEP_1: + if(count_microsteps) + microstep_down(time_received); + + last_travel_dir = COUNTERCLOCK; // Started turning counter-clockwise + break; + } + break; - // Extract the current and last encoder states from the received value - stateA = (bool)(received & STATE_A_MASK); - stateB = (bool)(received & STATE_B_MASK); - uint8_t states = (received & STATES_MASK) >> 28; + //-------------------------------------------------- + case MICROSTEP_3: + switch(CURR_STATE(states)) { + // A _________ + // B ‾‾‾‾|____ + case MICROSTEP_0: + if(count_microsteps) + microstep_up(time_received); + break; - // Extract the time (in cycles) it has been since the last received - int32_t time_received = (received & TIME_MASK) + ENC_DEBOUNCE_TIME; + // A ____|‾‾‾‾ + // B ‾‾‾‾‾‾‾‾‾ + case MICROSTEP_2: + if(count_microsteps || last_travel_dir == COUNTERCLOCK) + microstep_down(time_received); - // For rotary encoders, only every fourth transition is cared about, causing an inaccurate time value - // To address this we accumulate the times received and zero it when a transition 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; - } - - // Determine what transition occurred - switch(LAST_STATE(states)) { - //-------------------------------------------------- - case MICROSTEP_0: - switch(CURR_STATE(states)) { - // A ____|‾‾‾‾ - // B _________ - case MICROSTEP_1: - if(count_microsteps) - microstep_up(time_received); - break; - - // A _________ - // B ____|‾‾‾‾ - case MICROSTEP_3: - if(count_microsteps) - microstep_down(time_received); - break; - } - break; - - //-------------------------------------------------- - case MICROSTEP_1: - switch(CURR_STATE(states)) { - // A ‾‾‾‾‾‾‾‾‾ - // B ____|‾‾‾‾ - case MICROSTEP_2: - if(count_microsteps || last_travel_dir == CLOCKWISE) - microstep_up(time_received); - - last_travel_dir = NO_DIR; // Finished turning clockwise - break; - - // A ‾‾‾‾|____ - // B _________ - case MICROSTEP_0: - if(count_microsteps) - microstep_down(time_received); - break; - } - break; - - //-------------------------------------------------- - case MICROSTEP_2: - switch(CURR_STATE(states)) { - // A ‾‾‾‾|____ - // B ‾‾‾‾‾‾‾‾‾ - case MICROSTEP_3: - if(count_microsteps) - microstep_up(time_received); - - last_travel_dir = CLOCKWISE; // Started turning clockwise - break; - - // A ‾‾‾‾‾‾‾‾‾ - // B ‾‾‾‾|____ - case MICROSTEP_1: - if(count_microsteps) - microstep_down(time_received); - - last_travel_dir = COUNTERCLOCK; // Started turning counter-clockwise - break; - } - break; - - //-------------------------------------------------- - case MICROSTEP_3: - switch(CURR_STATE(states)) { - // A _________ - // B ‾‾‾‾|____ - case MICROSTEP_0: - if(count_microsteps) - microstep_up(time_received); - break; - - // A ____|‾‾‾‾ - // B ‾‾‾‾‾‾‾‾‾ - case MICROSTEP_2: - if(count_microsteps || last_travel_dir == COUNTERCLOCK) - microstep_down(time_received); - - last_travel_dir = NO_DIR; // Finished turning counter-clockwise - break; - } - break; - } + last_travel_dir = NO_DIR; // Finished turning counter-clockwise + break; + } + break; } } - //////////////////////////////////////////////////////////////////////////////////////////////////// - //////////////////////////////////////////////////////////////////////////////////////////////////// +} } \ No newline at end of file diff --git a/drivers/encoder-pio/encoder.hpp b/drivers/encoder-pio/encoder.hpp index 535db71c..ec353a62 100644 --- a/drivers/encoder-pio/encoder.hpp +++ b/drivers/encoder-pio/encoder.hpp @@ -1,10 +1,25 @@ #pragma once #include "hardware/pio.h" +#include "common/pimoroni_common.hpp" #include "capture.hpp" namespace pimoroni { + 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) {} + }; + class Encoder { //-------------------------------------------------- // Constants @@ -13,7 +28,6 @@ namespace pimoroni { static constexpr float DEFAULT_COUNTS_PER_REV = 24; static const uint16_t DEFAULT_COUNT_MICROSTEPS = false; static const uint16_t DEFAULT_FREQ_DIVIDER = 1; - static const uint8_t PIN_UNUSED = UINT8_MAX; private: static const uint32_t STATE_A_MASK = 0x80000000; @@ -41,16 +55,16 @@ namespace pimoroni { CLOCKWISE = 1, COUNTERCLOCK = -1, }; - - + + //-------------------------------------------------- // Variables //-------------------------------------------------- private: - const PIO enc_pio = pio0; - const uint8_t pinA = PIN_UNUSED; - const uint8_t pinB = PIN_UNUSED; - const uint8_t pinC = PIN_UNUSED; + PIO pio; + uint sm; + pin_pair enc_pins; + uint pin_c; const float counts_per_revolution = DEFAULT_COUNTS_PER_REV; const bool count_microsteps = DEFAULT_COUNT_MICROSTEPS; @@ -59,12 +73,9 @@ namespace pimoroni { //-------------------------------------------------- - uint enc_sm = 0; - uint enc_offset = 0; - - volatile bool stateA = false; - volatile bool stateB = false; - volatile int32_t count = 0; + volatile bool enc_state_a = false; + volatile bool enc_state_b = false; + volatile int32_t enc_count = 0; volatile int32_t time_since = 0; volatile Direction last_travel_dir = NO_DIR; volatile int32_t microstep_time = 0; @@ -73,15 +84,18 @@ namespace pimoroni { int32_t count_offset = 0; int32_t last_captured_count = 0; + bool initialised = false; + //-------------------------------------------------- // Statics //-------------------------------------------------- - public: - static Encoder* pio_encoders[NUM_PIOS][NUM_PIO_STATE_MACHINES]; - static uint8_t pio_claimed_sms[NUM_PIOS]; - static void pio0_interrupt_callback(); - static void pio1_interrupt_callback(); + 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(); //-------------------------------------------------- @@ -89,7 +103,7 @@ namespace pimoroni { //-------------------------------------------------- public: Encoder() {} - Encoder(PIO pio, uint8_t pinA, uint8_t pinB, uint8_t pinC = PIN_UNUSED, + Encoder(PIO pio, uint sm, const pin_pair &pins, uint pin_c = PIN_UNUSED, float counts_per_revolution = DEFAULT_COUNTS_PER_REV, bool count_microsteps = DEFAULT_COUNT_MICROSTEPS, uint16_t freq_divider = DEFAULT_FREQ_DIVIDER); ~Encoder(); @@ -98,24 +112,26 @@ namespace pimoroni { //-------------------------------------------------- // Methods //-------------------------------------------------- - public: + public: bool init(); - bool get_state_a() const; - bool get_state_b() const; - int32_t get_count() const; - float get_revolutions() const; - float get_angle_degrees() const; - float get_angle_radians() const; + // For print access in micropython + pin_pair pins() const; - float get_frequency() const; - float get_revolutions_per_second() const; - float get_revolutions_per_minute() const; - float get_degrees_per_second() const; - float get_radians_per_second() const; + bool_pair state() const; + int32_t count() const; + float revolutions() const; + float angle_degrees() const; + float angle_radians() const; + + float frequency() const; + float revolutions_per_second() const; + float revolutions_per_minute() const; + float degrees_per_second() const; + float radians_per_second() const; void zero_count(); - Capture perform_capture(); + Capture perform_capture(); //TODO rename capture to snapshot private: void microstep_up(int32_t time_since); diff --git a/drivers/encoder-pio/encoder.pio b/drivers/encoder-pio/encoder.pio index 26138986..0c124afb 100644 --- a/drivers/encoder-pio/encoder.pio +++ b/drivers/encoder-pio/encoder.pio @@ -3,30 +3,35 @@ ; 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. +; 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 +; - 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. +; 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 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 +; 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 @@ -35,14 +40,16 @@ .wrap_target loop: - ; Copy the state-change timer from OSR, decrement it, and save it back + ; 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 + ; Read the state of both encoder pins and check + ; if they are different from the last state jmp pin encA_was_high mov isr, null jmp read_encB @@ -57,10 +64,13 @@ read_encB: .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 + ; 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 + 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 @@ -70,7 +80,8 @@ state_changed: 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 + ; 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: @@ -86,34 +97,6 @@ y_dec: 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 +// 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; - - -static inline void encoder_program_init(PIO pio, uint sm, uint offset, uint pinA, uint pinB, uint16_t divider) { - pio_sm_config c = encoder_program_get_default_config(offset); - - sm_config_set_jmp_pin(&c, pinA); - sm_config_set_in_pins(&c, pinB); - sm_config_set_in_shift(&c, false, false, 1); - sm_config_set_fifo_join(&c, PIO_FIFO_JOIN_RX); - pio_gpio_init(pio, pinA); - pio_gpio_init(pio, pinB); - gpio_pull_up(pinA); - gpio_pull_up(pinB); - pio_sm_set_consecutive_pindirs(pio, sm, pinA, 1, 0); - pio_sm_set_consecutive_pindirs(pio, sm, pinB, 1, 0); - sm_config_set_clkdiv_int_frac(&c, divider, 0); - pio_sm_init(pio, sm, offset, &c); -} - -static inline void encoder_program_start(PIO pio, uint sm, bool stateA, bool stateB) { - pio_sm_exec(pio, sm, pio_encode_set(pio_x, (uint)stateA << 1 | (uint)stateB)); - pio_sm_set_enabled(pio, sm, true); -} - -static inline void encoder_program_release(PIO pio, uint sm) { - pio_sm_set_enabled(pio, sm, false); - pio_sm_unclaim(pio, sm); -} %} \ No newline at end of file diff --git a/examples/pico_explorer_encoder/demo.cpp b/examples/pico_explorer_encoder/demo.cpp index fbf17dff..e51e8393 100644 --- a/examples/pico_explorer_encoder/demo.cpp +++ b/examples/pico_explorer_encoder/demo.cpp @@ -11,10 +11,9 @@ using namespace pimoroni; //-------------------------------------------------- // Constants //-------------------------------------------------- -static const uint8_t ENCODER_PIN_A = 1; -static const uint8_t ENCODER_PIN_B = 0; -static const uint8_t ENCODER_PIN_C = Encoder::PIN_UNUSED; -static const uint8_t ENCODER_SWITCH_PIN = 4; +static const pin_pair ENCODER_PINS = {1, 0}; +static const uint ENCODER_PIN_C = PIN_UNUSED; +static const uint ENCODER_SWITCH_PIN = 4; static constexpr float COUNTS_PER_REVOLUTION = 24; // 24 is for rotary encoders. For motor magnetic encoders uses // 12 times the gear ratio (e.g. 12 * 20 with a 20:1 ratio motor @@ -55,7 +54,7 @@ enum DrawState { uint16_t buffer[PicoExplorer::WIDTH * PicoExplorer::HEIGHT]; PicoExplorer pico_explorer(buffer); -Encoder encoder(pio0, ENCODER_PIN_A, ENCODER_PIN_B, ENCODER_PIN_C, COUNTS_PER_REVOLUTION, COUNT_MICROSTEPS, FREQ_DIVIDER); +Encoder encoder(pio0, 0, ENCODER_PINS, ENCODER_PIN_C, COUNTS_PER_REVOLUTION, COUNT_MICROSTEPS, FREQ_DIVIDER); volatile bool encA_readings[READINGS_SIZE]; volatile bool encB_readings[READINGS_SIZE]; @@ -146,13 +145,13 @@ uint32_t draw_plot(Point p1, Point p2, volatile bool (&readings)[READINGS_SIZE], //////////////////////////////////////////////////////////////////////////////////////////////////// bool repeating_timer_callback(struct repeating_timer *t) { if(drawing_to_screen && next_scratch_index < SCRATCH_SIZE) { - encA_scratch[next_scratch_index] = encoder.get_state_a(); - encB_scratch[next_scratch_index] = encoder.get_state_b(); + encA_scratch[next_scratch_index] = encoder.state().a; + encB_scratch[next_scratch_index] = encoder.state().b; next_scratch_index++; } else { - encA_readings[next_reading_index] = encoder.get_state_a(); - encB_readings[next_reading_index] = encoder.get_state_b(); + encA_readings[next_reading_index] = encoder.state().a; + encB_readings[next_reading_index] = encoder.state().b; next_reading_index++; if(next_reading_index >= READINGS_SIZE) @@ -169,7 +168,7 @@ void setup() { gpio_init(PICO_DEFAULT_LED_PIN); gpio_set_dir(PICO_DEFAULT_LED_PIN, GPIO_OUT); - if(ENCODER_SWITCH_PIN != Encoder::PIN_UNUSED) { + 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); @@ -182,8 +181,8 @@ void setup() { encoder.init(); - bool encA = encoder.get_state_a(); - bool encB = encoder.get_state_b(); + bool encA = encoder.state().a; + bool encB = encoder.state().b; for(uint i = 0; i < READINGS_SIZE; i++) { encA_readings[i] = encA; encB_readings[i] = encB; @@ -226,7 +225,7 @@ int main() { 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 != Encoder::PIN_UNUSED && gpio_get(ENCODER_SWITCH_PIN)) { + if(ENCODER_SWITCH_PIN != PIN_UNUSED && gpio_get(ENCODER_SWITCH_PIN)) { encoder.zero_count(); } @@ -317,21 +316,21 @@ int main() { { std::stringstream sstream; - sstream << capture.get_count(); + 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.get_frequency() << "hz"; + 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.get_revolutions_per_minute(); + 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); } From 564fecf1de187d252026862f14e32eb12ab3f59f Mon Sep 17 00:00:00 2001 From: ZodiusInfuser Date: Wed, 13 Apr 2022 20:13:27 +0100 Subject: [PATCH 17/60] First pass at MP support --- micropython/modules/encoder/encoder.c | 65 +++++ micropython/modules/encoder/encoder.cpp | 229 ++++++++++++++++++ micropython/modules/encoder/encoder.h | 23 ++ micropython/modules/encoder/micropython.cmake | 22 ++ micropython/modules/micropython.cmake | 1 + 5 files changed, 340 insertions(+) create mode 100644 micropython/modules/encoder/encoder.c create mode 100644 micropython/modules/encoder/encoder.cpp create mode 100644 micropython/modules/encoder/encoder.h create mode 100644 micropython/modules/encoder/micropython.cmake diff --git a/micropython/modules/encoder/encoder.c b/micropython/modules/encoder/encoder.c new file mode 100644 index 00000000..13a2d766 --- /dev/null +++ b/micropython/modules/encoder/encoder.c @@ -0,0 +1,65 @@ +#include "encoder.h" + + +/***** Methods *****/ +MP_DEFINE_CONST_FUN_OBJ_1(Encoder___del___obj, Encoder___del__); +MP_DEFINE_CONST_FUN_OBJ_1(Encoder_pins_obj, Encoder_pins); +MP_DEFINE_CONST_FUN_OBJ_1(Encoder_state_obj, Encoder_state); +MP_DEFINE_CONST_FUN_OBJ_1(Encoder_count_obj, Encoder_count); +MP_DEFINE_CONST_FUN_OBJ_1(Encoder_revolutions_obj, Encoder_revolutions); +MP_DEFINE_CONST_FUN_OBJ_1(Encoder_angle_degrees_obj, Encoder_angle_degrees); +MP_DEFINE_CONST_FUN_OBJ_1(Encoder_angle_radians_obj, Encoder_angle_radians); +MP_DEFINE_CONST_FUN_OBJ_1(Encoder_frequency_obj, Encoder_frequency); +MP_DEFINE_CONST_FUN_OBJ_1(Encoder_revolutions_per_second_obj, Encoder_revolutions_per_second); +MP_DEFINE_CONST_FUN_OBJ_1(Encoder_revolutions_per_minute_obj, Encoder_revolutions_per_minute); +MP_DEFINE_CONST_FUN_OBJ_1(Encoder_degrees_per_second_obj, Encoder_degrees_per_second); +MP_DEFINE_CONST_FUN_OBJ_1(Encoder_radians_per_second_obj, Encoder_radians_per_second); +MP_DEFINE_CONST_FUN_OBJ_1(Encoder_zero_count_obj, Encoder_zero_count); +MP_DEFINE_CONST_FUN_OBJ_1(Encoder_take_snapshot_obj, Encoder_take_snapshot); + +/***** Binding of Methods *****/ +STATIC const mp_rom_map_elem_t Encoder_locals_dict_table[] = { + { MP_ROM_QSTR(MP_QSTR___del__), MP_ROM_PTR(&Encoder___del___obj) }, + { MP_ROM_QSTR(MP_QSTR_pins), MP_ROM_PTR(&Encoder_pins_obj) }, + { MP_ROM_QSTR(MP_QSTR_state), MP_ROM_PTR(&Encoder_state_obj) }, + { MP_ROM_QSTR(MP_QSTR_count), MP_ROM_PTR(&Encoder_count_obj) }, + { MP_ROM_QSTR(MP_QSTR_revolutions), MP_ROM_PTR(&Encoder_revolutions_obj) }, + { MP_ROM_QSTR(MP_QSTR_angle_degrees), MP_ROM_PTR(&Encoder_angle_degrees_obj) }, + { MP_ROM_QSTR(MP_QSTR_angle_radians), MP_ROM_PTR(&Encoder_angle_radians_obj) }, + { MP_ROM_QSTR(MP_QSTR_frequency), MP_ROM_PTR(&Encoder_frequency_obj) }, + { MP_ROM_QSTR(MP_QSTR_revolutions_per_second), MP_ROM_PTR(&Encoder_revolutions_per_second_obj) }, + { MP_ROM_QSTR(MP_QSTR_revolutions_per_minute), MP_ROM_PTR(&Encoder_revolutions_per_minute_obj) }, + { MP_ROM_QSTR(MP_QSTR_degrees_per_second), MP_ROM_PTR(&Encoder_degrees_per_second_obj) }, + { MP_ROM_QSTR(MP_QSTR_radians_per_second), MP_ROM_PTR(&Encoder_radians_per_second_obj) }, + { MP_ROM_QSTR(MP_QSTR_zero_count), MP_ROM_PTR(&Encoder_zero_count_obj) }, + { MP_ROM_QSTR(MP_QSTR_take_snapshot), MP_ROM_PTR(&Encoder_take_snapshot_obj) }, +}; + +STATIC MP_DEFINE_CONST_DICT(Encoder_locals_dict, Encoder_locals_dict_table); + +/***** Class Definition *****/ +const mp_obj_type_t Encoder_type = { + { &mp_type_type }, + .name = MP_QSTR_encoder, + .print = Encoder_print, + .make_new = Encoder_make_new, + .locals_dict = (mp_obj_dict_t*)&Encoder_locals_dict, +}; + +/***** Globals Table *****/ +STATIC const mp_map_elem_t encoder_globals_table[] = { + { MP_OBJ_NEW_QSTR(MP_QSTR___name__), MP_OBJ_NEW_QSTR(MP_QSTR_encoder) }, + { MP_OBJ_NEW_QSTR(MP_QSTR_Encoder), (mp_obj_t)&Encoder_type }, + + { MP_ROM_QSTR(MP_QSTR_ANGULAR), MP_ROM_INT(0x00) }, + { MP_ROM_QSTR(MP_QSTR_LINEAR), MP_ROM_INT(0x01) }, + { MP_ROM_QSTR(MP_QSTR_CONTINUOUS), MP_ROM_INT(0x02) }, +}; +STATIC MP_DEFINE_CONST_DICT(mp_module_encoder_globals, encoder_globals_table); + +/***** Module Definition *****/ +const mp_obj_module_t encoder_user_cmodule = { + .base = { &mp_type_module }, + .globals = (mp_obj_dict_t*)&mp_module_encoder_globals, +}; +MP_REGISTER_MODULE(MP_QSTR_encoder, encoder_user_cmodule, MODULE_ENCODER_ENABLED); diff --git a/micropython/modules/encoder/encoder.cpp b/micropython/modules/encoder/encoder.cpp new file mode 100644 index 00000000..69a7da85 --- /dev/null +++ b/micropython/modules/encoder/encoder.cpp @@ -0,0 +1,229 @@ +#include "drivers/encoder-pio/capture.hpp" +#include "drivers/encoder-pio/encoder.hpp" +#include + +#define MP_OBJ_TO_PTR2(o, t) ((t *)(uintptr_t)(o)) + +using namespace pimoroni; +//using namespace encoder; + +extern "C" { +#include "encoder.h" +#include "py/builtin.h" + + +/********** Encoder **********/ + +/***** Variables Struct *****/ +typedef struct _Encoder_obj_t { + mp_obj_base_t base; + Encoder* encoder; +} _Encoder_obj_t; + + +/***** Print *****/ +void Encoder_print(const mp_print_t *print, mp_obj_t self_in, mp_print_kind_t kind) { + (void)kind; //Unused input parameter + _Encoder_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Encoder_obj_t); + mp_print_str(print, "Encoder("); + + mp_print_str(print, "pins = ("); + pin_pair pins = self->encoder->pins(); + mp_obj_print_helper(print, mp_obj_new_int(pins.a), PRINT_REPR); + mp_print_str(print, ", "); + mp_obj_print_helper(print, mp_obj_new_int(pins.b), PRINT_REPR); + mp_print_str(print, "), enabled = "); + /*mp_obj_print_helper(print, self->encoder->is_enabled() ? mp_const_true : mp_const_false, PRINT_REPR); + mp_print_str(print, ", pulse = "); + mp_obj_print_helper(print, mp_obj_new_float(self->encoder->pulse()), PRINT_REPR); + mp_print_str(print, ", value = "); + mp_obj_print_helper(print, mp_obj_new_float(self->encoder->value()), PRINT_REPR); + mp_print_str(print, ", freq = "); + mp_obj_print_helper(print, mp_obj_new_float(self->encoder->frequency()), PRINT_REPR); +*/ + mp_print_str(print, ")"); +} + + +/***** Constructor *****/ +mp_obj_t Encoder_make_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw, const mp_obj_t *all_args) { + _Encoder_obj_t *self = nullptr; + + enum { ARG_pio, ARG_sm, ARG_pins, ARG_pin_c, ARG_calibration, ARG_freq }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_pio, MP_ARG_REQUIRED | MP_ARG_INT }, + { MP_QSTR_sm, MP_ARG_REQUIRED | MP_ARG_INT }, + { MP_QSTR_pins, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_pin_c, MP_ARG_INT, {.u_int = PIN_UNUSED} }, + { MP_QSTR_calibration, MP_ARG_OBJ, {.u_obj = mp_const_none} }, + { MP_QSTR_freq, MP_ARG_OBJ, {.u_obj = mp_const_none} }, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all_kw_array(n_args, n_kw, all_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + PIO pio = args[ARG_pio].u_int == 0 ? pio0 : pio1; + int sm = args[ARG_sm].u_int; + + size_t pin_count = 0; + pin_pair pins; + + // Determine what pair of pins this encoder will use + const mp_obj_t object = args[ARG_pins].u_obj; + mp_obj_t *items = nullptr; + if(mp_obj_is_type(object, &mp_type_list)) { + mp_obj_list_t *list = MP_OBJ_TO_PTR2(object, mp_obj_list_t); + pin_count = list->len; + items = list->items; + } + else if(mp_obj_is_type(object, &mp_type_tuple)) { + mp_obj_tuple_t *tuple = MP_OBJ_TO_PTR2(object, mp_obj_tuple_t); + pin_count = tuple->len; + items = tuple->items; + } + + if(items == nullptr) + mp_raise_TypeError("cannot convert object to a list or tuple of pins"); + else if(pin_count != 2) + mp_raise_TypeError("list or tuple must only contain two integers"); + else { + int a = mp_obj_get_int(items[0]); + int b = mp_obj_get_int(items[1]); + if((a < 0 || a >= (int)NUM_BANK0_GPIOS) || + (b < 0 || b >= (int)NUM_BANK0_GPIOS)) { + mp_raise_ValueError("a pin in the list or tuple is out of range. Expected 0 to 29"); + } + else if(a == b) { + mp_raise_ValueError("cannot use the same pin for encoder A and B"); + } + + pins.a = (uint8_t)a; + pins.b = (uint8_t)b; + } +/* + encoder::Calibration *calib = nullptr; + encoder::CalibrationType calibration_type = encoder::CalibrationType::ANGULAR; + const mp_obj_t calib_object = args[ARG_calibration].u_obj; + if(calib_object != mp_const_none) { + if(mp_obj_is_int(calib_object)) { + int type = mp_obj_get_int(calib_object); + if(type < 0 || type >= 3) { + mp_raise_ValueError("type out of range. Expected ANGULAR (0), LINEAR (1) or CONTINUOUS (2)"); + } + calibration_type = (encoder::CalibrationType)type; + } + else if(mp_obj_is_type(calib_object, &Calibration_type)) { + calib = (MP_OBJ_TO_PTR2(calib_object, _Calibration_obj_t)->calibration); + } + else { + mp_raise_TypeError("cannot convert object to an integer or a Calibration class instance"); + } + } + + float freq = encoder::EncoderState::DEFAULT_FREQUENCY; + if(args[ARG_freq].u_obj != mp_const_none) { + freq = mp_obj_get_float(args[ARG_freq].u_obj); + } +*/ + + Encoder *encoder = new Encoder(pio, sm, pins, args[ARG_pin_c].u_int); + if(!encoder->init()) { + delete encoder; + mp_raise_msg(&mp_type_RuntimeError, "unable to allocate the hardware resources needed to initialise this Encoder. Try running `import gc` followed by `gc.collect()` before creating it"); + } + + self = m_new_obj_with_finaliser(_Encoder_obj_t); + self->base.type = &Encoder_type; + self->encoder = encoder; + + return MP_OBJ_FROM_PTR(self); +} + + +/***** Destructor ******/ +mp_obj_t Encoder___del__(mp_obj_t self_in) { + _Encoder_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Encoder_obj_t); + delete self->encoder; + return mp_const_none; +} + + +/***** Methods *****/ +extern mp_obj_t Encoder_pins(mp_obj_t self_in) { + _Encoder_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Encoder_obj_t); + pin_pair pins = self->encoder->pins(); + + mp_obj_t tuple[2]; + tuple[0] = mp_obj_new_int(pins.a); + tuple[1] = mp_obj_new_int(pins.b); + return mp_obj_new_tuple(2, tuple); +} + +extern mp_obj_t Encoder_state(mp_obj_t self_in) { + _Encoder_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Encoder_obj_t); + bool_pair state = self->encoder->state(); + + mp_obj_t tuple[2]; + tuple[0] = state.a ? mp_const_true : mp_const_false; + tuple[1] = state.b ? mp_const_true : mp_const_false; + return mp_obj_new_tuple(2, tuple); +} + +extern mp_obj_t Encoder_count(mp_obj_t self_in) { + _Encoder_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Encoder_obj_t); + return mp_obj_new_int(self->encoder->count()); +} + +extern mp_obj_t Encoder_revolutions(mp_obj_t self_in) { + _Encoder_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Encoder_obj_t); + return mp_obj_new_float(self->encoder->revolutions()); +} + +extern mp_obj_t Encoder_angle_degrees(mp_obj_t self_in) { + _Encoder_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Encoder_obj_t); + return mp_obj_new_float(self->encoder->angle_degrees()); +} + +extern mp_obj_t Encoder_angle_radians(mp_obj_t self_in) { + _Encoder_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Encoder_obj_t); + return mp_obj_new_float(self->encoder->angle_radians()); +} + +extern mp_obj_t Encoder_frequency(mp_obj_t self_in) { + _Encoder_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Encoder_obj_t); + return mp_obj_new_float(self->encoder->frequency()); +} + +extern mp_obj_t Encoder_revolutions_per_second(mp_obj_t self_in) { + _Encoder_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Encoder_obj_t); + return mp_obj_new_float(self->encoder->revolutions_per_second()); +} + +extern mp_obj_t Encoder_revolutions_per_minute(mp_obj_t self_in) { + _Encoder_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Encoder_obj_t); + return mp_obj_new_float(self->encoder->revolutions_per_minute()); +} + +extern mp_obj_t Encoder_degrees_per_second(mp_obj_t self_in) { + _Encoder_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Encoder_obj_t); + return mp_obj_new_float(self->encoder->degrees_per_second()); +} + +extern mp_obj_t Encoder_radians_per_second(mp_obj_t self_in) { + _Encoder_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Encoder_obj_t); + return mp_obj_new_float(self->encoder->radians_per_second()); +} + +extern mp_obj_t Encoder_zero_count(mp_obj_t self_in) { + _Encoder_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Encoder_obj_t); + self->encoder->zero_count(); + return mp_const_none; +} + +extern mp_obj_t Encoder_take_snapshot(mp_obj_t self_in) { + //_Encoder_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Encoder_obj_t); + //Capture capture = self->encoder->perform_capture(); + return mp_const_none; +} +} diff --git a/micropython/modules/encoder/encoder.h b/micropython/modules/encoder/encoder.h new file mode 100644 index 00000000..e4ef8251 --- /dev/null +++ b/micropython/modules/encoder/encoder.h @@ -0,0 +1,23 @@ +// Include MicroPython API. +#include "py/runtime.h" + +/***** Extern of Class Definition *****/ +extern const mp_obj_type_t Encoder_type; + +/***** Extern of Class Methods *****/ +extern void Encoder_print(const mp_print_t *print, mp_obj_t self_in, mp_print_kind_t kind); +extern mp_obj_t Encoder_make_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw, const mp_obj_t *all_args); +extern mp_obj_t Encoder___del__(mp_obj_t self_in); +extern mp_obj_t Encoder_pins(mp_obj_t self_in); +extern mp_obj_t Encoder_state(mp_obj_t self_in); +extern mp_obj_t Encoder_count(mp_obj_t self_in); +extern mp_obj_t Encoder_revolutions(mp_obj_t self_in); +extern mp_obj_t Encoder_angle_degrees(mp_obj_t self_in); +extern mp_obj_t Encoder_angle_radians(mp_obj_t self_in); +extern mp_obj_t Encoder_frequency(mp_obj_t self_in); +extern mp_obj_t Encoder_revolutions_per_second(mp_obj_t self_in); +extern mp_obj_t Encoder_revolutions_per_minute(mp_obj_t self_in); +extern mp_obj_t Encoder_degrees_per_second(mp_obj_t self_in); +extern mp_obj_t Encoder_radians_per_second(mp_obj_t self_in); +extern mp_obj_t Encoder_zero_count(mp_obj_t self_in); +extern mp_obj_t Encoder_take_snapshot(mp_obj_t self_in); \ No newline at end of file diff --git a/micropython/modules/encoder/micropython.cmake b/micropython/modules/encoder/micropython.cmake new file mode 100644 index 00000000..2f4ed7bf --- /dev/null +++ b/micropython/modules/encoder/micropython.cmake @@ -0,0 +1,22 @@ +set(MOD_NAME encoder) +string(TOUPPER ${MOD_NAME} MOD_NAME_UPPER) +add_library(usermod_${MOD_NAME} INTERFACE) + +target_sources(usermod_${MOD_NAME} INTERFACE + ${CMAKE_CURRENT_LIST_DIR}/${MOD_NAME}.c + ${CMAKE_CURRENT_LIST_DIR}/${MOD_NAME}.cpp + ${CMAKE_CURRENT_LIST_DIR}/../../../drivers/encoder-pio/capture.cpp + ${CMAKE_CURRENT_LIST_DIR}/../../../drivers/encoder-pio/encoder.cpp +) +pico_generate_pio_header(usermod_${MOD_NAME} ${CMAKE_CURRENT_LIST_DIR}/../../../drivers/encoder-pio/encoder.pio) + +target_include_directories(usermod_${MOD_NAME} INTERFACE + ${CMAKE_CURRENT_LIST_DIR} + ${CMAKE_CURRENT_LIST_DIR}/../../../drivers/encoder-pio/ +) + +target_compile_definitions(usermod_${MOD_NAME} INTERFACE + MODULE_ENCODER_ENABLED=1 +) + +target_link_libraries(usermod INTERFACE usermod_${MOD_NAME}) \ No newline at end of file diff --git a/micropython/modules/micropython.cmake b/micropython/modules/micropython.cmake index 8aa983c2..30153784 100644 --- a/micropython/modules/micropython.cmake +++ b/micropython/modules/micropython.cmake @@ -43,6 +43,7 @@ include(bitmap_fonts/micropython) include(plasma/micropython) include(hub75/micropython) include(servo/micropython) +include(encoder/micropython) include(ulab/code/micropython) include(qrcode/micropython/micropython) From 90fd4b8fb9b5d5c6e462ea19009fa6ad5c952650 Mon Sep 17 00:00:00 2001 From: ZodiusInfuser Date: Sun, 17 Apr 2022 16:16:59 +0100 Subject: [PATCH 18/60] Encoder mostly implemented, now with MP support --- drivers/encoder-pio/capture.cpp | 70 ---- drivers/encoder-pio/capture.hpp | 45 --- drivers/encoder-pio/encoder-pio.cmake | 1 - drivers/encoder-pio/encoder.cpp | 250 ++++++++++----- drivers/encoder-pio/encoder.hpp | 144 ++++++--- examples/pico_explorer_encoder/demo.cpp | 14 +- micropython/modules/encoder/encoder.c | 81 +++-- micropython/modules/encoder/encoder.cpp | 301 ++++++++++++++---- micropython/modules/encoder/encoder.h | 35 +- micropython/modules/encoder/micropython.cmake | 1 - 10 files changed, 595 insertions(+), 347 deletions(-) delete mode 100644 drivers/encoder-pio/capture.cpp delete mode 100644 drivers/encoder-pio/capture.hpp diff --git a/drivers/encoder-pio/capture.cpp b/drivers/encoder-pio/capture.cpp deleted file mode 100644 index 9e1b428d..00000000 --- a/drivers/encoder-pio/capture.cpp +++ /dev/null @@ -1,70 +0,0 @@ -#include -#include -#include "capture.hpp" - -namespace pimoroni { - - //////////////////////////////////////////////////////////////////////////////////////////////////// - // CONSTRUCTORS - //////////////////////////////////////////////////////////////////////////////////////////////////// - Capture::Capture(int32_t captured_count, int32_t count_change, float average_frequency, float counts_per_revolution) : - captured_count(captured_count), capture_count_change(count_change), average_frequency(average_frequency), - counts_per_revolution(MAX(counts_per_revolution, FLT_MIN)) { //Clamp counts_per_rev to avoid potential NaN - } - - - - //////////////////////////////////////////////////////////////////////////////////////////////////// - // METHODS - //////////////////////////////////////////////////////////////////////////////////////////////////// - int32_t Capture::count() const { - return captured_count; - } - - //////////////////////////////////////////////////////////////////////////////////////////////////// - float Capture::revolutions() const { - return (float)count() / counts_per_revolution; - } - - //////////////////////////////////////////////////////////////////////////////////////////////////// - float Capture::angle_degrees() const { - return revolutions() * 360.0f; - } - - //////////////////////////////////////////////////////////////////////////////////////////////////// - float Capture::angle_radians() const { - return revolutions() * M_TWOPI; - } - - //////////////////////////////////////////////////////////////////////////////////////////////////// - int32_t Capture::count_change() const { - return capture_count_change; - } - - //////////////////////////////////////////////////////////////////////////////////////////////////// - float Capture::frequency() const { - return average_frequency; - } - - //////////////////////////////////////////////////////////////////////////////////////////////////// - float Capture::revolutions_per_second() const { - return frequency() / counts_per_revolution; - } - - //////////////////////////////////////////////////////////////////////////////////////////////////// - float Capture::revolutions_per_minute() const { - return revolutions_per_second() * 60.0f; - } - - //////////////////////////////////////////////////////////////////////////////////////////////////// - float Capture::degrees_per_second() const { - return revolutions_per_second() * 360.0f; - } - - //////////////////////////////////////////////////////////////////////////////////////////////////// - float Capture::radians_per_second() const { - return revolutions_per_second() * M_TWOPI; - } - //////////////////////////////////////////////////////////////////////////////////////////////////// - //////////////////////////////////////////////////////////////////////////////////////////////////// -} \ No newline at end of file diff --git a/drivers/encoder-pio/capture.hpp b/drivers/encoder-pio/capture.hpp deleted file mode 100644 index d5e46168..00000000 --- a/drivers/encoder-pio/capture.hpp +++ /dev/null @@ -1,45 +0,0 @@ -#pragma once - -#include "pico/stdlib.h" - -namespace pimoroni { - - class Capture { - //-------------------------------------------------- - // Variables - //-------------------------------------------------- - private: - const int32_t captured_count = 0; - const int32_t capture_count_change = 0; - const float average_frequency = 0.0f; - const float counts_per_revolution = 1; - - - //-------------------------------------------------- - // Constructors - //-------------------------------------------------- - public: - Capture() {} - Capture(int32_t captured_count, int32_t count_change, - float average_frequency, float counts_per_revolution); - - - //-------------------------------------------------- - // Methods - //-------------------------------------------------- - public: - int32_t count() const; - float revolutions() const; - float angle_degrees() const; - float angle_radians() const; - - int32_t count_change() const; - - float frequency() const; - float revolutions_per_second() const; - float revolutions_per_minute() const; - float degrees_per_second() const; - float radians_per_second() const; - }; - -} \ No newline at end of file diff --git a/drivers/encoder-pio/encoder-pio.cmake b/drivers/encoder-pio/encoder-pio.cmake index 74737526..923ecbe0 100644 --- a/drivers/encoder-pio/encoder-pio.cmake +++ b/drivers/encoder-pio/encoder-pio.cmake @@ -3,7 +3,6 @@ add_library(${DRIVER_NAME} INTERFACE) target_sources(${DRIVER_NAME} INTERFACE ${CMAKE_CURRENT_LIST_DIR}/encoder.cpp - ${CMAKE_CURRENT_LIST_DIR}/capture.cpp ) pico_generate_pio_header(${DRIVER_NAME} ${CMAKE_CURRENT_LIST_DIR}/encoder.pio) diff --git a/drivers/encoder-pio/encoder.cpp b/drivers/encoder-pio/encoder.cpp index 873d7673..0f9873bf 100644 --- a/drivers/encoder-pio/encoder.cpp +++ b/drivers/encoder-pio/encoder.cpp @@ -1,4 +1,5 @@ #include +#include #include #include "hardware/irq.h" #include "encoder.hpp" @@ -17,14 +18,63 @@ uint8_t Encoder::claimed_sms[] = { 0x0, 0x0 }; uint Encoder::pio_program_offset[] = { 0, 0 }; -Encoder::Encoder(PIO pio, uint sm, const pin_pair &pins, uint pin_c, - float counts_per_revolution, bool count_microsteps, - uint16_t freq_divider) +Encoder::Snapshot::Snapshot() +: count(0), delta(0), frequency(0.0f), counts_per_rev(INT32_MAX) { +} + +Encoder::Snapshot::Snapshot(int32_t count, int32_t delta, float frequency, float counts_per_rev) +: count(count), delta(delta), frequency(frequency) +, counts_per_rev(MAX(counts_per_rev, FLT_EPSILON)) { //Clamp counts_per_rev to avoid potential NaN +} + +float Encoder::Snapshot::revolutions() const { + return (float)count / counts_per_rev; +} + +float Encoder::Snapshot::degrees() const { + return revolutions() * 360.0f; +} + +float Encoder::Snapshot::radians() const { + return revolutions() * M_TWOPI; +} + +float Encoder::Snapshot::revolutions_delta() const { + return (float)delta / counts_per_rev; +} + +float Encoder::Snapshot::degrees_delta() const { + return revolutions_delta() * 360.0f; +} + +float Encoder::Snapshot::radians_delta() const { + return revolutions_delta() * M_TWOPI; +} + +float Encoder::Snapshot::revolutions_per_second() const { + return frequency / counts_per_rev; +} + +float Encoder::Snapshot::revolutions_per_minute() const { + return revolutions_per_second() * 60.0f; +} + +float Encoder::Snapshot::degrees_per_second() const { + return revolutions_per_second() * 360.0f; +} + +float Encoder::Snapshot::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) -, pin_c(pin_c) -, counts_per_revolution(counts_per_revolution) +, 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))) { @@ -57,8 +107,8 @@ Encoder::~Encoder() { gpio_set_function(enc_pins.a, GPIO_FUNC_NULL); gpio_set_function(enc_pins.b, GPIO_FUNC_NULL); - if(pin_c != PIN_UNUSED) { - gpio_set_function(pin_c, GPIO_FUNC_NULL); + if(enc_common_pin != PIN_UNUSED) { + gpio_set_function(enc_common_pin, GPIO_FUNC_NULL); } } } @@ -68,7 +118,7 @@ void Encoder::pio_interrupt_handler(uint pio_idx) { // 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]->check_for_transition(); + encoders[pio_idx][sm]->process_steps(); } } } @@ -87,10 +137,10 @@ bool Encoder::init() { 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((pin_c != PIN_UNUSED) && (pin_c < NUM_BANK0_GPIOS)) { - gpio_init(pin_c); - gpio_set_dir(pin_c, GPIO_OUT); - gpio_put(pin_c, false); + 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); @@ -119,8 +169,6 @@ bool Encoder::init() { sm_config_set_fifo_join(&c, PIO_FIFO_JOIN_RX); sm_config_set_clkdiv_int_frac(&c, freq_divider, 0); - //float div = clock_get_hz(clk_sys) / 500000; - //sm_config_set_clkdiv(&c, div); pio_sm_init(pio, sm, pio_program_offset[pio_idx], &c); @@ -157,92 +205,97 @@ 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 - count_offset; + 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_snapshot_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() / counts_per_revolution; + return (float)count() / enc_counts_per_rev; } -float Encoder::angle_degrees() const { +float Encoder::degrees() const { return revolutions() * 360.0f; } -float Encoder::angle_radians() const { +float Encoder::radians() const { return revolutions() * M_TWOPI; } -float Encoder::frequency() const { - return clocks_per_time / (float)time_since; +Direction Encoder::direction() const { + return enc_direction; } -float Encoder::revolutions_per_second() const { - return frequency() / counts_per_revolution; +void Encoder::direction(Direction direction) { + enc_direction = direction; } -float Encoder::revolutions_per_minute() const { - return revolutions_per_second() * 60.0f; +float Encoder::counts_per_revolution() const { + return enc_counts_per_rev; } -float Encoder::degrees_per_second() const { - return revolutions_per_second() * 360.0f; +void Encoder::counts_per_revolution(float counts_per_rev) { + enc_counts_per_rev = MAX(counts_per_rev, FLT_EPSILON); } -float Encoder::radians_per_second() const { - return revolutions_per_second() * M_TWOPI; -} +Encoder::Snapshot Encoder::take_snapshot() { + // Take a snapshot of the current values + int32_t count = enc_count; + int32_t cumulative_time = enc_cumulative_time; + enc_cumulative_time = 0; -void Encoder::zero_count() { - count_offset = enc_count; -} - -Capture Encoder::perform_capture() { - // Capture the current values - int32_t captured_count = enc_count; - int32_t captured_cumulative_time = cumulative_time; - cumulative_time = 0; - - // Determine the change in counts since the last capture was performed - int32_t count_change = captured_count - last_captured_count; - last_captured_count = captured_count; + // Determine the change in counts since the last snapshot was taken + int32_t change = count - last_snapshot_count; + last_snapshot_count = count; // Calculate the average frequency of state transitions - float average_frequency = 0.0f; - if(count_change != 0 && captured_cumulative_time != INT_MAX) { - average_frequency = (clocks_per_time * (float)count_change) / (float)captured_cumulative_time; + float frequency = 0.0f; + if(change != 0 && cumulative_time != INT32_MAX) { + frequency = (clocks_per_time * (float)change) / (float)cumulative_time; } - return Capture(captured_count, count_change, average_frequency, counts_per_revolution); + return Snapshot(count, change, frequency, enc_counts_per_rev); } -void Encoder::microstep_up(int32_t time) { - enc_count++; - time_since = time; - microstep_time = 0; - - if(time + cumulative_time < time) //Check to avoid integer overflow - cumulative_time = INT_MAX; - else - cumulative_time += time; -} - -void Encoder::microstep_down(int32_t time) { - enc_count--; - time_since = 0 - time; - microstep_time = 0; - - if(time + cumulative_time < time) //Check to avoid integer overflow - cumulative_time = INT_MAX; - else - cumulative_time += time; -} - -void Encoder::check_for_transition() { +void Encoder::process_steps() { while(pio->ints1 & (PIO_IRQ1_INTS_SM0_RXNEMPTY_BITS << sm)) { uint32_t received = pio_sm_get(pio, sm); @@ -264,6 +317,8 @@ void Encoder::check_for_transition() { microstep_time = time_received; } + bool up = (enc_direction == NORMAL); + // Determine what transition occurred switch(LAST_STATE(states)) { //-------------------------------------------------- @@ -273,14 +328,14 @@ void Encoder::check_for_transition() { // B _________ case MICROSTEP_1: if(count_microsteps) - microstep_up(time_received); + microstep(time_received, up); break; // A _________ // B ____|‾‾‾‾ case MICROSTEP_3: if(count_microsteps) - microstep_down(time_received); + microstep(time_received, !up); break; } break; @@ -291,21 +346,21 @@ void Encoder::check_for_transition() { // A ‾‾‾‾‾‾‾‾‾ // B ____|‾‾‾‾ case MICROSTEP_2: - if(count_microsteps || last_travel_dir == CLOCKWISE) - microstep_up(time_received); + if(count_microsteps || step_dir == INCREASING) + microstep(time_received, up); - last_travel_dir = NO_DIR; // Finished turning clockwise + step_dir = NO_DIR; // Finished increasing break; // A ‾‾‾‾|____ // B _________ case MICROSTEP_0: if(count_microsteps) - microstep_down(time_received); + microstep(time_received, !up); break; } break; - + //-------------------------------------------------- case MICROSTEP_2: switch(CURR_STATE(states)) { @@ -313,18 +368,18 @@ void Encoder::check_for_transition() { // B ‾‾‾‾‾‾‾‾‾ case MICROSTEP_3: if(count_microsteps) - microstep_up(time_received); - - last_travel_dir = CLOCKWISE; // Started turning clockwise + microstep(time_received, up); + + step_dir = INCREASING; // Started increasing break; // A ‾‾‾‾‾‾‾‾‾ // B ‾‾‾‾|____ case MICROSTEP_1: if(count_microsteps) - microstep_down(time_received); - - last_travel_dir = COUNTERCLOCK; // Started turning counter-clockwise + microstep(time_received, !up); + + step_dir = DECREASING; // Started decreasing break; } break; @@ -336,20 +391,43 @@ void Encoder::check_for_transition() { // B ‾‾‾‾|____ case MICROSTEP_0: if(count_microsteps) - microstep_up(time_received); + microstep(time_received, up); break; // A ____|‾‾‾‾ // B ‾‾‾‾‾‾‾‾‾ case MICROSTEP_2: - if(count_microsteps || last_travel_dir == COUNTERCLOCK) - microstep_down(time_received); + if(count_microsteps || step_dir == DECREASING) + microstep(time_received, !up); - last_travel_dir = NO_DIR; // Finished turning counter-clockwise + 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; +} } \ No newline at end of file diff --git a/drivers/encoder-pio/encoder.hpp b/drivers/encoder-pio/encoder.hpp index ec353a62..eb8c06b9 100644 --- a/drivers/encoder-pio/encoder.hpp +++ b/drivers/encoder-pio/encoder.hpp @@ -2,10 +2,15 @@ #include "hardware/pio.h" #include "common/pimoroni_common.hpp" -#include "capture.hpp" +#include "snapshot.hpp" namespace pimoroni { + enum Direction { + NORMAL = 0, + REVERSED = 1, + }; + struct bool_pair { union { bool first; @@ -26,7 +31,7 @@ namespace pimoroni { //-------------------------------------------------- public: static constexpr float DEFAULT_COUNTS_PER_REV = 24; - static const uint16_t DEFAULT_COUNT_MICROSTEPS = false; + static const bool DEFAULT_COUNT_MICROSTEPS = false; static const uint16_t DEFAULT_FREQ_DIVIDER = 1; private: @@ -40,20 +45,65 @@ namespace pimoroni { static const uint32_t TIME_MASK = 0x0fffffff; - static const uint8_t MICROSTEP_0 = 0b00; - static const uint8_t MICROSTEP_1 = 0b10; - static const uint8_t MICROSTEP_2 = 0b11; - static const uint8_t MICROSTEP_3 = 0b01; - //-------------------------------------------------- // Enums //-------------------------------------------------- private: - enum Direction { - NO_DIR = 0, - CLOCKWISE = 1, - COUNTERCLOCK = -1, + 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 Snapshot { + //-------------------------------------------------- + // Variables + //-------------------------------------------------- + public: + const int32_t count; + const int32_t delta; + const float frequency; + private: + float counts_per_rev; + + + //-------------------------------------------------- + // Constructors + //-------------------------------------------------- + public: + Snapshot(); + Snapshot(int32_t count, int32_t delta, float frequency, float counts_per_rev); + + + //-------------------------------------------------- + // Methods + //-------------------------------------------------- + public: + 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; }; @@ -64,27 +114,30 @@ namespace pimoroni { PIO pio; uint sm; pin_pair enc_pins; - uint pin_c; + uint enc_common_pin; + Direction enc_direction; + float enc_counts_per_rev; - const float counts_per_revolution = DEFAULT_COUNTS_PER_REV; - const bool count_microsteps = DEFAULT_COUNT_MICROSTEPS; - const uint16_t freq_divider = DEFAULT_FREQ_DIVIDER; - const float clocks_per_time = 0; + 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 time_since = 0; - volatile Direction last_travel_dir = NO_DIR; - volatile int32_t microstep_time = 0; - volatile int32_t cumulative_time = 0; + 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; - int32_t count_offset = 0; - int32_t last_captured_count = 0; + volatile int32_t microstep_time = 0; + volatile StepDir step_dir = NO_DIR; - bool initialised = false; + int32_t last_count = 0; + int32_t last_snapshot_count = 0; + + bool initialised = false; //-------------------------------------------------- @@ -102,9 +155,8 @@ namespace pimoroni { // Constructors/Destructor //-------------------------------------------------- public: - Encoder() {} - Encoder(PIO pio, uint sm, const pin_pair &pins, uint pin_c = PIN_UNUSED, - float counts_per_revolution = DEFAULT_COUNTS_PER_REV, bool count_microsteps = DEFAULT_COUNT_MICROSTEPS, + Encoder(PIO pio, uint sm, const pin_pair &pins, uint common_pin = PIN_UNUSED, Direction direction = NORMAL, + float counts_per_rev = DEFAULT_COUNTS_PER_REV, bool count_microsteps = DEFAULT_COUNT_MICROSTEPS, uint16_t freq_divider = DEFAULT_FREQ_DIVIDER); ~Encoder(); @@ -117,26 +169,34 @@ namespace pimoroni { // 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 angle_degrees() const; - float angle_radians() const; + float degrees() const; + float radians() const; - float frequency() const; - float revolutions_per_second() const; - float revolutions_per_minute() const; - float degrees_per_second() const; - float radians_per_second() const; - - void zero_count(); - Capture perform_capture(); //TODO rename capture to snapshot + //-------------------------------------------------- + Direction direction() const; + void direction(Direction direction); + + float counts_per_revolution() const; + void counts_per_revolution(float counts_per_rev); + + Snapshot take_snapshot(); + + //-------------------------------------------------- private: - void microstep_up(int32_t time_since); - void microstep_down(int32_t time_since); - void check_for_transition(); + void process_steps(); + void microstep(int32_t time_since, bool up); }; } \ No newline at end of file diff --git a/examples/pico_explorer_encoder/demo.cpp b/examples/pico_explorer_encoder/demo.cpp index e51e8393..bdb2080d 100644 --- a/examples/pico_explorer_encoder/demo.cpp +++ b/examples/pico_explorer_encoder/demo.cpp @@ -54,7 +54,7 @@ enum DrawState { uint16_t buffer[PicoExplorer::WIDTH * PicoExplorer::HEIGHT]; PicoExplorer pico_explorer(buffer); -Encoder encoder(pio0, 0, ENCODER_PINS, ENCODER_PIN_C, COUNTS_PER_REVOLUTION, COUNT_MICROSTEPS, FREQ_DIVIDER); +Encoder encoder(pio0, 0, ENCODER_PINS, ENCODER_PIN_C, NORMAL, COUNTS_PER_REVOLUTION, COUNT_MICROSTEPS, FREQ_DIVIDER); volatile bool encA_readings[READINGS_SIZE]; volatile bool encB_readings[READINGS_SIZE]; @@ -226,11 +226,11 @@ int main() { // 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)) { - encoder.zero_count(); + encoder.zero(); } - // Take a capture, or snapshot of the current encoder state - Capture capture = encoder.perform_capture(); + // Take a snapshot of the current encoder state + Encoder::Snapshot snapshot = encoder.take_snapshot(); // 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)) { @@ -316,21 +316,21 @@ int main() { { std::stringstream sstream; - sstream << capture.count(); + sstream << snapshot.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"; + sstream << std::fixed << std::setprecision(1) << snapshot.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(); + sstream << std::fixed << std::setprecision(1) << snapshot.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); } diff --git a/micropython/modules/encoder/encoder.c b/micropython/modules/encoder/encoder.c index 13a2d766..2cd4bc1e 100644 --- a/micropython/modules/encoder/encoder.c +++ b/micropython/modules/encoder/encoder.c @@ -2,42 +2,85 @@ /***** Methods *****/ +MP_DEFINE_CONST_FUN_OBJ_1(Snapshot___del___obj, Snapshot___del__); +MP_DEFINE_CONST_FUN_OBJ_1(Snapshot_count_obj, Snapshot_count); +MP_DEFINE_CONST_FUN_OBJ_1(Snapshot_delta_obj, Snapshot_delta); +MP_DEFINE_CONST_FUN_OBJ_1(Snapshot_frequency_obj, Snapshot_frequency); +MP_DEFINE_CONST_FUN_OBJ_1(Snapshot_revolutions_obj, Snapshot_revolutions); +MP_DEFINE_CONST_FUN_OBJ_1(Snapshot_degrees_obj, Snapshot_degrees); +MP_DEFINE_CONST_FUN_OBJ_1(Snapshot_radians_obj, Snapshot_radians); +MP_DEFINE_CONST_FUN_OBJ_1(Snapshot_revolutions_delta_obj, Snapshot_revolutions_delta); +MP_DEFINE_CONST_FUN_OBJ_1(Snapshot_degrees_delta_obj, Snapshot_degrees_delta); +MP_DEFINE_CONST_FUN_OBJ_1(Snapshot_radians_delta_obj, Snapshot_radians_delta); +MP_DEFINE_CONST_FUN_OBJ_1(Snapshot_revolutions_per_second_obj, Snapshot_revolutions_per_second); +MP_DEFINE_CONST_FUN_OBJ_1(Snapshot_revolutions_per_minute_obj, Snapshot_revolutions_per_minute); +MP_DEFINE_CONST_FUN_OBJ_1(Snapshot_degrees_per_second_obj, Snapshot_degrees_per_second); +MP_DEFINE_CONST_FUN_OBJ_1(Snapshot_radians_per_second_obj, Snapshot_radians_per_second); + MP_DEFINE_CONST_FUN_OBJ_1(Encoder___del___obj, Encoder___del__); MP_DEFINE_CONST_FUN_OBJ_1(Encoder_pins_obj, Encoder_pins); +MP_DEFINE_CONST_FUN_OBJ_1(Encoder_common_pin_obj, Encoder_common_pin); MP_DEFINE_CONST_FUN_OBJ_1(Encoder_state_obj, Encoder_state); MP_DEFINE_CONST_FUN_OBJ_1(Encoder_count_obj, Encoder_count); +MP_DEFINE_CONST_FUN_OBJ_1(Encoder_delta_obj, Encoder_delta); +MP_DEFINE_CONST_FUN_OBJ_1(Encoder_zero_obj, Encoder_zero); +MP_DEFINE_CONST_FUN_OBJ_1(Encoder_step_obj, Encoder_step); +MP_DEFINE_CONST_FUN_OBJ_1(Encoder_turn_obj, Encoder_turn); MP_DEFINE_CONST_FUN_OBJ_1(Encoder_revolutions_obj, Encoder_revolutions); -MP_DEFINE_CONST_FUN_OBJ_1(Encoder_angle_degrees_obj, Encoder_angle_degrees); -MP_DEFINE_CONST_FUN_OBJ_1(Encoder_angle_radians_obj, Encoder_angle_radians); -MP_DEFINE_CONST_FUN_OBJ_1(Encoder_frequency_obj, Encoder_frequency); -MP_DEFINE_CONST_FUN_OBJ_1(Encoder_revolutions_per_second_obj, Encoder_revolutions_per_second); -MP_DEFINE_CONST_FUN_OBJ_1(Encoder_revolutions_per_minute_obj, Encoder_revolutions_per_minute); -MP_DEFINE_CONST_FUN_OBJ_1(Encoder_degrees_per_second_obj, Encoder_degrees_per_second); -MP_DEFINE_CONST_FUN_OBJ_1(Encoder_radians_per_second_obj, Encoder_radians_per_second); -MP_DEFINE_CONST_FUN_OBJ_1(Encoder_zero_count_obj, Encoder_zero_count); +MP_DEFINE_CONST_FUN_OBJ_1(Encoder_degrees_obj, Encoder_degrees); +MP_DEFINE_CONST_FUN_OBJ_1(Encoder_radians_obj, Encoder_radians); +MP_DEFINE_CONST_FUN_OBJ_KW(Encoder_direction_obj, 1, Encoder_direction); +MP_DEFINE_CONST_FUN_OBJ_KW(Encoder_counts_per_revolution_obj, 1, Encoder_counts_per_revolution); MP_DEFINE_CONST_FUN_OBJ_1(Encoder_take_snapshot_obj, Encoder_take_snapshot); /***** Binding of Methods *****/ +STATIC const mp_rom_map_elem_t Snapshot_locals_dict_table[] = { + { MP_ROM_QSTR(MP_QSTR___del__), MP_ROM_PTR(&Snapshot___del___obj) }, + { MP_ROM_QSTR(MP_QSTR_count), MP_ROM_PTR(&Snapshot_count_obj) }, + { MP_ROM_QSTR(MP_QSTR_delta), MP_ROM_PTR(&Snapshot_delta_obj) }, + { MP_ROM_QSTR(MP_QSTR_frequency), MP_ROM_PTR(&Snapshot_frequency_obj) }, + { MP_ROM_QSTR(MP_QSTR_revolutions), MP_ROM_PTR(&Snapshot_revolutions_obj) }, + { MP_ROM_QSTR(MP_QSTR_degrees), MP_ROM_PTR(&Snapshot_degrees_obj) }, + { MP_ROM_QSTR(MP_QSTR_radians), MP_ROM_PTR(&Snapshot_radians_obj) }, + { MP_ROM_QSTR(MP_QSTR_revolutions_delta), MP_ROM_PTR(&Snapshot_revolutions_delta_obj) }, + { MP_ROM_QSTR(MP_QSTR_degrees_delta), MP_ROM_PTR(&Snapshot_degrees_delta_obj) }, + { MP_ROM_QSTR(MP_QSTR_radians_delta), MP_ROM_PTR(&Snapshot_radians_delta_obj) }, + { MP_ROM_QSTR(MP_QSTR_revolutions_per_second), MP_ROM_PTR(&Snapshot_revolutions_per_second_obj) }, + { MP_ROM_QSTR(MP_QSTR_revolutions_per_minute), MP_ROM_PTR(&Snapshot_revolutions_per_minute_obj) }, + { MP_ROM_QSTR(MP_QSTR_degrees_per_second), MP_ROM_PTR(&Snapshot_degrees_per_second_obj) }, + { MP_ROM_QSTR(MP_QSTR_radians_per_second), MP_ROM_PTR(&Snapshot_radians_per_second_obj) }, +}; + STATIC const mp_rom_map_elem_t Encoder_locals_dict_table[] = { { MP_ROM_QSTR(MP_QSTR___del__), MP_ROM_PTR(&Encoder___del___obj) }, { MP_ROM_QSTR(MP_QSTR_pins), MP_ROM_PTR(&Encoder_pins_obj) }, + { MP_ROM_QSTR(MP_QSTR_common_pin), MP_ROM_PTR(&Encoder_common_pin_obj) }, { MP_ROM_QSTR(MP_QSTR_state), MP_ROM_PTR(&Encoder_state_obj) }, { MP_ROM_QSTR(MP_QSTR_count), MP_ROM_PTR(&Encoder_count_obj) }, + { MP_ROM_QSTR(MP_QSTR_delta), MP_ROM_PTR(&Encoder_delta_obj) }, + { MP_ROM_QSTR(MP_QSTR_zero), MP_ROM_PTR(&Encoder_zero_obj) }, + { MP_ROM_QSTR(MP_QSTR_step), MP_ROM_PTR(&Encoder_step_obj) }, + { MP_ROM_QSTR(MP_QSTR_turn), MP_ROM_PTR(&Encoder_turn_obj) }, { MP_ROM_QSTR(MP_QSTR_revolutions), MP_ROM_PTR(&Encoder_revolutions_obj) }, - { MP_ROM_QSTR(MP_QSTR_angle_degrees), MP_ROM_PTR(&Encoder_angle_degrees_obj) }, - { MP_ROM_QSTR(MP_QSTR_angle_radians), MP_ROM_PTR(&Encoder_angle_radians_obj) }, - { MP_ROM_QSTR(MP_QSTR_frequency), MP_ROM_PTR(&Encoder_frequency_obj) }, - { MP_ROM_QSTR(MP_QSTR_revolutions_per_second), MP_ROM_PTR(&Encoder_revolutions_per_second_obj) }, - { MP_ROM_QSTR(MP_QSTR_revolutions_per_minute), MP_ROM_PTR(&Encoder_revolutions_per_minute_obj) }, - { MP_ROM_QSTR(MP_QSTR_degrees_per_second), MP_ROM_PTR(&Encoder_degrees_per_second_obj) }, - { MP_ROM_QSTR(MP_QSTR_radians_per_second), MP_ROM_PTR(&Encoder_radians_per_second_obj) }, - { MP_ROM_QSTR(MP_QSTR_zero_count), MP_ROM_PTR(&Encoder_zero_count_obj) }, + { MP_ROM_QSTR(MP_QSTR_degrees), MP_ROM_PTR(&Encoder_degrees_obj) }, + { MP_ROM_QSTR(MP_QSTR_radians), MP_ROM_PTR(&Encoder_radians_obj) }, + { MP_ROM_QSTR(MP_QSTR_direction), MP_ROM_PTR(&Encoder_direction_obj) }, + { MP_ROM_QSTR(MP_QSTR_counts_per_revolution), MP_ROM_PTR(&Encoder_counts_per_revolution_obj) }, { MP_ROM_QSTR(MP_QSTR_take_snapshot), MP_ROM_PTR(&Encoder_take_snapshot_obj) }, }; +STATIC MP_DEFINE_CONST_DICT(Snapshot_locals_dict, Snapshot_locals_dict_table); STATIC MP_DEFINE_CONST_DICT(Encoder_locals_dict, Encoder_locals_dict_table); /***** Class Definition *****/ +const mp_obj_type_t Snapshot_type = { + { &mp_type_type }, + .name = MP_QSTR_snapshot, + .print = Snapshot_print, + .make_new = Snapshot_make_new, + .locals_dict = (mp_obj_dict_t*)&Snapshot_locals_dict, +}; + const mp_obj_type_t Encoder_type = { { &mp_type_type }, .name = MP_QSTR_encoder, @@ -50,10 +93,10 @@ const mp_obj_type_t Encoder_type = { STATIC const mp_map_elem_t encoder_globals_table[] = { { MP_OBJ_NEW_QSTR(MP_QSTR___name__), MP_OBJ_NEW_QSTR(MP_QSTR_encoder) }, { MP_OBJ_NEW_QSTR(MP_QSTR_Encoder), (mp_obj_t)&Encoder_type }, + { MP_OBJ_NEW_QSTR(MP_QSTR_Snapshot), (mp_obj_t)&Snapshot_type }, - { MP_ROM_QSTR(MP_QSTR_ANGULAR), MP_ROM_INT(0x00) }, - { MP_ROM_QSTR(MP_QSTR_LINEAR), MP_ROM_INT(0x01) }, - { MP_ROM_QSTR(MP_QSTR_CONTINUOUS), MP_ROM_INT(0x02) }, + { MP_ROM_QSTR(MP_QSTR_NORMAL), MP_ROM_INT(0x00) }, + { MP_ROM_QSTR(MP_QSTR_REVERSED), MP_ROM_INT(0x01) }, }; STATIC MP_DEFINE_CONST_DICT(mp_module_encoder_globals, encoder_globals_table); diff --git a/micropython/modules/encoder/encoder.cpp b/micropython/modules/encoder/encoder.cpp index 69a7da85..10f012d9 100644 --- a/micropython/modules/encoder/encoder.cpp +++ b/micropython/modules/encoder/encoder.cpp @@ -1,6 +1,6 @@ -#include "drivers/encoder-pio/capture.hpp" #include "drivers/encoder-pio/encoder.hpp" #include +#include #define MP_OBJ_TO_PTR2(o, t) ((t *)(uintptr_t)(o)) @@ -12,6 +12,112 @@ extern "C" { #include "py/builtin.h" +/********** Snapshot **********/ + +/***** Variables Struct *****/ +typedef struct _Snapshot_obj_t { + mp_obj_base_t base; + Encoder::Snapshot *snapshot; +} _Snapshot_obj_t; + + +/***** Print *****/ +void Snapshot_print(const mp_print_t *print, mp_obj_t self_in, mp_print_kind_t kind) { + (void)kind; //Unused input parameter + _Snapshot_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Snapshot_obj_t); + Encoder::Snapshot* snap = self->snapshot; + mp_print_str(print, "Snapshot(count = "); + mp_obj_print_helper(print, mp_obj_new_int(snap->count), PRINT_REPR); + mp_print_str(print, ", delta = "); + mp_obj_print_helper(print, mp_obj_new_int(snap->delta), PRINT_REPR); + mp_print_str(print, ", freq = "); + mp_obj_print_helper(print, mp_obj_new_float(snap->frequency), PRINT_REPR); + mp_print_str(print, ")"); +} + + +/***** Constructor *****/ +mp_obj_t Snapshot_make_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw, const mp_obj_t *all_args) { + mp_raise_TypeError("cannot create an instance of Snapshot. They can only be created by calling take_snapshot on an Encoder object"); + return mp_const_none; +} + + +/***** Destructor ******/ +mp_obj_t Snapshot___del__(mp_obj_t self_in) { + _Snapshot_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Snapshot_obj_t); + delete self->snapshot; + return mp_const_none; +} + + +/***** Methods *****/ +mp_obj_t Snapshot_count(mp_obj_t self_in) { + _Snapshot_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Snapshot_obj_t); + return mp_obj_new_int(self->snapshot->count); +} + +mp_obj_t Snapshot_delta(mp_obj_t self_in) { + _Snapshot_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Snapshot_obj_t); + return mp_obj_new_int(self->snapshot->delta); +} + +mp_obj_t Snapshot_frequency(mp_obj_t self_in) { + _Snapshot_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Snapshot_obj_t); + return mp_obj_new_float(self->snapshot->frequency); +} + +mp_obj_t Snapshot_revolutions(mp_obj_t self_in) { + _Snapshot_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Snapshot_obj_t); + return mp_obj_new_int(self->snapshot->revolutions()); +} + +mp_obj_t Snapshot_degrees(mp_obj_t self_in) { + _Snapshot_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Snapshot_obj_t); + return mp_obj_new_int(self->snapshot->degrees()); +} + +mp_obj_t Snapshot_radians(mp_obj_t self_in) { + _Snapshot_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Snapshot_obj_t); + return mp_obj_new_int(self->snapshot->radians()); +} + +mp_obj_t Snapshot_revolutions_delta(mp_obj_t self_in) { + _Snapshot_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Snapshot_obj_t); + return mp_obj_new_int(self->snapshot->revolutions_delta()); +} + +mp_obj_t Snapshot_degrees_delta(mp_obj_t self_in) { + _Snapshot_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Snapshot_obj_t); + return mp_obj_new_int(self->snapshot->degrees_delta()); +} + +mp_obj_t Snapshot_radians_delta(mp_obj_t self_in) { + _Snapshot_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Snapshot_obj_t); + return mp_obj_new_int(self->snapshot->radians_delta()); +} + +mp_obj_t Snapshot_revolutions_per_second(mp_obj_t self_in) { + _Snapshot_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Snapshot_obj_t); + return mp_obj_new_float(self->snapshot->revolutions_per_second()); +} + +mp_obj_t Snapshot_revolutions_per_minute(mp_obj_t self_in) { + _Snapshot_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Snapshot_obj_t); + return mp_obj_new_float(self->snapshot->revolutions_per_minute()); +} + +mp_obj_t Snapshot_degrees_per_second(mp_obj_t self_in) { + _Snapshot_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Snapshot_obj_t); + return mp_obj_new_float(self->snapshot->degrees_per_second()); +} + +mp_obj_t Snapshot_radians_per_second(mp_obj_t self_in) { + _Snapshot_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Snapshot_obj_t); + return mp_obj_new_float(self->snapshot->radians_per_second()); +} + + /********** Encoder **********/ /***** Variables Struct *****/ @@ -32,15 +138,17 @@ void Encoder_print(const mp_print_t *print, mp_obj_t self_in, mp_print_kind_t ki mp_obj_print_helper(print, mp_obj_new_int(pins.a), PRINT_REPR); mp_print_str(print, ", "); mp_obj_print_helper(print, mp_obj_new_int(pins.b), PRINT_REPR); - mp_print_str(print, "), enabled = "); - /*mp_obj_print_helper(print, self->encoder->is_enabled() ? mp_const_true : mp_const_false, PRINT_REPR); - mp_print_str(print, ", pulse = "); - mp_obj_print_helper(print, mp_obj_new_float(self->encoder->pulse()), PRINT_REPR); - mp_print_str(print, ", value = "); - mp_obj_print_helper(print, mp_obj_new_float(self->encoder->value()), PRINT_REPR); - mp_print_str(print, ", freq = "); - mp_obj_print_helper(print, mp_obj_new_float(self->encoder->frequency()), PRINT_REPR); -*/ + mp_print_str(print, ", "); + mp_obj_print_helper(print, mp_obj_new_int(self->encoder->common_pin()), PRINT_REPR); + mp_print_str(print, ")"); + + if(self->encoder->direction() == NORMAL) + mp_print_str(print, ", direction = NORMAL"); + else + mp_print_str(print, ", direction = REVERSED"); + + mp_print_str(print, ", counts_per_rev = "); + mp_obj_print_helper(print, mp_obj_new_float(self->encoder->counts_per_revolution()), PRINT_REPR); mp_print_str(print, ")"); } @@ -49,14 +157,14 @@ void Encoder_print(const mp_print_t *print, mp_obj_t self_in, mp_print_kind_t ki mp_obj_t Encoder_make_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw, const mp_obj_t *all_args) { _Encoder_obj_t *self = nullptr; - enum { ARG_pio, ARG_sm, ARG_pins, ARG_pin_c, ARG_calibration, ARG_freq }; + enum { ARG_pio, ARG_sm, ARG_pins, ARG_common_pin, ARG_count_microsteps, ARG_freq_divider }; static const mp_arg_t allowed_args[] = { { MP_QSTR_pio, MP_ARG_REQUIRED | MP_ARG_INT }, { MP_QSTR_sm, MP_ARG_REQUIRED | MP_ARG_INT }, { MP_QSTR_pins, MP_ARG_REQUIRED | MP_ARG_OBJ }, - { MP_QSTR_pin_c, MP_ARG_INT, {.u_int = PIN_UNUSED} }, - { MP_QSTR_calibration, MP_ARG_OBJ, {.u_obj = mp_const_none} }, - { MP_QSTR_freq, MP_ARG_OBJ, {.u_obj = mp_const_none} }, + { MP_QSTR_common_pin, MP_ARG_INT, {.u_int = PIN_UNUSED} }, + { MP_QSTR_count_microsteps, MP_ARG_BOOL, {.u_bool = false} }, + { MP_QSTR_freq_divider, MP_ARG_OBJ, {.u_obj = mp_const_none} }, }; // Parse args. @@ -101,33 +209,15 @@ mp_obj_t Encoder_make_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw, pins.a = (uint8_t)a; pins.b = (uint8_t)b; } -/* - encoder::Calibration *calib = nullptr; - encoder::CalibrationType calibration_type = encoder::CalibrationType::ANGULAR; - const mp_obj_t calib_object = args[ARG_calibration].u_obj; - if(calib_object != mp_const_none) { - if(mp_obj_is_int(calib_object)) { - int type = mp_obj_get_int(calib_object); - if(type < 0 || type >= 3) { - mp_raise_ValueError("type out of range. Expected ANGULAR (0), LINEAR (1) or CONTINUOUS (2)"); - } - calibration_type = (encoder::CalibrationType)type; - } - else if(mp_obj_is_type(calib_object, &Calibration_type)) { - calib = (MP_OBJ_TO_PTR2(calib_object, _Calibration_obj_t)->calibration); - } - else { - mp_raise_TypeError("cannot convert object to an integer or a Calibration class instance"); - } + + float freq_divider = Encoder::DEFAULT_FREQ_DIVIDER; + if(args[ARG_freq_divider].u_obj != mp_const_none) { + freq_divider = mp_obj_get_float(args[ARG_freq_divider].u_obj); } - float freq = encoder::EncoderState::DEFAULT_FREQUENCY; - if(args[ARG_freq].u_obj != mp_const_none) { - freq = mp_obj_get_float(args[ARG_freq].u_obj); - } -*/ + bool count_microsteps = args[ARG_count_microsteps].u_bool; - Encoder *encoder = new Encoder(pio, sm, pins, args[ARG_pin_c].u_int); + Encoder *encoder = new Encoder(pio, sm, pins, args[ARG_common_pin].u_int, NORMAL, Encoder::DEFAULT_COUNTS_PER_REV, count_microsteps, freq_divider); if(!encoder->init()) { delete encoder; mp_raise_msg(&mp_type_RuntimeError, "unable to allocate the hardware resources needed to initialise this Encoder. Try running `import gc` followed by `gc.collect()` before creating it"); @@ -160,6 +250,12 @@ extern mp_obj_t Encoder_pins(mp_obj_t self_in) { return mp_obj_new_tuple(2, tuple); } +extern mp_obj_t Encoder_common_pin(mp_obj_t self_in) { + _Encoder_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Encoder_obj_t); + return mp_obj_new_int(self->encoder->common_pin()); +} + + extern mp_obj_t Encoder_state(mp_obj_t self_in) { _Encoder_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Encoder_obj_t); bool_pair state = self->encoder->state(); @@ -175,55 +271,124 @@ extern mp_obj_t Encoder_count(mp_obj_t self_in) { return mp_obj_new_int(self->encoder->count()); } +extern mp_obj_t Encoder_delta(mp_obj_t self_in) { + _Encoder_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Encoder_obj_t); + return mp_obj_new_int(self->encoder->delta()); +} + +extern mp_obj_t Encoder_zero(mp_obj_t self_in) { + _Encoder_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Encoder_obj_t); + self->encoder->zero(); + return mp_const_none; +} + +extern mp_obj_t Encoder_step(mp_obj_t self_in) { + _Encoder_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Encoder_obj_t); + return mp_obj_new_int(self->encoder->step()); +} + +extern mp_obj_t Encoder_turn(mp_obj_t self_in) { + _Encoder_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Encoder_obj_t); + return mp_obj_new_int(self->encoder->turn()); +} + extern mp_obj_t Encoder_revolutions(mp_obj_t self_in) { _Encoder_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Encoder_obj_t); return mp_obj_new_float(self->encoder->revolutions()); } -extern mp_obj_t Encoder_angle_degrees(mp_obj_t self_in) { +extern mp_obj_t Encoder_degrees(mp_obj_t self_in) { _Encoder_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Encoder_obj_t); - return mp_obj_new_float(self->encoder->angle_degrees()); + return mp_obj_new_float(self->encoder->degrees()); } -extern mp_obj_t Encoder_angle_radians(mp_obj_t self_in) { +extern mp_obj_t Encoder_radians(mp_obj_t self_in) { _Encoder_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Encoder_obj_t); - return mp_obj_new_float(self->encoder->angle_radians()); + return mp_obj_new_float(self->encoder->radians()); } -extern mp_obj_t Encoder_frequency(mp_obj_t self_in) { - _Encoder_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Encoder_obj_t); - return mp_obj_new_float(self->encoder->frequency()); +extern mp_obj_t Encoder_direction(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { + if(n_args <= 1) { + enum { ARG_self }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _Encoder_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Encoder_obj_t); + + return mp_obj_new_int(self->encoder->direction()); + } + else { + enum { ARG_self, ARG_direction }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_direction, MP_ARG_REQUIRED | MP_ARG_INT }, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _Encoder_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Encoder_obj_t); + + int direction = args[ARG_direction].u_int; + if(direction < 0 || direction > 1) { + mp_raise_ValueError("direction out of range. Expected NORMAL (0) or REVERSED (1)"); + } + self->encoder->direction((Direction)direction); + return mp_const_none; + } } -extern mp_obj_t Encoder_revolutions_per_second(mp_obj_t self_in) { - _Encoder_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Encoder_obj_t); - return mp_obj_new_float(self->encoder->revolutions_per_second()); -} +extern mp_obj_t Encoder_counts_per_revolution(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { + if(n_args <= 1) { + enum { ARG_self }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + }; -extern mp_obj_t Encoder_revolutions_per_minute(mp_obj_t self_in) { - _Encoder_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Encoder_obj_t); - return mp_obj_new_float(self->encoder->revolutions_per_minute()); -} + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); -extern mp_obj_t Encoder_degrees_per_second(mp_obj_t self_in) { - _Encoder_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Encoder_obj_t); - return mp_obj_new_float(self->encoder->degrees_per_second()); -} + _Encoder_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Encoder_obj_t); -extern mp_obj_t Encoder_radians_per_second(mp_obj_t self_in) { - _Encoder_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Encoder_obj_t); - return mp_obj_new_float(self->encoder->radians_per_second()); -} + return mp_obj_new_float(self->encoder->counts_per_revolution()); + } + else { + enum { ARG_self, ARG_counts_per_rev }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_counts_per_rev, MP_ARG_REQUIRED | MP_ARG_OBJ }, + }; -extern mp_obj_t Encoder_zero_count(mp_obj_t self_in) { - _Encoder_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Encoder_obj_t); - self->encoder->zero_count(); - return mp_const_none; + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _Encoder_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Encoder_obj_t); + + float counts_per_rev = mp_obj_get_float(args[ARG_counts_per_rev].u_obj); + if(counts_per_rev < FLT_EPSILON) { + mp_raise_ValueError("counts_per_rev out of range. Expected greater than 0.0"); + } + self->encoder->counts_per_revolution(counts_per_rev); + return mp_const_none; + } } extern mp_obj_t Encoder_take_snapshot(mp_obj_t self_in) { - //_Encoder_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Encoder_obj_t); - //Capture capture = self->encoder->perform_capture(); - return mp_const_none; + _Encoder_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Encoder_obj_t); + + // Create a new MP Snapshot instance and assign a copy of the encoder's snapshot to it + _Snapshot_obj_t *snap = m_new_obj_with_finaliser(_Snapshot_obj_t); + snap->base.type = &Snapshot_type; + + snap->snapshot = new Encoder::Snapshot(self->encoder->take_snapshot()); + return MP_OBJ_FROM_PTR(snap); } } diff --git a/micropython/modules/encoder/encoder.h b/micropython/modules/encoder/encoder.h index e4ef8251..80bbc4d7 100644 --- a/micropython/modules/encoder/encoder.h +++ b/micropython/modules/encoder/encoder.h @@ -2,22 +2,41 @@ #include "py/runtime.h" /***** Extern of Class Definition *****/ +extern const mp_obj_type_t Snapshot_type; extern const mp_obj_type_t Encoder_type; /***** Extern of Class Methods *****/ +extern void Snapshot_print(const mp_print_t *print, mp_obj_t self_in, mp_print_kind_t kind); +extern mp_obj_t Snapshot_make_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw, const mp_obj_t *all_args); +extern mp_obj_t Snapshot___del__(mp_obj_t self_in); +extern mp_obj_t Snapshot_count(mp_obj_t self_in); +extern mp_obj_t Snapshot_delta(mp_obj_t self_in); +extern mp_obj_t Snapshot_frequency(mp_obj_t self_in); +extern mp_obj_t Snapshot_revolutions(mp_obj_t self_in); +extern mp_obj_t Snapshot_degrees(mp_obj_t self_in); +extern mp_obj_t Snapshot_radians(mp_obj_t self_in); +extern mp_obj_t Snapshot_revolutions_delta(mp_obj_t self_in); +extern mp_obj_t Snapshot_degrees_delta(mp_obj_t self_in); +extern mp_obj_t Snapshot_radians_delta(mp_obj_t self_in); +extern mp_obj_t Snapshot_revolutions_per_second(mp_obj_t self_in); +extern mp_obj_t Snapshot_revolutions_per_minute(mp_obj_t self_in); +extern mp_obj_t Snapshot_degrees_per_second(mp_obj_t self_in); +extern mp_obj_t Snapshot_radians_per_second(mp_obj_t self_in); + extern void Encoder_print(const mp_print_t *print, mp_obj_t self_in, mp_print_kind_t kind); extern mp_obj_t Encoder_make_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw, const mp_obj_t *all_args); extern mp_obj_t Encoder___del__(mp_obj_t self_in); extern mp_obj_t Encoder_pins(mp_obj_t self_in); +extern mp_obj_t Encoder_common_pin(mp_obj_t self_in); extern mp_obj_t Encoder_state(mp_obj_t self_in); extern mp_obj_t Encoder_count(mp_obj_t self_in); +extern mp_obj_t Encoder_delta(mp_obj_t self_in); +extern mp_obj_t Encoder_zero(mp_obj_t self_in); +extern mp_obj_t Encoder_step(mp_obj_t self_in); +extern mp_obj_t Encoder_turn(mp_obj_t self_in); extern mp_obj_t Encoder_revolutions(mp_obj_t self_in); -extern mp_obj_t Encoder_angle_degrees(mp_obj_t self_in); -extern mp_obj_t Encoder_angle_radians(mp_obj_t self_in); -extern mp_obj_t Encoder_frequency(mp_obj_t self_in); -extern mp_obj_t Encoder_revolutions_per_second(mp_obj_t self_in); -extern mp_obj_t Encoder_revolutions_per_minute(mp_obj_t self_in); -extern mp_obj_t Encoder_degrees_per_second(mp_obj_t self_in); -extern mp_obj_t Encoder_radians_per_second(mp_obj_t self_in); -extern mp_obj_t Encoder_zero_count(mp_obj_t self_in); +extern mp_obj_t Encoder_degrees(mp_obj_t self_in); +extern mp_obj_t Encoder_radians(mp_obj_t self_in); +extern mp_obj_t Encoder_direction(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); +extern mp_obj_t Encoder_counts_per_revolution(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); extern mp_obj_t Encoder_take_snapshot(mp_obj_t self_in); \ No newline at end of file diff --git a/micropython/modules/encoder/micropython.cmake b/micropython/modules/encoder/micropython.cmake index 2f4ed7bf..9f7204c3 100644 --- a/micropython/modules/encoder/micropython.cmake +++ b/micropython/modules/encoder/micropython.cmake @@ -5,7 +5,6 @@ add_library(usermod_${MOD_NAME} INTERFACE) target_sources(usermod_${MOD_NAME} INTERFACE ${CMAKE_CURRENT_LIST_DIR}/${MOD_NAME}.c ${CMAKE_CURRENT_LIST_DIR}/${MOD_NAME}.cpp - ${CMAKE_CURRENT_LIST_DIR}/../../../drivers/encoder-pio/capture.cpp ${CMAKE_CURRENT_LIST_DIR}/../../../drivers/encoder-pio/encoder.cpp ) pico_generate_pio_header(usermod_${MOD_NAME} ${CMAKE_CURRENT_LIST_DIR}/../../../drivers/encoder-pio/encoder.pio) From 0bbd07164d8e252c36064ac651b797ece482ffb5 Mon Sep 17 00:00:00 2001 From: ZodiusInfuser Date: Sun, 17 Apr 2022 16:52:16 +0100 Subject: [PATCH 19/60] Removed unneeded include --- drivers/encoder-pio/encoder.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/drivers/encoder-pio/encoder.hpp b/drivers/encoder-pio/encoder.hpp index eb8c06b9..eee2ccff 100644 --- a/drivers/encoder-pio/encoder.hpp +++ b/drivers/encoder-pio/encoder.hpp @@ -2,7 +2,6 @@ #include "hardware/pio.h" #include "common/pimoroni_common.hpp" -#include "snapshot.hpp" namespace pimoroni { From 9472a1f109169c6d72974cb3201218b80d05f245 Mon Sep 17 00:00:00 2001 From: ZodiusInfuser Date: Tue, 19 Apr 2022 15:44:31 +0100 Subject: [PATCH 20/60] Fix for hard lock when taking many snapshots --- drivers/encoder-pio/encoder.cpp | 22 ++++++++++---- drivers/encoder-pio/encoder.hpp | 11 ++++--- examples/pico_explorer_encoder/demo.cpp | 4 +-- micropython/modules/encoder/encoder.cpp | 40 ++++++++++++------------- 4 files changed, 45 insertions(+), 32 deletions(-) diff --git a/drivers/encoder-pio/encoder.cpp b/drivers/encoder-pio/encoder.cpp index 0f9873bf..653a534f 100644 --- a/drivers/encoder-pio/encoder.cpp +++ b/drivers/encoder-pio/encoder.cpp @@ -19,16 +19,28 @@ uint Encoder::pio_program_offset[] = { 0, 0 }; Encoder::Snapshot::Snapshot() -: count(0), delta(0), frequency(0.0f), counts_per_rev(INT32_MAX) { +: captured_count(0), captured_delta(0), captured_frequency(0.0f), counts_per_rev(INT32_MAX) { } Encoder::Snapshot::Snapshot(int32_t count, int32_t delta, float frequency, float counts_per_rev) -: count(count), delta(delta), frequency(frequency) +: 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::Snapshot::count() const { + return captured_count; +} + +int32_t Encoder::Snapshot::delta() const { + return captured_delta; +} + +float Encoder::Snapshot::frequency() const { + return captured_frequency; +} + float Encoder::Snapshot::revolutions() const { - return (float)count / counts_per_rev; + return (float)captured_count / counts_per_rev; } float Encoder::Snapshot::degrees() const { @@ -40,7 +52,7 @@ float Encoder::Snapshot::radians() const { } float Encoder::Snapshot::revolutions_delta() const { - return (float)delta / counts_per_rev; + return (float)captured_delta / counts_per_rev; } float Encoder::Snapshot::degrees_delta() const { @@ -52,7 +64,7 @@ float Encoder::Snapshot::radians_delta() const { } float Encoder::Snapshot::revolutions_per_second() const { - return frequency / counts_per_rev; + return captured_frequency / counts_per_rev; } float Encoder::Snapshot::revolutions_per_minute() const { diff --git a/drivers/encoder-pio/encoder.hpp b/drivers/encoder-pio/encoder.hpp index eee2ccff..daea37e0 100644 --- a/drivers/encoder-pio/encoder.hpp +++ b/drivers/encoder-pio/encoder.hpp @@ -71,11 +71,10 @@ namespace pimoroni { //-------------------------------------------------- // Variables //-------------------------------------------------- - public: - const int32_t count; - const int32_t delta; - const float frequency; private: + int32_t captured_count; + int32_t captured_delta; + float captured_frequency; float counts_per_rev; @@ -91,6 +90,10 @@ namespace pimoroni { // Methods //-------------------------------------------------- public: + int32_t count() const; + int32_t delta() const; + float frequency() const; + float revolutions() const; float degrees() const; float radians() const; diff --git a/examples/pico_explorer_encoder/demo.cpp b/examples/pico_explorer_encoder/demo.cpp index bdb2080d..26cc65fb 100644 --- a/examples/pico_explorer_encoder/demo.cpp +++ b/examples/pico_explorer_encoder/demo.cpp @@ -316,14 +316,14 @@ int main() { { std::stringstream sstream; - sstream << snapshot.count; + sstream << snapshot.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) << snapshot.frequency << "hz"; + sstream << std::fixed << std::setprecision(1) << snapshot.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); } diff --git a/micropython/modules/encoder/encoder.cpp b/micropython/modules/encoder/encoder.cpp index 10f012d9..88c63f80 100644 --- a/micropython/modules/encoder/encoder.cpp +++ b/micropython/modules/encoder/encoder.cpp @@ -17,7 +17,7 @@ extern "C" { /***** Variables Struct *****/ typedef struct _Snapshot_obj_t { mp_obj_base_t base; - Encoder::Snapshot *snapshot; + Encoder::Snapshot snapshot; } _Snapshot_obj_t; @@ -25,13 +25,13 @@ typedef struct _Snapshot_obj_t { void Snapshot_print(const mp_print_t *print, mp_obj_t self_in, mp_print_kind_t kind) { (void)kind; //Unused input parameter _Snapshot_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Snapshot_obj_t); - Encoder::Snapshot* snap = self->snapshot; + Encoder::Snapshot& snap = self->snapshot; mp_print_str(print, "Snapshot(count = "); - mp_obj_print_helper(print, mp_obj_new_int(snap->count), PRINT_REPR); + mp_obj_print_helper(print, mp_obj_new_int(snap.count()), PRINT_REPR); mp_print_str(print, ", delta = "); - mp_obj_print_helper(print, mp_obj_new_int(snap->delta), PRINT_REPR); + mp_obj_print_helper(print, mp_obj_new_int(snap.delta()), PRINT_REPR); mp_print_str(print, ", freq = "); - mp_obj_print_helper(print, mp_obj_new_float(snap->frequency), PRINT_REPR); + mp_obj_print_helper(print, mp_obj_new_float(snap.frequency()), PRINT_REPR); mp_print_str(print, ")"); } @@ -45,8 +45,6 @@ mp_obj_t Snapshot_make_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw /***** Destructor ******/ mp_obj_t Snapshot___del__(mp_obj_t self_in) { - _Snapshot_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Snapshot_obj_t); - delete self->snapshot; return mp_const_none; } @@ -54,67 +52,67 @@ mp_obj_t Snapshot___del__(mp_obj_t self_in) { /***** Methods *****/ mp_obj_t Snapshot_count(mp_obj_t self_in) { _Snapshot_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Snapshot_obj_t); - return mp_obj_new_int(self->snapshot->count); + return mp_obj_new_int(self->snapshot.count()); } mp_obj_t Snapshot_delta(mp_obj_t self_in) { _Snapshot_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Snapshot_obj_t); - return mp_obj_new_int(self->snapshot->delta); + return mp_obj_new_int(self->snapshot.delta()); } mp_obj_t Snapshot_frequency(mp_obj_t self_in) { _Snapshot_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Snapshot_obj_t); - return mp_obj_new_float(self->snapshot->frequency); + return mp_obj_new_float(self->snapshot.frequency()); } mp_obj_t Snapshot_revolutions(mp_obj_t self_in) { _Snapshot_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Snapshot_obj_t); - return mp_obj_new_int(self->snapshot->revolutions()); + return mp_obj_new_int(self->snapshot.revolutions()); } mp_obj_t Snapshot_degrees(mp_obj_t self_in) { _Snapshot_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Snapshot_obj_t); - return mp_obj_new_int(self->snapshot->degrees()); + return mp_obj_new_int(self->snapshot.degrees()); } mp_obj_t Snapshot_radians(mp_obj_t self_in) { _Snapshot_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Snapshot_obj_t); - return mp_obj_new_int(self->snapshot->radians()); + return mp_obj_new_int(self->snapshot.radians()); } mp_obj_t Snapshot_revolutions_delta(mp_obj_t self_in) { _Snapshot_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Snapshot_obj_t); - return mp_obj_new_int(self->snapshot->revolutions_delta()); + return mp_obj_new_int(self->snapshot.revolutions_delta()); } mp_obj_t Snapshot_degrees_delta(mp_obj_t self_in) { _Snapshot_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Snapshot_obj_t); - return mp_obj_new_int(self->snapshot->degrees_delta()); + return mp_obj_new_int(self->snapshot.degrees_delta()); } mp_obj_t Snapshot_radians_delta(mp_obj_t self_in) { _Snapshot_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Snapshot_obj_t); - return mp_obj_new_int(self->snapshot->radians_delta()); + return mp_obj_new_int(self->snapshot.radians_delta()); } mp_obj_t Snapshot_revolutions_per_second(mp_obj_t self_in) { _Snapshot_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Snapshot_obj_t); - return mp_obj_new_float(self->snapshot->revolutions_per_second()); + return mp_obj_new_float(self->snapshot.revolutions_per_second()); } mp_obj_t Snapshot_revolutions_per_minute(mp_obj_t self_in) { _Snapshot_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Snapshot_obj_t); - return mp_obj_new_float(self->snapshot->revolutions_per_minute()); + return mp_obj_new_float(self->snapshot.revolutions_per_minute()); } mp_obj_t Snapshot_degrees_per_second(mp_obj_t self_in) { _Snapshot_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Snapshot_obj_t); - return mp_obj_new_float(self->snapshot->degrees_per_second()); + return mp_obj_new_float(self->snapshot.degrees_per_second()); } mp_obj_t Snapshot_radians_per_second(mp_obj_t self_in) { _Snapshot_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Snapshot_obj_t); - return mp_obj_new_float(self->snapshot->radians_per_second()); + return mp_obj_new_float(self->snapshot.radians_per_second()); } @@ -388,7 +386,7 @@ extern mp_obj_t Encoder_take_snapshot(mp_obj_t self_in) { _Snapshot_obj_t *snap = m_new_obj_with_finaliser(_Snapshot_obj_t); snap->base.type = &Snapshot_type; - snap->snapshot = new Encoder::Snapshot(self->encoder->take_snapshot()); + snap->snapshot = self->encoder->take_snapshot(); return MP_OBJ_FROM_PTR(snap); } } From 114c83e04e7122f98780bd4d5ba53fe32b18153e Mon Sep 17 00:00:00 2001 From: ZodiusInfuser Date: Tue, 19 Apr 2022 20:22:35 +0100 Subject: [PATCH 21/60] More encoder MP work --- .../encoder/rotary_encoder_capture.py | 32 ++++++++++++ .../examples/encoder/rotary_encoder_delta.py | 32 ++++++++++++ .../examples/encoder/rotary_encoder_simple.py | 31 +++++++++++ .../examples/motor2040/motor_encoders.py | 40 +++++++++++++++ .../motor2040/motor_encoders_capture.py | 50 ++++++++++++++++++ micropython/modules/encoder/encoder.c | 2 + micropython/modules/encoder/encoder.cpp | 51 ++++++++++++++----- 7 files changed, 226 insertions(+), 12 deletions(-) create mode 100644 micropython/examples/encoder/rotary_encoder_capture.py create mode 100644 micropython/examples/encoder/rotary_encoder_delta.py create mode 100644 micropython/examples/encoder/rotary_encoder_simple.py create mode 100644 micropython/examples/motor2040/motor_encoders.py create mode 100644 micropython/examples/motor2040/motor_encoders_capture.py diff --git a/micropython/examples/encoder/rotary_encoder_capture.py b/micropython/examples/encoder/rotary_encoder_capture.py new file mode 100644 index 00000000..afbb3fc1 --- /dev/null +++ b/micropython/examples/encoder/rotary_encoder_capture.py @@ -0,0 +1,32 @@ +import gc +import time +from encoder import Encoder +# from encoder import REVERSED + +""" +An example of how to read a mechanical rotary encoder, only when it has turned +""" + +# 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) + +# Loop forever +while True: + capture = enc.take_snapshot() + + 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) + diff --git a/micropython/examples/encoder/rotary_encoder_delta.py b/micropython/examples/encoder/rotary_encoder_delta.py new file mode 100644 index 00000000..6e7430a5 --- /dev/null +++ b/micropython/examples/encoder/rotary_encoder_delta.py @@ -0,0 +1,32 @@ +import gc +from encoder import Encoder +# from encoder import REVERSED + +""" +An example of how to read a mechanical rotary encoder, only when it has turned +""" + +# 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) + +# 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()) \ No newline at end of file diff --git a/micropython/examples/encoder/rotary_encoder_simple.py b/micropython/examples/encoder/rotary_encoder_simple.py new file mode 100644 index 00000000..60056bb7 --- /dev/null +++ b/micropython/examples/encoder/rotary_encoder_simple.py @@ -0,0 +1,31 @@ +import gc +import time +from encoder import Encoder +# from encoder import REVERSED + +""" +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) + +# 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) diff --git a/micropython/examples/motor2040/motor_encoders.py b/micropython/examples/motor2040/motor_encoders.py new file mode 100644 index 00000000..bdcbef48 --- /dev/null +++ b/micropython/examples/motor2040/motor_encoders.py @@ -0,0 +1,40 @@ +import gc +import time +from pimoroni import Button +from encoder import Encoder +# from encoder import REVERSED +from motor import motor2040 + +""" +Demonstrates how to read the counts of multiple motor encoders. + +Press "Boot" to exit the program. +""" + +# 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 +enc_a = Encoder(0, 0, motor2040.ENCODER_A, count_microsteps=True) +enc_b = Encoder(0, 1, motor2040.ENCODER_B, count_microsteps=True) +enc_c = Encoder(0, 2, motor2040.ENCODER_C, count_microsteps=True) +enc_d = Encoder(0, 3, motor2040.ENCODER_D, count_microsteps=True) + +# Uncomment the below line (and the top import) to reverse the counting direction +# enc_a.direction(REVERSED) +# enc_b.direction(REVERSED) +# enc_c.direction(REVERSED) +# enc_d.direction(REVERSED) + +# Create the user button +user_sw = Button(motor2040.USER_SW) + +# Read the encoders until the user button is pressed +while user_sw.raw() is not True: + print("A =", enc_a.count(), end=", ") + print("B =", enc_b.count(), end=", ") + print("C =", enc_c.count(), end=", ") + print("D =", enc_d.count()) + + time.sleep(0.1) + diff --git a/micropython/examples/motor2040/motor_encoders_capture.py b/micropython/examples/motor2040/motor_encoders_capture.py new file mode 100644 index 00000000..8e1d4e3f --- /dev/null +++ b/micropython/examples/motor2040/motor_encoders_capture.py @@ -0,0 +1,50 @@ +import gc +import time +from pimoroni import Button +from encoder import Encoder, MMME_CPR +# from encoder import REVERSED +from motor import motor2040 + +""" +Demonstrates how to read the counts of multiple motor encoders. + +Press "Boot" to exit the program. +""" + +# Free up hardware resources ahead of creating a new Encoder +gc.collect() + +# The gear ratio of the motor the encoder is attached to. +GEAR_RATIO = 50 +CPR = MMME_CPR * GEAR_RATIO + + +# Create a list of motors +ENCODER_PINS = [ motor2040.ENCODER_A, motor2040.ENCODER_B, motor2040.ENCODER_C, motor2040.ENCODER_D ] +ENCODER_NAMES = [ "A", "B", "C", "D" ] +NUM_ENCODERS = len(ENCODER_PINS) +encoders = [Encoder(0, i, ENCODER_PINS[i], counts_per_rev=CPR, count_microsteps=True) for i in range(NUM_ENCODERS)] + +# Uncomment the below line (and the top import) to reverse the counting direction +# encoders[0].direction(REVERSED) +# encoders[1].direction(REVERSED) +# encoders[2].direction(REVERSED) +# encoders[3].direction(REVERSED) + +# Create the user button +user_sw = Button(motor2040.USER_SW) + +captures = [None] * NUM_ENCODERS + +# Read the encoders until the user button is pressed +while user_sw.raw() is not True: + # Capture the state of all encoders, at as close to the same moment as possible + for i in range(NUM_ENCODERS): + captures[i] = encoders[i].take_snapshot() + + # Print out the angle and speed of each encoder + for i in range(NUM_ENCODERS): + print(ENCODER_NAMES[i], "=", captures[i].degrees(), "|", captures[i].degrees_per_second(), end=", ") + + print() + time.sleep(0.1) diff --git a/micropython/modules/encoder/encoder.c b/micropython/modules/encoder/encoder.c index 2cd4bc1e..2cd19741 100644 --- a/micropython/modules/encoder/encoder.c +++ b/micropython/modules/encoder/encoder.c @@ -97,6 +97,8 @@ STATIC const mp_map_elem_t encoder_globals_table[] = { { MP_ROM_QSTR(MP_QSTR_NORMAL), MP_ROM_INT(0x00) }, { MP_ROM_QSTR(MP_QSTR_REVERSED), MP_ROM_INT(0x01) }, + { MP_ROM_QSTR(MP_QSTR_MMME_CPR), MP_ROM_INT(12) }, + { MP_ROM_QSTR(MP_QSTR_ROTARY_CPR), MP_ROM_INT(24) }, }; STATIC MP_DEFINE_CONST_DICT(mp_module_encoder_globals, encoder_globals_table); diff --git a/micropython/modules/encoder/encoder.cpp b/micropython/modules/encoder/encoder.cpp index 88c63f80..ee36d433 100644 --- a/micropython/modules/encoder/encoder.cpp +++ b/micropython/modules/encoder/encoder.cpp @@ -67,32 +67,32 @@ mp_obj_t Snapshot_frequency(mp_obj_t self_in) { mp_obj_t Snapshot_revolutions(mp_obj_t self_in) { _Snapshot_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Snapshot_obj_t); - return mp_obj_new_int(self->snapshot.revolutions()); + return mp_obj_new_float(self->snapshot.revolutions()); } mp_obj_t Snapshot_degrees(mp_obj_t self_in) { _Snapshot_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Snapshot_obj_t); - return mp_obj_new_int(self->snapshot.degrees()); + return mp_obj_new_float(self->snapshot.degrees()); } mp_obj_t Snapshot_radians(mp_obj_t self_in) { _Snapshot_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Snapshot_obj_t); - return mp_obj_new_int(self->snapshot.radians()); + return mp_obj_new_float(self->snapshot.radians()); } mp_obj_t Snapshot_revolutions_delta(mp_obj_t self_in) { _Snapshot_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Snapshot_obj_t); - return mp_obj_new_int(self->snapshot.revolutions_delta()); + return mp_obj_new_float(self->snapshot.revolutions_delta()); } mp_obj_t Snapshot_degrees_delta(mp_obj_t self_in) { _Snapshot_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Snapshot_obj_t); - return mp_obj_new_int(self->snapshot.degrees_delta()); + return mp_obj_new_float(self->snapshot.degrees_delta()); } mp_obj_t Snapshot_radians_delta(mp_obj_t self_in) { _Snapshot_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Snapshot_obj_t); - return mp_obj_new_int(self->snapshot.radians_delta()); + return mp_obj_new_float(self->snapshot.radians_delta()); } mp_obj_t Snapshot_revolutions_per_second(mp_obj_t self_in) { @@ -137,7 +137,11 @@ void Encoder_print(const mp_print_t *print, mp_obj_t self_in, mp_print_kind_t ki mp_print_str(print, ", "); mp_obj_print_helper(print, mp_obj_new_int(pins.b), PRINT_REPR); mp_print_str(print, ", "); - mp_obj_print_helper(print, mp_obj_new_int(self->encoder->common_pin()), PRINT_REPR); + uint common_pin = self->encoder->common_pin(); + if(common_pin == PIN_UNUSED) + mp_print_str(print, "PIN_UNUSED"); + else + mp_obj_print_helper(print, mp_obj_new_int(self->encoder->common_pin()), PRINT_REPR); mp_print_str(print, ")"); if(self->encoder->direction() == NORMAL) @@ -155,12 +159,14 @@ void Encoder_print(const mp_print_t *print, mp_obj_t self_in, mp_print_kind_t ki mp_obj_t Encoder_make_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw, const mp_obj_t *all_args) { _Encoder_obj_t *self = nullptr; - enum { ARG_pio, ARG_sm, ARG_pins, ARG_common_pin, ARG_count_microsteps, ARG_freq_divider }; + enum { ARG_pio, ARG_sm, ARG_pins, ARG_common_pin, ARG_direction, ARG_counts_per_rev, ARG_count_microsteps, ARG_freq_divider }; static const mp_arg_t allowed_args[] = { { MP_QSTR_pio, MP_ARG_REQUIRED | MP_ARG_INT }, { MP_QSTR_sm, MP_ARG_REQUIRED | MP_ARG_INT }, { MP_QSTR_pins, MP_ARG_REQUIRED | MP_ARG_OBJ }, { MP_QSTR_common_pin, MP_ARG_INT, {.u_int = PIN_UNUSED} }, + { MP_QSTR_direction, MP_ARG_INT, {.u_int = NORMAL} }, + { MP_QSTR_counts_per_rev, MP_ARG_OBJ, {.u_obj = mp_const_none} }, { MP_QSTR_count_microsteps, MP_ARG_BOOL, {.u_bool = false} }, { MP_QSTR_freq_divider, MP_ARG_OBJ, {.u_obj = mp_const_none} }, }; @@ -169,8 +175,16 @@ mp_obj_t Encoder_make_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw, mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; mp_arg_parse_all_kw_array(n_args, n_kw, all_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); - PIO pio = args[ARG_pio].u_int == 0 ? pio0 : pio1; + int pio_int = args[ARG_pio].u_int; + if(pio_int < 0 || pio_int > (int)NUM_PIOS) { + mp_raise_ValueError("pio out of range. Expected 0 to 1"); + } + PIO pio = pio_int ? pio0 : pio1; + int sm = args[ARG_sm].u_int; + if(sm < 0 || sm > (int)NUM_PIO_STATE_MACHINES) { + mp_raise_ValueError("sm out of range. Expected 0 to 3"); + } size_t pin_count = 0; pin_pair pins; @@ -208,14 +222,27 @@ mp_obj_t Encoder_make_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw, pins.b = (uint8_t)b; } + int direction = args[ARG_direction].u_int; + if(direction < 0 || direction > 1) { + mp_raise_ValueError("direction out of range. Expected NORMAL (0) or REVERSED (1)"); + } + + float counts_per_rev = Encoder::DEFAULT_COUNTS_PER_REV; + if(args[ARG_counts_per_rev].u_obj != mp_const_none) { + counts_per_rev = mp_obj_get_float(args[ARG_counts_per_rev].u_obj); + if(counts_per_rev < FLT_EPSILON) { + mp_raise_ValueError("counts_per_rev out of range. Expected greater than 0.0"); + } + } + + bool count_microsteps = args[ARG_count_microsteps].u_bool; + float freq_divider = Encoder::DEFAULT_FREQ_DIVIDER; if(args[ARG_freq_divider].u_obj != mp_const_none) { freq_divider = mp_obj_get_float(args[ARG_freq_divider].u_obj); } - bool count_microsteps = args[ARG_count_microsteps].u_bool; - - Encoder *encoder = new Encoder(pio, sm, pins, args[ARG_common_pin].u_int, NORMAL, Encoder::DEFAULT_COUNTS_PER_REV, count_microsteps, freq_divider); + Encoder *encoder = new Encoder(pio, sm, pins, args[ARG_common_pin].u_int, (Direction)direction, counts_per_rev, count_microsteps, freq_divider); if(!encoder->init()) { delete encoder; mp_raise_msg(&mp_type_RuntimeError, "unable to allocate the hardware resources needed to initialise this Encoder. Try running `import gc` followed by `gc.collect()` before creating it"); From ae191697f2f2095ce23553f5b5b71e1eb25b4d36 Mon Sep 17 00:00:00 2001 From: ZodiusInfuser Date: Wed, 20 Apr 2022 14:26:32 +0100 Subject: [PATCH 22/60] Added a motor profiler example and finished constructor args --- .../examples/motor2040/motor_profiler.py | 89 +++++++++++++ micropython/modules/motor/motor.cpp | 125 ++++++++++-------- 2 files changed, 162 insertions(+), 52 deletions(-) create mode 100644 micropython/examples/motor2040/motor_profiler.py diff --git a/micropython/examples/motor2040/motor_profiler.py b/micropython/examples/motor2040/motor_profiler.py new file mode 100644 index 00000000..28cc875d --- /dev/null +++ b/micropython/examples/motor2040/motor_profiler.py @@ -0,0 +1,89 @@ +import gc +import time +from motor import Motor, motor2040 +from encoder import Encoder, MMME_CPR +from encoder import REVERSED + +""" +A program that profiles the speed of a motor across its PWM +duty cycle range using the attached encoder for feedback. +""" + +MOTOR_PINS = motor2040.MOTOR_A # The pins of the motor being profiled +ENCODER_PINS = motor2040.ENCODER_A # The pins of the encoder attached to the profiled motor +GEAR_RATIO = 50 # The gear ratio of the motor +COUNTS_PER_REV = MMME_CPR * GEAR_RATIO # The counts per revolution of the motor's output shaft + +SPEED_SCALE = 5.4 # The scaling to apply to the motor's speed + # Set this to the maximum measured speed + +DUTY_STEPS = 100 # How many duty cycle steps to sample the speed of +SETTLE_TIME = 0.1 # How long to wait after changing motor duty cycle +CAPTURE_TIME = 0.2 # How long to capture the motor's speed at each step + +# Free up hardware resources ahead of creating a new Encoder +gc.collect() + +# Create a motor and set its speed scale, and give it a zero deadzone +m = Motor(MOTOR_PINS, speed_scale=SPEED_SCALE, deadzone=0.0) + +# Create an encoder, using PIO 0 and State Machine 0 +enc = Encoder(0, 0, ENCODER_PINS, counts_per_rev=COUNTS_PER_REV, count_microsteps=True) + +# Uncomment the below line (and the top import) to reverse the counting direction +enc.direction(REVERSED) + + +# Function that performs a single profiling step +def profile_at_duty(duty): + # Set the motor to a new duty cycle and wait for it to settle + m.duty(duty) + time.sleep(SETTLE_TIME) + + # Perform a dummy capture to clear the encoder + enc.take_snapshot() + + # Wait for the capture time to pass + time.sleep(CAPTURE_TIME) + + # Perform a capture and read the measured speed + capture = enc.take_snapshot() + measured_speed = capture.revolutions_per_second() + + # These are some alternate speed measurements from the encoder + # measured_speed = capture.revolutions_per_minute() + # measured_speed = capture.degrees_per_second() + # measured_speed = capture.radians_per_second() + + # Print out the expected and measured speeds, as well as their difference + print("Duty = ", m.duty(), ", Expected = ", m.speed(), ", Measured = ", measured_speed, ", Diff = ", m.speed() - measured_speed, sep="") + + +# Enable the motor to get started +m.enable() + +print("Profiler Starting...") + +# Profile from 0% up to one step below 100% +for i in range(DUTY_STEPS): + profile_at_duty(i / DUTY_STEPS) + +# Profile from 100% down to one step above 0% +for i in range(DUTY_STEPS): + profile_at_duty((DUTY_STEPS - i) / DUTY_STEPS) + +# Profile from 0% down to one step above -100% +for i in range(DUTY_STEPS): + profile_at_duty(-i / DUTY_STEPS) + +# Profile from -100% up to one step below 0% +for i in range(DUTY_STEPS): + profile_at_duty(-(DUTY_STEPS - i) / DUTY_STEPS) + +# Profile 0% again +profile_at_duty(0) + +print("Profiler Finished...") + +# Disable the motor now the profiler has finished +m.disable() diff --git a/micropython/modules/motor/motor.cpp b/micropython/modules/motor/motor.cpp index 27f3f16e..6a3d3bc9 100644 --- a/micropython/modules/motor/motor.cpp +++ b/micropython/modules/motor/motor.cpp @@ -63,13 +63,15 @@ void Motor_print(const mp_print_t *print, mp_obj_t self_in, mp_print_kind_t kind mp_obj_t Motor_make_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw, const mp_obj_t *all_args) { _Motor_obj_t *self = nullptr; - enum { ARG_pins, ARG_calibration, ARG_freq }; + enum { ARG_pins, ARG_direction, ARG_speed_scale, ARG_deadzone, ARG_freq, ARG_mode }; static const mp_arg_t allowed_args[] = { { MP_QSTR_pins, MP_ARG_REQUIRED | MP_ARG_OBJ }, - { MP_QSTR_calibration, MP_ARG_OBJ, {.u_obj = mp_const_none} }, + { MP_QSTR_direction, MP_ARG_INT, {.u_int = NORMAL} }, + { MP_QSTR_speed_scale, MP_ARG_OBJ, {.u_obj = mp_const_none} }, + { MP_QSTR_deadzone, MP_ARG_OBJ, {.u_obj = mp_const_none} }, { MP_QSTR_freq, MP_ARG_OBJ, {.u_obj = mp_const_none} }, + { MP_QSTR_mode, MP_ARG_INT, {.u_int = MotorState::DEFAULT_DECAY_MODE} }, }; - //TODO ^ // Parse args. mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; @@ -111,37 +113,41 @@ mp_obj_t Motor_make_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw, c pins.negative = (uint8_t)neg; } - //motor::Calibration *calib = nullptr; - //motor::CalibrationType calibration_type = motor::CalibrationType::ANGULAR; - const mp_obj_t calib_object = args[ARG_calibration].u_obj; - if(calib_object != mp_const_none) { - /*if(mp_obj_is_int(calib_object)) { - int type = mp_obj_get_int(calib_object); - if(type < 0 || type >= 3) { - mp_raise_ValueError("type out of range. Expected ANGULAR (0), LINEAR (1) or CONTINUOUS (2)"); - } - calibration_type = (motor::CalibrationType)type; - } - else if(mp_obj_is_type(calib_object, &Calibration_type)) { - calib = (MP_OBJ_TO_PTR2(calib_object, _Calibration_obj_t)->calibration); - } - else { - mp_raise_TypeError("cannot convert object to an integer or a Calibration class instance"); - }*/ + int direction = args[ARG_direction].u_int; + if(direction < 0 || direction > 1) { + mp_raise_ValueError("direction out of range. Expected NORMAL (0) or REVERSED (1)"); } - /*float freq = motor::MotorState::DEFAULT_FREQUENCY; + float speed_scale = MotorState::DEFAULT_SPEED_SCALE; + if(args[ARG_speed_scale].u_obj != mp_const_none) { + speed_scale = mp_obj_get_float(args[ARG_speed_scale].u_obj); + if(speed_scale < FLT_EPSILON) { + mp_raise_ValueError("speed_scale out of range. Expected greater than 0.0"); + } + } + + float deadzone = MotorState::DEFAULT_DEADZONE; + if(args[ARG_deadzone].u_obj != mp_const_none) { + deadzone = mp_obj_get_float(args[ARG_deadzone].u_obj); + if(deadzone < 0.0f || deadzone > 1.0f) { + mp_raise_ValueError("deadzone out of range. Expected 0.0 to 1.0"); + } + } + + float freq = MotorState::DEFAULT_FREQUENCY; if(args[ARG_freq].u_obj != mp_const_none) { freq = mp_obj_get_float(args[ARG_freq].u_obj); - }*/ + } + + int mode = args[ARG_mode].u_int; + if(mode < 0 || mode > 1) { + mp_raise_ValueError("mode out of range. Expected FAST_DECAY (0) or SLOW_DECAY (1)"); + } self = m_new_obj_with_finaliser(_Motor_obj_t); self->base.type = &Motor_type; - //if(calib != nullptr) - // self->motor = new Motor(pin, *calib, freq); - //else - self->motor = new Motor2(pins);//TODO, calibration_type, freq); + self->motor = new Motor2(pins, (Direction)direction, speed_scale, deadzone, freq, (DecayMode)mode); self->motor->init(); return MP_OBJ_FROM_PTR(self); @@ -591,23 +597,33 @@ void MotorCluster_print(const mp_print_t *print, mp_obj_t self_in, mp_print_kind mp_obj_t MotorCluster_make_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw, const mp_obj_t *all_args) { _MotorCluster_obj_t *self = nullptr; - enum { ARG_pio, ARG_sm, ARG_pins, ARG_calibration, ARG_freq, ARG_auto_phase }; + enum { ARG_pio, ARG_sm, ARG_pins, ARG_direction, ARG_speed_scale, ARG_deadzone, ARG_freq, ARG_mode, ARG_auto_phase }; static const mp_arg_t allowed_args[] = { { MP_QSTR_pio, MP_ARG_REQUIRED | MP_ARG_INT }, { MP_QSTR_sm, MP_ARG_REQUIRED | MP_ARG_INT }, { MP_QSTR_pins, MP_ARG_REQUIRED | MP_ARG_OBJ }, - { MP_QSTR_calibration, MP_ARG_OBJ, {.u_obj = mp_const_none} }, + { MP_QSTR_direction, MP_ARG_INT, {.u_int = NORMAL} }, + { MP_QSTR_speed_scale, MP_ARG_OBJ, {.u_obj = mp_const_none} }, + { MP_QSTR_deadzone, MP_ARG_OBJ, {.u_obj = mp_const_none} }, { MP_QSTR_freq, MP_ARG_OBJ, {.u_obj = mp_const_none} }, + { MP_QSTR_mode, MP_ARG_INT, {.u_int = MotorState::DEFAULT_DECAY_MODE} }, { MP_QSTR_auto_phase, MP_ARG_BOOL, {.u_bool = true} }, }; - //TODO ^ // Parse args. mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; mp_arg_parse_all_kw_array(n_args, n_kw, all_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); - PIO pio = args[ARG_pio].u_int == 0 ? pio0 : pio1; + int pio_int = args[ARG_pio].u_int; + if(pio_int < 0 || pio_int > (int)NUM_PIOS) { + mp_raise_ValueError("pio out of range. Expected 0 to 1"); + } + PIO pio = pio_int ? pio0 : pio1; + int sm = args[ARG_sm].u_int; + if(sm < 0 || sm > (int)NUM_PIO_STATE_MACHINES) { + mp_raise_ValueError("sm out of range. Expected 0 to 3"); + } size_t pair_count = 0; pin_pair *pins = nullptr; @@ -685,39 +701,44 @@ mp_obj_t MotorCluster_make_new(const mp_obj_type_t *type, size_t n_args, size_t } } - //motor::Calibration *calib = nullptr; - //motor::CalibrationType calibration_type = motor::CalibrationType::ANGULAR; - const mp_obj_t calib_object = args[ARG_calibration].u_obj; - if(calib_object != mp_const_none) { - /*if(mp_obj_is_int(calib_object)) { - int type = mp_obj_get_int(calib_object); - if(type < 0 || type >= 3) { - mp_raise_ValueError("type out of range. Expected ANGULAR (0), LINEAR (1) or CONTINUOUS (2)"); - } - calibration_type = (motor::CalibrationType)mp_obj_get_int(calib_object); - } - else if(mp_obj_is_type(calib_object, &Calibration_type)) { - calib = (MP_OBJ_TO_PTR2(calib_object, _Calibration_obj_t)->calibration); - } - else { - mp_raise_TypeError("cannot convert object to an integer or a Calibration class instance"); - }*/ + int direction = args[ARG_direction].u_int; + if(direction < 0 || direction > 1) { + mp_raise_ValueError("direction out of range. Expected NORMAL (0) or REVERSED (1)"); } - float freq = motor::MotorState::DEFAULT_FREQUENCY; + float speed_scale = MotorState::DEFAULT_SPEED_SCALE; + if(args[ARG_speed_scale].u_obj != mp_const_none) { + speed_scale = mp_obj_get_float(args[ARG_speed_scale].u_obj); + if(speed_scale < FLT_EPSILON) { + mp_raise_ValueError("speed_scale out of range. Expected greater than 0.0"); + } + } + + float deadzone = MotorState::DEFAULT_DEADZONE; + if(args[ARG_deadzone].u_obj != mp_const_none) { + deadzone = mp_obj_get_float(args[ARG_deadzone].u_obj); + if(deadzone < 0.0f || deadzone > 1.0f) { + mp_raise_ValueError("deadzone out of range. Expected 0.0 to 1.0"); + } + } + + float freq = MotorState::DEFAULT_FREQUENCY; if(args[ARG_freq].u_obj != mp_const_none) { freq = mp_obj_get_float(args[ARG_freq].u_obj); } + int mode = args[ARG_mode].u_int; + if(mode < 0 || mode > 1) { + mp_raise_ValueError("mode out of range. Expected FAST_DECAY (0) or SLOW_DECAY (1)"); + } + bool auto_phase = args[ARG_auto_phase].u_bool; MotorCluster *cluster; PWMCluster::Sequence *seq_buffer = m_new(PWMCluster::Sequence, PWMCluster::NUM_BUFFERS * 2); PWMCluster::TransitionData *dat_buffer = m_new(PWMCluster::TransitionData, PWMCluster::BUFFER_SIZE * 2); - //if(calib != nullptr) - //cluster = new MotorCluster(pio, sm, pins, pair_count, *calib, freq, auto_phase, seq_buffer, dat_buffer); - //else - cluster = new MotorCluster(pio, sm, pins, pair_count, NORMAL, 1.0f, 0.0f, freq, SLOW_DECAY, auto_phase, seq_buffer, dat_buffer); + cluster = new MotorCluster(pio, sm, pins, pair_count, (Direction)direction, speed_scale, deadzone, + freq, (DecayMode)mode, auto_phase, seq_buffer, dat_buffer); // Cleanup the pins array if(pins != nullptr) From 8ef0d33f0cb4795931f8a5837ac42f8007010c5f Mon Sep 17 00:00:00 2001 From: ZodiusInfuser Date: Wed, 20 Apr 2022 18:55:39 +0100 Subject: [PATCH 23/60] Finalised more examples --- drivers/motor/motor_state.cpp | 4 +- libraries/motor2040/motor2040.hpp | 11 ++-- .../examples/motor2040/motor_cluster.py | 56 ++++++++++--------- .../examples/motor2040/motor_profiler.py | 14 ++--- micropython/examples/motor2040/motor_wave.py | 22 ++++---- .../examples/motor2040/multiple_motors.py | 32 ++++++----- .../examples/motor2040/single_motor.py | 33 ++++++----- .../examples/motor2040/turn_off_motors.py | 8 +-- micropython/modules/motor/motor.c | 8 +-- 9 files changed, 102 insertions(+), 86 deletions(-) diff --git a/drivers/motor/motor_state.cpp b/drivers/motor/motor_state.cpp index 6ae19373..ce66db91 100644 --- a/drivers/motor/motor_state.cpp +++ b/drivers/motor/motor_state.cpp @@ -62,7 +62,7 @@ namespace motor { speed = 0.0f - speed; // Clamp the speed between the hard limits - motor_speed = CLAMP(speed, -motor_scale, motor_scale); + motor_speed = CLAMP(speed, 0.0f - motor_scale, motor_scale); last_enabled_duty = motor_speed / motor_scale; return enable_with_return(); @@ -81,7 +81,7 @@ namespace motor { } 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_speed, motor_speed); + float speed = MotorState::map_float(in, in_min, in_max, 0.0f - motor_scale, motor_scale); return set_speed_with_return(speed); } diff --git a/libraries/motor2040/motor2040.hpp b/libraries/motor2040/motor2040.hpp index 17dc22d4..acb8f17a 100644 --- a/libraries/motor2040/motor2040.hpp +++ b/libraries/motor2040/motor2040.hpp @@ -31,10 +31,13 @@ namespace motor { const uint ENCODER_D_A = 14; const uint ENCODER_D_B = 15; - const pin_pair ENCODER_A(ENCODER_A_A, ENCODER_A_B); - const pin_pair ENCODER_B(ENCODER_B_A, ENCODER_B_B); - const pin_pair ENCODER_C(ENCODER_C_A, ENCODER_C_B); - const pin_pair ENCODER_D(ENCODER_D_A, ENCODER_D_B); + // 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; diff --git a/micropython/examples/motor2040/motor_cluster.py b/micropython/examples/motor2040/motor_cluster.py index 400e364d..b06a8685 100644 --- a/micropython/examples/motor2040/motor_cluster.py +++ b/micropython/examples/motor2040/motor_cluster.py @@ -1,60 +1,64 @@ import gc import time import math -from servo import ServoCluster, servo2040 +from motor import MotorCluster, motor2040 """ -Demonstrates how to create a ServoCluster object to control multiple servos at once. +Demonstrates how to create a MotorCluster object to control multiple motors at once. -NOTE: ServoCluster uses the RP2040's PIO system, and as +NOTE: MotorCluster uses the RP2040's PIO system, and as such may have problems when running code multiple times. If you encounter issues, try resetting your board. """ -# Free up hardware resources ahead of creating a new ServoCluster +# Free up hardware resources ahead of creating a new MotorCluster gc.collect() -# Create a servo cluster for pins 0 to 3, using PIO 0 and State Machine 0 -START_PIN = servo2040.SERVO_1 -END_PIN = servo2040.SERVO_4 -servos = ServoCluster(pio=0, sm=0, pins=list(range(START_PIN, END_PIN + 1))) +# Create a motor cluster, using PIO 0 and State Machine 0 +MOTOR_PINS = [motor2040.MOTOR_A, motor2040.MOTOR_B, motor2040.MOTOR_C, motor2040.MOTOR_D] +motors = MotorCluster(pio=0, sm=0, pins=MOTOR_PINS) -# Enable all servos (this puts them at the middle) -servos.enable_all() +# Enable all motors +motors.enable_all() time.sleep(2) -# Go to min -servos.all_to_min() +# Drive at full positive +motors.all_full_positive() time.sleep(2) -# Go to max -servos.all_to_max() +# Stop all moving +motors.stop_all() time.sleep(2) -# Go back to mid -servos.all_to_mid() +# Drive at full negative +motors.all_full_negative() time.sleep(2) -SWEEPS = 3 # How many sweeps of the servo to perform +# Coast all to a gradual stop +motors.coast_all() +time.sleep(2) + + +SWEEPS = 2 # How many sweeps of the motors to perform STEPS = 10 # The number of discrete sweep steps STEPS_INTERVAL = 0.5 # The time in seconds between each step of the sequence -SWEEP_EXTENT = 90.0 # How far from zero to move the servos when sweeping +SPEED_EXTENT = 1.0 # How far from zero to drive the motors when sweeping -# Do a sine sweep +# Do a sine speed sweep for j in range(SWEEPS): for i in range(360): - value = math.sin(math.radians(i)) * SWEEP_EXTENT - servos.all_to_value(value) + speed = math.sin(math.radians(i)) * SPEED_EXTENT + motors.all_to_speed(speed) time.sleep(0.02) -# Do a stepped sweep +# Do a stepped speed sweep for j in range(SWEEPS): for i in range(0, STEPS): - servos.all_to_percent(i, 0, STEPS, 0.0 - SWEEP_EXTENT, SWEEP_EXTENT) + motors.all_to_percent(i, 0, STEPS, 0.0 - SPEED_EXTENT, SPEED_EXTENT) time.sleep(STEPS_INTERVAL) for i in range(0, STEPS): - servos.all_to_percent(i, STEPS, 0, 0.0 - SWEEP_EXTENT, SWEEP_EXTENT) + motors.all_to_percent(i, STEPS, 0, 0.0 - SPEED_EXTENT, SPEED_EXTENT) time.sleep(STEPS_INTERVAL) -# Disable the servos -servos.disable_all() +# Disable the motors +motors.disable_all() diff --git a/micropython/examples/motor2040/motor_profiler.py b/micropython/examples/motor2040/motor_profiler.py index 28cc875d..d930e0d4 100644 --- a/micropython/examples/motor2040/motor_profiler.py +++ b/micropython/examples/motor2040/motor_profiler.py @@ -2,7 +2,6 @@ import gc import time from motor import Motor, motor2040 from encoder import Encoder, MMME_CPR -from encoder import REVERSED """ A program that profiles the speed of a motor across its PWM @@ -14,6 +13,7 @@ ENCODER_PINS = motor2040.ENCODER_A # The pins of the encoder attached to th GEAR_RATIO = 50 # The gear ratio of the motor COUNTS_PER_REV = MMME_CPR * GEAR_RATIO # The counts per revolution of the motor's output shaft +DIRECTION = 0 # The direction to spin the motor in. NORMAL (0), REVERSED (1) SPEED_SCALE = 5.4 # The scaling to apply to the motor's speed # Set this to the maximum measured speed @@ -25,19 +25,19 @@ CAPTURE_TIME = 0.2 # How long to capture the motor's speed gc.collect() # Create a motor and set its speed scale, and give it a zero deadzone -m = Motor(MOTOR_PINS, speed_scale=SPEED_SCALE, deadzone=0.0) +m = Motor(MOTOR_PINS, direction=DIRECTION, speed_scale=SPEED_SCALE, deadzone=0.0) # Create an encoder, using PIO 0 and State Machine 0 -enc = Encoder(0, 0, ENCODER_PINS, counts_per_rev=COUNTS_PER_REV, count_microsteps=True) - -# Uncomment the below line (and the top import) to reverse the counting direction -enc.direction(REVERSED) +enc = Encoder(0, 0, ENCODER_PINS, direction=DIRECTION, counts_per_rev=COUNTS_PER_REV, count_microsteps=True) # Function that performs a single profiling step def profile_at_duty(duty): # Set the motor to a new duty cycle and wait for it to settle - m.duty(duty) + if DIRECTION == 1: + m.duty(0.0 - duty) + else: + m.duty(duty) time.sleep(SETTLE_TIME) # Perform a dummy capture to clear the encoder diff --git a/micropython/examples/motor2040/motor_wave.py b/micropython/examples/motor2040/motor_wave.py index 7d5c228e..369f326c 100644 --- a/micropython/examples/motor2040/motor_wave.py +++ b/micropython/examples/motor2040/motor_wave.py @@ -3,29 +3,28 @@ import time import math from pimoroni import Button from plasma import WS2812 -from motor import Motor, MotorCluster, motor2040 +from motor import Motor, motor2040 """ An example of applying a wave pattern to a group of motors and the LED. Press "Boot" to exit the program. -NOTE: MotorCluster and Plasma WS2812 use the RP2040's PIO system, -and as such may have problems when running code multiple times. +NOTE: Plasma WS2812 uses the RP2040's PIO system, and as +such may have problems when running code multiple times. If you encounter issues, try resetting your board. """ -SPEED = 5 # The speed that the LEDs will cycle at +SPEED = 5 # The speed that the LEDs and motors will cycle at BRIGHTNESS = 0.4 # The brightness of the LEDs -UPDATES = 50 # How many times to update LEDs and Motors per second -MOTOR_EXTENT = 1.0 # How far from zero to move the motors +UPDATES = 50 # How many times to update LEDs and motors per second +SPEED_EXTENT = 1.0 # How far from zero to drive the motors # Free up hardware resources ahead of creating a new MotorCluster gc.collect() -# Create a motor cluster for pins 0 to 7, using PIO 0 and State Machine 0 -# motors = MotorCluster(pio=0, sm=0, pins=list(range(START_PIN, END_PIN + 1))) -MOTOR_PINS = [ motor2040.MOTOR_1, motor2040.MOTOR_2, motor2040.MOTOR_3, motor2040.MOTOR_4] +# Create a list of motors +MOTOR_PINS = [motor2040.MOTOR_A, motor2040.MOTOR_B, motor2040.MOTOR_C, motor2040.MOTOR_D] motors = [Motor(pins) for pins in MOTOR_PINS] # Create the LED, using PIO 1 and State Machine 0 @@ -49,11 +48,10 @@ while user_sw.raw() is not True: led.set_hsv(0, offset / 2, 1.0, BRIGHTNESS) # Update all the MOTORs - #for i in range(motors.count()): for i in range(len(motors)): angle = ((i / len(motors)) + offset) * math.pi - motors[i].speed(math.sin(angle) * MOTOR_EXTENT) - + motors[i].speed(math.sin(angle) * SPEED_EXTENT) + time.sleep(1.0 / UPDATES) # Stop all the motors diff --git a/micropython/examples/motor2040/multiple_motors.py b/micropython/examples/motor2040/multiple_motors.py index 32696607..19ebacd5 100644 --- a/micropython/examples/motor2040/multiple_motors.py +++ b/micropython/examples/motor2040/multiple_motors.py @@ -7,35 +7,41 @@ Demonstrates how to create multiple Motor objects and control them together. """ # Create a list of motors -MOTOR_PINS = [ motor2040.MOTOR_1, motor2040.MOTOR_2, motor2040.MOTOR_3, motor2040.MOTOR_4] +MOTOR_PINS = [motor2040.MOTOR_A, motor2040.MOTOR_B, motor2040.MOTOR_C, motor2040.MOTOR_D] motors = [Motor(pins) for pins in MOTOR_PINS] -# Enable all motors (this puts them at the middle) +# Enable all motors for m in motors: m.enable() time.sleep(2) -# Go to min +# Drive at full positive for m in motors: m.full_positive() time.sleep(2) -# Go to max -for m in motors: - m.full_negative() -time.sleep(2) - -# Go back to mid +# Stop moving for m in motors: m.stop() time.sleep(2) -SWEEPS = 3 # How many sweeps of the motor to perform +# Drive at full negative +for m in motors: + m.full_negative() +time.sleep(2) + +# Coast to a gradual stop +for m in motors: + m.coast() +time.sleep(2) + + +SWEEPS = 2 # How many speed sweeps of the motor to perform STEPS = 10 # The number of discrete sweep steps STEPS_INTERVAL = 0.5 # The time in seconds between each step of the sequence -SPEED_EXTENT = 1.0 # How far from zero to move the motor when sweeping +SPEED_EXTENT = 1.0 # How far from zero to drive the motors when sweeping -# Do a sine sweep +# Do a sine speed sweep for j in range(SWEEPS): for i in range(360): speed = math.sin(math.radians(i)) * SPEED_EXTENT @@ -43,7 +49,7 @@ for j in range(SWEEPS): s.speed(speed) time.sleep(0.02) -# Do a stepped sweep +# Do a stepped speed sweep for j in range(SWEEPS): for i in range(0, STEPS): for m in motors: diff --git a/micropython/examples/motor2040/single_motor.py b/micropython/examples/motor2040/single_motor.py index 3c7e81d4..6f6b1bf2 100644 --- a/micropython/examples/motor2040/single_motor.py +++ b/micropython/examples/motor2040/single_motor.py @@ -6,43 +6,48 @@ from motor import Motor, motor2040 Demonstrates how to create a Motor object and control it. """ -# Create a motor on pins 4 and 5 -m = Motor(motor2040.MOTOR_1) +# Create a motor +m = Motor(motor2040.MOTOR_A) -# Enable the motor (this puts it at the middle) +# Enable the motor m.enable() time.sleep(2) -# Go to full positive +# Drive at full positive m.full_positive() time.sleep(2) -# Go to full negative -m.full_negative() -time.sleep(2) - # Stop moving m.stop() time.sleep(2) +# Drive at full negative +m.full_negative() +time.sleep(2) -SWEEPS = 3 # How many sweeps of the motor to perform +# Coast to a gradual stop +m.coast() +time.sleep(2) + + +SWEEPS = 2 # How many speed sweeps of the motor to perform STEPS = 10 # The number of discrete sweep steps STEPS_INTERVAL = 0.5 # The time in seconds between each step of the sequence +SPEED_EXTENT = 1.0 # How far from zero to drive the motor when sweeping -# Do a sine sweep +# Do a sine speed sweep for j in range(SWEEPS): for i in range(360): - m.speed(math.sin(math.radians(i))) + m.speed(math.sin(math.radians(i)) * SPEED_EXTENT) time.sleep(0.02) -# Do a stepped sweep +# Do a stepped speed sweep for j in range(SWEEPS): for i in range(0, STEPS): - m.to_percent(i, 0, STEPS) + m.to_percent(i, 0, STEPS, 0.0 - SPEED_EXTENT, SPEED_EXTENT) time.sleep(STEPS_INTERVAL) for i in range(0, STEPS): - m.to_percent(i, STEPS, 0) + m.to_percent(i, STEPS, 0, 0.0 - SPEED_EXTENT, SPEED_EXTENT) time.sleep(STEPS_INTERVAL) # Disable the motor diff --git a/micropython/examples/motor2040/turn_off_motors.py b/micropython/examples/motor2040/turn_off_motors.py index 1a33dd9c..bed8fcc2 100644 --- a/micropython/examples/motor2040/turn_off_motors.py +++ b/micropython/examples/motor2040/turn_off_motors.py @@ -7,7 +7,7 @@ A simple program that turns off the motors. # Create four motor objects. # This will initialise the pins, stopping any # previous movement that may be stuck on -m1 = Motor(motor2040.MOTOR_1) -m2 = Motor(motor2040.MOTOR_2) -m3 = Motor(motor2040.MOTOR_3) -m4 = Motor(motor2040.MOTOR_4) +mA = Motor(motor2040.MOTOR_A) +mB = Motor(motor2040.MOTOR_B) +mC = Motor(motor2040.MOTOR_C) +mD = Motor(motor2040.MOTOR_D) diff --git a/micropython/modules/motor/motor.c b/micropython/modules/motor/motor.c index 87f40ff7..a79ddb11 100644 --- a/micropython/modules/motor/motor.c +++ b/micropython/modules/motor/motor.c @@ -160,16 +160,16 @@ const mp_rom_obj_tuple_t motor2040_motorD_pins = { {&mp_type_tuple}, 2, { MP_ROM_INT(10), MP_ROM_INT(11) }, }; const mp_rom_obj_tuple_t motor2040_encoderA_pins = { - {&mp_type_tuple}, 2, { MP_ROM_INT(0), MP_ROM_INT(1) }, + {&mp_type_tuple}, 2, { MP_ROM_INT(1), MP_ROM_INT(0) }, }; const mp_rom_obj_tuple_t motor2040_encoderB_pins = { - {&mp_type_tuple}, 2, { MP_ROM_INT(2), MP_ROM_INT(3) }, + {&mp_type_tuple}, 2, { MP_ROM_INT(3), MP_ROM_INT(2) }, }; const mp_rom_obj_tuple_t motor2040_encoderC_pins = { - {&mp_type_tuple}, 2, { MP_ROM_INT(12), MP_ROM_INT(13) }, + {&mp_type_tuple}, 2, { MP_ROM_INT(13), MP_ROM_INT(12) }, }; const mp_rom_obj_tuple_t motor2040_encoderD_pins = { - {&mp_type_tuple}, 2, { MP_ROM_INT(14), MP_ROM_INT(15) }, + {&mp_type_tuple}, 2, { MP_ROM_INT(15), MP_ROM_INT(14) }, }; typedef struct _mp_obj_float_t { From 0f792c1e0bbc5b97eb1cb0f659d6de6d74beccee Mon Sep 17 00:00:00 2001 From: ZodiusInfuser Date: Wed, 20 Apr 2022 21:59:20 +0100 Subject: [PATCH 24/60] Added attrtuple for capture, and some position control examples --- .../examples/motor2040/motor_profiler.py | 12 +- .../examples/motor2040/position_control.py | 106 ++++++++++++++++++ micropython/examples/motor2040/tuning.py | 89 +++++++++++++++ micropython/modules/encoder/encoder.c | 2 + micropython/modules/encoder/encoder.cpp | 40 +++++++ micropython/modules/encoder/encoder.h | 3 +- 6 files changed, 245 insertions(+), 7 deletions(-) create mode 100644 micropython/examples/motor2040/position_control.py create mode 100644 micropython/examples/motor2040/tuning.py diff --git a/micropython/examples/motor2040/motor_profiler.py b/micropython/examples/motor2040/motor_profiler.py index d930e0d4..e25ec6fb 100644 --- a/micropython/examples/motor2040/motor_profiler.py +++ b/micropython/examples/motor2040/motor_profiler.py @@ -41,19 +41,19 @@ def profile_at_duty(duty): time.sleep(SETTLE_TIME) # Perform a dummy capture to clear the encoder - enc.take_snapshot() + enc.capture() # Wait for the capture time to pass time.sleep(CAPTURE_TIME) # Perform a capture and read the measured speed - capture = enc.take_snapshot() - measured_speed = capture.revolutions_per_second() + capture = enc.capture() + measured_speed = capture.revolutions_per_second # These are some alternate speed measurements from the encoder - # measured_speed = capture.revolutions_per_minute() - # measured_speed = capture.degrees_per_second() - # measured_speed = capture.radians_per_second() + # measured_speed = capture.revolutions_per_minute + # measured_speed = capture.degrees_per_second + # measured_speed = capture.radians_per_second # Print out the expected and measured speeds, as well as their difference print("Duty = ", m.duty(), ", Expected = ", m.speed(), ", Measured = ", measured_speed, ", Diff = ", m.speed() - measured_speed, sep="") diff --git a/micropython/examples/motor2040/position_control.py b/micropython/examples/motor2040/position_control.py new file mode 100644 index 00000000..4fc99859 --- /dev/null +++ b/micropython/examples/motor2040/position_control.py @@ -0,0 +1,106 @@ +import gc +import time +import math +import random +from pimoroni import Button +from motor import Motor, motor2040 +from encoder import Encoder, MMME_CPR + +""" +A program that profiles the speed of a motor across its PWM +duty cycle range using the attached encoder for feedback. +""" + +MOTOR_PINS = motor2040.MOTOR_A # The pins of the motor being profiled +ENCODER_PINS = motor2040.ENCODER_A # The pins of the encoder attached to the profiled motor +GEAR_RATIO = 50 # The gear ratio of the motor +COUNTS_PER_REV = MMME_CPR * GEAR_RATIO # The counts per revolution of the motor's output shaft + +DIRECTION = 0 # The direction to spin the motor in. NORMAL (0), REVERSED (1) +SPEED_SCALE = 5.4 # The scaling to apply to the motor's speed + # Set this to the maximum measured speed + +UPDATES = 100 # How many times to update the motor per second +TIME_FOR_EACH_MOVE = 1 # The time to travel between each random value +UPDATES_PER_MOVE = TIME_FOR_EACH_MOVE * UPDATES + +MOTOR_EXTENT = 180 # How far from zero to move the motor +USE_COSINE = True # Whether or not to use a cosine path between values + +# Free up hardware resources ahead of creating a new Encoder +gc.collect() + +# Create a motor and set its speed scale +m = Motor(MOTOR_PINS, direction=DIRECTION, speed_scale=SPEED_SCALE, deadzone=0.05) + +# Create an encoder, using PIO 0 and State Machine 0 +enc = Encoder(0, 0, ENCODER_PINS, direction=DIRECTION, counts_per_rev=COUNTS_PER_REV, count_microsteps=True) + + +# Enable the motor to get started +m.enable() + + +# Set th initial value and create a random end value between the extents +start_value = 0.0 +end_value = 180.0#random.uniform(-MOTOR_EXTENT, MOTOR_EXTENT) + +# Create the user button +user_sw = Button(motor2040.USER_SW) + + +update = 0 +target = 0 +error_sum = 0 + +# Values for 50Hz update +# kP = 0.08 +# kI = 0.000 +# kD = 0.06 + +# Values for 100Hz update +kP = 0.14 +kI = 0.000 +kD = 0.2 + +# Continually move the servo until the user button is pressed +while user_sw.raw() is not True: + + # Calculate how far along this movement to be + percent_along = min(update / UPDATES_PER_MOVE, 1.0) + + if USE_COSINE: + # Move the motor between values using cosine + target = (((-math.cos(percent_along * math.pi) + 1.0) / 2.0) * (end_value - start_value)) + start_value; + else: + # Move the motor linearly between values + target = (percent_along * (end_value - start_value)) + start_value; + + capture = enc.capture() + error = target - capture.degrees + error_sum += (kI * error) + motor_speed = (error * kP) + error_sum - (kD * capture.degrees_delta) + + m.speed(motor_speed) + + # Print out the target value the motor will move towards + print("C=", round(capture.degrees, 3), ", T=", round(target, 3), ", S=", round(motor_speed, 3),sep="") + + # Move along in time + update += 1 + + # Have we reached the end of this movement? + if update >= UPDATES_PER_MOVE: + + #if update >= UPDATES_PER_MOVE + 10: + # Reset the counter + update = 0 + + # Set the start as the last end and create a new random end value + start_value = end_value + end_value = random.uniform(-MOTOR_EXTENT, MOTOR_EXTENT) + + time.sleep(1.0 / UPDATES) + +# Disable the servo +m.disable() \ No newline at end of file diff --git a/micropython/examples/motor2040/tuning.py b/micropython/examples/motor2040/tuning.py new file mode 100644 index 00000000..1d53b95b --- /dev/null +++ b/micropython/examples/motor2040/tuning.py @@ -0,0 +1,89 @@ +import gc +import time +import math +from pimoroni import Button +from motor import Motor, motor2040 +from encoder import Encoder, MMME_CPR + +""" +A program that profiles the speed of a motor across its PWM +duty cycle range using the attached encoder for feedback. +""" + +MOTOR_PINS = motor2040.MOTOR_A # The pins of the motor being profiled +ENCODER_PINS = motor2040.ENCODER_A # The pins of the encoder attached to the profiled motor +GEAR_RATIO = 50 # The gear ratio of the motor +COUNTS_PER_REV = MMME_CPR * GEAR_RATIO # The counts per revolution of the motor's output shaft + +DIRECTION = 0 # The direction to spin the motor in. NORMAL (0), REVERSED (1) +SPEED_SCALE = 5.4 # The scaling to apply to the motor's speed + # Set this to the maximum measured speed + +UPDATES = 100 # How many times to update the motor per second +TIME_FOR_EACH_MOVE = 0.25 # The time to travel between each random value +UPDATES_PER_MOVE = TIME_FOR_EACH_MOVE * UPDATES +FURTHER_DELAY = 2 # The time to travel between each random value +FURTHER_UPDATES = FURTHER_DELAY * UPDATES + +MOTOR_EXTENT = 90 # How far from zero to move the motor + +# Free up hardware resources ahead of creating a new Encoder +gc.collect() + +# Create a motor and set its speed scale +m = Motor(MOTOR_PINS, direction=DIRECTION, speed_scale=SPEED_SCALE, deadzone=0.1) + +# Create an encoder, using PIO 0 and State Machine 0 +enc = Encoder(0, 0, ENCODER_PINS, direction=DIRECTION, counts_per_rev=COUNTS_PER_REV, count_microsteps=True) + +# Create the user button +user_sw = Button(motor2040.USER_SW) + + +update = 0 +target = MOTOR_EXTENT +error_sum = 0 + +# Values for 50Hz update +# kP = 0.08 +# kI = 0.000 +# kD = 0.06 + +# Values for 100Hz update +kP = 0.14 +kI = 0.000 +kD = 0.2 + +# Enable the motor to get started +m.enable() + +last_angle = 0 + +# Continually move the servo until the user button is pressed +while user_sw.raw() is not True: + + capture = enc.capture() + error = target - capture.degrees + error_sum += (kI * error) + motor_speed = (error * kP) + error_sum - (kD * capture.degrees_delta) + + m.speed(motor_speed) + + if update < UPDATES_PER_MOVE: + # Print out the target value the motor will move towards + print("Current = ", round(capture.degrees, 3), ", Target = ", round(target, 3), ", Speed x10= ", round(m.speed() * 10, 3),sep="") + + # Move along in time + update += 1 + + # Have we reached the end of this movement? + if update >= UPDATES_PER_MOVE + FURTHER_UPDATES: + # Reset the counter + update = 0 + + target = -target + + time.sleep(1.0 / UPDATES) + +# Disable the servo +m.disable() \ No newline at end of file diff --git a/micropython/modules/encoder/encoder.c b/micropython/modules/encoder/encoder.c index 2cd19741..1a2afdc1 100644 --- a/micropython/modules/encoder/encoder.c +++ b/micropython/modules/encoder/encoder.c @@ -32,6 +32,7 @@ MP_DEFINE_CONST_FUN_OBJ_1(Encoder_radians_obj, Encoder_radians); MP_DEFINE_CONST_FUN_OBJ_KW(Encoder_direction_obj, 1, Encoder_direction); MP_DEFINE_CONST_FUN_OBJ_KW(Encoder_counts_per_revolution_obj, 1, Encoder_counts_per_revolution); MP_DEFINE_CONST_FUN_OBJ_1(Encoder_take_snapshot_obj, Encoder_take_snapshot); +MP_DEFINE_CONST_FUN_OBJ_1(Encoder_capture_obj, Encoder_capture); /***** Binding of Methods *****/ STATIC const mp_rom_map_elem_t Snapshot_locals_dict_table[] = { @@ -67,6 +68,7 @@ STATIC const mp_rom_map_elem_t Encoder_locals_dict_table[] = { { MP_ROM_QSTR(MP_QSTR_direction), MP_ROM_PTR(&Encoder_direction_obj) }, { MP_ROM_QSTR(MP_QSTR_counts_per_revolution), MP_ROM_PTR(&Encoder_counts_per_revolution_obj) }, { MP_ROM_QSTR(MP_QSTR_take_snapshot), MP_ROM_PTR(&Encoder_take_snapshot_obj) }, + { MP_ROM_QSTR(MP_QSTR_capture), MP_ROM_PTR(&Encoder_capture_obj) }, }; STATIC MP_DEFINE_CONST_DICT(Snapshot_locals_dict, Snapshot_locals_dict_table); diff --git a/micropython/modules/encoder/encoder.cpp b/micropython/modules/encoder/encoder.cpp index ee36d433..5ee3ea0d 100644 --- a/micropython/modules/encoder/encoder.cpp +++ b/micropython/modules/encoder/encoder.cpp @@ -416,4 +416,44 @@ extern mp_obj_t Encoder_take_snapshot(mp_obj_t self_in) { snap->snapshot = self->encoder->take_snapshot(); return MP_OBJ_FROM_PTR(snap); } + +extern mp_obj_t Encoder_capture(mp_obj_t self_in) { + _Encoder_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Encoder_obj_t); + + Encoder::Snapshot snapshot = self->encoder->take_snapshot(); + + mp_obj_t tuple[] = { + mp_obj_new_int(snapshot.count()), + mp_obj_new_int(snapshot.delta()), + mp_obj_new_float(snapshot.frequency()), + mp_obj_new_float(snapshot.revolutions()), + mp_obj_new_float(snapshot.degrees()), + mp_obj_new_float(snapshot.radians()), + mp_obj_new_float(snapshot.revolutions_delta()), + mp_obj_new_float(snapshot.degrees_delta()), + mp_obj_new_float(snapshot.radians_delta()), + mp_obj_new_float(snapshot.revolutions_per_second()), + mp_obj_new_float(snapshot.revolutions_per_minute()), + mp_obj_new_float(snapshot.degrees_per_second()), + mp_obj_new_float(snapshot.radians_per_second()), + }; + + STATIC const qstr tuple_fields[] = { + MP_QSTR_count, + MP_QSTR_delta, + MP_QSTR_frequency, + MP_QSTR_revolutions, + MP_QSTR_degrees, + MP_QSTR_radians, + MP_QSTR_revolutions_delta, + MP_QSTR_degrees_delta, + MP_QSTR_radians_delta, + MP_QSTR_revolutions_per_second, + MP_QSTR_revolutions_per_minute, + MP_QSTR_degrees_per_second, + MP_QSTR_radians_per_second, + }; + + return mp_obj_new_attrtuple(tuple_fields, sizeof(tuple) / sizeof(mp_obj_t), tuple); +} } diff --git a/micropython/modules/encoder/encoder.h b/micropython/modules/encoder/encoder.h index 80bbc4d7..257a5bd3 100644 --- a/micropython/modules/encoder/encoder.h +++ b/micropython/modules/encoder/encoder.h @@ -39,4 +39,5 @@ extern mp_obj_t Encoder_degrees(mp_obj_t self_in); extern mp_obj_t Encoder_radians(mp_obj_t self_in); extern mp_obj_t Encoder_direction(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); extern mp_obj_t Encoder_counts_per_revolution(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); -extern mp_obj_t Encoder_take_snapshot(mp_obj_t self_in); \ No newline at end of file +extern mp_obj_t Encoder_take_snapshot(mp_obj_t self_in); +extern mp_obj_t Encoder_capture(mp_obj_t self_in); \ No newline at end of file From c837081fe273a5c88557ffb5e09d96ecb2226daa Mon Sep 17 00:00:00 2001 From: ZodiusInfuser Date: Thu, 21 Apr 2022 16:47:37 +0100 Subject: [PATCH 25/60] Added PID class and MP tuning examples --- .../motor2040/position_on_velocity_tuning.py | 116 ++++++++++++++++++ .../examples/motor2040/position_tuning.py | 100 +++++++++++++++ .../examples/motor2040/velocity_tuning.py | 101 +++++++++++++++ micropython/modules_py/pimoroni.py | 86 +++---------- 4 files changed, 335 insertions(+), 68 deletions(-) create mode 100644 micropython/examples/motor2040/position_on_velocity_tuning.py create mode 100644 micropython/examples/motor2040/position_tuning.py create mode 100644 micropython/examples/motor2040/velocity_tuning.py diff --git a/micropython/examples/motor2040/position_on_velocity_tuning.py b/micropython/examples/motor2040/position_on_velocity_tuning.py new file mode 100644 index 00000000..7c03c113 --- /dev/null +++ b/micropython/examples/motor2040/position_on_velocity_tuning.py @@ -0,0 +1,116 @@ +import gc +import time +from pimoroni import Button, PID +from motor import Motor, motor2040 +from encoder import Encoder, MMME_CPR + +""" +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 target +angles and plots the measured response. + +Press "Boot" to exit the program. +""" + +MOTOR_PINS = motor2040.MOTOR_A # The pins of the motor being profiled +ENCODER_PINS = motor2040.ENCODER_A # The pins of the encoder attached to the profiled motor +GEAR_RATIO = 50 # The gear ratio of the motor +COUNTS_PER_REV = MMME_CPR * GEAR_RATIO # The counts per revolution of the motor's output shaft + +DIRECTION = 0 # The direction to spin the motor in. NORMAL (0), REVERSED (1) +SPEED_SCALE = 5.4 # The scaling to apply to the motor's speed to match its real-world speed + +UPDATES = 100 # How many times to update the motor per second +UPDATE_RATE = 1 / UPDATES +PRINT_WINDOW = 1.0 # The time (in seconds) after a new target, to display print out motor values +MOVEMENT_WINDOW = 2.0 # The time (in seconds) between each new target being set +PRINT_DIVIDER = 4 # How many of the updates should be printed (i.e. 2 would be every other update) + +# Multipliers for the different printed values, so they appear nicely on the Thonny plotter +ACC_PRINT_SCALE = 1 # Acceleration multiplier +SPD_PRINT_SCALE = 20 # Driving Speed multipler + +POSITION_EXTENT = 180 # How far from zero to move the motor, in degrees +MAX_SPEED = 2.0 # The maximum speed to move the motor at, in revolutions per second + +# PID values +POS_KP = 0.025 # Position proportional (P) gain +POS_KI = 0.0 # Position integral (I) gain +POS_KD = 0.0 # Position derivative (D) gain + +VEL_KP = 30.0 # Velocity proportional (P) gain +VEL_KI = 0.0 # Velocity integral (I) gain +VEL_KD = 0.4 # Velocity derivative (D) gain + + +# Free up hardware resources ahead of creating a new Encoder +gc.collect() + +# Create a motor and set its speed scale +m = Motor(MOTOR_PINS, direction=DIRECTION, speed_scale=SPEED_SCALE, deadzone=0.0) + +# Create an encoder, using PIO 0 and State Machine 0 +enc = Encoder(0, 0, ENCODER_PINS, direction=DIRECTION, counts_per_rev=COUNTS_PER_REV, count_microsteps=True) + +# Create the user button +user_sw = Button(motor2040.USER_SW) + +# Create PID objects for both position and velocity control +pos_pid = PID(POS_KP, POS_KI, POS_KD, UPDATE_RATE) +vel_pid = PID(VEL_KP, VEL_KI, VEL_KD, UPDATE_RATE) + +# Enable the motor to get started +m.enable() + +# Set the initial target position +pos_pid.target = POSITION_EXTENT + + +update = 0 +print_count = 0 + +# Continually move the motor until the user button is pressed +while user_sw.raw() is not True: + + # Capture the state of the encoder + capture = enc.capture() + + # Calculate the velocity to move the motor closer to the position target + vel = pos_pid.calculate(capture.degrees, capture.degrees_per_second) + + # Limit the velocity between user defined limits, and set it as the new target of the velocity PID + vel_pid.target = max(min(vel, MAX_SPEED), -MAX_SPEED) + + # Calculate the acceleration to apply to the motor to move it closer to the velocity target + 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 targets, + # but only for the first few updates and only every multiple + if update < (PRINT_WINDOW * UPDATES) and print_count == 0: + print("Pos =", capture.degrees, end=", ") + print("Targ Pos =", pos_pid.target, end=", ") + print("Vel =", capture.revolutions_per_second * SPD_PRINT_SCALE, end=", ") + print("Targ Vel =", vel_pid.target * SPD_PRINT_SCALE, end=", ") + print("Accel =", accel * ACC_PRINT_SCALE, end=", ") + print("Speed =", m.speed() * SPD_PRINT_SCALE) + + # Increment the print count, and wrap it + print_count = (print_count + 1) % PRINT_DIVIDER + + update += 1 # Move along in time + + # Have we reached the end of this time window? + if update >= (MOVEMENT_WINDOW * UPDATES): + update = 0 # Reset the counter + + # Set the new position target to be the inverse of the current target + pos_pid.target = 0.0 - pos_pid.target + + time.sleep(UPDATE_RATE) + +# Disable the motor +m.disable() diff --git a/micropython/examples/motor2040/position_tuning.py b/micropython/examples/motor2040/position_tuning.py new file mode 100644 index 00000000..eda408ca --- /dev/null +++ b/micropython/examples/motor2040/position_tuning.py @@ -0,0 +1,100 @@ +import gc +import time +from pimoroni import Button, PID +from motor import Motor, motor2040 +from encoder import Encoder, MMME_CPR + +""" +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 target angles and +plots the measured response. + +Press "Boot" to exit the program. +""" + +MOTOR_PINS = motor2040.MOTOR_A # The pins of the motor being profiled +ENCODER_PINS = motor2040.ENCODER_A # The pins of the encoder attached to the profiled motor +GEAR_RATIO = 50 # The gear ratio of the motor +COUNTS_PER_REV = MMME_CPR * GEAR_RATIO # The counts per revolution of the motor's output shaft + +DIRECTION = 0 # The direction to spin the motor in. NORMAL (0), REVERSED (1) +SPEED_SCALE = 5.4 # The scaling to apply to the motor's speed to match its real-world speed + +UPDATES = 100 # How many times to update the motor per second +UPDATE_RATE = 1 / UPDATES +PRINT_WINDOW = 0.25 # The time (in seconds) after a new target, to display print out motor values +MOVEMENT_WINDOW = 2.0 # The time (in seconds) between each new target being set +PRINT_DIVIDER = 1 # How many of the updates should be printed (i.e. 2 would be every other update) + +# Multipliers for the different printed values, so they appear nicely on the Thonny plotter +SPD_PRINT_SCALE = 10 # Driving Speed multipler + +POSITION_EXTENT = 90 # How far from zero to move the motor, in degrees + +# PID values +POS_KP = 0.14 # Position proportional (P) gain +POS_KI = 0.0 # Position integral (I) gain +POS_KD = 0.0022 # Position derivative (D) gain + + +# Free up hardware resources ahead of creating a new Encoder +gc.collect() + +# Create a motor and set its speed scale +m = Motor(MOTOR_PINS, direction=DIRECTION, speed_scale=SPEED_SCALE, deadzone=0.0) + +# Create an encoder, using PIO 0 and State Machine 0 +enc = Encoder(0, 0, ENCODER_PINS, direction=DIRECTION, counts_per_rev=COUNTS_PER_REV, count_microsteps=True) + +# Create the user button +user_sw = Button(motor2040.USER_SW) + +# Create PID object for position control +pos_pid = PID(POS_KP, POS_KI, POS_KD, UPDATE_RATE) + +# Enable the motor to get started +m.enable() + +# Set the initial target position +pos_pid.target = POSITION_EXTENT + + +update = 0 +print_count = 0 + +# Continually move the motor until the user button is pressed +while user_sw.raw() is not True: + + # Capture the state of the encoder + capture = enc.capture() + + # Calculate the velocity to move the motor closer to the position target + 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 targets, + # but only for the first few updates and only every multiple + if update < (PRINT_WINDOW * UPDATES) and print_count == 0: + print("Pos =", capture.degrees, end=", ") + print("Targ Pos =", pos_pid.target, end=", ") + print("Speed = ", m.speed() * SPD_PRINT_SCALE) + + # Increment the print count, and wrap it + print_count = (print_count + 1) % PRINT_DIVIDER + + update += 1 # Move along in time + + # Have we reached the end of this time window? + if update >= (MOVEMENT_WINDOW * UPDATES): + update = 0 # Reset the counter + + # Set the new position target to be the inverse of the current target + pos_pid.target = 0.0 - pos_pid.target + + time.sleep(UPDATE_RATE) + +# Disable the servo +m.disable() diff --git a/micropython/examples/motor2040/velocity_tuning.py b/micropython/examples/motor2040/velocity_tuning.py new file mode 100644 index 00000000..f46bae52 --- /dev/null +++ b/micropython/examples/motor2040/velocity_tuning.py @@ -0,0 +1,101 @@ +import gc +import time +from pimoroni import Button, PID +from motor import Motor, motor2040 +from encoder import Encoder, MMME_CPR + +""" +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 target speeds and +plots the measured response. + +Press "Boot" to exit the program. +""" + +MOTOR_PINS = motor2040.MOTOR_A # The pins of the motor being profiled +ENCODER_PINS = motor2040.ENCODER_A # The pins of the encoder attached to the profiled motor +GEAR_RATIO = 50 # The gear ratio of the motor +COUNTS_PER_REV = MMME_CPR * GEAR_RATIO # The counts per revolution of the motor's output shaft + +DIRECTION = 0 # The direction to spin the motor in. NORMAL (0), REVERSED (1) +SPEED_SCALE = 5.4 # The scaling to apply to the motor's speed to match its real-world speed + +UPDATES = 100 # How many times to update the motor per second +UPDATE_RATE = 1 / UPDATES +PRINT_WINDOW = 1.0 # The time (in seconds) after a new target, to display print out motor values +MOVEMENT_WINDOW = 2.0 # The time (in seconds) between each new target being set +PRINT_DIVIDER = 4 # How many of the updates should be printed (i.e. 2 would be every other update) + +# Multipliers for the different printed values, so they appear nicely on the Thonny plotter +ACC_PRINT_SCALE = 0.01 # Acceleration multiplier + +VELOCITY_EXTENT = 3 # How far from zero to drive the motor at, in revolutions per second + +# PID values +VEL_KP = 30.0 # Velocity proportional (P) gain +VEL_KI = 0.0 # Velocity integral (I) gain +VEL_KD = 0.4 # Velocity derivative (D) gain + + +# Free up hardware resources ahead of creating a new Encoder +gc.collect() + +# Create a motor and set its speed scale +m = Motor(MOTOR_PINS, direction=DIRECTION, speed_scale=SPEED_SCALE, deadzone=0.0) + +# Create an encoder, using PIO 0 and State Machine 0 +enc = Encoder(0, 0, ENCODER_PINS, direction=DIRECTION, counts_per_rev=COUNTS_PER_REV, count_microsteps=True) + +# Create the user button +user_sw = Button(motor2040.USER_SW) + +# Create PID object for velocity control +vel_pid = PID(VEL_KP, VEL_KI, VEL_KD, UPDATE_RATE) + +# Enable the motor to get started +m.enable() + +# Set the initial target velocity +vel_pid.target = VELOCITY_EXTENT + + +update = 0 +print_count = 0 + +# Continually move the motor until the user button is pressed +while user_sw.raw() is not True: + + # Capture the state of the encoder + capture = enc.capture() + + # Calculate the acceleration to apply to the motor to move it closer to the velocity target + 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 targets, + # but only for the first few updates and only every multiple + if update < (PRINT_WINDOW * UPDATES) and print_count == 0: + print("Vel =", capture.revolutions_per_second, end=", ") + print("Targ Vel =", vel_pid.target, end=", ") + print("Accel =", accel * ACC_PRINT_SCALE, end=", ") + print("Speed =", m.speed()) + + # Increment the print count, and wrap it + print_count = (print_count + 1) % PRINT_DIVIDER + + update += 1 # Move along in time + + # Have we reached the end of this time window? + if update >= (MOVEMENT_WINDOW * UPDATES): + update = 0 # Reset the counter + + # Set the new velocity target to be the inverse of the current target + vel_pid.target = 0.0 - vel_pid.target + + time.sleep(UPDATE_RATE) + +# Disable the motor +m.disable() diff --git a/micropython/modules_py/pimoroni.py b/micropython/modules_py/pimoroni.py index d051833f..17a33bf8 100644 --- a/micropython/modules_py/pimoroni.py +++ b/micropython/modules_py/pimoroni.py @@ -143,74 +143,24 @@ class RGBLED: self.led_b.duty_u16(int((b * 65535) / 255)) -class Motor: - FAST_DECAY = 0 # Recirculation current fast decay mode (coasting) - SLOW_DECAY = 1 # Recirculation current slow decay mode (braking) +# A simple class for handling Proportional, Integral & Derivative (PID) control calculations +class PID: + def __init__(self, kp, ki, kd, sample_rate): + self.kp = kp + self.ki = ki + self.kd = kd + self.target = 0 + self._error_sum = 0 + self._last_value = 0 + self._sample_rate = sample_rate - def __init__(self, pos, neg, freq=25000, decay_mode=SLOW_DECAY): - self.speed = 0.0 - self.freq = freq - if decay_mode in (self.FAST_DECAY, self.SLOW_DECAY): - self.decay_mode = decay_mode + def calculate(self, value, value_change=None): + error = self.target - value + self._error_sum += error * self._sample_rate + if value_change == None: + rate_error = (value - self._last_value) / self._sample_rate else: - raise ValueError("Decay mode value must be either Motor.FAST_DECAY or Motor.SLOW_DECAY") + rate_error = value_change + self._last_value = value - self.pos_pwm = PWM(Pin(pos)) - self.pos_pwm.freq(freq) - self.neg_pwm = PWM(Pin(neg)) - self.neg_pwm.freq(freq) - - def get_speed(self): - return self.speed - - def set_speed(self, speed): - if speed > 1.0 or speed < -1.0: - raise ValueError("Speed must be between -1.0 and +1.0") - self.speed = speed - self._update_pwm() - - def get_frequency(self): - return self.freq - - def set_frequency(self, freq): - self.pos_pwm.freq(freq) - self.neg_pwm.freq(freq) - self._update_pwm() - - def get_decay_mode(self): - return self.decay_mode - - def set_decay_mode(self, mode): - if mode in (self.FAST_DECAY, self.SLOW_DECAY): - self.decay_mode = mode - self._update_pwm() - else: - raise ValueError("Decay mode value must be either Motor.FAST_DECAY or Motor.SLOW_DECAY") - - def stop(self): - self.speed = 0.0 - self._update_pwm() - - def disable(self): - self.speed = 0.0 - self.pos_pwm.duty_u16(0) - self.neg_pwm.duty_u16(0) - - def _update_pwm(self): - signed_duty_cycle = int(self.speed * 0xFFFF) - - if self.decay_mode is self.SLOW_DECAY: # aka 'Braking' - if signed_duty_cycle >= 0: - self.pos_pwm.duty_u16(0xFFFF) - self.neg_pwm.duty_u16(0xFFFF - signed_duty_cycle) - else: - self.pos_pwm.duty_u16(0xFFFF + signed_duty_cycle) - self.neg_pwm.duty_u16(0xFFFF) - - else: # aka 'Coasting' - if signed_duty_cycle >= 0: - self.pos_pwm.duty_u16(signed_duty_cycle) - self.neg_pwm.duty_u16(0) - else: - self.pos_pwm.duty_u16(0) - self.neg_pwm.duty_u16(0 - signed_duty_cycle) + return (error * self.kp) + (self._error_sum * self.ki) - (rate_error * self.kd) From 70d1368a2b2a3712fb066ae1535c172a482fa14a Mon Sep 17 00:00:00 2001 From: ZodiusInfuser Date: Thu, 21 Apr 2022 17:38:37 +0100 Subject: [PATCH 26/60] Extended tuning examples to be general control examples --- .../examples/motor2040/position_control.py | 103 ++++++++------ .../motor2040/position_on_velocity_control.py | 134 ++++++++++++++++++ micropython/examples/motor2040/tuning.py | 89 ------------ .../examples/motor2040/velocity_control.py | 119 ++++++++++++++++ 4 files changed, 309 insertions(+), 136 deletions(-) create mode 100644 micropython/examples/motor2040/position_on_velocity_control.py delete mode 100644 micropython/examples/motor2040/tuning.py create mode 100644 micropython/examples/motor2040/velocity_control.py diff --git a/micropython/examples/motor2040/position_control.py b/micropython/examples/motor2040/position_control.py index 4fc99859..18ee63e8 100644 --- a/micropython/examples/motor2040/position_control.py +++ b/micropython/examples/motor2040/position_control.py @@ -2,13 +2,15 @@ import gc import time import math import random -from pimoroni import Button +from pimoroni import Button, PID from motor import Motor, motor2040 from encoder import Encoder, MMME_CPR """ -A program that profiles the speed of a motor across its PWM -duty cycle range using the attached encoder for feedback. +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. """ MOTOR_PINS = motor2040.MOTOR_A # The pins of the motor being profiled @@ -17,15 +19,25 @@ GEAR_RATIO = 50 # The gear ratio of the motor COUNTS_PER_REV = MMME_CPR * GEAR_RATIO # The counts per revolution of the motor's output shaft DIRECTION = 0 # The direction to spin the motor in. NORMAL (0), REVERSED (1) -SPEED_SCALE = 5.4 # The scaling to apply to the motor's speed - # Set this to the maximum measured speed +SPEED_SCALE = 5.4 # The scaling to apply to the motor's speed to match its real-world speed -UPDATES = 100 # How many times to update the motor per second +UPDATES = 100 # How many times to update the motor per second +UPDATE_RATE = 1 / UPDATES TIME_FOR_EACH_MOVE = 1 # The time to travel between each random value UPDATES_PER_MOVE = TIME_FOR_EACH_MOVE * UPDATES +PRINT_DIVIDER = 4 # How many of the updates should be printed (i.e. 2 would be every other update) + +# Multipliers for the different printed values, so they appear nicely on the Thonny plotter +SPD_PRINT_SCALE = 20 # Driving Speed multipler + +POSITION_EXTENT = 180 # How far from zero to move the motor, in degrees +INTERP_MODE = 2 # The interpolating mode between targets. STEP (0), LINEAR (1), COSINE (2) + +# PID values +POS_KP = 0.14 # Position proportional (P) gain +POS_KI = 0.0 # Position integral (I) gain +POS_KD = 0.0022 # Position derivative (D) gain -MOTOR_EXTENT = 180 # How far from zero to move the motor -USE_COSINE = True # Whether or not to use a cosine path between values # Free up hardware resources ahead of creating a new Encoder gc.collect() @@ -36,71 +48,68 @@ m = Motor(MOTOR_PINS, direction=DIRECTION, speed_scale=SPEED_SCALE, deadzone=0.0 # Create an encoder, using PIO 0 and State Machine 0 enc = Encoder(0, 0, ENCODER_PINS, direction=DIRECTION, counts_per_rev=COUNTS_PER_REV, count_microsteps=True) +# Create the user button +user_sw = Button(motor2040.USER_SW) + +# Create PID object for position control +pos_pid = PID(POS_KP, POS_KI, POS_KD, UPDATE_RATE) # Enable the motor to get started m.enable() -# Set th initial value and create a random end value between the extents -start_value = 0.0 -end_value = 180.0#random.uniform(-MOTOR_EXTENT, MOTOR_EXTENT) - -# Create the user button -user_sw = Button(motor2040.USER_SW) - - update = 0 -target = 0 -error_sum = 0 +print_count = 0 -# Values for 50Hz update -# kP = 0.08 -# kI = 0.000 -# kD = 0.06 +# Set the initial value and create a random end value between the extents +start_value = 0.0 +end_value = random.uniform(-POSITION_EXTENT, POSITION_EXTENT) -# Values for 100Hz update -kP = 0.14 -kI = 0.000 -kD = 0.2 - -# Continually move the servo until the user button is pressed +# Continually move the motor until the user button is pressed while user_sw.raw() is not True: + # Capture the state of the encoder + capture = enc.capture() + # Calculate how far along this movement to be percent_along = min(update / UPDATES_PER_MOVE, 1.0) - if USE_COSINE: + if INTERP_MODE == 0: + # Move the motor instantly to the end value + pos_pid.target = end_value + elif INTERP_MODE == 2: # Move the motor between values using cosine - target = (((-math.cos(percent_along * math.pi) + 1.0) / 2.0) * (end_value - start_value)) + start_value; + pos_pid.target = (((-math.cos(percent_along * math.pi) + 1.0) / 2.0) * (end_value - start_value)) + start_value else: # Move the motor linearly between values - target = (percent_along * (end_value - start_value)) + start_value; + pos_pid.target = (percent_along * (end_value - start_value)) + start_value - capture = enc.capture() - error = target - capture.degrees - error_sum += (kI * error) - motor_speed = (error * kP) + error_sum - (kD * capture.degrees_delta) + # Calculate the velocity to move the motor closer to the position target + vel = pos_pid.calculate(capture.degrees, capture.degrees_per_second) - m.speed(motor_speed) + # Set the new motor driving speed + m.speed(vel) - # Print out the target value the motor will move towards - print("C=", round(capture.degrees, 3), ", T=", round(target, 3), ", S=", round(motor_speed, 3),sep="") + # Print out the current motor values and their targets, but only on every multiple + if print_count == 0: + print("Pos =", capture.degrees, end=", ") + print("Targ Pos =", pos_pid.target, end=", ") + print("Speed = ", m.speed() * SPD_PRINT_SCALE) - # Move along in time - update += 1 + # Increment the print count, and wrap it + print_count = (print_count + 1) % PRINT_DIVIDER + + update += 1 # Move along in time # Have we reached the end of this movement? if update >= UPDATES_PER_MOVE: - - #if update >= UPDATES_PER_MOVE + 10: - # Reset the counter - update = 0 + 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 = random.uniform(-MOTOR_EXTENT, MOTOR_EXTENT) + end_value = random.uniform(-POSITION_EXTENT, POSITION_EXTENT) - time.sleep(1.0 / UPDATES) + time.sleep(UPDATE_RATE) # Disable the servo -m.disable() \ No newline at end of file +m.disable() diff --git a/micropython/examples/motor2040/position_on_velocity_control.py b/micropython/examples/motor2040/position_on_velocity_control.py new file mode 100644 index 00000000..d6da2fcc --- /dev/null +++ b/micropython/examples/motor2040/position_on_velocity_control.py @@ -0,0 +1,134 @@ +import gc +import time +import math +import random +from pimoroni import Button, PID +from motor import Motor, motor2040 +from encoder import Encoder, MMME_CPR + +""" +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. +""" + +MOTOR_PINS = motor2040.MOTOR_A # The pins of the motor being profiled +ENCODER_PINS = motor2040.ENCODER_A # The pins of the encoder attached to the profiled motor +GEAR_RATIO = 50 # The gear ratio of the motor +COUNTS_PER_REV = MMME_CPR * GEAR_RATIO # The counts per revolution of the motor's output shaft + +DIRECTION = 0 # The direction to spin the motor in. NORMAL (0), REVERSED (1) +SPEED_SCALE = 5.4 # The scaling to apply to the motor's speed to match its real-world speed + +UPDATES = 100 # How many times to update the motor per second +UPDATE_RATE = 1 / UPDATES +TIME_FOR_EACH_MOVE = 1 # The time to travel between each random value, in seconds +UPDATES_PER_MOVE = TIME_FOR_EACH_MOVE * UPDATES +PRINT_DIVIDER = 4 # How many of the updates should be printed (i.e. 2 would be every other update) + +# Multipliers for the different printed values, so they appear nicely on the Thonny plotter +ACC_PRINT_SCALE = 2 # Acceleration multiplier +SPD_PRINT_SCALE = 40 # Driving Speed multipler + +POSITION_EXTENT = 180 # How far from zero to move the motor, in degrees +MAX_SPEED = 1.0 # The maximum speed to move the motor at, in revolutions per second +INTERP_MODE = 0 # The interpolating mode between targets. STEP (0), LINEAR (1), COSINE (2) + +# PID values +POS_KP = 0.025 # Position proportional (P) gain +POS_KI = 0.0 # Position integral (I) gain +POS_KD = 0.0 # Position derivative (D) gain + +VEL_KP = 30.0 # Velocity proportional (P) gain +VEL_KI = 0.0 # Velocity integral (I) gain +VEL_KD = 0.4 # Velocity derivative (D) gain + + +# Free up hardware resources ahead of creating a new Encoder +gc.collect() + +# Create a motor and set its speed scale +m = Motor(MOTOR_PINS, direction=DIRECTION, speed_scale=SPEED_SCALE, deadzone=0.05) + +# Create an encoder, using PIO 0 and State Machine 0 +enc = Encoder(0, 0, ENCODER_PINS, direction=DIRECTION, counts_per_rev=COUNTS_PER_REV, count_microsteps=True) + +# Create the user button +user_sw = Button(motor2040.USER_SW) + +# Create PID objects for both position and velocity control +pos_pid = PID(POS_KP, POS_KI, POS_KD, UPDATE_RATE) +vel_pid = PID(VEL_KP, VEL_KI, VEL_KD, UPDATE_RATE) + +# Enable the motor to get started +m.enable() + +# Set the initial target position +pos_pid.target = POSITION_EXTENT + + +update = 0 +print_count = 0 + +# Set the initial value and create a random end value between the extents +start_value = 0.0 +end_value = random.uniform(-POSITION_EXTENT, POSITION_EXTENT) + +# Continually move the motor until the user button is pressed +while user_sw.raw() is not True: + + # Capture the state of the encoder + capture = enc.capture() + + # Calculate how far along this movement to be + percent_along = min(update / UPDATES_PER_MOVE, 1.0) + + if INTERP_MODE == 0: + # Move the motor instantly to the end value + pos_pid.target = end_value + elif INTERP_MODE == 2: + # Move the motor between values using cosine + pos_pid.target = (((-math.cos(percent_along * math.pi) + 1.0) / 2.0) * (end_value - start_value)) + start_value + else: + # Move the motor linearly between values + pos_pid.target = (percent_along * (end_value - start_value)) + start_value + + # Calculate the velocity to move the motor closer to the position target + vel = pos_pid.calculate(capture.degrees, capture.degrees_per_second) + + # Limit the velocity between user defined limits, and set it as the new target of the velocity PID + vel_pid.target = max(min(vel, MAX_SPEED), -MAX_SPEED) + + # Calculate the acceleration to apply to the motor to move it closer to the velocity target + 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 targets, but only on every multiple + if print_count == 0: + print("Pos =", capture.degrees, end=", ") + print("Targ Pos =", pos_pid.target, end=", ") + print("Vel =", capture.revolutions_per_second * SPD_PRINT_SCALE, end=", ") + print("Targ Vel =", vel_pid.target * SPD_PRINT_SCALE, end=", ") + print("Accel =", accel * ACC_PRINT_SCALE, end=", ") + print("Speed =", m.speed() * SPD_PRINT_SCALE) + + # Increment the print count, and wrap it + print_count = (print_count + 1) % PRINT_DIVIDER + + update += 1 # 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 = random.uniform(-POSITION_EXTENT, POSITION_EXTENT) + + time.sleep(UPDATE_RATE) + +# Disable the motor +m.disable() diff --git a/micropython/examples/motor2040/tuning.py b/micropython/examples/motor2040/tuning.py deleted file mode 100644 index 1d53b95b..00000000 --- a/micropython/examples/motor2040/tuning.py +++ /dev/null @@ -1,89 +0,0 @@ -import gc -import time -import math -from pimoroni import Button -from motor import Motor, motor2040 -from encoder import Encoder, MMME_CPR - -""" -A program that profiles the speed of a motor across its PWM -duty cycle range using the attached encoder for feedback. -""" - -MOTOR_PINS = motor2040.MOTOR_A # The pins of the motor being profiled -ENCODER_PINS = motor2040.ENCODER_A # The pins of the encoder attached to the profiled motor -GEAR_RATIO = 50 # The gear ratio of the motor -COUNTS_PER_REV = MMME_CPR * GEAR_RATIO # The counts per revolution of the motor's output shaft - -DIRECTION = 0 # The direction to spin the motor in. NORMAL (0), REVERSED (1) -SPEED_SCALE = 5.4 # The scaling to apply to the motor's speed - # Set this to the maximum measured speed - -UPDATES = 100 # How many times to update the motor per second -TIME_FOR_EACH_MOVE = 0.25 # The time to travel between each random value -UPDATES_PER_MOVE = TIME_FOR_EACH_MOVE * UPDATES -FURTHER_DELAY = 2 # The time to travel between each random value -FURTHER_UPDATES = FURTHER_DELAY * UPDATES - -MOTOR_EXTENT = 90 # How far from zero to move the motor - -# Free up hardware resources ahead of creating a new Encoder -gc.collect() - -# Create a motor and set its speed scale -m = Motor(MOTOR_PINS, direction=DIRECTION, speed_scale=SPEED_SCALE, deadzone=0.1) - -# Create an encoder, using PIO 0 and State Machine 0 -enc = Encoder(0, 0, ENCODER_PINS, direction=DIRECTION, counts_per_rev=COUNTS_PER_REV, count_microsteps=True) - -# Create the user button -user_sw = Button(motor2040.USER_SW) - - -update = 0 -target = MOTOR_EXTENT -error_sum = 0 - -# Values for 50Hz update -# kP = 0.08 -# kI = 0.000 -# kD = 0.06 - -# Values for 100Hz update -kP = 0.14 -kI = 0.000 -kD = 0.2 - -# Enable the motor to get started -m.enable() - -last_angle = 0 - -# Continually move the servo until the user button is pressed -while user_sw.raw() is not True: - - capture = enc.capture() - error = target - capture.degrees - error_sum += (kI * error) - motor_speed = (error * kP) + error_sum - (kD * capture.degrees_delta) - - m.speed(motor_speed) - - if update < UPDATES_PER_MOVE: - # Print out the target value the motor will move towards - print("Current = ", round(capture.degrees, 3), ", Target = ", round(target, 3), ", Speed x10= ", round(m.speed() * 10, 3),sep="") - - # Move along in time - update += 1 - - # Have we reached the end of this movement? - if update >= UPDATES_PER_MOVE + FURTHER_UPDATES: - # Reset the counter - update = 0 - - target = -target - - time.sleep(1.0 / UPDATES) - -# Disable the servo -m.disable() \ No newline at end of file diff --git a/micropython/examples/motor2040/velocity_control.py b/micropython/examples/motor2040/velocity_control.py new file mode 100644 index 00000000..565e2b4b --- /dev/null +++ b/micropython/examples/motor2040/velocity_control.py @@ -0,0 +1,119 @@ +import gc +import time +import math +import random +from pimoroni import Button, PID +from motor import Motor, motor2040 +from encoder import Encoder, MMME_CPR + +""" +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. +""" + +MOTOR_PINS = motor2040.MOTOR_A # The pins of the motor being profiled +ENCODER_PINS = motor2040.ENCODER_A # The pins of the encoder attached to the profiled motor +GEAR_RATIO = 50 # The gear ratio of the motor +COUNTS_PER_REV = MMME_CPR * GEAR_RATIO # The counts per revolution of the motor's output shaft + +DIRECTION = 0 # The direction to spin the motor in. NORMAL (0), REVERSED (1) +SPEED_SCALE = 5.4 # The scaling to apply to the motor's speed to match its real-world speed + +UPDATES = 100 # How many times to update the motor per second +UPDATE_RATE = 1 / UPDATES +TIME_FOR_EACH_MOVE = 1 # The time to travel between each random value, in seconds +UPDATES_PER_MOVE = TIME_FOR_EACH_MOVE * UPDATES +PRINT_DIVIDER = 4 # How many of the updates should be printed (i.e. 2 would be every other update) + +# Multipliers for the different printed values, so they appear nicely on the Thonny plotter +ACC_PRINT_SCALE = 0.05 # Acceleration multiplier + +VELOCITY_EXTENT = 3 # How far from zero to drive the motor at, in revolutions per second +INTERP_MODE = 2 # The interpolating mode between targets. STEP (0), LINEAR (1), COSINE (2) + +# PID values +VEL_KP = 30.0 # Velocity proportional (P) gain +VEL_KI = 0.0 # Velocity integral (I) gain +VEL_KD = 0.4 # Velocity derivative (D) gain + + +# Free up hardware resources ahead of creating a new Encoder +gc.collect() + +# Create a motor and set its speed scale +m = Motor(MOTOR_PINS, direction=DIRECTION, speed_scale=SPEED_SCALE, deadzone=0.05) + +# Create an encoder, using PIO 0 and State Machine 0 +enc = Encoder(0, 0, ENCODER_PINS, direction=DIRECTION, counts_per_rev=COUNTS_PER_REV, count_microsteps=True) + +# Create the user button +user_sw = Button(motor2040.USER_SW) + +# Create PID object for velocity control +vel_pid = PID(VEL_KP, VEL_KI, VEL_KD, UPDATE_RATE) + +# Enable the motor to get started +m.enable() + +# Set the initial target velocity +vel_pid.target = VELOCITY_EXTENT + + +update = 0 +print_count = 0 + +# Set the initial value and create a random end value between the extents +start_value = 0.0 +end_value = random.uniform(-VELOCITY_EXTENT, VELOCITY_EXTENT) + +# Continually move the motor until the user button is pressed +while user_sw.raw() is not True: + + # Capture the state of the encoder + capture = enc.capture() + + # Calculate how far along this movement to be + percent_along = min(update / UPDATES_PER_MOVE, 1.0) + + if INTERP_MODE == 0: + # Move the motor instantly to the end value + vel_pid.target = end_value + elif INTERP_MODE == 2: + # Move the motor between values using cosine + vel_pid.target = (((-math.cos(percent_along * math.pi) + 1.0) / 2.0) * (end_value - start_value)) + start_value + else: + # Move the motor linearly between values + vel_pid.target = (percent_along * (end_value - start_value)) + start_value + + # Calculate the acceleration to apply to the motor to move it closer to the velocity target + 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 targets, but only on every multiple + if print_count == 0: + print("Vel =", capture.revolutions_per_second, end=", ") + print("Targ Vel =", vel_pid.target, end=", ") + print("Accel =", accel * ACC_PRINT_SCALE, end=", ") + print("Speed =", m.speed()) + + # Increment the print count, and wrap it + print_count = (print_count + 1) % PRINT_DIVIDER + + update += 1 # 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 = random.uniform(-VELOCITY_EXTENT, VELOCITY_EXTENT) + + time.sleep(UPDATE_RATE) + +# Disable the motor +m.disable() From 6b3ba659a2e8298a2da4c7acee5cd5653f50e83e Mon Sep 17 00:00:00 2001 From: ZodiusInfuser Date: Thu, 21 Apr 2022 21:14:41 +0100 Subject: [PATCH 27/60] Added a reactive encoder example --- micropython/examples/motor2040/README.md | 94 +++++++++++-- .../examples/motor2040/motor_angles.py | 45 ++++++ .../examples/motor2040/motor_encoders.py | 40 ------ .../motor2040/motor_encoders_capture.py | 50 ------- .../examples/motor2040/reactive_encoder.py | 132 ++++++++++++++++++ .../examples/motor2040/simple_easing.py | 64 --------- 6 files changed, 257 insertions(+), 168 deletions(-) create mode 100644 micropython/examples/motor2040/motor_angles.py delete mode 100644 micropython/examples/motor2040/motor_encoders.py delete mode 100644 micropython/examples/motor2040/motor_encoders_capture.py create mode 100644 micropython/examples/motor2040/reactive_encoder.py delete mode 100644 micropython/examples/motor2040/simple_easing.py diff --git a/micropython/examples/motor2040/README.md b/micropython/examples/motor2040/README.md index 4bb84e0a..5c0fed62 100644 --- a/micropython/examples/motor2040/README.md +++ b/micropython/examples/motor2040/README.md @@ -4,15 +4,26 @@ - [Single Motor](#single-motor) - [Multiple Motors](#multiple-motors) - [Motor Cluster](#motor-cluster) - - [Simple Easing](#simple-easing) - [Motor Wave](#motor-wave) - - [Calibration](#calibration) + - [Turn Off Motors](#turn-off-motors) +- [Encoder Examples](#encoder-examples) + - [Motor Angles](#motor-angles) + - [Motor Profiler](#motor-profiler) - [Function Examples](#function-examples) - [Read Sensors](#read-sensors) - [Sensor Feedback](#sensor-feedback) - [Current Meter](#current-meter) - [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) +- [Tuning Examples](#tuning-examples) + - [Position Tuning](#position-tuning) + - [Velocity Tuning](#velocity-tuning) + - [Position on Velocity Tuning](#position-on-velocity-tuning) ## Motor Examples @@ -35,41 +46,50 @@ Demonstrates how to create multiple Motor objects and control them together. Demonstrates how to create a MotorCluster object to control multiple motors at once. -### Simple Easing -[simple_easing.py](simple_easing.py) - -An example of how to move a motor smoothly between random positions. - - ### Motor Wave [motor_wave.py](motor_wave.py) An example of applying a wave pattern to a group of motors and the LEDs. -### Calibration -[calibration.py](calibration.py) +### Turn Off Motors +[turn_off_motors.py](turn_off_motors.py) -Shows how to create motors with different common calibrations, modify a motor's existing calibration, and create a motor with a custom calibration. +A simple program that turns off the motors. + + +## Encoder Examples + +### Motor Angles +[motor_angles.py](motor_angles.py) + +Demonstrates how to read the angles of Motor 2040's four encoders. + + +### Motor Profiler +[motor_profiler.py](motor_profiler.py) + +A program that profiles the speed of a motor across its PWM +duty cycle range using the attached encoder for feedback. ## Function Examples ### Read Sensors [read_sensors.py](read_sensors.py) - +TODO Shows how to initialise and read the 6 external and 2 internal sensors of Motor 2040. ### Sensor Feedback [sensor_feedback.py](sensor_feedback.py) - +TODO Show how to read the 6 external sensors and display their values on the neighbouring LEDs. ### Current Meter [current_meter.py](current_meter.py) - +TODO An example of how to use Motor 2040's current measuring ability and display the value on the onboard LED bar. @@ -83,3 +103,49 @@ Displays a rotating rainbow pattern on the Motor 2040's onboard LED. [turn_off_led.py](turn_off_led.py) A simple program that turns off the onboard LED. + + +## Control Examples + +### Position Control +[position_control.py](position_control.py) + +An example of how to move a motor smoothly between random positions, with the help of it's attached encoder and PID control. + + +### Velocity Control +[velocity_control.py](velocity_control.py) + +An example of how to drive a motor smoothly between random speeds, with the help of it's attached encoder and PID control. + + +### Position on Velocity Control +[position_on_velocity_control.py](position_on_velocity_control.py) + +An example of how to move a motor smoothly between random positions, with velocity limits, with the help of it's attached encoder and PID control. + + +### Reactive Encoder +[reactive_encoder.py](reactive_encoder.py) + +A demonstration of how a motor with an encoder can be used as a programmable rotary encoder for user input, with force-feedback for arbitrary detents and end stops. + + +## Tuning Examples + +### Position Tuning +[position_tuning.py](position_tuning.py) + +A program to aid in the discovery and tuning of motor PID values for position control. It does this by commanding the motor to move repeatedly between two target angles and plots the measured response. + + +### Velocity Tuning +[velocity_tuning.py](velocity_tuning.py) + +A program to aid in the discovery and tuning of motor PID values for velocity control. It does this by commanding the motor to drive repeatedly between two target speeds and plots the measured response. + + +### Position on Velocity Tuning +[position_on_velocity_tuning.py](position_on_velocity_tuning.py) + +A program to aid in the discovery and tuning of motor PID values for position on velocity control. It does this by commanding the motor to move repeatedly between two target angles and plots the measured response. diff --git a/micropython/examples/motor2040/motor_angles.py b/micropython/examples/motor2040/motor_angles.py new file mode 100644 index 00000000..40d6010e --- /dev/null +++ b/micropython/examples/motor2040/motor_angles.py @@ -0,0 +1,45 @@ +import gc +import time +from pimoroni import Button +from motor import motor2040 +from encoder import Encoder, MMME_CPR +# from encoder import REVERSED + +""" +Demonstrates how to read the angles of Motor 2040's four encoders. + +Press "Boot" to exit the program. +""" + +GEAR_RATIO = 50 # The gear ratio of the motor +COUNTS_PER_REV = MMME_CPR * GEAR_RATIO # The counts per revolution of the motor's output shaft + + +# Free up hardware resources ahead of creating a new Encoder +gc.collect() + +# Create a list of encoders +ENCODER_PINS = [motor2040.ENCODER_A, motor2040.ENCODER_B, motor2040.ENCODER_C, motor2040.ENCODER_D] +ENCODER_NAMES = ["A", "B", "C", "D"] +NUM_ENCODERS = len(ENCODER_PINS) +encoders = [Encoder(0, i, ENCODER_PINS[i], counts_per_rev=COUNTS_PER_REV, count_microsteps=True) for i in range(NUM_ENCODERS)] + +# Uncomment the below lines (and the top import) to +# reverse the counting direction of an encoder +# encoders[0].direction(REVERSED) +# encoders[1].direction(REVERSED) +# encoders[2].direction(REVERSED) +# encoders[3].direction(REVERSED) + +# Create the user button +user_sw = Button(motor2040.USER_SW) + +# Read the encoders until the user button is pressed +while user_sw.raw() is not True: + + # Print out the angle of each encoder + for i in range(NUM_ENCODERS): + print(ENCODER_NAMES[i], "=", encoders[i].degrees(), end=", ") + print() + + time.sleep(0.1) diff --git a/micropython/examples/motor2040/motor_encoders.py b/micropython/examples/motor2040/motor_encoders.py deleted file mode 100644 index bdcbef48..00000000 --- a/micropython/examples/motor2040/motor_encoders.py +++ /dev/null @@ -1,40 +0,0 @@ -import gc -import time -from pimoroni import Button -from encoder import Encoder -# from encoder import REVERSED -from motor import motor2040 - -""" -Demonstrates how to read the counts of multiple motor encoders. - -Press "Boot" to exit the program. -""" - -# 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 -enc_a = Encoder(0, 0, motor2040.ENCODER_A, count_microsteps=True) -enc_b = Encoder(0, 1, motor2040.ENCODER_B, count_microsteps=True) -enc_c = Encoder(0, 2, motor2040.ENCODER_C, count_microsteps=True) -enc_d = Encoder(0, 3, motor2040.ENCODER_D, count_microsteps=True) - -# Uncomment the below line (and the top import) to reverse the counting direction -# enc_a.direction(REVERSED) -# enc_b.direction(REVERSED) -# enc_c.direction(REVERSED) -# enc_d.direction(REVERSED) - -# Create the user button -user_sw = Button(motor2040.USER_SW) - -# Read the encoders until the user button is pressed -while user_sw.raw() is not True: - print("A =", enc_a.count(), end=", ") - print("B =", enc_b.count(), end=", ") - print("C =", enc_c.count(), end=", ") - print("D =", enc_d.count()) - - time.sleep(0.1) - diff --git a/micropython/examples/motor2040/motor_encoders_capture.py b/micropython/examples/motor2040/motor_encoders_capture.py deleted file mode 100644 index 8e1d4e3f..00000000 --- a/micropython/examples/motor2040/motor_encoders_capture.py +++ /dev/null @@ -1,50 +0,0 @@ -import gc -import time -from pimoroni import Button -from encoder import Encoder, MMME_CPR -# from encoder import REVERSED -from motor import motor2040 - -""" -Demonstrates how to read the counts of multiple motor encoders. - -Press "Boot" to exit the program. -""" - -# Free up hardware resources ahead of creating a new Encoder -gc.collect() - -# The gear ratio of the motor the encoder is attached to. -GEAR_RATIO = 50 -CPR = MMME_CPR * GEAR_RATIO - - -# Create a list of motors -ENCODER_PINS = [ motor2040.ENCODER_A, motor2040.ENCODER_B, motor2040.ENCODER_C, motor2040.ENCODER_D ] -ENCODER_NAMES = [ "A", "B", "C", "D" ] -NUM_ENCODERS = len(ENCODER_PINS) -encoders = [Encoder(0, i, ENCODER_PINS[i], counts_per_rev=CPR, count_microsteps=True) for i in range(NUM_ENCODERS)] - -# Uncomment the below line (and the top import) to reverse the counting direction -# encoders[0].direction(REVERSED) -# encoders[1].direction(REVERSED) -# encoders[2].direction(REVERSED) -# encoders[3].direction(REVERSED) - -# Create the user button -user_sw = Button(motor2040.USER_SW) - -captures = [None] * NUM_ENCODERS - -# Read the encoders until the user button is pressed -while user_sw.raw() is not True: - # Capture the state of all encoders, at as close to the same moment as possible - for i in range(NUM_ENCODERS): - captures[i] = encoders[i].take_snapshot() - - # Print out the angle and speed of each encoder - for i in range(NUM_ENCODERS): - print(ENCODER_NAMES[i], "=", captures[i].degrees(), "|", captures[i].degrees_per_second(), end=", ") - - print() - time.sleep(0.1) diff --git a/micropython/examples/motor2040/reactive_encoder.py b/micropython/examples/motor2040/reactive_encoder.py new file mode 100644 index 00000000..a6f62d2c --- /dev/null +++ b/micropython/examples/motor2040/reactive_encoder.py @@ -0,0 +1,132 @@ +import gc +import time +from pimoroni import Button, PID +from plasma import WS2812 +from motor import Motor, motor2040 +from encoder import Encoder, MMME_CPR + +""" +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. + +NOTE: Plasma WS2812 uses the RP2040's PIO system, and as +such may have problems when running code multiple times. +If you encounter issues, try resetting your board. +""" + +MOTOR_PINS = motor2040.MOTOR_A # The pins of the motor being profiled +ENCODER_PINS = motor2040.ENCODER_A # The pins of the encoder attached to the profiled motor +GEAR_RATIO = 50 # The gear ratio of the motor +COUNTS_PER_REV = MMME_CPR * GEAR_RATIO # The counts per revolution of the motor's output shaft + +DIRECTION = 0 # The direction to spin the motor in. NORMAL (0), REVERSED (1) +SPEED_SCALE = 5.4 # The scaling to apply to the motor's speed to match its real-world speed + +UPDATES = 100 # How many times to update the motor per second +UPDATE_RATE = 1 / UPDATES +PRINT_DIVIDER = 4 # How many of the updates should be printed (i.e. 2 would be every other update) + +# Multipliers for the different printed values, so they appear nicely on the Thonny plotter +SPD_PRINT_SCALE = 20 # Driving Speed multipler + +DETENT_SIZE = 20 # The size (in degrees) of each detent region +MIN_DETENT = -12 # The minimum detent that can be counted to +MAX_DETENT = +12 # The maximum detent that can be counted to +MAX_DRIVE_PERCENT = 0.5 # The maximum drive force (as a percent) to apply when crossing detents + +# PID values +POS_KP = 0.14 # Position proportional (P) gain +POS_KI = 0.0 # Position integral (I) gain +POS_KD = 0.0022 # Position derivative (D) gain + +BRIGHTNESS = 0.4 # The brightness of the LEDs + + +# Free up hardware resources ahead of creating a new Encoder +gc.collect() + +# Create a motor and set its speed scale +m = Motor(MOTOR_PINS, direction=DIRECTION, speed_scale=SPEED_SCALE, deadzone=0.05) + +# Create an encoder, using PIO 0 and State Machine 0 +enc = Encoder(0, 0, ENCODER_PINS, direction=DIRECTION, counts_per_rev=COUNTS_PER_REV, count_microsteps=True) + +# Create the LED, using PIO 0 and State Machine 1 +led = WS2812(motor2040.NUM_LEDS, 0, 1, motor2040.LED_DATA) + +# Create the user button +user_sw = Button(motor2040.USER_SW) + +# Create PID object for position control +pos_pid = PID(POS_KP, POS_KI, POS_KD, UPDATE_RATE) + +# Start updating the LED +led.start() + +# Enable the motor to get started +m.enable() + + +current_detent = 0 + + +# Function to deal with a detent change +def detent_change(change): + global current_detent + + # Update the current detent and pid target + current_detent += change + pos_pid.target = (current_detent * DETENT_SIZE) # Update the motor position target + print("Detent =", current_detent) + + # Convert the current detent to a hue and set the onboard led to it + hue = (current_detent - MIN_DETENT) / (MAX_DETENT - MIN_DETENT) + led.set_hsv(0, hue, 1.0, BRIGHTNESS) + + +# Call the function once to set the target and print the value +detent_change(0) + + +# Continually move the motor until the user button is pressed +while user_sw.raw() is not True: + + # Capture the state of the encoder + capture = enc.capture() + + # Get the current detent's centre angle + detent_angle = (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? + elif 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 target + 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) and (capture.degrees <= MAX_DETENT * DETENT_SIZE): + vel = max(min(vel, MAX_DRIVE_PERCENT), -MAX_DRIVE_PERCENT) + + # Set the new motor driving speed + m.speed(vel) + + time.sleep(UPDATE_RATE) + +# Disable the motor +m.disable() + +# Turn off the LED +led.clear() diff --git a/micropython/examples/motor2040/simple_easing.py b/micropython/examples/motor2040/simple_easing.py deleted file mode 100644 index 54647ebc..00000000 --- a/micropython/examples/motor2040/simple_easing.py +++ /dev/null @@ -1,64 +0,0 @@ -import time -import math -import random -from pimoroni import Button -from servo import Servo, servo2040 - -""" -An example of how to move a servo smoothly between random positions. - -Press "Boot" to exit the program. -""" - -UPDATES = 50 # How many times to update Servos per second -TIME_FOR_EACH_MOVE = 2 # The time to travel between each random value -UPDATES_PER_MOVE = TIME_FOR_EACH_MOVE * UPDATES - -SERVO_EXTENT = 80 # How far from zero to move the servo -USE_COSINE = True # Whether or not to use a cosine path between values - -# Create a servo on pin 0 -s = Servo(servo2040.SERVO_1) - -# Get the initial value and create a random end value between the extents -start_value = s.mid_value() -end_value = random.uniform(-SERVO_EXTENT, SERVO_EXTENT) - -# Create the user button -user_sw = Button(servo2040.USER_SW) - - -update = 0 - -# Continually move the servo until the user button is pressed -while user_sw.raw() is not True: - - # Calculate how far along this movement to be - percent_along = update / UPDATES_PER_MOVE - - if USE_COSINE: - # Move the servo between values using cosine - s.to_percent(math.cos(percent_along * math.pi), 1.0, -1.0, start_value, end_value) - else: - # Move the servo linearly between values - s.to_percent(percent_along, 0.0, 1.0, start_value, end_value) - - # Print out the value the servo is now at - print("Value = ", round(s.value(), 3), sep="") - - # Move along in time - update += 1 - - # Have we reached the end of this movement? - if update >= UPDATES_PER_MOVE: - # Reset the counter - update = 0 - - # Set the start as the last end and create a new random end value - start_value = end_value - end_value = random.uniform(-SERVO_EXTENT, SERVO_EXTENT) - - time.sleep(1.0 / UPDATES) - -# Disable the servo -s.disable() From 98c57741b63eddce7b38a108bd7bb9fc3f321bba Mon Sep 17 00:00:00 2001 From: ZodiusInfuser Date: Thu, 21 Apr 2022 21:22:35 +0100 Subject: [PATCH 28/60] linting fix --- micropython/modules_py/pimoroni.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/micropython/modules_py/pimoroni.py b/micropython/modules_py/pimoroni.py index 17a33bf8..4b39e1c2 100644 --- a/micropython/modules_py/pimoroni.py +++ b/micropython/modules_py/pimoroni.py @@ -157,7 +157,7 @@ class PID: def calculate(self, value, value_change=None): error = self.target - value self._error_sum += error * self._sample_rate - if value_change == None: + if value_change is None: rate_error = (value - self._last_value) / self._sample_rate else: rate_error = value_change From 652612fc311f2c750615fad3a1b192aa8169c27c Mon Sep 17 00:00:00 2001 From: ZodiusInfuser Date: Fri, 22 Apr 2022 13:59:40 +0100 Subject: [PATCH 29/60] Removed old C++ motor implementation --- drivers/motor/CMakeLists.txt | 1 - drivers/motor/motor.cpp | 288 +++++++++++------- drivers/motor/motor.hpp | 77 ++--- drivers/motor/motor2.cmake | 15 - drivers/motor/motor2.cpp | 223 -------------- drivers/motor/motor2.hpp | 84 ----- examples/motor2040/motor2040_single_motor.cpp | 2 +- examples/pico_motor_shim/balance/demo.cpp | 4 +- examples/pico_motor_shim/sequence/demo.cpp | 12 +- examples/pico_motor_shim/song/demo.cpp | 22 +- libraries/motor2040/motor2040.cmake | 2 +- libraries/motor2040/motor2040.hpp | 2 +- libraries/pico_motor_shim/pico_motor_shim.hpp | 2 +- micropython/modules/motor/micropython.cmake | 2 +- micropython/modules/motor/motor.cpp | 6 +- 15 files changed, 252 insertions(+), 490 deletions(-) delete mode 100644 drivers/motor/motor2.cmake delete mode 100644 drivers/motor/motor2.cpp delete mode 100644 drivers/motor/motor2.hpp diff --git a/drivers/motor/CMakeLists.txt b/drivers/motor/CMakeLists.txt index af7cdfff..36dba68a 100644 --- a/drivers/motor/CMakeLists.txt +++ b/drivers/motor/CMakeLists.txt @@ -1,3 +1,2 @@ include(motor.cmake) -include(motor2.cmake) include(motor_cluster.cmake) \ No newline at end of file diff --git a/drivers/motor/motor.cpp b/drivers/motor/motor.cpp index 73c812c4..d08fcc29 100644 --- a/drivers/motor/motor.cpp +++ b/drivers/motor/motor.cpp @@ -1,14 +1,16 @@ #include "motor.hpp" +#include "hardware/clocks.h" #include "pwm.hpp" +#include "math.h" namespace motor { - Motor::Motor(const pin_pair &pins, float freq, DecayMode mode) - : pins(pins), pwm_frequency(freq), motor_decay_mode(mode) { + Motor::Motor(const pin_pair &pins, Direction direction, float speed_scale, float deadzone, float freq, DecayMode mode) + : motor_pins(pins), state(direction, speed_scale, deadzone), pwm_frequency(freq), motor_mode(mode) { } Motor::~Motor() { - gpio_set_function(pins.positive, GPIO_FUNC_NULL); - gpio_set_function(pins.negative, GPIO_FUNC_NULL); + gpio_set_function(motor_pins.positive, GPIO_FUNC_NULL); + gpio_set_function(motor_pins.negative, GPIO_FUNC_NULL); } bool Motor::init() { @@ -20,128 +22,202 @@ namespace motor { 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); + // Set the new wrap (should be 1 less than the period to get full 0 to 100%) + pwm_config_set_wrap(&pwm_cfg, pwm_period - 1); - //Apply the divider - pwm_config_set_clkdiv(&pwm_cfg, (float)div16 / 16.0f); + // 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... - pwm_init(pwm_gpio_to_slice_num(pins.positive), &pwm_cfg, true); - gpio_set_function(pins.positive, GPIO_FUNC_PWM); + 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); - pwm_init(pwm_gpio_to_slice_num(pins.negative), &pwm_cfg, true); - gpio_set_function(pins.negative, GPIO_FUNC_PWM); - update_pwm(); + 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); success = true; } return success; } - float Motor::get_speed() { - return motor_speed; + pin_pair Motor::pins() const { + return motor_pins; } - void Motor::set_speed(float speed) { - motor_speed = MIN(MAX(speed, -1.0f), 1.0f); - update_pwm(); - } - - float Motor::get_frequency() { - return pwm_frequency; - } - - bool Motor::set_frequency(float freq) { - bool success = false; - - //Calculate a suitable pwm wrap period for this frequency - uint16_t period; uint16_t div16; - if(pimoroni::calculate_pwm_factors(freq, period, div16)) { - - //Record if the new period will be larger or smaller. - //This is used to apply new pwm values 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; - pwm_frequency = freq; - - uint pos_num = pwm_gpio_to_slice_num(pins.positive); - uint neg_num = pwm_gpio_to_slice_num(pins.negative); - - //Apply the new divider - uint8_t div = div16 >> 4; - uint8_t mod = div16 % 16; - pwm_set_clkdiv_int_frac(pos_num, div, mod); - if(neg_num != pos_num) { - pwm_set_clkdiv_int_frac(neg_num, div, mod); - } - - //If the the period is larger, update the pwm before setting the new wraps - if(pre_update_pwm) - update_pwm(); - - //Set the new wrap (should be 1 less than the period to get full 0 to 100%) - pwm_set_wrap(pos_num, pwm_period - 1); - if(neg_num != pos_num) { - pwm_set_wrap(neg_num, pwm_period - 1); - } - - //If the the period is smaller, update the pwm after setting the new wraps - if(!pre_update_pwm) - update_pwm(); - - success = true; - } - return success; - } - - Motor::DecayMode Motor::get_decay_mode() { - return motor_decay_mode; - } - - void Motor::set_decay_mode(Motor::DecayMode mode) { - motor_decay_mode = mode; - update_pwm(); - } - - void Motor::stop() { - motor_speed = 0.0f; - update_pwm(); + void Motor::enable() { + apply_duty(state.enable_with_return(), motor_mode); } void Motor::disable() { - motor_speed = 0.0f; - pwm_set_gpio_level(pins.positive, 0); - pwm_set_gpio_level(pins.negative, 0); + apply_duty(state.disable_with_return(), motor_mode); } - void Motor::update_pwm() { - int32_t signed_duty_cycle = (int32_t)(motor_speed * (float)pwm_period); + bool Motor::is_enabled() const { + return state.is_enabled(); + } - switch(motor_decay_mode) { - case SLOW_DECAY: //aka 'Braking' - if(signed_duty_cycle >= 0) { - pwm_set_gpio_level(pins.positive, pwm_period); - pwm_set_gpio_level(pins.negative, pwm_period - signed_duty_cycle); - } - else { - pwm_set_gpio_level(pins.positive, pwm_period + signed_duty_cycle); - pwm_set_gpio_level(pins.negative, pwm_period); - } - break; + float Motor::duty() const { + return state.get_duty(); + } - case FAST_DECAY: //aka 'Coasting' - default: - if(signed_duty_cycle >= 0) { - pwm_set_gpio_level(pins.positive, signed_duty_cycle); - pwm_set_gpio_level(pins.negative, 0); + void Motor::duty(float duty) { + apply_duty(state.set_duty_with_return(duty), motor_mode); + } + + float Motor::speed() const { + return state.get_speed(); + } + + void Motor::speed(float speed) { + 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)) { + + // 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; + pwm_frequency = freq; + + uint pos_pin_num = pwm_gpio_to_slice_num(motor_pins.positive); + uint neg_pin_num = pwm_gpio_to_slice_num(motor_pins.negative); + + // Apply the new divider + uint8_t div = div16 >> 4; + uint8_t mod = div16 % 16; + pwm_set_clkdiv_int_frac(pos_pin_num, div, mod); + if(neg_pin_num != pos_pin_num) + pwm_set_clkdiv_int_frac(neg_pin_num, div, mod); + + // If the period is larger, update the pwm before setting the new wraps + if(pre_update_pwm) { + apply_duty(state.get_deadzoned_duty(), motor_mode); + } + + // Set the new wrap (should be 1 less than the period to get full 0 to 100%) + pwm_set_wrap(pos_pin_num, pwm_period - 1); + if(neg_pin_num != pos_pin_num) + pwm_set_wrap(neg_pin_num, pwm_period - 1); + + // If the period is smaller, update the pwm after setting the new wraps + if(!pre_update_pwm) { + apply_duty(state.get_deadzoned_duty(), motor_mode); + } + + success = true; } - else { - pwm_set_gpio_level(pins.positive, 0); - pwm_set_gpio_level(pins.negative, 0 - signed_duty_cycle); + } + return success; + } + + void Motor::stop() { + apply_duty(state.stop_with_return(), motor_mode); + } + + void Motor::coast() { + apply_duty(state.stop_with_return(), FAST_DECAY); + } + + void Motor::brake() { + apply_duty(state.stop_with_return(), SLOW_DECAY); + } + + void Motor::full_negative() { + apply_duty(state.full_negative_with_return(), motor_mode); + } + + void Motor::full_positive() { + apply_duty(state.full_positive_with_return(), motor_mode); + } + + void Motor::to_percent(float in, float in_min, float in_max) { + 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) { + 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::deadzone() const { + return state.get_deadzone(); + } + + void Motor::deadzone(float deadzone) { + 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; + apply_duty(state.get_deadzoned_duty(), motor_mode); + } + + void Motor::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; } - break; + } + else { + pwm_set_gpio_level(motor_pins.positive, 0); + pwm_set_gpio_level(motor_pins.negative, 0); } } }; \ No newline at end of file diff --git a/drivers/motor/motor.hpp b/drivers/motor/motor.hpp index 921fbff5..45638097 100644 --- a/drivers/motor/motor.hpp +++ b/drivers/motor/motor.hpp @@ -10,42 +10,23 @@ using namespace pimoroni; namespace motor { class Motor { - //-------------------------------------------------- - // Enums - //-------------------------------------------------- - public: - enum DecayMode { - FAST_DECAY = 0, //aka 'Coasting' - SLOW_DECAY = 1, //aka 'Braking' - }; - - //-------------------------------------------------- - // Constants - //-------------------------------------------------- - public: - static const uint16_t DEFAULT_PWM_FREQUENCY = 25000; // Chose 25KHz because it is outside of hearing - // and divides nicely into the RP2040's 125MHz PWM frequency - static const DecayMode DEFAULT_DECAY_MODE = SLOW_DECAY; - - //-------------------------------------------------- // Variables //-------------------------------------------------- private: - pin_pair pins; + pin_pair motor_pins; + MotorState state; pwm_config pwm_cfg; uint16_t pwm_period; - float pwm_frequency = DEFAULT_PWM_FREQUENCY; - - DecayMode motor_decay_mode = DEFAULT_DECAY_MODE; - float motor_speed = 0.0f; - + float pwm_frequency; + DecayMode motor_mode; //-------------------------------------------------- // Constructors/Destructor //-------------------------------------------------- public: - Motor(const pin_pair &pins, float freq = DEFAULT_PWM_FREQUENCY, DecayMode mode = DEFAULT_DECAY_MODE); + Motor(const pin_pair &pins, Direction direction = NORMAL, float speed_scale = MotorState::DEFAULT_SPEED_SCALE, + float deadzone = MotorState::DEFAULT_DEADZONE, float freq = MotorState::DEFAULT_FREQUENCY, DecayMode mode = MotorState::DEFAULT_DECAY_MODE); ~Motor(); @@ -55,21 +36,49 @@ namespace motor { public: bool init(); - float get_speed(); - void set_speed(float speed); + // For print access in micropython + pin_pair pins() const; - float get_frequency(); - bool set_frequency(float freq); + void enable(); + void disable(); + bool is_enabled() const; - DecayMode get_decay_mode(); - void set_decay_mode(DecayMode mode); + float duty() const; + void duty(float duty); + + float speed() const; + void speed(float speed); + + float frequency() const; + bool frequency(float freq); + + //-------------------------------------------------- void stop(); - void disable(); + 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 deadzone() const; + void deadzone(float deadzone); + + DecayMode decay_mode(); + void decay_mode(DecayMode mode); //-------------------------------------------------- private: - void update_pwm(); + void apply_duty(float duty, DecayMode mode); }; -} +} \ No newline at end of file diff --git a/drivers/motor/motor2.cmake b/drivers/motor/motor2.cmake deleted file mode 100644 index 85ad531d..00000000 --- a/drivers/motor/motor2.cmake +++ /dev/null @@ -1,15 +0,0 @@ -set(DRIVER_NAME motor2) -add_library(${DRIVER_NAME} INTERFACE) - -target_sources(${DRIVER_NAME} INTERFACE - ${CMAKE_CURRENT_LIST_DIR}/motor2.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 - ) \ No newline at end of file diff --git a/drivers/motor/motor2.cpp b/drivers/motor/motor2.cpp deleted file mode 100644 index 71c0c4fa..00000000 --- a/drivers/motor/motor2.cpp +++ /dev/null @@ -1,223 +0,0 @@ -#include "motor2.hpp" -#include "hardware/clocks.h" -#include "pwm.hpp" -#include "math.h" - -namespace motor { - Motor2::Motor2(const pin_pair &pins, Direction direction, float speed_scale, float deadzone, float freq, DecayMode mode) - : motor_pins(pins), state(direction, speed_scale, deadzone), pwm_frequency(freq), motor_mode(mode) { - } - - Motor2::~Motor2() { - gpio_set_function(motor_pins.positive, GPIO_FUNC_NULL); - gpio_set_function(motor_pins.negative, GPIO_FUNC_NULL); - } - - bool Motor2::init() { - bool success = false; - - uint16_t period; uint16_t div16; - if(pimoroni::calculate_pwm_factors(pwm_frequency, period, div16)) { - pwm_period = period; - - 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, pwm_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... - - 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); - - success = true; - } - return success; - } - - pin_pair Motor2::pins() const { - return motor_pins; - } - - void Motor2::enable() { - apply_duty(state.enable_with_return(), motor_mode); - } - - void Motor2::disable() { - apply_duty(state.disable_with_return(), motor_mode); - } - - bool Motor2::is_enabled() const { - return state.is_enabled(); - } - - float Motor2::duty() const { - return state.get_duty(); - } - - void Motor2::duty(float duty) { - apply_duty(state.set_duty_with_return(duty), motor_mode); - } - - float Motor2::speed() const { - return state.get_speed(); - } - - void Motor2::speed(float speed) { - apply_duty(state.set_speed_with_return(speed), motor_mode); - } - - float Motor2::frequency() const { - return pwm_frequency; - } - - bool Motor2::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)) { - - // 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; - pwm_frequency = freq; - - uint pos_pin_num = pwm_gpio_to_slice_num(motor_pins.positive); - uint neg_pin_num = pwm_gpio_to_slice_num(motor_pins.negative); - - // Apply the new divider - uint8_t div = div16 >> 4; - uint8_t mod = div16 % 16; - pwm_set_clkdiv_int_frac(pos_pin_num, div, mod); - if(neg_pin_num != pos_pin_num) - pwm_set_clkdiv_int_frac(neg_pin_num, div, mod); - - // If the period is larger, update the pwm before setting the new wraps - if(pre_update_pwm) { - apply_duty(state.get_deadzoned_duty(), motor_mode); - } - - // Set the new wrap (should be 1 less than the period to get full 0 to 100%) - pwm_set_wrap(pos_pin_num, pwm_period - 1); - if(neg_pin_num != pos_pin_num) - pwm_set_wrap(neg_pin_num, pwm_period - 1); - - // If the period is smaller, update the pwm after setting the new wraps - if(!pre_update_pwm) { - apply_duty(state.get_deadzoned_duty(), motor_mode); - } - - success = true; - } - } - return success; - } - - void Motor2::stop() { - apply_duty(state.stop_with_return(), motor_mode); - } - - void Motor2::coast() { - apply_duty(state.stop_with_return(), FAST_DECAY); - } - - void Motor2::brake() { - apply_duty(state.stop_with_return(), SLOW_DECAY); - } - - void Motor2::full_negative() { - apply_duty(state.full_negative_with_return(), motor_mode); - } - - void Motor2::full_positive() { - apply_duty(state.full_positive_with_return(), motor_mode); - } - - void Motor2::to_percent(float in, float in_min, float in_max) { - apply_duty(state.to_percent_with_return(in, in_min, in_max), motor_mode); - } - - void Motor2::to_percent(float in, float in_min, float in_max, float speed_min, float speed_max) { - apply_duty(state.to_percent_with_return(in, in_min, in_max, speed_min, speed_max), motor_mode); - } - - Direction Motor2::direction() const { - return state.get_direction(); - } - - void Motor2::direction(Direction direction) { - state.set_direction(direction); - } - - float Motor2::speed_scale() const { - return state.get_speed_scale(); - } - - void Motor2::speed_scale(float speed_scale) { - state.set_speed_scale(speed_scale); - } - - float Motor2::deadzone() const { - return state.get_deadzone(); - } - - void Motor2::deadzone(float deadzone) { - apply_duty(state.set_deadzone_with_return(deadzone), motor_mode); - } - - DecayMode Motor2::decay_mode() { - return motor_mode; - } - - void Motor2::decay_mode(DecayMode mode) { - motor_mode = mode; - apply_duty(state.get_deadzoned_duty(), motor_mode); - } - - void Motor2::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); - } - } -}; \ No newline at end of file diff --git a/drivers/motor/motor2.hpp b/drivers/motor/motor2.hpp deleted file mode 100644 index ee1a095c..00000000 --- a/drivers/motor/motor2.hpp +++ /dev/null @@ -1,84 +0,0 @@ -#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 Motor2 { - //-------------------------------------------------- - // Variables - //-------------------------------------------------- - private: - pin_pair motor_pins; - MotorState state; - pwm_config pwm_cfg; - uint16_t pwm_period; - float pwm_frequency; - DecayMode motor_mode; - - //-------------------------------------------------- - // Constructors/Destructor - //-------------------------------------------------- - public: - Motor2(const pin_pair &pins, Direction direction = NORMAL, float speed_scale = MotorState::DEFAULT_SPEED_SCALE, - float deadzone = MotorState::DEFAULT_DEADZONE, float freq = MotorState::DEFAULT_FREQUENCY, DecayMode mode = MotorState::DEFAULT_DECAY_MODE); - ~Motor2(); - - - //-------------------------------------------------- - // 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 deadzone() const; - void deadzone(float deadzone); - - DecayMode decay_mode(); - void decay_mode(DecayMode mode); - - //-------------------------------------------------- - private: - void apply_duty(float duty, DecayMode mode); - }; - -} \ No newline at end of file diff --git a/examples/motor2040/motor2040_single_motor.cpp b/examples/motor2040/motor2040_single_motor.cpp index a151227c..d08767a1 100644 --- a/examples/motor2040/motor2040_single_motor.cpp +++ b/examples/motor2040/motor2040_single_motor.cpp @@ -22,7 +22,7 @@ constexpr float SWEEP_EXTENT = 90.0f; // Create a motor on pin 0 and 1 -Motor2 m = Motor2(motor2040::MOTOR_A); +Motor m = Motor(motor2040::MOTOR_A); int main() { diff --git a/examples/pico_motor_shim/balance/demo.cpp b/examples/pico_motor_shim/balance/demo.cpp index 95e98aa7..39ae4000 100644 --- a/examples/pico_motor_shim/balance/demo.cpp +++ b/examples/pico_motor_shim/balance/demo.cpp @@ -103,8 +103,8 @@ int main() { printf("Y: %f, Z: %f, AngErr: %f\n", y, z, angle_error); float output = angle_error * PROPORTIONAL; //No need to clamp this value as set_speed does this internally - motor_1.set_speed(output); - motor_2.set_speed(-output); + motor_1.speed(output); + motor_2.speed(-output); sleep_ms(1); } diff --git a/examples/pico_motor_shim/sequence/demo.cpp b/examples/pico_motor_shim/sequence/demo.cpp index 88a4a830..61ffda82 100644 --- a/examples/pico_motor_shim/sequence/demo.cpp +++ b/examples/pico_motor_shim/sequence/demo.cpp @@ -65,8 +65,8 @@ bool accelerate_over(float left_speed, float right_speed, uint32_t duration_ms) uint32_t ellapsed = 0; //Get the current motor speeds - float last_left = motor_1.get_speed(); - float last_right = motor_2.get_speed(); + float last_left = motor_1.speed(); + float last_right = motor_2.speed(); //Loops until the duration has elapsed, checking the button state every millisecond, and updating motor speeds while(ellapsed <= duration_ms) { @@ -75,16 +75,16 @@ bool accelerate_over(float left_speed, float right_speed, uint32_t duration_ms) //Calculate and set the new motor speeds float percentage = (float)ellapsed / (float)duration_ms; - motor_1.set_speed(((left_speed - last_left) * percentage) + last_left); - motor_2.set_speed(((right_speed - last_right) * percentage) + last_right); + motor_1.speed(((left_speed - last_left) * percentage) + last_left); + motor_2.speed(((right_speed - last_right) * percentage) + last_right); sleep_ms(1); ellapsed = millis() - start_time; } //Set the final motor speeds as loop may not reach 100% - motor_1.set_speed(left_speed); - motor_2.set_speed(right_speed); + motor_1.speed(left_speed); + motor_2.speed(right_speed); return true; } diff --git a/examples/pico_motor_shim/song/demo.cpp b/examples/pico_motor_shim/song/demo.cpp index b6404863..8f6df835 100644 --- a/examples/pico_motor_shim/song/demo.cpp +++ b/examples/pico_motor_shim/song/demo.cpp @@ -40,11 +40,11 @@ static const uint STATIONARY_TOGGLE_US = 2000; Button button_a(pico_motor_shim::BUTTON_A, Polarity::ACTIVE_LOW, 0); #ifdef USE_FAST_DECAY - Motor motor_1(pico_motor_shim::MOTOR_1, Motor::DEFAULT_PWM_FREQUENCY, Motor::FAST_DECAY); - Motor motor_2(pico_motor_shim::MOTOR_2, Motor::DEFAULT_PWM_FREQUENCY, Motor::FAST_DECAY); + Motor motor_1(pico_motor_shim::MOTOR_1, NORMAL, MotorState::DEFAULT_SPEED_SCALE, MotorState::DEFAULT_FREQUENCY, FAST_DECAY); + Motor motor_2(pico_motor_shim::MOTOR_2, NORMAL, MotorState::DEFAULT_SPEED_SCALE, MotorState::DEFAULT_FREQUENCY, FAST_DECAY); #else - Motor motor_1(pico_motor_shim::MOTOR_1, Motor::DEFAULT_PWM_FREQUENCY, Motor::SLOW_DECAY); - Motor motor_2(pico_motor_shim::MOTOR_2, Motor::DEFAULT_PWM_FREQUENCY, Motor::SLOW_DECAY); + Motor motor_1(pico_motor_shim::MOTOR_1, NORMAL, MotorState::DEFAULT_SPEED_SCALE, MotorState::DEFAULT_FREQUENCY, SLOW_DECAY); + Motor motor_2(pico_motor_shim::MOTOR_2, NORMAL, MotorState::DEFAULT_SPEED_SCALE, MotorState::DEFAULT_FREQUENCY, SLOW_DECAY); #endif static bool button_toggle = false; @@ -83,26 +83,26 @@ int main() { //Play the song for(uint i = 0; i < SONG_LENGTH && check_button_toggle(); i++) { - if(motor_1.set_frequency(SONG[i]) && motor_2.set_frequency(SONG[i])) { + if(motor_1.frequency(SONG[i]) && motor_2.frequency(SONG[i])) { #ifdef 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.set_speed(0.5f); - motor_2.set_speed(0.5f); + motor_1.duty(0.5f); + motor_2.duty(0.5f); sleep_us(STATIONARY_TOGGLE_US); t += STATIONARY_TOGGLE_US; - motor_1.set_speed(-0.5f); - motor_2.set_speed(-0.5f); + 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.set_speed(0.5f); - motor_2.set_speed(0.5f); + motor_1.duty(0.5f); + motor_2.duty(0.5f); sleep_ms(NOTE_DURATION_MS); #endif } diff --git a/libraries/motor2040/motor2040.cmake b/libraries/motor2040/motor2040.cmake index 60f67543..73ae2487 100644 --- a/libraries/motor2040/motor2040.cmake +++ b/libraries/motor2040/motor2040.cmake @@ -3,4 +3,4 @@ add_library(motor2040 INTERFACE) target_include_directories(motor2040 INTERFACE ${CMAKE_CURRENT_LIST_DIR}) # Pull in pico libraries that we need -target_link_libraries(motor2040 INTERFACE pico_stdlib plasma motor2 motor_cluster) \ No newline at end of file +target_link_libraries(motor2040 INTERFACE pico_stdlib plasma motor motor_cluster) \ No newline at end of file diff --git a/libraries/motor2040/motor2040.hpp b/libraries/motor2040/motor2040.hpp index acb8f17a..a89fd549 100644 --- a/libraries/motor2040/motor2040.hpp +++ b/libraries/motor2040/motor2040.hpp @@ -2,7 +2,7 @@ #include "pico/stdlib.h" #include "ws2812.hpp" -#include "motor2.hpp" +#include "motor.hpp" #include "motor_cluster.hpp" namespace motor { diff --git a/libraries/pico_motor_shim/pico_motor_shim.hpp b/libraries/pico_motor_shim/pico_motor_shim.hpp index 9cb90c20..e130ef0a 100644 --- a/libraries/pico_motor_shim/pico_motor_shim.hpp +++ b/libraries/pico_motor_shim/pico_motor_shim.hpp @@ -1,7 +1,7 @@ #pragma once #include "pico/stdlib.h" -#include "motor2.hpp" +#include "motor.hpp" namespace motor { namespace pico_motor_shim { diff --git a/micropython/modules/motor/micropython.cmake b/micropython/modules/motor/micropython.cmake index 80fb2aea..af0024db 100644 --- a/micropython/modules/motor/micropython.cmake +++ b/micropython/modules/motor/micropython.cmake @@ -7,7 +7,7 @@ target_sources(usermod_${MOD_NAME} INTERFACE ${CMAKE_CURRENT_LIST_DIR}/${MOD_NAME}.cpp ${CMAKE_CURRENT_LIST_DIR}/../../../drivers/pwm/pwm.cpp ${CMAKE_CURRENT_LIST_DIR}/../../../drivers/pwm/pwm_cluster.cpp - ${CMAKE_CURRENT_LIST_DIR}/../../../drivers/motor/motor2.cpp + ${CMAKE_CURRENT_LIST_DIR}/../../../drivers/motor/motor.cpp ${CMAKE_CURRENT_LIST_DIR}/../../../drivers/motor/motor_cluster.cpp ${CMAKE_CURRENT_LIST_DIR}/../../../drivers/motor/motor_state.cpp ) diff --git a/micropython/modules/motor/motor.cpp b/micropython/modules/motor/motor.cpp index 6a3d3bc9..0f35c73d 100644 --- a/micropython/modules/motor/motor.cpp +++ b/micropython/modules/motor/motor.cpp @@ -1,4 +1,4 @@ -#include "drivers/motor/motor2.hpp" +#include "drivers/motor/motor.hpp" #include "drivers/motor/motor_cluster.hpp" #include "common/pimoroni_common.hpp" #include @@ -19,7 +19,7 @@ extern "C" { /***** Variables Struct *****/ typedef struct _Motor_obj_t { mp_obj_base_t base; - Motor2* motor; + Motor* motor; } _Motor_obj_t; @@ -147,7 +147,7 @@ mp_obj_t Motor_make_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw, c self = m_new_obj_with_finaliser(_Motor_obj_t); self->base.type = &Motor_type; - self->motor = new Motor2(pins, (Direction)direction, speed_scale, deadzone, freq, (DecayMode)mode); + self->motor = new Motor(pins, (Direction)direction, speed_scale, deadzone, freq, (DecayMode)mode); self->motor->init(); return MP_OBJ_FROM_PTR(self); From ae606e7bf5eaba98c3fd8f75e0dd5caa62fe571d Mon Sep 17 00:00:00 2001 From: ZodiusInfuser Date: Fri, 22 Apr 2022 15:14:30 +0100 Subject: [PATCH 30/60] Removed MP accesors to Snapshot class --- micropython/modules/encoder/encoder.c | 44 --------- micropython/modules/encoder/encoder.cpp | 116 ------------------------ micropython/modules/encoder/encoder.h | 19 ---- 3 files changed, 179 deletions(-) diff --git a/micropython/modules/encoder/encoder.c b/micropython/modules/encoder/encoder.c index 1a2afdc1..23bec710 100644 --- a/micropython/modules/encoder/encoder.c +++ b/micropython/modules/encoder/encoder.c @@ -2,21 +2,6 @@ /***** Methods *****/ -MP_DEFINE_CONST_FUN_OBJ_1(Snapshot___del___obj, Snapshot___del__); -MP_DEFINE_CONST_FUN_OBJ_1(Snapshot_count_obj, Snapshot_count); -MP_DEFINE_CONST_FUN_OBJ_1(Snapshot_delta_obj, Snapshot_delta); -MP_DEFINE_CONST_FUN_OBJ_1(Snapshot_frequency_obj, Snapshot_frequency); -MP_DEFINE_CONST_FUN_OBJ_1(Snapshot_revolutions_obj, Snapshot_revolutions); -MP_DEFINE_CONST_FUN_OBJ_1(Snapshot_degrees_obj, Snapshot_degrees); -MP_DEFINE_CONST_FUN_OBJ_1(Snapshot_radians_obj, Snapshot_radians); -MP_DEFINE_CONST_FUN_OBJ_1(Snapshot_revolutions_delta_obj, Snapshot_revolutions_delta); -MP_DEFINE_CONST_FUN_OBJ_1(Snapshot_degrees_delta_obj, Snapshot_degrees_delta); -MP_DEFINE_CONST_FUN_OBJ_1(Snapshot_radians_delta_obj, Snapshot_radians_delta); -MP_DEFINE_CONST_FUN_OBJ_1(Snapshot_revolutions_per_second_obj, Snapshot_revolutions_per_second); -MP_DEFINE_CONST_FUN_OBJ_1(Snapshot_revolutions_per_minute_obj, Snapshot_revolutions_per_minute); -MP_DEFINE_CONST_FUN_OBJ_1(Snapshot_degrees_per_second_obj, Snapshot_degrees_per_second); -MP_DEFINE_CONST_FUN_OBJ_1(Snapshot_radians_per_second_obj, Snapshot_radians_per_second); - MP_DEFINE_CONST_FUN_OBJ_1(Encoder___del___obj, Encoder___del__); MP_DEFINE_CONST_FUN_OBJ_1(Encoder_pins_obj, Encoder_pins); MP_DEFINE_CONST_FUN_OBJ_1(Encoder_common_pin_obj, Encoder_common_pin); @@ -31,27 +16,9 @@ MP_DEFINE_CONST_FUN_OBJ_1(Encoder_degrees_obj, Encoder_degrees); MP_DEFINE_CONST_FUN_OBJ_1(Encoder_radians_obj, Encoder_radians); MP_DEFINE_CONST_FUN_OBJ_KW(Encoder_direction_obj, 1, Encoder_direction); MP_DEFINE_CONST_FUN_OBJ_KW(Encoder_counts_per_revolution_obj, 1, Encoder_counts_per_revolution); -MP_DEFINE_CONST_FUN_OBJ_1(Encoder_take_snapshot_obj, Encoder_take_snapshot); MP_DEFINE_CONST_FUN_OBJ_1(Encoder_capture_obj, Encoder_capture); /***** Binding of Methods *****/ -STATIC const mp_rom_map_elem_t Snapshot_locals_dict_table[] = { - { MP_ROM_QSTR(MP_QSTR___del__), MP_ROM_PTR(&Snapshot___del___obj) }, - { MP_ROM_QSTR(MP_QSTR_count), MP_ROM_PTR(&Snapshot_count_obj) }, - { MP_ROM_QSTR(MP_QSTR_delta), MP_ROM_PTR(&Snapshot_delta_obj) }, - { MP_ROM_QSTR(MP_QSTR_frequency), MP_ROM_PTR(&Snapshot_frequency_obj) }, - { MP_ROM_QSTR(MP_QSTR_revolutions), MP_ROM_PTR(&Snapshot_revolutions_obj) }, - { MP_ROM_QSTR(MP_QSTR_degrees), MP_ROM_PTR(&Snapshot_degrees_obj) }, - { MP_ROM_QSTR(MP_QSTR_radians), MP_ROM_PTR(&Snapshot_radians_obj) }, - { MP_ROM_QSTR(MP_QSTR_revolutions_delta), MP_ROM_PTR(&Snapshot_revolutions_delta_obj) }, - { MP_ROM_QSTR(MP_QSTR_degrees_delta), MP_ROM_PTR(&Snapshot_degrees_delta_obj) }, - { MP_ROM_QSTR(MP_QSTR_radians_delta), MP_ROM_PTR(&Snapshot_radians_delta_obj) }, - { MP_ROM_QSTR(MP_QSTR_revolutions_per_second), MP_ROM_PTR(&Snapshot_revolutions_per_second_obj) }, - { MP_ROM_QSTR(MP_QSTR_revolutions_per_minute), MP_ROM_PTR(&Snapshot_revolutions_per_minute_obj) }, - { MP_ROM_QSTR(MP_QSTR_degrees_per_second), MP_ROM_PTR(&Snapshot_degrees_per_second_obj) }, - { MP_ROM_QSTR(MP_QSTR_radians_per_second), MP_ROM_PTR(&Snapshot_radians_per_second_obj) }, -}; - STATIC const mp_rom_map_elem_t Encoder_locals_dict_table[] = { { MP_ROM_QSTR(MP_QSTR___del__), MP_ROM_PTR(&Encoder___del___obj) }, { MP_ROM_QSTR(MP_QSTR_pins), MP_ROM_PTR(&Encoder_pins_obj) }, @@ -67,22 +34,12 @@ STATIC const mp_rom_map_elem_t Encoder_locals_dict_table[] = { { MP_ROM_QSTR(MP_QSTR_radians), MP_ROM_PTR(&Encoder_radians_obj) }, { MP_ROM_QSTR(MP_QSTR_direction), MP_ROM_PTR(&Encoder_direction_obj) }, { MP_ROM_QSTR(MP_QSTR_counts_per_revolution), MP_ROM_PTR(&Encoder_counts_per_revolution_obj) }, - { MP_ROM_QSTR(MP_QSTR_take_snapshot), MP_ROM_PTR(&Encoder_take_snapshot_obj) }, { MP_ROM_QSTR(MP_QSTR_capture), MP_ROM_PTR(&Encoder_capture_obj) }, }; -STATIC MP_DEFINE_CONST_DICT(Snapshot_locals_dict, Snapshot_locals_dict_table); STATIC MP_DEFINE_CONST_DICT(Encoder_locals_dict, Encoder_locals_dict_table); /***** Class Definition *****/ -const mp_obj_type_t Snapshot_type = { - { &mp_type_type }, - .name = MP_QSTR_snapshot, - .print = Snapshot_print, - .make_new = Snapshot_make_new, - .locals_dict = (mp_obj_dict_t*)&Snapshot_locals_dict, -}; - const mp_obj_type_t Encoder_type = { { &mp_type_type }, .name = MP_QSTR_encoder, @@ -95,7 +52,6 @@ const mp_obj_type_t Encoder_type = { STATIC const mp_map_elem_t encoder_globals_table[] = { { MP_OBJ_NEW_QSTR(MP_QSTR___name__), MP_OBJ_NEW_QSTR(MP_QSTR_encoder) }, { MP_OBJ_NEW_QSTR(MP_QSTR_Encoder), (mp_obj_t)&Encoder_type }, - { MP_OBJ_NEW_QSTR(MP_QSTR_Snapshot), (mp_obj_t)&Snapshot_type }, { MP_ROM_QSTR(MP_QSTR_NORMAL), MP_ROM_INT(0x00) }, { MP_ROM_QSTR(MP_QSTR_REVERSED), MP_ROM_INT(0x01) }, diff --git a/micropython/modules/encoder/encoder.cpp b/micropython/modules/encoder/encoder.cpp index 5ee3ea0d..61d6ef78 100644 --- a/micropython/modules/encoder/encoder.cpp +++ b/micropython/modules/encoder/encoder.cpp @@ -12,110 +12,6 @@ extern "C" { #include "py/builtin.h" -/********** Snapshot **********/ - -/***** Variables Struct *****/ -typedef struct _Snapshot_obj_t { - mp_obj_base_t base; - Encoder::Snapshot snapshot; -} _Snapshot_obj_t; - - -/***** Print *****/ -void Snapshot_print(const mp_print_t *print, mp_obj_t self_in, mp_print_kind_t kind) { - (void)kind; //Unused input parameter - _Snapshot_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Snapshot_obj_t); - Encoder::Snapshot& snap = self->snapshot; - mp_print_str(print, "Snapshot(count = "); - mp_obj_print_helper(print, mp_obj_new_int(snap.count()), PRINT_REPR); - mp_print_str(print, ", delta = "); - mp_obj_print_helper(print, mp_obj_new_int(snap.delta()), PRINT_REPR); - mp_print_str(print, ", freq = "); - mp_obj_print_helper(print, mp_obj_new_float(snap.frequency()), PRINT_REPR); - mp_print_str(print, ")"); -} - - -/***** Constructor *****/ -mp_obj_t Snapshot_make_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw, const mp_obj_t *all_args) { - mp_raise_TypeError("cannot create an instance of Snapshot. They can only be created by calling take_snapshot on an Encoder object"); - return mp_const_none; -} - - -/***** Destructor ******/ -mp_obj_t Snapshot___del__(mp_obj_t self_in) { - return mp_const_none; -} - - -/***** Methods *****/ -mp_obj_t Snapshot_count(mp_obj_t self_in) { - _Snapshot_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Snapshot_obj_t); - return mp_obj_new_int(self->snapshot.count()); -} - -mp_obj_t Snapshot_delta(mp_obj_t self_in) { - _Snapshot_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Snapshot_obj_t); - return mp_obj_new_int(self->snapshot.delta()); -} - -mp_obj_t Snapshot_frequency(mp_obj_t self_in) { - _Snapshot_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Snapshot_obj_t); - return mp_obj_new_float(self->snapshot.frequency()); -} - -mp_obj_t Snapshot_revolutions(mp_obj_t self_in) { - _Snapshot_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Snapshot_obj_t); - return mp_obj_new_float(self->snapshot.revolutions()); -} - -mp_obj_t Snapshot_degrees(mp_obj_t self_in) { - _Snapshot_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Snapshot_obj_t); - return mp_obj_new_float(self->snapshot.degrees()); -} - -mp_obj_t Snapshot_radians(mp_obj_t self_in) { - _Snapshot_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Snapshot_obj_t); - return mp_obj_new_float(self->snapshot.radians()); -} - -mp_obj_t Snapshot_revolutions_delta(mp_obj_t self_in) { - _Snapshot_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Snapshot_obj_t); - return mp_obj_new_float(self->snapshot.revolutions_delta()); -} - -mp_obj_t Snapshot_degrees_delta(mp_obj_t self_in) { - _Snapshot_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Snapshot_obj_t); - return mp_obj_new_float(self->snapshot.degrees_delta()); -} - -mp_obj_t Snapshot_radians_delta(mp_obj_t self_in) { - _Snapshot_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Snapshot_obj_t); - return mp_obj_new_float(self->snapshot.radians_delta()); -} - -mp_obj_t Snapshot_revolutions_per_second(mp_obj_t self_in) { - _Snapshot_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Snapshot_obj_t); - return mp_obj_new_float(self->snapshot.revolutions_per_second()); -} - -mp_obj_t Snapshot_revolutions_per_minute(mp_obj_t self_in) { - _Snapshot_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Snapshot_obj_t); - return mp_obj_new_float(self->snapshot.revolutions_per_minute()); -} - -mp_obj_t Snapshot_degrees_per_second(mp_obj_t self_in) { - _Snapshot_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Snapshot_obj_t); - return mp_obj_new_float(self->snapshot.degrees_per_second()); -} - -mp_obj_t Snapshot_radians_per_second(mp_obj_t self_in) { - _Snapshot_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Snapshot_obj_t); - return mp_obj_new_float(self->snapshot.radians_per_second()); -} - - /********** Encoder **********/ /***** Variables Struct *****/ @@ -280,7 +176,6 @@ extern mp_obj_t Encoder_common_pin(mp_obj_t self_in) { return mp_obj_new_int(self->encoder->common_pin()); } - extern mp_obj_t Encoder_state(mp_obj_t self_in) { _Encoder_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Encoder_obj_t); bool_pair state = self->encoder->state(); @@ -406,17 +301,6 @@ extern mp_obj_t Encoder_counts_per_revolution(size_t n_args, const mp_obj_t *pos } } -extern mp_obj_t Encoder_take_snapshot(mp_obj_t self_in) { - _Encoder_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Encoder_obj_t); - - // Create a new MP Snapshot instance and assign a copy of the encoder's snapshot to it - _Snapshot_obj_t *snap = m_new_obj_with_finaliser(_Snapshot_obj_t); - snap->base.type = &Snapshot_type; - - snap->snapshot = self->encoder->take_snapshot(); - return MP_OBJ_FROM_PTR(snap); -} - extern mp_obj_t Encoder_capture(mp_obj_t self_in) { _Encoder_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Encoder_obj_t); diff --git a/micropython/modules/encoder/encoder.h b/micropython/modules/encoder/encoder.h index 257a5bd3..53eac0ec 100644 --- a/micropython/modules/encoder/encoder.h +++ b/micropython/modules/encoder/encoder.h @@ -2,27 +2,9 @@ #include "py/runtime.h" /***** Extern of Class Definition *****/ -extern const mp_obj_type_t Snapshot_type; extern const mp_obj_type_t Encoder_type; /***** Extern of Class Methods *****/ -extern void Snapshot_print(const mp_print_t *print, mp_obj_t self_in, mp_print_kind_t kind); -extern mp_obj_t Snapshot_make_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw, const mp_obj_t *all_args); -extern mp_obj_t Snapshot___del__(mp_obj_t self_in); -extern mp_obj_t Snapshot_count(mp_obj_t self_in); -extern mp_obj_t Snapshot_delta(mp_obj_t self_in); -extern mp_obj_t Snapshot_frequency(mp_obj_t self_in); -extern mp_obj_t Snapshot_revolutions(mp_obj_t self_in); -extern mp_obj_t Snapshot_degrees(mp_obj_t self_in); -extern mp_obj_t Snapshot_radians(mp_obj_t self_in); -extern mp_obj_t Snapshot_revolutions_delta(mp_obj_t self_in); -extern mp_obj_t Snapshot_degrees_delta(mp_obj_t self_in); -extern mp_obj_t Snapshot_radians_delta(mp_obj_t self_in); -extern mp_obj_t Snapshot_revolutions_per_second(mp_obj_t self_in); -extern mp_obj_t Snapshot_revolutions_per_minute(mp_obj_t self_in); -extern mp_obj_t Snapshot_degrees_per_second(mp_obj_t self_in); -extern mp_obj_t Snapshot_radians_per_second(mp_obj_t self_in); - extern void Encoder_print(const mp_print_t *print, mp_obj_t self_in, mp_print_kind_t kind); extern mp_obj_t Encoder_make_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw, const mp_obj_t *all_args); extern mp_obj_t Encoder___del__(mp_obj_t self_in); @@ -39,5 +21,4 @@ extern mp_obj_t Encoder_degrees(mp_obj_t self_in); extern mp_obj_t Encoder_radians(mp_obj_t self_in); extern mp_obj_t Encoder_direction(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); extern mp_obj_t Encoder_counts_per_revolution(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); -extern mp_obj_t Encoder_take_snapshot(mp_obj_t self_in); extern mp_obj_t Encoder_capture(mp_obj_t self_in); \ No newline at end of file From 1bde0acbefaecb3aa9771c75654a18152a6d883f Mon Sep 17 00:00:00 2001 From: ZodiusInfuser Date: Fri, 22 Apr 2022 15:22:32 +0100 Subject: [PATCH 31/60] Renamed target to setpoint to be more accurated to PID literature --- micropython/examples/motor2040/README.md | 6 ++-- .../examples/motor2040/position_control.py | 14 +++++----- .../motor2040/position_on_velocity_control.py | 26 ++++++++--------- .../motor2040/position_on_velocity_tuning.py | 28 +++++++++---------- .../examples/motor2040/position_tuning.py | 20 ++++++------- .../examples/motor2040/reactive_encoder.py | 8 +++--- .../examples/motor2040/velocity_control.py | 18 ++++++------ .../examples/motor2040/velocity_tuning.py | 20 ++++++------- micropython/modules_py/pimoroni.py | 4 +-- 9 files changed, 72 insertions(+), 72 deletions(-) diff --git a/micropython/examples/motor2040/README.md b/micropython/examples/motor2040/README.md index 5c0fed62..50f4af13 100644 --- a/micropython/examples/motor2040/README.md +++ b/micropython/examples/motor2040/README.md @@ -136,16 +136,16 @@ A demonstration of how a motor with an encoder can be used as a programmable rot ### Position Tuning [position_tuning.py](position_tuning.py) -A program to aid in the discovery and tuning of motor PID values for position control. It does this by commanding the motor to move repeatedly between two target angles and plots the measured response. +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 [velocity_tuning.py](velocity_tuning.py) -A program to aid in the discovery and tuning of motor PID values for velocity control. It does this by commanding the motor to drive repeatedly between two target speeds and plots the measured response. +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 [position_on_velocity_tuning.py](position_on_velocity_tuning.py) -A program to aid in the discovery and tuning of motor PID values for position on velocity control. It does this by commanding the motor to move repeatedly between two target angles and plots the measured response. +A program to aid in the discovery and tuning of motor PID values for position on velocity control. It does this by commanding the motor to move repeatedly between two setpoint angles and plots the measured response. diff --git a/micropython/examples/motor2040/position_control.py b/micropython/examples/motor2040/position_control.py index 18ee63e8..9f6c9516 100644 --- a/micropython/examples/motor2040/position_control.py +++ b/micropython/examples/motor2040/position_control.py @@ -31,7 +31,7 @@ PRINT_DIVIDER = 4 # How many of the updates should be prin SPD_PRINT_SCALE = 20 # Driving Speed multipler POSITION_EXTENT = 180 # How far from zero to move the motor, in degrees -INTERP_MODE = 2 # The interpolating mode between targets. STEP (0), LINEAR (1), COSINE (2) +INTERP_MODE = 2 # The interpolating mode between setpoints. STEP (0), LINEAR (1), COSINE (2) # PID values POS_KP = 0.14 # Position proportional (P) gain @@ -76,24 +76,24 @@ while user_sw.raw() is not True: if INTERP_MODE == 0: # Move the motor instantly to the end value - pos_pid.target = end_value + pos_pid.setpoint = end_value elif INTERP_MODE == 2: # Move the motor between values using cosine - pos_pid.target = (((-math.cos(percent_along * math.pi) + 1.0) / 2.0) * (end_value - start_value)) + start_value + pos_pid.setpoint = (((-math.cos(percent_along * math.pi) + 1.0) / 2.0) * (end_value - start_value)) + start_value else: # Move the motor linearly between values - pos_pid.target = (percent_along * (end_value - start_value)) + start_value + pos_pid.setpoint = (percent_along * (end_value - start_value)) + start_value - # Calculate the velocity to move the motor closer to the position target + # Calculate the velocity to move the motor closer to the position setpoint 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 targets, but only on every multiple + # Print out the current motor values and their setpoints, but only on every multiple if print_count == 0: print("Pos =", capture.degrees, end=", ") - print("Targ Pos =", pos_pid.target, end=", ") + print("Pos SP =", pos_pid.setpoint, end=", ") print("Speed = ", m.speed() * SPD_PRINT_SCALE) # Increment the print count, and wrap it diff --git a/micropython/examples/motor2040/position_on_velocity_control.py b/micropython/examples/motor2040/position_on_velocity_control.py index d6da2fcc..e569a29d 100644 --- a/micropython/examples/motor2040/position_on_velocity_control.py +++ b/micropython/examples/motor2040/position_on_velocity_control.py @@ -33,7 +33,7 @@ SPD_PRINT_SCALE = 40 # Driving Speed multipler POSITION_EXTENT = 180 # How far from zero to move the motor, in degrees MAX_SPEED = 1.0 # The maximum speed to move the motor at, in revolutions per second -INTERP_MODE = 0 # The interpolating mode between targets. STEP (0), LINEAR (1), COSINE (2) +INTERP_MODE = 0 # The interpolating mode between setpoints. STEP (0), LINEAR (1), COSINE (2) # PID values POS_KP = 0.025 # Position proportional (P) gain @@ -64,8 +64,8 @@ vel_pid = PID(VEL_KP, VEL_KI, VEL_KD, UPDATE_RATE) # Enable the motor to get started m.enable() -# Set the initial target position -pos_pid.target = POSITION_EXTENT +# Set the initial setpoint position +pos_pid.setpoint = POSITION_EXTENT update = 0 @@ -86,32 +86,32 @@ while user_sw.raw() is not True: if INTERP_MODE == 0: # Move the motor instantly to the end value - pos_pid.target = end_value + pos_pid.setpoint = end_value elif INTERP_MODE == 2: # Move the motor between values using cosine - pos_pid.target = (((-math.cos(percent_along * math.pi) + 1.0) / 2.0) * (end_value - start_value)) + start_value + pos_pid.setpoint = (((-math.cos(percent_along * math.pi) + 1.0) / 2.0) * (end_value - start_value)) + start_value else: # Move the motor linearly between values - pos_pid.target = (percent_along * (end_value - start_value)) + start_value + pos_pid.setpoint = (percent_along * (end_value - start_value)) + start_value - # Calculate the velocity to move the motor closer to the position target + # Calculate the velocity to move the motor closer to the position setpoint vel = pos_pid.calculate(capture.degrees, capture.degrees_per_second) - # Limit the velocity between user defined limits, and set it as the new target of the velocity PID - vel_pid.target = max(min(vel, MAX_SPEED), -MAX_SPEED) + # Limit the velocity between user defined limits, and set it as the new setpoint of the velocity PID + vel_pid.setpoint = max(min(vel, MAX_SPEED), -MAX_SPEED) - # Calculate the acceleration to apply to the motor to move it closer to the velocity target + # Calculate the acceleration to apply to the motor to move it closer to the velocity setpoint 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 targets, but only on every multiple + # Print out the current motor values and their setpoints, but only on every multiple if print_count == 0: print("Pos =", capture.degrees, end=", ") - print("Targ Pos =", pos_pid.target, end=", ") + print("Pos SP =", pos_pid.setpoint, end=", ") print("Vel =", capture.revolutions_per_second * SPD_PRINT_SCALE, end=", ") - print("Targ Vel =", vel_pid.target * SPD_PRINT_SCALE, end=", ") + print("Vel SP =", vel_pid.setpoint * SPD_PRINT_SCALE, end=", ") print("Accel =", accel * ACC_PRINT_SCALE, end=", ") print("Speed =", m.speed() * SPD_PRINT_SCALE) diff --git a/micropython/examples/motor2040/position_on_velocity_tuning.py b/micropython/examples/motor2040/position_on_velocity_tuning.py index 7c03c113..d787f895 100644 --- a/micropython/examples/motor2040/position_on_velocity_tuning.py +++ b/micropython/examples/motor2040/position_on_velocity_tuning.py @@ -7,7 +7,7 @@ from encoder import Encoder, MMME_CPR """ 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 target +commanding the motor to move repeatedly between two setpoint angles and plots the measured response. Press "Boot" to exit the program. @@ -23,8 +23,8 @@ SPEED_SCALE = 5.4 # The scaling to apply to the motor's sp UPDATES = 100 # How many times to update the motor per second UPDATE_RATE = 1 / UPDATES -PRINT_WINDOW = 1.0 # The time (in seconds) after a new target, to display print out motor values -MOVEMENT_WINDOW = 2.0 # The time (in seconds) between each new target being set +PRINT_WINDOW = 1.0 # The time (in seconds) after a new setpoint, to display print out motor values +MOVEMENT_WINDOW = 2.0 # The time (in seconds) between each new setpoint being set PRINT_DIVIDER = 4 # How many of the updates should be printed (i.e. 2 would be every other update) # Multipliers for the different printed values, so they appear nicely on the Thonny plotter @@ -63,8 +63,8 @@ vel_pid = PID(VEL_KP, VEL_KI, VEL_KD, UPDATE_RATE) # Enable the motor to get started m.enable() -# Set the initial target position -pos_pid.target = POSITION_EXTENT +# Set the initial setpoint position +pos_pid.setpoint = POSITION_EXTENT update = 0 @@ -76,25 +76,25 @@ while user_sw.raw() is not True: # Capture the state of the encoder capture = enc.capture() - # Calculate the velocity to move the motor closer to the position target + # Calculate the velocity to move the motor closer to the position setpoint vel = pos_pid.calculate(capture.degrees, capture.degrees_per_second) - # Limit the velocity between user defined limits, and set it as the new target of the velocity PID - vel_pid.target = max(min(vel, MAX_SPEED), -MAX_SPEED) + # Limit the velocity between user defined limits, and set it as the new setpoint of the velocity PID + vel_pid.setpoint = max(min(vel, MAX_SPEED), -MAX_SPEED) - # Calculate the acceleration to apply to the motor to move it closer to the velocity target + # Calculate the acceleration to apply to the motor to move it closer to the velocity setpoint 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 targets, + # Print out the current motor values and their setpoints, # but only for the first few updates and only every multiple if update < (PRINT_WINDOW * UPDATES) and print_count == 0: print("Pos =", capture.degrees, end=", ") - print("Targ Pos =", pos_pid.target, end=", ") + print("Pos SP =", pos_pid.setpoint, end=", ") print("Vel =", capture.revolutions_per_second * SPD_PRINT_SCALE, end=", ") - print("Targ Vel =", vel_pid.target * SPD_PRINT_SCALE, end=", ") + print("Vel SP =", vel_pid.setpoint * SPD_PRINT_SCALE, end=", ") print("Accel =", accel * ACC_PRINT_SCALE, end=", ") print("Speed =", m.speed() * SPD_PRINT_SCALE) @@ -107,8 +107,8 @@ while user_sw.raw() is not True: if update >= (MOVEMENT_WINDOW * UPDATES): update = 0 # Reset the counter - # Set the new position target to be the inverse of the current target - pos_pid.target = 0.0 - pos_pid.target + # Set the new position setpoint to be the inverse of the current setpoint + pos_pid.setpoint = 0.0 - pos_pid.setpoint time.sleep(UPDATE_RATE) diff --git a/micropython/examples/motor2040/position_tuning.py b/micropython/examples/motor2040/position_tuning.py index eda408ca..44f3d1bd 100644 --- a/micropython/examples/motor2040/position_tuning.py +++ b/micropython/examples/motor2040/position_tuning.py @@ -7,7 +7,7 @@ from encoder import Encoder, MMME_CPR """ 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 target angles and +motor to move repeatedly between two setpoint angles and plots the measured response. Press "Boot" to exit the program. @@ -23,8 +23,8 @@ SPEED_SCALE = 5.4 # The scaling to apply to the motor's sp UPDATES = 100 # How many times to update the motor per second UPDATE_RATE = 1 / UPDATES -PRINT_WINDOW = 0.25 # The time (in seconds) after a new target, to display print out motor values -MOVEMENT_WINDOW = 2.0 # The time (in seconds) between each new target being set +PRINT_WINDOW = 0.25 # The time (in seconds) after a new setpoint, to display print out motor values +MOVEMENT_WINDOW = 2.0 # The time (in seconds) between each new setpoint being set PRINT_DIVIDER = 1 # How many of the updates should be printed (i.e. 2 would be every other update) # Multipliers for the different printed values, so they appear nicely on the Thonny plotter @@ -56,8 +56,8 @@ pos_pid = PID(POS_KP, POS_KI, POS_KD, UPDATE_RATE) # Enable the motor to get started m.enable() -# Set the initial target position -pos_pid.target = POSITION_EXTENT +# Set the initial setpoint position +pos_pid.setpoint = POSITION_EXTENT update = 0 @@ -69,17 +69,17 @@ while user_sw.raw() is not True: # Capture the state of the encoder capture = enc.capture() - # Calculate the velocity to move the motor closer to the position target + # Calculate the velocity to move the motor closer to the position setpoint 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 targets, + # Print out the current motor values and their setpoints, # but only for the first few updates and only every multiple if update < (PRINT_WINDOW * UPDATES) and print_count == 0: print("Pos =", capture.degrees, end=", ") - print("Targ Pos =", pos_pid.target, end=", ") + print("Pos SP =", pos_pid.setpoint, end=", ") print("Speed = ", m.speed() * SPD_PRINT_SCALE) # Increment the print count, and wrap it @@ -91,8 +91,8 @@ while user_sw.raw() is not True: if update >= (MOVEMENT_WINDOW * UPDATES): update = 0 # Reset the counter - # Set the new position target to be the inverse of the current target - pos_pid.target = 0.0 - pos_pid.target + # Set the new position setpoint to be the inverse of the current setpoint + pos_pid.setpoint = 0.0 - pos_pid.setpoint time.sleep(UPDATE_RATE) diff --git a/micropython/examples/motor2040/reactive_encoder.py b/micropython/examples/motor2040/reactive_encoder.py index a6f62d2c..d9fbbea9 100644 --- a/micropython/examples/motor2040/reactive_encoder.py +++ b/micropython/examples/motor2040/reactive_encoder.py @@ -77,9 +77,9 @@ current_detent = 0 def detent_change(change): global current_detent - # Update the current detent and pid target + # Update the current detent and pid setpoint current_detent += change - pos_pid.target = (current_detent * DETENT_SIZE) # Update the motor position target + pos_pid.setpoint = (current_detent * DETENT_SIZE) # Update the motor position setpoint print("Detent =", current_detent) # Convert the current detent to a hue and set the onboard led to it @@ -87,7 +87,7 @@ def detent_change(change): led.set_hsv(0, hue, 1.0, BRIGHTNESS) -# Call the function once to set the target and print the value +# Call the function once to set the setpoint and print the value detent_change(0) @@ -112,7 +112,7 @@ while user_sw.raw() is not True: if current_detent > MIN_DETENT: detent_change(-1) # Decrement to the next detent - # Calculate the velocity to move the motor closer to the position target + # Calculate the velocity to move the motor closer to the position setpoint vel = pos_pid.calculate(capture.degrees, capture.degrees_per_second) # If the current angle is within the detent range, limit the max vel diff --git a/micropython/examples/motor2040/velocity_control.py b/micropython/examples/motor2040/velocity_control.py index 565e2b4b..0afe5493 100644 --- a/micropython/examples/motor2040/velocity_control.py +++ b/micropython/examples/motor2040/velocity_control.py @@ -31,7 +31,7 @@ PRINT_DIVIDER = 4 # How many of the updates should be prin ACC_PRINT_SCALE = 0.05 # Acceleration multiplier VELOCITY_EXTENT = 3 # How far from zero to drive the motor at, in revolutions per second -INTERP_MODE = 2 # The interpolating mode between targets. STEP (0), LINEAR (1), COSINE (2) +INTERP_MODE = 2 # The interpolating mode between setpoints. STEP (0), LINEAR (1), COSINE (2) # PID values VEL_KP = 30.0 # Velocity proportional (P) gain @@ -57,8 +57,8 @@ vel_pid = PID(VEL_KP, VEL_KI, VEL_KD, UPDATE_RATE) # Enable the motor to get started m.enable() -# Set the initial target velocity -vel_pid.target = VELOCITY_EXTENT +# Set the initial setpoint velocity +vel_pid.setpoint = VELOCITY_EXTENT update = 0 @@ -79,24 +79,24 @@ while user_sw.raw() is not True: if INTERP_MODE == 0: # Move the motor instantly to the end value - vel_pid.target = end_value + vel_pid.setpoint = end_value elif INTERP_MODE == 2: # Move the motor between values using cosine - vel_pid.target = (((-math.cos(percent_along * math.pi) + 1.0) / 2.0) * (end_value - start_value)) + start_value + vel_pid.setpoint = (((-math.cos(percent_along * math.pi) + 1.0) / 2.0) * (end_value - start_value)) + start_value else: # Move the motor linearly between values - vel_pid.target = (percent_along * (end_value - start_value)) + start_value + 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 target + # Calculate the acceleration to apply to the motor to move it closer to the velocity setpoint 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 targets, but only on every multiple + # Print out the current motor values and their setpoints, but only on every multiple if print_count == 0: print("Vel =", capture.revolutions_per_second, end=", ") - print("Targ Vel =", vel_pid.target, end=", ") + print("Vel SP =", vel_pid.setpoint, end=", ") print("Accel =", accel * ACC_PRINT_SCALE, end=", ") print("Speed =", m.speed()) diff --git a/micropython/examples/motor2040/velocity_tuning.py b/micropython/examples/motor2040/velocity_tuning.py index f46bae52..72f38786 100644 --- a/micropython/examples/motor2040/velocity_tuning.py +++ b/micropython/examples/motor2040/velocity_tuning.py @@ -7,7 +7,7 @@ from encoder import Encoder, MMME_CPR """ 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 target speeds and +motor to drive repeatedly between two setpoint speeds and plots the measured response. Press "Boot" to exit the program. @@ -23,8 +23,8 @@ SPEED_SCALE = 5.4 # The scaling to apply to the motor's sp UPDATES = 100 # How many times to update the motor per second UPDATE_RATE = 1 / UPDATES -PRINT_WINDOW = 1.0 # The time (in seconds) after a new target, to display print out motor values -MOVEMENT_WINDOW = 2.0 # The time (in seconds) between each new target being set +PRINT_WINDOW = 1.0 # The time (in seconds) after a new setpoint, to display print out motor values +MOVEMENT_WINDOW = 2.0 # The time (in seconds) between each new setpoint being set PRINT_DIVIDER = 4 # How many of the updates should be printed (i.e. 2 would be every other update) # Multipliers for the different printed values, so they appear nicely on the Thonny plotter @@ -56,8 +56,8 @@ vel_pid = PID(VEL_KP, VEL_KI, VEL_KD, UPDATE_RATE) # Enable the motor to get started m.enable() -# Set the initial target velocity -vel_pid.target = VELOCITY_EXTENT +# Set the initial setpoint velocity +vel_pid.setpoint = VELOCITY_EXTENT update = 0 @@ -69,17 +69,17 @@ while user_sw.raw() is not True: # Capture the state of the encoder capture = enc.capture() - # Calculate the acceleration to apply to the motor to move it closer to the velocity target + # Calculate the acceleration to apply to the motor to move it closer to the velocity setpoint 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 targets, + # Print out the current motor values and their setpoints, # but only for the first few updates and only every multiple if update < (PRINT_WINDOW * UPDATES) and print_count == 0: print("Vel =", capture.revolutions_per_second, end=", ") - print("Targ Vel =", vel_pid.target, end=", ") + print("Vel SP =", vel_pid.setpoint, end=", ") print("Accel =", accel * ACC_PRINT_SCALE, end=", ") print("Speed =", m.speed()) @@ -92,8 +92,8 @@ while user_sw.raw() is not True: if update >= (MOVEMENT_WINDOW * UPDATES): update = 0 # Reset the counter - # Set the new velocity target to be the inverse of the current target - vel_pid.target = 0.0 - vel_pid.target + # Set the new velocity setpoint to be the inverse of the current setpoint + vel_pid.setpoint = 0.0 - vel_pid.setpoint time.sleep(UPDATE_RATE) diff --git a/micropython/modules_py/pimoroni.py b/micropython/modules_py/pimoroni.py index 4b39e1c2..d7776168 100644 --- a/micropython/modules_py/pimoroni.py +++ b/micropython/modules_py/pimoroni.py @@ -149,13 +149,13 @@ class PID: self.kp = kp self.ki = ki self.kd = kd - self.target = 0 + self.setpoint = 0 self._error_sum = 0 self._last_value = 0 self._sample_rate = sample_rate def calculate(self, value, value_change=None): - error = self.target - value + error = self.setpoint - value self._error_sum += error * self._sample_rate if value_change is None: rate_error = (value - self._last_value) / self._sample_rate From ba076ccc7b6c28f6a44e864ccc9997c593187ea9 Mon Sep 17 00:00:00 2001 From: ZodiusInfuser Date: Fri, 22 Apr 2022 16:06:54 +0100 Subject: [PATCH 32/60] Renamed Snapshot back to Capture --- common/pimoroni_common.hpp | 14 +++++++ drivers/encoder-pio/encoder.cpp | 55 +++++++++++++------------ drivers/encoder-pio/encoder.hpp | 28 ++++--------- drivers/encoder-pio/encoder.pio | 12 +++--- examples/pico_explorer_encoder/demo.cpp | 35 ++++++++-------- micropython/modules/encoder/encoder.cpp | 30 +++++++------- 6 files changed, 88 insertions(+), 86 deletions(-) diff --git a/common/pimoroni_common.hpp b/common/pimoroni_common.hpp index 3a788732..b7b14aef 100644 --- a/common/pimoroni_common.hpp +++ b/common/pimoroni_common.hpp @@ -96,4 +96,18 @@ namespace pimoroni { 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) {} + }; } \ No newline at end of file diff --git a/drivers/encoder-pio/encoder.cpp b/drivers/encoder-pio/encoder.cpp index 653a534f..53e08d7b 100644 --- a/drivers/encoder-pio/encoder.cpp +++ b/drivers/encoder-pio/encoder.cpp @@ -2,13 +2,14 @@ #include #include #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 pimoroni { +namespace encoder { //////////////////////////////////////////////////////////////////////////////////////////////////// // STATICS @@ -18,64 +19,64 @@ uint8_t Encoder::claimed_sms[] = { 0x0, 0x0 }; uint Encoder::pio_program_offset[] = { 0, 0 }; -Encoder::Snapshot::Snapshot() +Encoder::Capture::Capture() : captured_count(0), captured_delta(0), captured_frequency(0.0f), counts_per_rev(INT32_MAX) { } -Encoder::Snapshot::Snapshot(int32_t count, int32_t delta, float frequency, float counts_per_rev) +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::Snapshot::count() const { +int32_t Encoder::Capture::count() const { return captured_count; } -int32_t Encoder::Snapshot::delta() const { +int32_t Encoder::Capture::delta() const { return captured_delta; } -float Encoder::Snapshot::frequency() const { +float Encoder::Capture::frequency() const { return captured_frequency; } -float Encoder::Snapshot::revolutions() const { +float Encoder::Capture::revolutions() const { return (float)captured_count / counts_per_rev; } -float Encoder::Snapshot::degrees() const { +float Encoder::Capture::degrees() const { return revolutions() * 360.0f; } -float Encoder::Snapshot::radians() const { +float Encoder::Capture::radians() const { return revolutions() * M_TWOPI; } -float Encoder::Snapshot::revolutions_delta() const { +float Encoder::Capture::revolutions_delta() const { return (float)captured_delta / counts_per_rev; } -float Encoder::Snapshot::degrees_delta() const { +float Encoder::Capture::degrees_delta() const { return revolutions_delta() * 360.0f; } -float Encoder::Snapshot::radians_delta() const { +float Encoder::Capture::radians_delta() const { return revolutions_delta() * M_TWOPI; } -float Encoder::Snapshot::revolutions_per_second() const { +float Encoder::Capture::revolutions_per_second() const { return captured_frequency / counts_per_rev; } -float Encoder::Snapshot::revolutions_per_minute() const { +float Encoder::Capture::revolutions_per_minute() const { return revolutions_per_second() * 60.0f; } -float Encoder::Snapshot::degrees_per_second() const { +float Encoder::Capture::degrees_per_second() const { return revolutions_per_second() * 360.0f; } -float Encoder::Snapshot::radians_per_second() const { +float Encoder::Capture::radians_per_second() const { return revolutions_per_second() * M_TWOPI; } @@ -249,7 +250,7 @@ void Encoder::zero() { step_dir = NO_DIR; // may not be wanted? last_count = 0; - last_snapshot_count = 0; + last_capture_count = 0; } int16_t Encoder::step() const { @@ -288,23 +289,23 @@ void Encoder::counts_per_revolution(float counts_per_rev) { enc_counts_per_rev = MAX(counts_per_rev, FLT_EPSILON); } -Encoder::Snapshot Encoder::take_snapshot() { - // Take a snapshot of the current values +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 snapshot was taken - int32_t change = count - last_snapshot_count; - last_snapshot_count = count; + // 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 state transitions + // 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 Snapshot(count, change, frequency, enc_counts_per_rev); + return Capture(count, change, frequency, enc_counts_per_rev); } void Encoder::process_steps() { @@ -319,8 +320,8 @@ void Encoder::process_steps() { // 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 transition is cared about, causing an inaccurate time value - // To address this we accumulate the times received and zero it when a transition is counted + // 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; @@ -331,7 +332,7 @@ void Encoder::process_steps() { bool up = (enc_direction == NORMAL); - // Determine what transition occurred + // Determine what step occurred switch(LAST_STATE(states)) { //-------------------------------------------------- case MICROSTEP_0: diff --git a/drivers/encoder-pio/encoder.hpp b/drivers/encoder-pio/encoder.hpp index daea37e0..2a3f3833 100644 --- a/drivers/encoder-pio/encoder.hpp +++ b/drivers/encoder-pio/encoder.hpp @@ -3,27 +3,15 @@ #include "hardware/pio.h" #include "common/pimoroni_common.hpp" -namespace pimoroni { +using namespace pimoroni; + +namespace encoder { enum Direction { NORMAL = 0, REVERSED = 1, }; - 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) {} - }; - class Encoder { //-------------------------------------------------- // Constants @@ -67,7 +55,7 @@ namespace pimoroni { // Substructures //-------------------------------------------------- public: - class Snapshot { + class Capture { //-------------------------------------------------- // Variables //-------------------------------------------------- @@ -82,8 +70,8 @@ namespace pimoroni { // Constructors //-------------------------------------------------- public: - Snapshot(); - Snapshot(int32_t count, int32_t delta, float frequency, float counts_per_rev); + Capture(); + Capture(int32_t count, int32_t delta, float frequency, float counts_per_rev); //-------------------------------------------------- @@ -137,7 +125,7 @@ namespace pimoroni { volatile StepDir step_dir = NO_DIR; int32_t last_count = 0; - int32_t last_snapshot_count = 0; + int32_t last_capture_count = 0; bool initialised = false; @@ -193,7 +181,7 @@ namespace pimoroni { float counts_per_revolution() const; void counts_per_revolution(float counts_per_rev); - Snapshot take_snapshot(); + Capture capture(); //-------------------------------------------------- private: diff --git a/drivers/encoder-pio/encoder.pio b/drivers/encoder-pio/encoder.pio index 0c124afb..630fa6a6 100644 --- a/drivers/encoder-pio/encoder.pio +++ b/drivers/encoder-pio/encoder.pio @@ -50,13 +50,13 @@ osr_dec: ; Read the state of both encoder pins and check ; if they are different from the last state - jmp pin encA_was_high + jmp pin enc_a_was_high mov isr, null - jmp read_encB -encA_was_high: + jmp read_enc_b +enc_a_was_high: set y, 1 mov isr, y -read_encB: +read_enc_b: in pins, 1 mov y, isr jmp x!=y state_changed [1] @@ -77,7 +77,7 @@ state_changed: ; Perform a delay to debounce switch inputs set y, (ITERATIONS - 1) [SET_CYCLES - 1] -debounce_loop: +debounce_loop: jmp y-- debounce_loop [JMP_CYCLES - 1] ; Initialise the timer, as an inverse, and decrement @@ -93,8 +93,6 @@ y_dec: ; Initialisation Code ; -------------------------------------------------- % c-sdk { -#include "hardware/clocks.h" - 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 diff --git a/examples/pico_explorer_encoder/demo.cpp b/examples/pico_explorer_encoder/demo.cpp index 26cc65fb..4ebfb7ae 100644 --- a/examples/pico_explorer_encoder/demo.cpp +++ b/examples/pico_explorer_encoder/demo.cpp @@ -6,14 +6,15 @@ #include "quadrature_out.pio.h" using namespace pimoroni; +using namespace encoder; //-------------------------------------------------- // Constants //-------------------------------------------------- -static const pin_pair ENCODER_PINS = {1, 0}; -static const uint ENCODER_PIN_C = PIN_UNUSED; -static const uint ENCODER_SWITCH_PIN = 4; +static const pin_pair ENCODER_PINS = {1, 0}; +static const uint ENCODER_PIN_C = PIN_UNUSED; +static const uint ENCODER_SWITCH_PIN = 4; static constexpr float COUNTS_PER_REVOLUTION = 24; // 24 is for rotary encoders. For motor magnetic encoders uses // 12 times the gear ratio (e.g. 12 * 20 with a 20:1 ratio motor @@ -54,7 +55,7 @@ enum DrawState { uint16_t buffer[PicoExplorer::WIDTH * PicoExplorer::HEIGHT]; PicoExplorer pico_explorer(buffer); -Encoder encoder(pio0, 0, ENCODER_PINS, ENCODER_PIN_C, NORMAL, COUNTS_PER_REVOLUTION, COUNT_MICROSTEPS, FREQ_DIVIDER); +Encoder enc(pio0, 0, ENCODER_PINS, ENCODER_PIN_C, NORMAL, COUNTS_PER_REVOLUTION, COUNT_MICROSTEPS, FREQ_DIVIDER); volatile bool encA_readings[READINGS_SIZE]; volatile bool encB_readings[READINGS_SIZE]; @@ -145,13 +146,13 @@ uint32_t draw_plot(Point p1, Point p2, volatile bool (&readings)[READINGS_SIZE], //////////////////////////////////////////////////////////////////////////////////////////////////// bool repeating_timer_callback(struct repeating_timer *t) { if(drawing_to_screen && next_scratch_index < SCRATCH_SIZE) { - encA_scratch[next_scratch_index] = encoder.state().a; - encB_scratch[next_scratch_index] = encoder.state().b; + encA_scratch[next_scratch_index] = enc.state().a; + encB_scratch[next_scratch_index] = enc.state().b; next_scratch_index++; } else { - encA_readings[next_reading_index] = encoder.state().a; - encB_readings[next_reading_index] = encoder.state().b; + encA_readings[next_reading_index] = enc.state().a; + encB_readings[next_reading_index] = enc.state().b; next_reading_index++; if(next_reading_index >= READINGS_SIZE) @@ -179,10 +180,10 @@ void setup() { pico_explorer.clear(); pico_explorer.update(); - encoder.init(); + enc.init(); - bool encA = encoder.state().a; - bool encB = encoder.state().b; + bool encA = enc.state().a; + bool encB = enc.state().b; for(uint i = 0; i < READINGS_SIZE; i++) { encA_readings[i] = encA; encB_readings[i] = encB; @@ -226,11 +227,11 @@ int main() { // 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)) { - encoder.zero(); + enc.zero(); } - // Take a snapshot of the current encoder state - Encoder::Snapshot snapshot = encoder.take_snapshot(); + // 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)) { @@ -316,21 +317,21 @@ int main() { { std::stringstream sstream; - sstream << snapshot.count(); + 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) << snapshot.frequency() << "hz"; + 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) << snapshot.revolutions_per_minute(); + 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); } diff --git a/micropython/modules/encoder/encoder.cpp b/micropython/modules/encoder/encoder.cpp index 61d6ef78..fd7ffbb0 100644 --- a/micropython/modules/encoder/encoder.cpp +++ b/micropython/modules/encoder/encoder.cpp @@ -5,7 +5,7 @@ #define MP_OBJ_TO_PTR2(o, t) ((t *)(uintptr_t)(o)) using namespace pimoroni; -//using namespace encoder; +using namespace encoder; extern "C" { #include "encoder.h" @@ -304,22 +304,22 @@ extern mp_obj_t Encoder_counts_per_revolution(size_t n_args, const mp_obj_t *pos extern mp_obj_t Encoder_capture(mp_obj_t self_in) { _Encoder_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Encoder_obj_t); - Encoder::Snapshot snapshot = self->encoder->take_snapshot(); + Encoder::Capture capture = self->encoder->capture(); mp_obj_t tuple[] = { - mp_obj_new_int(snapshot.count()), - mp_obj_new_int(snapshot.delta()), - mp_obj_new_float(snapshot.frequency()), - mp_obj_new_float(snapshot.revolutions()), - mp_obj_new_float(snapshot.degrees()), - mp_obj_new_float(snapshot.radians()), - mp_obj_new_float(snapshot.revolutions_delta()), - mp_obj_new_float(snapshot.degrees_delta()), - mp_obj_new_float(snapshot.radians_delta()), - mp_obj_new_float(snapshot.revolutions_per_second()), - mp_obj_new_float(snapshot.revolutions_per_minute()), - mp_obj_new_float(snapshot.degrees_per_second()), - mp_obj_new_float(snapshot.radians_per_second()), + mp_obj_new_int(capture.count()), + mp_obj_new_int(capture.delta()), + mp_obj_new_float(capture.frequency()), + mp_obj_new_float(capture.revolutions()), + mp_obj_new_float(capture.degrees()), + mp_obj_new_float(capture.radians()), + mp_obj_new_float(capture.revolutions_delta()), + mp_obj_new_float(capture.degrees_delta()), + mp_obj_new_float(capture.radians_delta()), + mp_obj_new_float(capture.revolutions_per_second()), + mp_obj_new_float(capture.revolutions_per_minute()), + mp_obj_new_float(capture.degrees_per_second()), + mp_obj_new_float(capture.radians_per_second()), }; STATIC const qstr tuple_fields[] = { From 2eb6a0cf3ebbfaff900fead905771e06836c655d Mon Sep 17 00:00:00 2001 From: ZodiusInfuser Date: Fri, 22 Apr 2022 16:23:45 +0100 Subject: [PATCH 33/60] Changed encoder folder and gave it a namespace --- drivers/CMakeLists.txt | 2 +- drivers/encoder-pio/CMakeLists.txt | 1 - drivers/encoder/CMakeLists.txt | 1 + .../encoder-pio.cmake => encoder/encoder.cmake} | 2 +- drivers/{encoder-pio => encoder}/encoder.cpp | 0 drivers/{encoder-pio => encoder}/encoder.hpp | 0 drivers/{encoder-pio => encoder}/encoder.pio | 0 examples/pico_explorer_encoder/CMakeLists.txt | 2 +- micropython/modules/encoder/encoder.cpp | 2 +- micropython/modules/encoder/micropython.cmake | 6 +++--- 10 files changed, 8 insertions(+), 8 deletions(-) delete mode 100644 drivers/encoder-pio/CMakeLists.txt create mode 100644 drivers/encoder/CMakeLists.txt rename drivers/{encoder-pio/encoder-pio.cmake => encoder/encoder.cmake} (92%) rename drivers/{encoder-pio => encoder}/encoder.cpp (100%) rename drivers/{encoder-pio => encoder}/encoder.hpp (100%) rename drivers/{encoder-pio => encoder}/encoder.pio (100%) diff --git a/drivers/CMakeLists.txt b/drivers/CMakeLists.txt index d7fd1851..9fc45193 100644 --- a/drivers/CMakeLists.txt +++ b/drivers/CMakeLists.txt @@ -29,5 +29,5 @@ add_subdirectory(hub75) add_subdirectory(uc8151) add_subdirectory(pwm) add_subdirectory(servo) -add_subdirectory(encoder-pio) +add_subdirectory(encoder) add_subdirectory(motor) diff --git a/drivers/encoder-pio/CMakeLists.txt b/drivers/encoder-pio/CMakeLists.txt deleted file mode 100644 index 9dce7452..00000000 --- a/drivers/encoder-pio/CMakeLists.txt +++ /dev/null @@ -1 +0,0 @@ -include(encoder-pio.cmake) \ No newline at end of file diff --git a/drivers/encoder/CMakeLists.txt b/drivers/encoder/CMakeLists.txt new file mode 100644 index 00000000..34fa6cad --- /dev/null +++ b/drivers/encoder/CMakeLists.txt @@ -0,0 +1 @@ +include(encoder.cmake) \ No newline at end of file diff --git a/drivers/encoder-pio/encoder-pio.cmake b/drivers/encoder/encoder.cmake similarity index 92% rename from drivers/encoder-pio/encoder-pio.cmake rename to drivers/encoder/encoder.cmake index 923ecbe0..c5a694cf 100644 --- a/drivers/encoder-pio/encoder-pio.cmake +++ b/drivers/encoder/encoder.cmake @@ -1,4 +1,4 @@ -set(DRIVER_NAME encoder-pio) +set(DRIVER_NAME encoder) add_library(${DRIVER_NAME} INTERFACE) target_sources(${DRIVER_NAME} INTERFACE diff --git a/drivers/encoder-pio/encoder.cpp b/drivers/encoder/encoder.cpp similarity index 100% rename from drivers/encoder-pio/encoder.cpp rename to drivers/encoder/encoder.cpp diff --git a/drivers/encoder-pio/encoder.hpp b/drivers/encoder/encoder.hpp similarity index 100% rename from drivers/encoder-pio/encoder.hpp rename to drivers/encoder/encoder.hpp diff --git a/drivers/encoder-pio/encoder.pio b/drivers/encoder/encoder.pio similarity index 100% rename from drivers/encoder-pio/encoder.pio rename to drivers/encoder/encoder.pio diff --git a/examples/pico_explorer_encoder/CMakeLists.txt b/examples/pico_explorer_encoder/CMakeLists.txt index 1d598480..cab9a39f 100644 --- a/examples/pico_explorer_encoder/CMakeLists.txt +++ b/examples/pico_explorer_encoder/CMakeLists.txt @@ -6,7 +6,7 @@ add_executable( 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-pio) +target_link_libraries(explorerencoder pico_stdlib pico_explorer encoder) # create map/bin/hex file etc. pico_add_extra_outputs(explorerencoder) \ No newline at end of file diff --git a/micropython/modules/encoder/encoder.cpp b/micropython/modules/encoder/encoder.cpp index fd7ffbb0..f0dffc13 100644 --- a/micropython/modules/encoder/encoder.cpp +++ b/micropython/modules/encoder/encoder.cpp @@ -1,4 +1,4 @@ -#include "drivers/encoder-pio/encoder.hpp" +#include "drivers/encoder/encoder.hpp" #include #include diff --git a/micropython/modules/encoder/micropython.cmake b/micropython/modules/encoder/micropython.cmake index 9f7204c3..8b5ad9bc 100644 --- a/micropython/modules/encoder/micropython.cmake +++ b/micropython/modules/encoder/micropython.cmake @@ -5,13 +5,13 @@ add_library(usermod_${MOD_NAME} INTERFACE) target_sources(usermod_${MOD_NAME} INTERFACE ${CMAKE_CURRENT_LIST_DIR}/${MOD_NAME}.c ${CMAKE_CURRENT_LIST_DIR}/${MOD_NAME}.cpp - ${CMAKE_CURRENT_LIST_DIR}/../../../drivers/encoder-pio/encoder.cpp + ${CMAKE_CURRENT_LIST_DIR}/../../../drivers/encoder/encoder.cpp ) -pico_generate_pio_header(usermod_${MOD_NAME} ${CMAKE_CURRENT_LIST_DIR}/../../../drivers/encoder-pio/encoder.pio) +pico_generate_pio_header(usermod_${MOD_NAME} ${CMAKE_CURRENT_LIST_DIR}/../../../drivers/encoder/encoder.pio) target_include_directories(usermod_${MOD_NAME} INTERFACE ${CMAKE_CURRENT_LIST_DIR} - ${CMAKE_CURRENT_LIST_DIR}/../../../drivers/encoder-pio/ + ${CMAKE_CURRENT_LIST_DIR}/../../../drivers/encoder/ ) target_compile_definitions(usermod_${MOD_NAME} INTERFACE From 7608e3f2931500d4deab3839aac6df2633a4650b Mon Sep 17 00:00:00 2001 From: ZodiusInfuser Date: Mon, 25 Apr 2022 12:28:42 +0100 Subject: [PATCH 34/60] Encoder driver finalising --- drivers/encoder/encoder.cpp | 4 +- drivers/encoder/encoder.hpp | 6 +- micropython/examples/motor2040/README.md | 21 ++- .../examples/motor2040/motor_profiler.py | 6 +- .../examples/motor2040/position_control.py | 4 +- .../motor2040/position_on_velocity_control.py | 4 +- .../motor2040/position_on_velocity_tuning.py | 4 +- .../examples/motor2040/position_tuning.py | 4 +- .../examples/motor2040/quad_position_wave.py | 130 ++++++++++++++++++ .../examples/motor2040/reactive_encoder.py | 4 +- .../{motor_angles.py => read_encoders.py} | 11 +- .../{turn_off_motors.py => stop_motors.py} | 2 +- .../examples/motor2040/velocity_control.py | 4 +- .../examples/motor2040/velocity_tuning.py | 4 +- micropython/modules/encoder/encoder.c | 6 +- micropython/modules/encoder/encoder.cpp | 10 +- micropython/modules/encoder/encoder.h | 2 +- micropython/modules/motor/motor.cpp | 2 +- micropython/modules_py/pimoroni.py | 4 + 19 files changed, 185 insertions(+), 47 deletions(-) create mode 100644 micropython/examples/motor2040/quad_position_wave.py rename micropython/examples/motor2040/{motor_angles.py => read_encoders.py} (85%) rename micropython/examples/motor2040/{turn_off_motors.py => stop_motors.py} (86%) diff --git a/drivers/encoder/encoder.cpp b/drivers/encoder/encoder.cpp index 53e08d7b..427589c2 100644 --- a/drivers/encoder/encoder.cpp +++ b/drivers/encoder/encoder.cpp @@ -281,11 +281,11 @@ void Encoder::direction(Direction direction) { enc_direction = direction; } -float Encoder::counts_per_revolution() const { +float Encoder::counts_per_rev() const { return enc_counts_per_rev; } -void Encoder::counts_per_revolution(float counts_per_rev) { +void Encoder::counts_per_rev(float counts_per_rev) { enc_counts_per_rev = MAX(counts_per_rev, FLT_EPSILON); } diff --git a/drivers/encoder/encoder.hpp b/drivers/encoder/encoder.hpp index 2a3f3833..37f4a8dc 100644 --- a/drivers/encoder/encoder.hpp +++ b/drivers/encoder/encoder.hpp @@ -125,7 +125,7 @@ namespace encoder { volatile StepDir step_dir = NO_DIR; int32_t last_count = 0; - int32_t last_capture_count = 0; + int32_t last_capture_count = 0; bool initialised = false; @@ -178,8 +178,8 @@ namespace encoder { Direction direction() const; void direction(Direction direction); - float counts_per_revolution() const; - void counts_per_revolution(float counts_per_rev); + float counts_per_rev() const; + void counts_per_rev(float counts_per_rev); Capture capture(); diff --git a/micropython/examples/motor2040/README.md b/micropython/examples/motor2040/README.md index 50f4af13..042bfcc4 100644 --- a/micropython/examples/motor2040/README.md +++ b/micropython/examples/motor2040/README.md @@ -5,9 +5,9 @@ - [Multiple Motors](#multiple-motors) - [Motor Cluster](#motor-cluster) - [Motor Wave](#motor-wave) - - [Turn Off Motors](#turn-off-motors) + - [Stop Motors](#stop-motors) - [Encoder Examples](#encoder-examples) - - [Motor Angles](#motor-angles) + - [Read Encoders](#read-encoders) - [Motor Profiler](#motor-profiler) - [Function Examples](#function-examples) - [Read Sensors](#read-sensors) @@ -20,6 +20,7 @@ - [Velocity Control](#velocity-control) - [Position on Velocity Control](#position-on-velocity-control) - [Reactive Encoder](#reactive-encoder) + - [Quad Position Wave](#quad-position-wave) - [Tuning Examples](#tuning-examples) - [Position Tuning](#position-tuning) - [Velocity Tuning](#velocity-tuning) @@ -52,16 +53,16 @@ Demonstrates how to create a MotorCluster object to control multiple motors at o An example of applying a wave pattern to a group of motors and the LEDs. -### Turn Off Motors -[turn_off_motors.py](turn_off_motors.py) +### Stop Motors +[stop_motors.py](stop_motors.py) -A simple program that turns off the motors. +A simple program that stops the motors. ## Encoder Examples -### Motor Angles -[motor_angles.py](motor_angles.py) +### Read Encoders +[read_encoders.py](read_encoders.py) Demonstrates how to read the angles of Motor 2040's four encoders. @@ -131,6 +132,12 @@ An example of how to move a motor smoothly between random positions, with veloci 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 +[quad_position_wave.py](quad_position_wave.py) + +An demonstration of driving all four of Motor 2040's motor outputs between positions, with the help of their attached encoders and PID control. + + ## Tuning Examples ### Position Tuning diff --git a/micropython/examples/motor2040/motor_profiler.py b/micropython/examples/motor2040/motor_profiler.py index e25ec6fb..0c9532f8 100644 --- a/micropython/examples/motor2040/motor_profiler.py +++ b/micropython/examples/motor2040/motor_profiler.py @@ -2,6 +2,7 @@ import gc import time from motor import Motor, motor2040 from encoder import Encoder, MMME_CPR +from pimoroni import NORMAL_DIR # , REVERSED_DIR """ A program that profiles the speed of a motor across its PWM @@ -13,9 +14,8 @@ ENCODER_PINS = motor2040.ENCODER_A # The pins of the encoder attached to th GEAR_RATIO = 50 # The gear ratio of the motor COUNTS_PER_REV = MMME_CPR * GEAR_RATIO # The counts per revolution of the motor's output shaft -DIRECTION = 0 # The direction to spin the motor in. NORMAL (0), REVERSED (1) -SPEED_SCALE = 5.4 # The scaling to apply to the motor's speed - # Set this to the maximum measured speed +DIRECTION = NORMAL_DIR # The direction to spin the motor in. NORMAL_DIR (0), REVERSED_DIR (1) +SPEED_SCALE = 5.4 # The scaling to apply to the motor's speed. Set this to the maximum measured speed DUTY_STEPS = 100 # How many duty cycle steps to sample the speed of SETTLE_TIME = 0.1 # How long to wait after changing motor duty cycle diff --git a/micropython/examples/motor2040/position_control.py b/micropython/examples/motor2040/position_control.py index 9f6c9516..7d656527 100644 --- a/micropython/examples/motor2040/position_control.py +++ b/micropython/examples/motor2040/position_control.py @@ -2,9 +2,9 @@ import gc import time import math import random -from pimoroni import Button, PID from motor import Motor, motor2040 from encoder import Encoder, MMME_CPR +from pimoroni import Button, PID, NORMAL_DIR # , REVERSED_DIR """ An example of how to move a motor smoothly between random positions, @@ -18,7 +18,7 @@ ENCODER_PINS = motor2040.ENCODER_A # The pins of the encoder attached to th GEAR_RATIO = 50 # The gear ratio of the motor COUNTS_PER_REV = MMME_CPR * GEAR_RATIO # The counts per revolution of the motor's output shaft -DIRECTION = 0 # The direction to spin the motor in. NORMAL (0), REVERSED (1) +DIRECTION = NORMAL_DIR # The direction to spin the motor in. NORMAL_DIR (0), REVERSED_DIR (1) SPEED_SCALE = 5.4 # The scaling to apply to the motor's speed to match its real-world speed UPDATES = 100 # How many times to update the motor per second diff --git a/micropython/examples/motor2040/position_on_velocity_control.py b/micropython/examples/motor2040/position_on_velocity_control.py index e569a29d..718f6e93 100644 --- a/micropython/examples/motor2040/position_on_velocity_control.py +++ b/micropython/examples/motor2040/position_on_velocity_control.py @@ -2,9 +2,9 @@ import gc import time import math import random -from pimoroni import Button, PID from motor import Motor, motor2040 from encoder import Encoder, MMME_CPR +from pimoroni import Button, PID, NORMAL_DIR # , REVERSED_DIR """ An example of how to move a motor smoothly between random positions, @@ -18,7 +18,7 @@ ENCODER_PINS = motor2040.ENCODER_A # The pins of the encoder attached to th GEAR_RATIO = 50 # The gear ratio of the motor COUNTS_PER_REV = MMME_CPR * GEAR_RATIO # The counts per revolution of the motor's output shaft -DIRECTION = 0 # The direction to spin the motor in. NORMAL (0), REVERSED (1) +DIRECTION = NORMAL_DIR # The direction to spin the motor in. NORMAL_DIR (0), REVERSED_DIR (1) SPEED_SCALE = 5.4 # The scaling to apply to the motor's speed to match its real-world speed UPDATES = 100 # How many times to update the motor per second diff --git a/micropython/examples/motor2040/position_on_velocity_tuning.py b/micropython/examples/motor2040/position_on_velocity_tuning.py index d787f895..62778644 100644 --- a/micropython/examples/motor2040/position_on_velocity_tuning.py +++ b/micropython/examples/motor2040/position_on_velocity_tuning.py @@ -1,8 +1,8 @@ import gc import time -from pimoroni import Button, PID from motor import Motor, motor2040 from encoder import Encoder, MMME_CPR +from pimoroni import Button, PID, NORMAL_DIR # , REVERSED_DIR """ A program to aid in the discovery and tuning of motor PID @@ -18,7 +18,7 @@ ENCODER_PINS = motor2040.ENCODER_A # The pins of the encoder attached to th GEAR_RATIO = 50 # The gear ratio of the motor COUNTS_PER_REV = MMME_CPR * GEAR_RATIO # The counts per revolution of the motor's output shaft -DIRECTION = 0 # The direction to spin the motor in. NORMAL (0), REVERSED (1) +DIRECTION = NORMAL_DIR # The direction to spin the motor in. NORMAL_DIR (0), REVERSED_DIR (1) SPEED_SCALE = 5.4 # The scaling to apply to the motor's speed to match its real-world speed UPDATES = 100 # How many times to update the motor per second diff --git a/micropython/examples/motor2040/position_tuning.py b/micropython/examples/motor2040/position_tuning.py index 44f3d1bd..9ecfd13e 100644 --- a/micropython/examples/motor2040/position_tuning.py +++ b/micropython/examples/motor2040/position_tuning.py @@ -1,8 +1,8 @@ import gc import time -from pimoroni import Button, PID from motor import Motor, motor2040 from encoder import Encoder, MMME_CPR +from pimoroni import Button, PID, NORMAL_DIR # , REVERSED_DIR """ A program to aid in the discovery and tuning of motor PID @@ -18,7 +18,7 @@ ENCODER_PINS = motor2040.ENCODER_A # The pins of the encoder attached to th GEAR_RATIO = 50 # The gear ratio of the motor COUNTS_PER_REV = MMME_CPR * GEAR_RATIO # The counts per revolution of the motor's output shaft -DIRECTION = 0 # The direction to spin the motor in. NORMAL (0), REVERSED (1) +DIRECTION = NORMAL_DIR # The direction to spin the motor in. NORMAL_DIR (0), REVERSED_DIR (1) SPEED_SCALE = 5.4 # The scaling to apply to the motor's speed to match its real-world speed UPDATES = 100 # How many times to update the motor per second diff --git a/micropython/examples/motor2040/quad_position_wave.py b/micropython/examples/motor2040/quad_position_wave.py new file mode 100644 index 00000000..400a6af9 --- /dev/null +++ b/micropython/examples/motor2040/quad_position_wave.py @@ -0,0 +1,130 @@ +import gc +import time +import math +from plasma import WS2812 +from motor import Motor, motor2040 +from encoder import Encoder, MMME_CPR +from pimoroni import Button, PID, REVERSED_DIR + +""" +An 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. +""" + +GEAR_RATIO = 50 # The gear ratio of the motors +COUNTS_PER_REV = MMME_CPR * GEAR_RATIO # The counts per revolution of each motor's output shaft + +SPEED_SCALE = 5.4 # The scaling to apply to each motor's speed to match its real-world speed + +UPDATES = 100 # How many times to update the motor per second +UPDATE_RATE = 1 / UPDATES +TIME_FOR_EACH_MOVE = 2 # The time to travel between each value +UPDATES_PER_MOVE = TIME_FOR_EACH_MOVE * UPDATES +PRINT_DIVIDER = 4 # How many of the updates should be printed (i.e. 2 would be every other update) + +# LED constant +BRIGHTNESS = 0.4 # The brightness of the LEDs + +# PID values +POS_KP = 0.14 # Position proportional (P) gain +POS_KI = 0.0 # Position integral (I) gain +POS_KD = 0.0022 # Position derivative (D) gain + + +# Free up hardware resources ahead of creating a new Encoder +gc.collect() + +# Create a list of motors with a given speed scale +MOTOR_PINS = [motor2040.MOTOR_A, motor2040.MOTOR_B, motor2040.MOTOR_C, motor2040.MOTOR_D] +motors = [Motor(pins, speed_scale=SPEED_SCALE, deadzone=0.05) for pins in MOTOR_PINS] + +# Create a list of encoders, using PIO 0, with the given counts per revolution +ENCODER_PINS = [motor2040.ENCODER_A, motor2040.ENCODER_B, motor2040.ENCODER_C, motor2040.ENCODER_D] +ENCODER_NAMES = ["A", "B", "C", "D"] +encoders = [Encoder(0, i, ENCODER_PINS[i], counts_per_rev=COUNTS_PER_REV, count_microsteps=True) for i in range(motor2040.NUM_MOTORS)] + +# 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) + +# Create the LED, using PIO 1 and State Machine 0 +led = WS2812(motor2040.NUM_LEDS, 1, 0, motor2040.LED_DATA) + +# Create the user button +user_sw = Button(motor2040.USER_SW) + +# Create PID objects for position control +pos_pids = [PID(POS_KP, POS_KI, POS_KD, UPDATE_RATE) for i in range(motor2040.NUM_MOTORS)] + +# Start updating the LED +led.start() + +# Enable the motor to get started +for m in motors: + m.enable() + + +update = 0 +print_count = 0 + +# Set the initial and end values +start_value = 0.0 +end_value = 270.0 + +captures = [None] * motor2040.NUM_MOTORS + +# Continually move the motor until the user button is pressed +while user_sw.raw() is not True: + + # Capture the state of all the encoders + for i in range(motor2040.NUM_MOTORS): + captures[i] = encoders[i].capture() + + # Calculate how far along this movement to be + percent_along = min(update / UPDATES_PER_MOVE, 1.0) + + for i in range(motor2040.NUM_MOTORS): + # Move the motor between values using cosine + pos_pids[i].setpoint = (((-math.cos(percent_along * math.pi) + 1.0) / 2.0) * (end_value - start_value)) + start_value + + # Calculate the velocity to move the motor closer to the position setpoint + 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.0, BRIGHTNESS) + + # Print out the current motor values and their setpoints, but only on every multiple + if print_count == 0: + for i in range(len(motors)): + print(ENCODER_NAMES[i], "=", captures[i].degrees, end=", ") + print() + + # Increment the print count, and wrap it + print_count = (print_count + 1) % PRINT_DIVIDER + + update += 1 # 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 + temp = start_value + start_value = end_value + end_value = temp + + time.sleep(UPDATE_RATE) + +# Stop all the motors +for m in motors: + m.disable() + +# Turn off the LED bar +led.clear() diff --git a/micropython/examples/motor2040/reactive_encoder.py b/micropython/examples/motor2040/reactive_encoder.py index d9fbbea9..e5a2983b 100644 --- a/micropython/examples/motor2040/reactive_encoder.py +++ b/micropython/examples/motor2040/reactive_encoder.py @@ -1,9 +1,9 @@ import gc import time -from pimoroni import Button, PID from plasma import WS2812 from motor import Motor, motor2040 from encoder import Encoder, MMME_CPR +from pimoroni import Button, PID, NORMAL_DIR # , REVERSED_DIR """ A demonstration of how a motor with an encoder can be used @@ -22,7 +22,7 @@ ENCODER_PINS = motor2040.ENCODER_A # The pins of the encoder attached to th GEAR_RATIO = 50 # The gear ratio of the motor COUNTS_PER_REV = MMME_CPR * GEAR_RATIO # The counts per revolution of the motor's output shaft -DIRECTION = 0 # The direction to spin the motor in. NORMAL (0), REVERSED (1) +DIRECTION = NORMAL_DIR # The direction to spin the motor in. NORMAL_DIR (0), REVERSED_DIR (1) SPEED_SCALE = 5.4 # The scaling to apply to the motor's speed to match its real-world speed UPDATES = 100 # How many times to update the motor per second diff --git a/micropython/examples/motor2040/motor_angles.py b/micropython/examples/motor2040/read_encoders.py similarity index 85% rename from micropython/examples/motor2040/motor_angles.py rename to micropython/examples/motor2040/read_encoders.py index 40d6010e..3d07ef64 100644 --- a/micropython/examples/motor2040/motor_angles.py +++ b/micropython/examples/motor2040/read_encoders.py @@ -1,9 +1,8 @@ import gc import time -from pimoroni import Button from motor import motor2040 from encoder import Encoder, MMME_CPR -# from encoder import REVERSED +from pimoroni import Button # , REVERSED_DIR """ Demonstrates how to read the angles of Motor 2040's four encoders. @@ -26,10 +25,10 @@ encoders = [Encoder(0, i, ENCODER_PINS[i], counts_per_rev=COUNTS_PER_REV, count_ # Uncomment the below lines (and the top import) to # reverse the counting direction of an encoder -# encoders[0].direction(REVERSED) -# encoders[1].direction(REVERSED) -# encoders[2].direction(REVERSED) -# encoders[3].direction(REVERSED) +# encoders[0].direction(REVERSED_DIR) +# encoders[1].direction(REVERSED_DIR) +# encoders[2].direction(REVERSED_DIR) +# encoders[3].direction(REVERSED_DIR) # Create the user button user_sw = Button(motor2040.USER_SW) diff --git a/micropython/examples/motor2040/turn_off_motors.py b/micropython/examples/motor2040/stop_motors.py similarity index 86% rename from micropython/examples/motor2040/turn_off_motors.py rename to micropython/examples/motor2040/stop_motors.py index bed8fcc2..951a4353 100644 --- a/micropython/examples/motor2040/turn_off_motors.py +++ b/micropython/examples/motor2040/stop_motors.py @@ -1,7 +1,7 @@ from motor import Motor, motor2040 """ -A simple program that turns off the motors. +A simple program that stops the motors. """ # Create four motor objects. diff --git a/micropython/examples/motor2040/velocity_control.py b/micropython/examples/motor2040/velocity_control.py index 0afe5493..db05fa1b 100644 --- a/micropython/examples/motor2040/velocity_control.py +++ b/micropython/examples/motor2040/velocity_control.py @@ -2,9 +2,9 @@ import gc import time import math import random -from pimoroni import Button, PID from motor import Motor, motor2040 from encoder import Encoder, MMME_CPR +from pimoroni import Button, PID, NORMAL_DIR # , REVERSED_DIR """ An example of how to drive a motor smoothly between random speeds, @@ -18,7 +18,7 @@ ENCODER_PINS = motor2040.ENCODER_A # The pins of the encoder attached to th GEAR_RATIO = 50 # The gear ratio of the motor COUNTS_PER_REV = MMME_CPR * GEAR_RATIO # The counts per revolution of the motor's output shaft -DIRECTION = 0 # The direction to spin the motor in. NORMAL (0), REVERSED (1) +DIRECTION = NORMAL_DIR # The direction to spin the motor in. NORMAL_DIR (0), REVERSED_DIR (1) SPEED_SCALE = 5.4 # The scaling to apply to the motor's speed to match its real-world speed UPDATES = 100 # How many times to update the motor per second diff --git a/micropython/examples/motor2040/velocity_tuning.py b/micropython/examples/motor2040/velocity_tuning.py index 72f38786..e913e7ea 100644 --- a/micropython/examples/motor2040/velocity_tuning.py +++ b/micropython/examples/motor2040/velocity_tuning.py @@ -1,8 +1,8 @@ import gc import time -from pimoroni import Button, PID from motor import Motor, motor2040 from encoder import Encoder, MMME_CPR +from pimoroni import Button, PID, NORMAL_DIR # , REVERSED_DIR """ A program to aid in the discovery and tuning of motor PID @@ -18,7 +18,7 @@ ENCODER_PINS = motor2040.ENCODER_A # The pins of the encoder attached to th GEAR_RATIO = 50 # The gear ratio of the motor COUNTS_PER_REV = MMME_CPR * GEAR_RATIO # The counts per revolution of the motor's output shaft -DIRECTION = 0 # The direction to spin the motor in. NORMAL (0), REVERSED (1) +DIRECTION = NORMAL_DIR # The direction to spin the motor in. NORMAL_DIR (0), REVERSED_DIR (1) SPEED_SCALE = 5.4 # The scaling to apply to the motor's speed to match its real-world speed UPDATES = 100 # How many times to update the motor per second diff --git a/micropython/modules/encoder/encoder.c b/micropython/modules/encoder/encoder.c index 23bec710..2fedf54f 100644 --- a/micropython/modules/encoder/encoder.c +++ b/micropython/modules/encoder/encoder.c @@ -15,7 +15,7 @@ MP_DEFINE_CONST_FUN_OBJ_1(Encoder_revolutions_obj, Encoder_revolutions); MP_DEFINE_CONST_FUN_OBJ_1(Encoder_degrees_obj, Encoder_degrees); MP_DEFINE_CONST_FUN_OBJ_1(Encoder_radians_obj, Encoder_radians); MP_DEFINE_CONST_FUN_OBJ_KW(Encoder_direction_obj, 1, Encoder_direction); -MP_DEFINE_CONST_FUN_OBJ_KW(Encoder_counts_per_revolution_obj, 1, Encoder_counts_per_revolution); +MP_DEFINE_CONST_FUN_OBJ_KW(Encoder_counts_per_rev_obj, 1, Encoder_counts_per_rev); MP_DEFINE_CONST_FUN_OBJ_1(Encoder_capture_obj, Encoder_capture); /***** Binding of Methods *****/ @@ -33,7 +33,7 @@ STATIC const mp_rom_map_elem_t Encoder_locals_dict_table[] = { { MP_ROM_QSTR(MP_QSTR_degrees), MP_ROM_PTR(&Encoder_degrees_obj) }, { MP_ROM_QSTR(MP_QSTR_radians), MP_ROM_PTR(&Encoder_radians_obj) }, { MP_ROM_QSTR(MP_QSTR_direction), MP_ROM_PTR(&Encoder_direction_obj) }, - { MP_ROM_QSTR(MP_QSTR_counts_per_revolution), MP_ROM_PTR(&Encoder_counts_per_revolution_obj) }, + { MP_ROM_QSTR(MP_QSTR_counts_per_rev), MP_ROM_PTR(&Encoder_counts_per_rev_obj) }, { MP_ROM_QSTR(MP_QSTR_capture), MP_ROM_PTR(&Encoder_capture_obj) }, }; @@ -53,8 +53,6 @@ STATIC const mp_map_elem_t encoder_globals_table[] = { { MP_OBJ_NEW_QSTR(MP_QSTR___name__), MP_OBJ_NEW_QSTR(MP_QSTR_encoder) }, { MP_OBJ_NEW_QSTR(MP_QSTR_Encoder), (mp_obj_t)&Encoder_type }, - { MP_ROM_QSTR(MP_QSTR_NORMAL), MP_ROM_INT(0x00) }, - { MP_ROM_QSTR(MP_QSTR_REVERSED), MP_ROM_INT(0x01) }, { MP_ROM_QSTR(MP_QSTR_MMME_CPR), MP_ROM_INT(12) }, { MP_ROM_QSTR(MP_QSTR_ROTARY_CPR), MP_ROM_INT(24) }, }; diff --git a/micropython/modules/encoder/encoder.cpp b/micropython/modules/encoder/encoder.cpp index f0dffc13..c080316f 100644 --- a/micropython/modules/encoder/encoder.cpp +++ b/micropython/modules/encoder/encoder.cpp @@ -46,7 +46,7 @@ void Encoder_print(const mp_print_t *print, mp_obj_t self_in, mp_print_kind_t ki mp_print_str(print, ", direction = REVERSED"); mp_print_str(print, ", counts_per_rev = "); - mp_obj_print_helper(print, mp_obj_new_float(self->encoder->counts_per_revolution()), PRINT_REPR); + mp_obj_print_helper(print, mp_obj_new_float(self->encoder->counts_per_rev()), PRINT_REPR); mp_print_str(print, ")"); } @@ -75,7 +75,7 @@ mp_obj_t Encoder_make_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw, if(pio_int < 0 || pio_int > (int)NUM_PIOS) { mp_raise_ValueError("pio out of range. Expected 0 to 1"); } - PIO pio = pio_int ? pio0 : pio1; + PIO pio = pio_int == 0 ? pio0 : pio1; int sm = args[ARG_sm].u_int; if(sm < 0 || sm > (int)NUM_PIO_STATE_MACHINES) { @@ -264,7 +264,7 @@ extern mp_obj_t Encoder_direction(size_t n_args, const mp_obj_t *pos_args, mp_ma } } -extern mp_obj_t Encoder_counts_per_revolution(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { +extern mp_obj_t Encoder_counts_per_rev(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { if(n_args <= 1) { enum { ARG_self }; static const mp_arg_t allowed_args[] = { @@ -277,7 +277,7 @@ extern mp_obj_t Encoder_counts_per_revolution(size_t n_args, const mp_obj_t *pos _Encoder_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Encoder_obj_t); - return mp_obj_new_float(self->encoder->counts_per_revolution()); + return mp_obj_new_float(self->encoder->counts_per_rev()); } else { enum { ARG_self, ARG_counts_per_rev }; @@ -296,7 +296,7 @@ extern mp_obj_t Encoder_counts_per_revolution(size_t n_args, const mp_obj_t *pos if(counts_per_rev < FLT_EPSILON) { mp_raise_ValueError("counts_per_rev out of range. Expected greater than 0.0"); } - self->encoder->counts_per_revolution(counts_per_rev); + self->encoder->counts_per_rev(counts_per_rev); return mp_const_none; } } diff --git a/micropython/modules/encoder/encoder.h b/micropython/modules/encoder/encoder.h index 53eac0ec..105d312b 100644 --- a/micropython/modules/encoder/encoder.h +++ b/micropython/modules/encoder/encoder.h @@ -20,5 +20,5 @@ extern mp_obj_t Encoder_revolutions(mp_obj_t self_in); extern mp_obj_t Encoder_degrees(mp_obj_t self_in); extern mp_obj_t Encoder_radians(mp_obj_t self_in); extern mp_obj_t Encoder_direction(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); -extern mp_obj_t Encoder_counts_per_revolution(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); +extern mp_obj_t Encoder_counts_per_rev(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); extern mp_obj_t Encoder_capture(mp_obj_t self_in); \ No newline at end of file diff --git a/micropython/modules/motor/motor.cpp b/micropython/modules/motor/motor.cpp index 0f35c73d..1bbc2cf5 100644 --- a/micropython/modules/motor/motor.cpp +++ b/micropython/modules/motor/motor.cpp @@ -618,7 +618,7 @@ mp_obj_t MotorCluster_make_new(const mp_obj_type_t *type, size_t n_args, size_t if(pio_int < 0 || pio_int > (int)NUM_PIOS) { mp_raise_ValueError("pio out of range. Expected 0 to 1"); } - PIO pio = pio_int ? pio0 : pio1; + PIO pio = pio_int == 0 ? pio0 : pio1; int sm = args[ARG_sm].u_int; if(sm < 0 || sm > (int)NUM_PIO_STATE_MACHINES) { diff --git a/micropython/modules_py/pimoroni.py b/micropython/modules_py/pimoroni.py index d7776168..f833329a 100644 --- a/micropython/modules_py/pimoroni.py +++ b/micropython/modules_py/pimoroni.py @@ -6,6 +6,10 @@ BREAKOUT_GARDEN_I2C_PINS = {"sda": 4, "scl": 5} PICO_EXPLORER_I2C_PINS = {"sda": 20, "scl": 21} HEADER_I2C_PINS = {"sda": 20, "scl": 21} +# Motor and encoder directions +NORMAL_DIR = 0x00 +REVERSED_DIR = 0x01 + class Analog: def __init__(self, pin, amplifier_gain=1, resistor=0, offset=0): From 0efe210c4c9237d2dc5eb5d3d2ea50f69b4c800c Mon Sep 17 00:00:00 2001 From: ZodiusInfuser Date: Mon, 25 Apr 2022 16:16:02 +0100 Subject: [PATCH 35/60] Added in PH_EN motor driver, and zeropoint support --- common/pimoroni_common.hpp | 7 + drivers/encoder/encoder.cpp | 2 +- drivers/encoder/encoder.hpp | 7 +- drivers/motor/motor.cpp | 426 +++++++++++------- drivers/motor/motor.hpp | 84 +++- drivers/motor/motor_cluster.cpp | 48 +- drivers/motor/motor_cluster.hpp | 20 +- drivers/motor/motor_state.cpp | 43 +- drivers/motor/motor_state.hpp | 17 +- examples/pico_explorer_encoder/demo.cpp | 2 +- examples/pico_motor_shim/song/demo.cpp | 8 +- .../examples/motor2040/motor_profiler.py | 2 +- micropython/modules/encoder/encoder.cpp | 12 +- micropython/modules/motor/motor.c | 1 + micropython/modules/motor/motor.cpp | 88 +++- micropython/modules/motor/motor.h | 1 + 16 files changed, 528 insertions(+), 240 deletions(-) diff --git a/common/pimoroni_common.hpp b/common/pimoroni_common.hpp index b7b14aef..03b64f1f 100644 --- a/common/pimoroni_common.hpp +++ b/common/pimoroni_common.hpp @@ -59,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()); } @@ -86,11 +91,13 @@ namespace pimoroni { 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) {} diff --git a/drivers/encoder/encoder.cpp b/drivers/encoder/encoder.cpp index 427589c2..7fca1472 100644 --- a/drivers/encoder/encoder.cpp +++ b/drivers/encoder/encoder.cpp @@ -330,7 +330,7 @@ void Encoder::process_steps() { microstep_time = time_received; } - bool up = (enc_direction == NORMAL); + bool up = (enc_direction == NORMAL_DIR); // Determine what step occurred switch(LAST_STATE(states)) { diff --git a/drivers/encoder/encoder.hpp b/drivers/encoder/encoder.hpp index 37f4a8dc..45441ef3 100644 --- a/drivers/encoder/encoder.hpp +++ b/drivers/encoder/encoder.hpp @@ -7,11 +7,6 @@ using namespace pimoroni; namespace encoder { - enum Direction { - NORMAL = 0, - REVERSED = 1, - }; - class Encoder { //-------------------------------------------------- // Constants @@ -145,7 +140,7 @@ namespace encoder { // Constructors/Destructor //-------------------------------------------------- public: - Encoder(PIO pio, uint sm, const pin_pair &pins, uint common_pin = PIN_UNUSED, Direction direction = NORMAL, + 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(); diff --git a/drivers/motor/motor.cpp b/drivers/motor/motor.cpp index d08fcc29..fae17d5b 100644 --- a/drivers/motor/motor.cpp +++ b/drivers/motor/motor.cpp @@ -4,189 +4,64 @@ #include "math.h" namespace motor { - Motor::Motor(const pin_pair &pins, Direction direction, float speed_scale, float deadzone, float freq, DecayMode mode) - : motor_pins(pins), state(direction, speed_scale, deadzone), pwm_frequency(freq), motor_mode(mode) { + Motor::Driver::Driver(const pin_pair &pins) : motor_pins(pins), pwm_period(1) { } - Motor::~Motor() { - gpio_set_function(motor_pins.positive, GPIO_FUNC_NULL); - gpio_set_function(motor_pins.negative, GPIO_FUNC_NULL); + Motor::Driver::~Driver() { + gpio_set_function(motor_pins.first, GPIO_FUNC_NULL); + gpio_set_function(motor_pins.second, GPIO_FUNC_NULL); } - bool Motor::init() { - bool success = false; - - uint16_t period; uint16_t div16; - if(pimoroni::calculate_pwm_factors(pwm_frequency, period, div16)) { - pwm_period = period; - - 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, pwm_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... - - 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); - - success = true; - } - return success; - } - - pin_pair Motor::pins() const { + const pin_pair& Motor::Driver::pins() const { return motor_pins; } - void Motor::enable() { - apply_duty(state.enable_with_return(), motor_mode); + 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::disable() { - apply_duty(state.disable_with_return(), motor_mode); - } + 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; - bool Motor::is_enabled() const { - return state.is_enabled(); - } + uint pos_pin_slice = pwm_gpio_to_slice_num(motor_pins.positive); + uint neg_pin_slice = pwm_gpio_to_slice_num(motor_pins.negative); - float Motor::duty() const { - return state.get_duty(); - } + // 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); - void Motor::duty(float duty) { - apply_duty(state.set_duty_with_return(duty), motor_mode); - } - - float Motor::speed() const { - return state.get_speed(); - } - - void Motor::speed(float speed) { - 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)) { - - // 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; - pwm_frequency = freq; - - uint pos_pin_num = pwm_gpio_to_slice_num(motor_pins.positive); - uint neg_pin_num = pwm_gpio_to_slice_num(motor_pins.negative); - - // Apply the new divider - uint8_t div = div16 >> 4; - uint8_t mod = div16 % 16; - pwm_set_clkdiv_int_frac(pos_pin_num, div, mod); - if(neg_pin_num != pos_pin_num) - pwm_set_clkdiv_int_frac(neg_pin_num, div, mod); - - // If the period is larger, update the pwm before setting the new wraps - if(pre_update_pwm) { - apply_duty(state.get_deadzoned_duty(), motor_mode); - } - - // Set the new wrap (should be 1 less than the period to get full 0 to 100%) - pwm_set_wrap(pos_pin_num, pwm_period - 1); - if(neg_pin_num != pos_pin_num) - pwm_set_wrap(neg_pin_num, pwm_period - 1); - - // If the period is smaller, update the pwm after setting the new wraps - if(!pre_update_pwm) { - apply_duty(state.get_deadzoned_duty(), motor_mode); - } - - success = true; - } + // 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); } - return success; } - void Motor::stop() { - apply_duty(state.stop_with_return(), motor_mode); - } - - void Motor::coast() { - apply_duty(state.stop_with_return(), FAST_DECAY); - } - - void Motor::brake() { - apply_duty(state.stop_with_return(), SLOW_DECAY); - } - - void Motor::full_negative() { - apply_duty(state.full_negative_with_return(), motor_mode); - } - - void Motor::full_positive() { - apply_duty(state.full_positive_with_return(), motor_mode); - } - - void Motor::to_percent(float in, float in_min, float in_max) { - 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) { - 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::deadzone() const { - return state.get_deadzone(); - } - - void Motor::deadzone(float deadzone) { - 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; - apply_duty(state.get_deadzoned_duty(), motor_mode); - } - - void Motor::apply_duty(float duty, DecayMode mode) { + void Motor::DualPWMDriver::apply_duty(float duty, DecayMode mode) { if(isfinite(duty)) { int32_t signed_level = MotorState::duty_to_level(duty, pwm_period); @@ -220,4 +95,219 @@ namespace motor { 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); + } }; \ No newline at end of file diff --git a/drivers/motor/motor.hpp b/drivers/motor/motor.hpp index 45638097..d75f337f 100644 --- a/drivers/motor/motor.hpp +++ b/drivers/motor/motor.hpp @@ -10,23 +10,92 @@ 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: - pin_pair motor_pins; + Driver *driver; MotorState state; pwm_config pwm_cfg; - uint16_t pwm_period; float pwm_frequency; DecayMode motor_mode; + //-------------------------------------------------- // Constructors/Destructor //-------------------------------------------------- public: - Motor(const pin_pair &pins, Direction direction = NORMAL, float speed_scale = MotorState::DEFAULT_SPEED_SCALE, - float deadzone = MotorState::DEFAULT_DEADZONE, float freq = MotorState::DEFAULT_FREQUENCY, DecayMode mode = MotorState::DEFAULT_DECAY_MODE); + 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(); @@ -70,15 +139,14 @@ namespace motor { 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); - - //-------------------------------------------------- - private: - void apply_duty(float duty, DecayMode mode); }; } \ No newline at end of file diff --git a/drivers/motor/motor_cluster.cpp b/drivers/motor/motor_cluster.cpp index 4b77f48c..736d16d7 100644 --- a/drivers/motor/motor_cluster.cpp +++ b/drivers/motor/motor_cluster.cpp @@ -8,24 +8,24 @@ namespace motor { MotorCluster::MotorCluster(PIO pio, uint sm, uint pin_base, uint pin_pair_count, Direction direction, - float speed_scale, float deadzone, float freq, DecayMode mode, + 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), pwm_frequency(freq) { - create_motor_states(direction, speed_scale, deadzone, mode, auto_phase); + 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 deadzone, float freq, DecayMode mode, + 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), pwm_frequency(freq) { - create_motor_states(direction, speed_scale, deadzone, mode, auto_phase); + create_motor_states(direction, speed_scale, zeropoint, deadzone, mode, auto_phase); } MotorCluster::MotorCluster(PIO pio, uint sm, std::initializer_list pin_pairs, Direction direction, - float speed_scale, float deadzone, float freq, DecayMode mode, + 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), pwm_frequency(freq) { - create_motor_states(direction, speed_scale, deadzone, mode, auto_phase); + create_motor_states(direction, speed_scale, zeropoint, deadzone, mode, auto_phase); } MotorCluster::~MotorCluster() { @@ -581,6 +581,36 @@ namespace motor { } } + 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 motors, float zeropoint, bool load) { + for(auto motor : motors) { + this->zeropoint(motor, zeropoint); + } + } + + void MotorCluster::all_to_zeropoint(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(); @@ -679,7 +709,7 @@ namespace motor { } } - void MotorCluster::create_motor_states(Direction direction, float speed_scale, + 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) { @@ -687,10 +717,10 @@ namespace motor { configs = new motor_config[motor_count]; for(uint motor = 0; motor < motor_count; motor++) { - states[motor] = MotorState(direction, speed_scale, deadzone); + states[motor] = MotorState(direction, speed_scale, zeropoint, deadzone); configs[motor].phase = (auto_phase) ? (float)motor / (float)motor_count : 0.0f; configs[motor].mode = mode; } } } -}; \ No newline at end of file +} diff --git a/drivers/motor/motor_cluster.hpp b/drivers/motor/motor_cluster.hpp index d2468b21..785d9c15 100644 --- a/drivers/motor/motor_cluster.hpp +++ b/drivers/motor/motor_cluster.hpp @@ -34,14 +34,14 @@ namespace motor { // Constructors/Destructor //-------------------------------------------------- public: - MotorCluster(PIO pio, uint sm, uint pin_base, uint pin_pair_count, Direction direction = NORMAL, float speed_scale = MotorState::DEFAULT_SPEED_SCALE, - float deadzone = MotorState::DEFAULT_DEADZONE, float freq = MotorState::DEFAULT_FREQUENCY, DecayMode mode = MotorState::DEFAULT_DECAY_MODE, + 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, float speed_scale = MotorState::DEFAULT_SPEED_SCALE, - float deadzone = MotorState::DEFAULT_DEADZONE, float freq = MotorState::DEFAULT_FREQUENCY, DecayMode mode = MotorState::DEFAULT_DECAY_MODE, + 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_pairs, Direction direction = NORMAL, float speed_scale = MotorState::DEFAULT_SPEED_SCALE, - float deadzone = MotorState::DEFAULT_DEADZONE, float freq = MotorState::DEFAULT_FREQUENCY, DecayMode mode = MotorState::DEFAULT_DECAY_MODE, + MotorCluster(PIO pio, uint sm, std::initializer_list 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(); @@ -140,6 +140,12 @@ namespace motor { void speed_scale(std::initializer_list motors, float speed_scale); void all_to_speed_scale(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 motors, float zeropoint, bool load = true); + void all_to_zeropoint(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); @@ -155,7 +161,7 @@ namespace motor { //-------------------------------------------------- private: void apply_duty(uint8_t motor, float duty, DecayMode mode, bool load); - void create_motor_states(Direction direction, float speed_scale, float deadzone, DecayMode mode, bool auto_phase); + void create_motor_states(Direction direction, float speed_scale, float zeropoint, float deadzone, DecayMode mode, bool auto_phase); }; } \ No newline at end of file diff --git a/drivers/motor/motor_state.cpp b/drivers/motor/motor_state.cpp index ce66db91..4eeca27e 100644 --- a/drivers/motor/motor_state.cpp +++ b/drivers/motor/motor_state.cpp @@ -6,12 +6,12 @@ namespace motor { MotorState::MotorState() : motor_speed(0.0f), last_enabled_duty(0.0f), enabled(false), - motor_direction(NORMAL), motor_scale(DEFAULT_SPEED_SCALE), motor_deadzone(DEFAULT_DEADZONE){ + 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 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_deadzone(CLAMP(deadzone, 0.0f, 1.0f)) { + 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() { @@ -47,23 +47,38 @@ namespace motor { float MotorState::set_duty_with_return(float duty) { // Clamp the duty between the hard limits last_enabled_duty = CLAMP(duty, -1.0f, 1.0f); - motor_speed = last_enabled_duty * motor_scale; + + if(last_enabled_duty > motor_zeropoint) + motor_speed = map_float(last_enabled_duty, motor_zeropoint, 1.0f, 0.0f, motor_scale); + else if(last_enabled_duty < -motor_zeropoint) + motor_speed = map_float(last_enabled_duty, -motor_zeropoint, -1.0f, 0.0f, -motor_scale); + else + motor_speed = 0.0f; + + //motor_speed = last_enabled_duty * motor_scale; return enable_with_return(); } float MotorState::get_speed() const { - return (motor_direction == NORMAL) ? motor_speed : 0.0f - motor_speed; + 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) + 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); - last_enabled_duty = motor_speed / motor_scale; + + if(motor_speed > 0.0f) + last_enabled_duty = map_float(motor_speed / motor_scale, 0.0f, 1.0f, motor_zeropoint, 1.0f); + else if(motor_speed < 0.0f) + last_enabled_duty = map_float(motor_speed / motor_scale, 0.0f, 1.0f, motor_zeropoint, 1.0f); + else + last_enabled_duty = 0.0f; + //last_enabled_duty = motor_speed / motor_scale; return enable_with_return(); } @@ -107,6 +122,20 @@ namespace motor { motor_speed = last_enabled_duty * 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); + + if(last_enabled_duty > motor_zeropoint) + motor_speed = map_float(last_enabled_duty, motor_zeropoint, 1.0f, 0.0f, motor_scale); + else if(last_enabled_duty < -motor_zeropoint) + motor_speed = map_float(last_enabled_duty, -motor_zeropoint, -1.0f, 0.0f, -motor_scale); + else + motor_speed = 0.0f; + } float MotorState::get_deadzone() const { return motor_deadzone; diff --git a/drivers/motor/motor_state.hpp b/drivers/motor/motor_state.hpp index 990ecbd3..1a7f9cba 100644 --- a/drivers/motor/motor_state.hpp +++ b/drivers/motor/motor_state.hpp @@ -1,14 +1,12 @@ #pragma once #include "pico/stdlib.h" +#include "common/pimoroni_common.hpp" + +using namespace pimoroni; namespace motor { - enum Direction { - NORMAL = 0, - REVERSED = 1, - }; - enum DecayMode { FAST_DECAY = 0, //aka 'Coasting' SLOW_DECAY = 1, //aka 'Braking' @@ -20,7 +18,8 @@ namespace motor { //-------------------------------------------------- public: static constexpr float DEFAULT_SPEED_SCALE = 1.0f; // The standard motor speed scale - static constexpr float DEFAULT_DEADZONE = 0.1f; // The standard motor deadzone + static constexpr float DEFAULT_ZEROPOINT = 0.0f; // The standard motor deadzone + 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 @@ -42,6 +41,7 @@ namespace motor { // Customisation variables Direction motor_direction; float motor_scale; + float motor_zeropoint; float motor_deadzone; @@ -50,7 +50,7 @@ namespace motor { //-------------------------------------------------- public: MotorState(); - MotorState(Direction direction, float speed_scale, float deadzone); + MotorState(Direction direction, float speed_scale, float zeropoint, float deadzone); //-------------------------------------------------- @@ -84,6 +84,9 @@ namespace motor { 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); diff --git a/examples/pico_explorer_encoder/demo.cpp b/examples/pico_explorer_encoder/demo.cpp index 4ebfb7ae..0f4e9ce7 100644 --- a/examples/pico_explorer_encoder/demo.cpp +++ b/examples/pico_explorer_encoder/demo.cpp @@ -55,7 +55,7 @@ enum DrawState { uint16_t buffer[PicoExplorer::WIDTH * PicoExplorer::HEIGHT]; PicoExplorer pico_explorer(buffer); -Encoder enc(pio0, 0, ENCODER_PINS, ENCODER_PIN_C, NORMAL, COUNTS_PER_REVOLUTION, COUNT_MICROSTEPS, FREQ_DIVIDER); +Encoder enc(pio0, 0, ENCODER_PINS, ENCODER_PIN_C, NORMAL_DIR, COUNTS_PER_REVOLUTION, COUNT_MICROSTEPS, FREQ_DIVIDER); volatile bool encA_readings[READINGS_SIZE]; volatile bool encB_readings[READINGS_SIZE]; diff --git a/examples/pico_motor_shim/song/demo.cpp b/examples/pico_motor_shim/song/demo.cpp index 8f6df835..bd2f2ad5 100644 --- a/examples/pico_motor_shim/song/demo.cpp +++ b/examples/pico_motor_shim/song/demo.cpp @@ -40,11 +40,11 @@ static const uint STATIONARY_TOGGLE_US = 2000; Button button_a(pico_motor_shim::BUTTON_A, Polarity::ACTIVE_LOW, 0); #ifdef USE_FAST_DECAY - Motor motor_1(pico_motor_shim::MOTOR_1, NORMAL, MotorState::DEFAULT_SPEED_SCALE, MotorState::DEFAULT_FREQUENCY, FAST_DECAY); - Motor motor_2(pico_motor_shim::MOTOR_2, NORMAL, MotorState::DEFAULT_SPEED_SCALE, MotorState::DEFAULT_FREQUENCY, FAST_DECAY); + Motor motor_1(pico_motor_shim::MOTOR_1, NORMAL_DIR, MotorState::DEFAULT_SPEED_SCALE, MotorState::DEFAULT_FREQUENCY, FAST_DECAY); + Motor motor_2(pico_motor_shim::MOTOR_2, NORMAL_DIR, MotorState::DEFAULT_SPEED_SCALE, MotorState::DEFAULT_FREQUENCY, FAST_DECAY); #else - Motor motor_1(pico_motor_shim::MOTOR_1, NORMAL, MotorState::DEFAULT_SPEED_SCALE, MotorState::DEFAULT_FREQUENCY, SLOW_DECAY); - Motor motor_2(pico_motor_shim::MOTOR_2, NORMAL, MotorState::DEFAULT_SPEED_SCALE, MotorState::DEFAULT_FREQUENCY, SLOW_DECAY); + Motor motor_1(pico_motor_shim::MOTOR_1, NORMAL_DIR, MotorState::DEFAULT_SPEED_SCALE, MotorState::DEFAULT_FREQUENCY, SLOW_DECAY); + Motor motor_2(pico_motor_shim::MOTOR_2, NORMAL_DIR, MotorState::DEFAULT_SPEED_SCALE, MotorState::DEFAULT_FREQUENCY, SLOW_DECAY); #endif static bool button_toggle = false; diff --git a/micropython/examples/motor2040/motor_profiler.py b/micropython/examples/motor2040/motor_profiler.py index 0c9532f8..30d2896e 100644 --- a/micropython/examples/motor2040/motor_profiler.py +++ b/micropython/examples/motor2040/motor_profiler.py @@ -25,7 +25,7 @@ CAPTURE_TIME = 0.2 # How long to capture the motor's speed gc.collect() # Create a motor and set its speed scale, and give it a zero deadzone -m = Motor(MOTOR_PINS, direction=DIRECTION, speed_scale=SPEED_SCALE, deadzone=0.0) +m = Motor(MOTOR_PINS, direction=DIRECTION, speed_scale=SPEED_SCALE, zeropoint=0.0, deadzone=0.0) # Create an encoder, using PIO 0 and State Machine 0 enc = Encoder(0, 0, ENCODER_PINS, direction=DIRECTION, counts_per_rev=COUNTS_PER_REV, count_microsteps=True) diff --git a/micropython/modules/encoder/encoder.cpp b/micropython/modules/encoder/encoder.cpp index c080316f..4768f2c3 100644 --- a/micropython/modules/encoder/encoder.cpp +++ b/micropython/modules/encoder/encoder.cpp @@ -40,10 +40,10 @@ void Encoder_print(const mp_print_t *print, mp_obj_t self_in, mp_print_kind_t ki mp_obj_print_helper(print, mp_obj_new_int(self->encoder->common_pin()), PRINT_REPR); mp_print_str(print, ")"); - if(self->encoder->direction() == NORMAL) - mp_print_str(print, ", direction = NORMAL"); + if(self->encoder->direction() == NORMAL_DIR) + mp_print_str(print, ", direction = NORMAL_DIR"); else - mp_print_str(print, ", direction = REVERSED"); + mp_print_str(print, ", direction = REVERSED_DIR"); mp_print_str(print, ", counts_per_rev = "); mp_obj_print_helper(print, mp_obj_new_float(self->encoder->counts_per_rev()), PRINT_REPR); @@ -61,7 +61,7 @@ mp_obj_t Encoder_make_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw, { MP_QSTR_sm, MP_ARG_REQUIRED | MP_ARG_INT }, { MP_QSTR_pins, MP_ARG_REQUIRED | MP_ARG_OBJ }, { MP_QSTR_common_pin, MP_ARG_INT, {.u_int = PIN_UNUSED} }, - { MP_QSTR_direction, MP_ARG_INT, {.u_int = NORMAL} }, + { MP_QSTR_direction, MP_ARG_INT, {.u_int = NORMAL_DIR} }, { MP_QSTR_counts_per_rev, MP_ARG_OBJ, {.u_obj = mp_const_none} }, { MP_QSTR_count_microsteps, MP_ARG_BOOL, {.u_bool = false} }, { MP_QSTR_freq_divider, MP_ARG_OBJ, {.u_obj = mp_const_none} }, @@ -120,7 +120,7 @@ mp_obj_t Encoder_make_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw, int direction = args[ARG_direction].u_int; if(direction < 0 || direction > 1) { - mp_raise_ValueError("direction out of range. Expected NORMAL (0) or REVERSED (1)"); + mp_raise_ValueError("direction out of range. Expected NORMAL (0) or REVERSED_DIR (1)"); } float counts_per_rev = Encoder::DEFAULT_COUNTS_PER_REV; @@ -257,7 +257,7 @@ extern mp_obj_t Encoder_direction(size_t n_args, const mp_obj_t *pos_args, mp_ma int direction = args[ARG_direction].u_int; if(direction < 0 || direction > 1) { - mp_raise_ValueError("direction out of range. Expected NORMAL (0) or REVERSED (1)"); + mp_raise_ValueError("direction out of range. Expected NORMAL (0) or REVERSED_DIR (1)"); } self->encoder->direction((Direction)direction); return mp_const_none; diff --git a/micropython/modules/motor/motor.c b/micropython/modules/motor/motor.c index a79ddb11..715984bd 100644 --- a/micropython/modules/motor/motor.c +++ b/micropython/modules/motor/motor.c @@ -17,6 +17,7 @@ MP_DEFINE_CONST_FUN_OBJ_1(Motor_full_positive_obj, Motor_full_positive); MP_DEFINE_CONST_FUN_OBJ_KW(Motor_to_percent_obj, 2, Motor_to_percent); MP_DEFINE_CONST_FUN_OBJ_KW(Motor_direction_obj, 1, Motor_direction); MP_DEFINE_CONST_FUN_OBJ_KW(Motor_speed_scale_obj, 1, Motor_speed_scale); +MP_DEFINE_CONST_FUN_OBJ_KW(Motor_zeropoint_obj, 1, Motor_zeropoint); MP_DEFINE_CONST_FUN_OBJ_KW(Motor_deadzone_obj, 1, Motor_deadzone); MP_DEFINE_CONST_FUN_OBJ_KW(Motor_decay_mode_obj, 1, Motor_decay_mode); diff --git a/micropython/modules/motor/motor.cpp b/micropython/modules/motor/motor.cpp index 1bbc2cf5..8a7f5f6f 100644 --- a/micropython/modules/motor/motor.cpp +++ b/micropython/modules/motor/motor.cpp @@ -42,12 +42,14 @@ void Motor_print(const mp_print_t *print, mp_obj_t self_in, mp_print_kind_t kind mp_obj_print_helper(print, mp_obj_new_float(self->motor->speed()), PRINT_REPR); mp_print_str(print, ", freq = "); mp_obj_print_helper(print, mp_obj_new_float(self->motor->frequency()), PRINT_REPR); - if(self->motor->direction() == NORMAL) - mp_print_str(print, ", direction = NORMAL"); + if(self->motor->direction() == NORMAL_DIR) + mp_print_str(print, ", direction = NORMAL_DIR"); else - mp_print_str(print, ", direction = REVERSED"); + mp_print_str(print, ", direction = REVERSED_DIR"); mp_print_str(print, ", speed_scale = "); mp_obj_print_helper(print, mp_obj_new_float(self->motor->speed_scale()), PRINT_REPR); + mp_print_str(print, ", zeropoint = "); + mp_obj_print_helper(print, mp_obj_new_float(self->motor->zeropoint()), PRINT_REPR); mp_print_str(print, ", deadzone = "); mp_obj_print_helper(print, mp_obj_new_float(self->motor->deadzone()), PRINT_REPR); if(self->motor->decay_mode() == SLOW_DECAY) @@ -63,14 +65,16 @@ void Motor_print(const mp_print_t *print, mp_obj_t self_in, mp_print_kind_t kind mp_obj_t Motor_make_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw, const mp_obj_t *all_args) { _Motor_obj_t *self = nullptr; - enum { ARG_pins, ARG_direction, ARG_speed_scale, ARG_deadzone, ARG_freq, ARG_mode }; + enum { ARG_pins, ARG_direction, ARG_speed_scale, ARG_zeropoint, ARG_deadzone, ARG_freq, ARG_mode, ARG_ph_en_driver }; static const mp_arg_t allowed_args[] = { { MP_QSTR_pins, MP_ARG_REQUIRED | MP_ARG_OBJ }, - { MP_QSTR_direction, MP_ARG_INT, {.u_int = NORMAL} }, + { MP_QSTR_direction, MP_ARG_INT, {.u_int = NORMAL_DIR} }, { MP_QSTR_speed_scale, MP_ARG_OBJ, {.u_obj = mp_const_none} }, + { MP_QSTR_zeropoint, MP_ARG_OBJ, {.u_obj = mp_const_none} }, { MP_QSTR_deadzone, MP_ARG_OBJ, {.u_obj = mp_const_none} }, { MP_QSTR_freq, MP_ARG_OBJ, {.u_obj = mp_const_none} }, { MP_QSTR_mode, MP_ARG_INT, {.u_int = MotorState::DEFAULT_DECAY_MODE} }, + { MP_QSTR_ph_en_driver, MP_ARG_BOOL, {.u_bool = false} } }; // Parse args. @@ -115,7 +119,7 @@ mp_obj_t Motor_make_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw, c int direction = args[ARG_direction].u_int; if(direction < 0 || direction > 1) { - mp_raise_ValueError("direction out of range. Expected NORMAL (0) or REVERSED (1)"); + mp_raise_ValueError("direction out of range. Expected NORMAL_DIR (0) or REVERSED_DIR (1)"); } float speed_scale = MotorState::DEFAULT_SPEED_SCALE; @@ -126,6 +130,14 @@ mp_obj_t Motor_make_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw, c } } + float zeropoint = MotorState::DEFAULT_ZEROPOINT; + if(args[ARG_zeropoint].u_obj != mp_const_none) { + zeropoint = mp_obj_get_float(args[ARG_zeropoint].u_obj); + if(zeropoint < 0.0f || zeropoint > 1.0f - FLT_EPSILON) { + mp_raise_ValueError("zeropoint out of range. Expected 0.0 to less than 1.0"); + } + } + float deadzone = MotorState::DEFAULT_DEADZONE; if(args[ARG_deadzone].u_obj != mp_const_none) { deadzone = mp_obj_get_float(args[ARG_deadzone].u_obj); @@ -147,7 +159,7 @@ mp_obj_t Motor_make_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw, c self = m_new_obj_with_finaliser(_Motor_obj_t); self->base.type = &Motor_type; - self->motor = new Motor(pins, (Direction)direction, speed_scale, deadzone, freq, (DecayMode)mode); + self->motor = new Motor(pins, (Direction)direction, speed_scale, zeropoint, deadzone, freq, (DecayMode)mode, args[ARG_ph_en_driver].u_bool); self->motor->init(); return MP_OBJ_FROM_PTR(self); @@ -426,7 +438,7 @@ extern mp_obj_t Motor_direction(size_t n_args, const mp_obj_t *pos_args, mp_map_ int direction = args[ARG_direction].u_int; if(direction < 0 || direction > 1) { - mp_raise_ValueError("direction out of range. Expected NORMAL (0) or REVERSED (1)"); + mp_raise_ValueError("direction out of range. Expected NORMAL_DIR (0) or REVERSED_DIR (1)"); } self->motor->direction((Direction)direction); return mp_const_none; @@ -470,6 +482,43 @@ extern mp_obj_t Motor_speed_scale(size_t n_args, const mp_obj_t *pos_args, mp_ma } } +extern mp_obj_t Motor_zeropoint(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { + if(n_args <= 1) { + enum { ARG_self }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _Motor_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Motor_obj_t); + + return mp_obj_new_float(self->motor->zeropoint()); + } + else { + enum { ARG_self, ARG_zeropoint }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_zeropoint, MP_ARG_REQUIRED | MP_ARG_OBJ }, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _Motor_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Motor_obj_t); + + float zeropoint = mp_obj_get_float(args[ARG_zeropoint].u_obj); + if(zeropoint < 0.0f || zeropoint > 1.0f - FLT_EPSILON) { + mp_raise_ValueError("zeropoint out of range. Expected 0.0 to less than 1.0"); + } + self->motor->zeropoint(zeropoint); + return mp_const_none; + } +} + extern mp_obj_t Motor_deadzone(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { if(n_args <= 1) { enum { ARG_self }; @@ -597,13 +646,14 @@ void MotorCluster_print(const mp_print_t *print, mp_obj_t self_in, mp_print_kind mp_obj_t MotorCluster_make_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw, const mp_obj_t *all_args) { _MotorCluster_obj_t *self = nullptr; - enum { ARG_pio, ARG_sm, ARG_pins, ARG_direction, ARG_speed_scale, ARG_deadzone, ARG_freq, ARG_mode, ARG_auto_phase }; + enum { ARG_pio, ARG_sm, ARG_pins, ARG_direction, ARG_speed_scale, ARG_zeropoint, ARG_deadzone, ARG_freq, ARG_mode, ARG_auto_phase }; static const mp_arg_t allowed_args[] = { { MP_QSTR_pio, MP_ARG_REQUIRED | MP_ARG_INT }, { MP_QSTR_sm, MP_ARG_REQUIRED | MP_ARG_INT }, { MP_QSTR_pins, MP_ARG_REQUIRED | MP_ARG_OBJ }, - { MP_QSTR_direction, MP_ARG_INT, {.u_int = NORMAL} }, + { MP_QSTR_direction, MP_ARG_INT, {.u_int = NORMAL_DIR} }, { MP_QSTR_speed_scale, MP_ARG_OBJ, {.u_obj = mp_const_none} }, + { MP_QSTR_zeropoint, MP_ARG_OBJ, {.u_obj = mp_const_none} }, { MP_QSTR_deadzone, MP_ARG_OBJ, {.u_obj = mp_const_none} }, { MP_QSTR_freq, MP_ARG_OBJ, {.u_obj = mp_const_none} }, { MP_QSTR_mode, MP_ARG_INT, {.u_int = MotorState::DEFAULT_DECAY_MODE} }, @@ -703,7 +753,7 @@ mp_obj_t MotorCluster_make_new(const mp_obj_type_t *type, size_t n_args, size_t int direction = args[ARG_direction].u_int; if(direction < 0 || direction > 1) { - mp_raise_ValueError("direction out of range. Expected NORMAL (0) or REVERSED (1)"); + mp_raise_ValueError("direction out of range. Expected NORMAL_DIR (0) or REVERSED_DIR (1)"); } float speed_scale = MotorState::DEFAULT_SPEED_SCALE; @@ -714,6 +764,14 @@ mp_obj_t MotorCluster_make_new(const mp_obj_type_t *type, size_t n_args, size_t } } + float zeropoint = MotorState::DEFAULT_ZEROPOINT; + if(args[ARG_zeropoint].u_obj != mp_const_none) { + zeropoint = mp_obj_get_float(args[ARG_zeropoint].u_obj); + if(zeropoint < 0.0f || zeropoint > 1.0f - FLT_EPSILON) { + mp_raise_ValueError("zeropoint out of range. Expected 0.0 to less than 1.0"); + } + } + float deadzone = MotorState::DEFAULT_DEADZONE; if(args[ARG_deadzone].u_obj != mp_const_none) { deadzone = mp_obj_get_float(args[ARG_deadzone].u_obj); @@ -737,7 +795,7 @@ mp_obj_t MotorCluster_make_new(const mp_obj_type_t *type, size_t n_args, size_t MotorCluster *cluster; PWMCluster::Sequence *seq_buffer = m_new(PWMCluster::Sequence, PWMCluster::NUM_BUFFERS * 2); PWMCluster::TransitionData *dat_buffer = m_new(PWMCluster::TransitionData, PWMCluster::BUFFER_SIZE * 2); - cluster = new MotorCluster(pio, sm, pins, pair_count, (Direction)direction, speed_scale, deadzone, + cluster = new MotorCluster(pio, sm, pins, pair_count, (Direction)direction, speed_scale, zeropoint, deadzone, freq, (DecayMode)mode, auto_phase, seq_buffer, dat_buffer); // Cleanup the pins array @@ -2194,7 +2252,7 @@ extern mp_obj_t MotorCluster_direction(size_t n_args, const mp_obj_t *pos_args, else { int direction = args[ARG_direction].u_int; if(direction < 0 || direction > 1) { - mp_raise_ValueError("direction out of range. Expected NORMAL (0) or REVERSED (1)"); + mp_raise_ValueError("direction out of range. Expected NORMAL_DIR (0) or REVERSED_DIR (1)"); } self->cluster->direction((uint)motor, (Direction)direction); } @@ -2233,7 +2291,7 @@ extern mp_obj_t MotorCluster_direction(size_t n_args, const mp_obj_t *pos_args, int direction = args[ARG_direction].u_int; if(direction < 0 || direction > 1) { delete[] motors; - mp_raise_ValueError("direction out of range. Expected NORMAL (0) or REVERSED (1)"); + mp_raise_ValueError("direction out of range. Expected NORMAL_DIR (0) or REVERSED_DIR (1)"); } self->cluster->direction(motors, length, (Direction)direction); delete[] motors; @@ -2263,7 +2321,7 @@ extern mp_obj_t MotorCluster_all_to_direction(size_t n_args, const mp_obj_t *pos else { int direction = args[ARG_direction].u_int; if(direction < 0 || direction > 1) { - mp_raise_ValueError("direction out of range. Expected NORMAL (0) or REVERSED (1)"); + mp_raise_ValueError("direction out of range. Expected NORMAL_DIR (0) or REVERSED_DIR (1)"); } self->cluster->all_to_direction((Direction)direction); } diff --git a/micropython/modules/motor/motor.h b/micropython/modules/motor/motor.h index 1dff6978..7ce746a4 100644 --- a/micropython/modules/motor/motor.h +++ b/micropython/modules/motor/motor.h @@ -24,6 +24,7 @@ extern mp_obj_t Motor_full_positive(mp_obj_t self_in); extern mp_obj_t Motor_to_percent(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); extern mp_obj_t Motor_direction(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); extern mp_obj_t Motor_speed_scale(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); +extern mp_obj_t Motor_zeropoint(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); extern mp_obj_t Motor_deadzone(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); extern mp_obj_t Motor_decay_mode(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); From 56451bff302f6f536a8ec98d774da410282f3500 Mon Sep 17 00:00:00 2001 From: ZodiusInfuser Date: Tue, 26 Apr 2022 12:52:28 +0100 Subject: [PATCH 36/60] Finished MP examples --- micropython/examples/motor2040/README.md | 43 ++--- .../examples/motor2040/current_meter.py | 90 ---------- .../examples/motor2040/quad_position_wave.py | 2 +- .../motor2040/quad_velocity_sequence.py | 162 ++++++++++++++++++ .../examples/motor2040/read_sensors.py | 38 ++-- .../examples/motor2040/sensor_feedback.py | 58 ------- micropython/modules/motor/motor.c | 5 +- micropython/modules_py/pimoroni.py | 6 + 8 files changed, 210 insertions(+), 194 deletions(-) delete mode 100644 micropython/examples/motor2040/current_meter.py create mode 100644 micropython/examples/motor2040/quad_velocity_sequence.py delete mode 100644 micropython/examples/motor2040/sensor_feedback.py diff --git a/micropython/examples/motor2040/README.md b/micropython/examples/motor2040/README.md index 042bfcc4..2468e269 100644 --- a/micropython/examples/motor2040/README.md +++ b/micropython/examples/motor2040/README.md @@ -6,13 +6,10 @@ - [Motor Cluster](#motor-cluster) - [Motor Wave](#motor-wave) - [Stop Motors](#stop-motors) -- [Encoder Examples](#encoder-examples) - - [Read Encoders](#read-encoders) - - [Motor Profiler](#motor-profiler) - [Function Examples](#function-examples) - [Read Sensors](#read-sensors) - - [Sensor Feedback](#sensor-feedback) - - [Current Meter](#current-meter) + - [Read Encoders](#read-encoders) + - [Motor Profiler](#motor-profiler) - [LED Rainbow](#led-rainbow) - [Turn Off LEDs](#turn-off-leds) - [Control Examples](#control-examples) @@ -21,6 +18,7 @@ - [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) @@ -59,7 +57,13 @@ An example of applying a wave pattern to a group of motors and the LEDs. A simple program that stops the motors. -## Encoder Examples +## Function Examples + +### Read Sensors +[read_sensors.py](read_sensors.py) + +Shows how to initialise and read the 2 external and 6 internal sensors of Motor 2040. + ### Read Encoders [read_encoders.py](read_encoders.py) @@ -74,26 +78,6 @@ A program that profiles the speed of a motor across its PWM duty cycle range using the attached encoder for feedback. -## Function Examples - -### Read Sensors -[read_sensors.py](read_sensors.py) -TODO -Shows how to initialise and read the 6 external and 2 internal sensors of Motor 2040. - - -### Sensor Feedback -[sensor_feedback.py](sensor_feedback.py) -TODO -Show how to read the 6 external sensors and display their values on the neighbouring LEDs. - - -### Current Meter -[current_meter.py](current_meter.py) -TODO -An example of how to use Motor 2040's current measuring ability and display the value on the onboard LED bar. - - ### LED Rainbow [led_rainbow.py](led_rainbow.py) @@ -135,7 +119,12 @@ A demonstration of how a motor with an encoder can be used as a programmable rot ### Quad Position Wave [quad_position_wave.py](quad_position_wave.py) -An demonstration of driving all four of Motor 2040's motor outputs between positions, with the help of their attached encoders and PID control. +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 +[quad_velocity_sequence.py](quad_velocity_sequence.py) + +A demonstration of driving all four of Motor 2040's motor outputs through a sequence of velocities, with the help of their attached encoders and PID control. ## Tuning Examples diff --git a/micropython/examples/motor2040/current_meter.py b/micropython/examples/motor2040/current_meter.py deleted file mode 100644 index 5e4f4220..00000000 --- a/micropython/examples/motor2040/current_meter.py +++ /dev/null @@ -1,90 +0,0 @@ -import gc -import time -from machine import Pin -from pimoroni import Analog, AnalogMux, Button -from plasma import WS2812 -from servo import ServoCluster, servo2040 - -""" -An example of how to use Servo 2040's current measuring -ability and display the value on the onboard LED bar. - -Press "Boot" to exit the program. - -NOTE: ServoCluster and Plasma WS2812 use the RP2040's PIO system, -and as such may have problems when running code multiple times. -If you encounter issues, try resetting your board. -""" - -BRIGHTNESS = 0.4 # The brightness of the LEDs -UPDATES = 50 # How many times to update LEDs and Servos per second -MAX_CURRENT = 3.0 # The maximum current, in amps, to show on the meter -SAMPLES = 50 # The number of current measurements to take per reading -TIME_BETWEEN = 0.001 # The time between each current measurement - -# Free up hardware resources ahead of creating a new ServoCluster -gc.collect() - -# Create a servo cluster for pins 0 to 7, using PIO 0 and State Machine 0 -START_PIN = servo2040.SERVO_1 -END_PIN = servo2040.SERVO_8 -servos = ServoCluster(pio=0, sm=0, pins=list(range(START_PIN, END_PIN + 1))) - -# Set up the shared analog inputs -cur_adc = Analog(servo2040.SHARED_ADC, servo2040.CURRENT_GAIN, - servo2040.SHUNT_RESISTOR, servo2040.CURRENT_OFFSET) - -# Set up the analog multiplexer, including the pin for controlling pull-up/pull-down -mux = AnalogMux(servo2040.ADC_ADDR_0, servo2040.ADC_ADDR_1, servo2040.ADC_ADDR_2, - muxed_pin=Pin(servo2040.SHARED_ADC)) - -# Create the LED bar, using PIO 1 and State Machine 0 -led_bar = WS2812(servo2040.NUM_LEDS, 1, 0, servo2040.LED_DATA) - -# Create the user button -user_sw = Button(servo2040.USER_SW) - -# Start updating the LED bar -led_bar.start() - -# Enable all servos (this puts them at the middle). -# The servos are not going to be moved, but are activated to give a current draw -servos.enable_all() - -# Read sensors until the user button is pressed -while user_sw.raw() is not True: - - # Select the current sense - mux.select(servo2040.CURRENT_SENSE_ADDR) - - # Read the current sense several times and average the result - current = 0 - for i in range(SAMPLES): - current += cur_adc.read_current() - time.sleep(TIME_BETWEEN) - current /= SAMPLES - - # Print out the current sense value - print("Current =", round(current, 4)) - - # Convert the current to a percentage of the maximum we want to show - percent = (current / MAX_CURRENT) - - # Update all the LEDs - for i in range(servo2040.NUM_LEDS): - # Calculate the LED's hue, with Red for high currents and Green for low - hue = (1.0 - i / (servo2040.NUM_LEDS - 1)) * 0.333 - - # Calculate the current level the LED represents - level = (i + 0.5) / servo2040.NUM_LEDS - # If the percent is above the level, light the LED, otherwise turn it off - if percent >= level: - led_bar.set_hsv(i, hue, 1.0, BRIGHTNESS) - else: - led_bar.set_hsv(i, hue, 1.0, 0.0) - -# Disable the servos -servos.disable_all() - -# Turn off the LED bar -led_bar.clear() diff --git a/micropython/examples/motor2040/quad_position_wave.py b/micropython/examples/motor2040/quad_position_wave.py index 400a6af9..3ef04108 100644 --- a/micropython/examples/motor2040/quad_position_wave.py +++ b/micropython/examples/motor2040/quad_position_wave.py @@ -7,7 +7,7 @@ from encoder import Encoder, MMME_CPR from pimoroni import Button, PID, REVERSED_DIR """ -An demonstration of driving all four of Motor 2040's motor outputs between +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. diff --git a/micropython/examples/motor2040/quad_velocity_sequence.py b/micropython/examples/motor2040/quad_velocity_sequence.py new file mode 100644 index 00000000..e9fc46a2 --- /dev/null +++ b/micropython/examples/motor2040/quad_velocity_sequence.py @@ -0,0 +1,162 @@ +import gc +import time +from motor import Motor, motor2040 +from encoder import Encoder, MMME_CPR +from pimoroni import Button, PID, REVERSED_DIR + +""" +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. +""" + +GEAR_RATIO = 50 # The gear ratio of the motors +COUNTS_PER_REV = MMME_CPR * GEAR_RATIO # The counts per revolution of each motor's output shaft + +SPEED_SCALE = 5.4 # The scaling to apply to each motor's speed to match its real-world speed + +UPDATES = 100 # How many times to update the motor per second +UPDATE_RATE = 1 / UPDATES +TIME_FOR_EACH_MOVE = 2 # The time to travel between each value +UPDATES_PER_MOVE = TIME_FOR_EACH_MOVE * UPDATES +PRINT_DIVIDER = 4 # How many of the updates should be printed (i.e. 2 would be every other update) + +# PID values +VEL_KP = 30.0 # Velocity proportional (P) gain +VEL_KI = 0.0 # Velocity integral (I) gain +VEL_KD = 0.4 # Velocity derivative (D) gain + + +# Free up hardware resources ahead of creating a new Encoder +gc.collect() + +# Create a list of motors with a given speed scale +MOTOR_PINS = [motor2040.MOTOR_A, motor2040.MOTOR_B, motor2040.MOTOR_C, motor2040.MOTOR_D] +motors = [Motor(pins, speed_scale=SPEED_SCALE) for pins in MOTOR_PINS] + +# Create a list of encoders, using PIO 0, with the given counts per revolution +ENCODER_PINS = [motor2040.ENCODER_A, motor2040.ENCODER_B, motor2040.ENCODER_C, motor2040.ENCODER_D] +ENCODER_NAMES = ["RR", "RL", "FL", "FR"] +encoders = [Encoder(0, i, ENCODER_PINS[i], counts_per_rev=COUNTS_PER_REV, count_microsteps=True) for i in range(motor2040.NUM_MOTORS)] + +# Wheel friendly names +FL = 2 +FR = 3 +RL = 1 +RR = 0 + +# Reverse the direction of the B and D motors and encoders +motors[FL].direction(REVERSED_DIR) +motors[RL].direction(REVERSED_DIR) +encoders[FL].direction(REVERSED_DIR) +encoders[RL].direction(REVERSED_DIR) + +# Create the user button +user_sw = Button(motor2040.USER_SW) + +# Create PID objects for position control +vel_pids = [PID(VEL_KP, VEL_KI, VEL_KD, UPDATE_RATE) for i in range(motor2040.NUM_MOTORS)] + +# Enable the motor to get started +for m in motors: + m.enable() + + +update = 0 +print_count = 0 + + +# Helper functions for driving in common directions +def drive_forward(speed): + vel_pids[FL].setpoint = speed + vel_pids[FR].setpoint = speed + vel_pids[RL].setpoint = speed + vel_pids[RR].setpoint = speed + + +def turn_right(speed): + vel_pids[FL].setpoint = speed + vel_pids[FR].setpoint = -speed + vel_pids[RL].setpoint = speed + vel_pids[RR].setpoint = -speed + + +def strafe_right(speed): + vel_pids[FL].setpoint = speed + vel_pids[FR].setpoint = -speed + vel_pids[RL].setpoint = -speed + vel_pids[RR].setpoint = speed + + +def stop(): + vel_pids[FL].setpoint = 0 + vel_pids[FR].setpoint = 0 + vel_pids[RL].setpoint = 0 + vel_pids[RR].setpoint = 0 + + +sequence = 0 + +captures = [None] * motor2040.NUM_MOTORS + +# Continually move the motor until the user button is pressed +while user_sw.raw() is not True: + + # Capture the state of all the encoders + for i in range(motor2040.NUM_MOTORS): + captures[i] = encoders[i].capture() + + # Calculate how far along this movement to be + percent_along = min(update / UPDATES_PER_MOVE, 1.0) + + for i in range(motor2040.NUM_MOTORS): + # Calculate the acceleration to apply to the motor to move it closer to the velocity setpoint + accel = vel_pids[i].calculate(captures[i].revolutions_per_second) + + # Accelerate or decelerate the motor + motors[i].speed(motors[i].speed() + (accel * UPDATE_RATE)) + + # Print out the current motor values, but only on every multiple + if print_count == 0: + for i in range(len(motors)): + print(ENCODER_NAMES[i], "=", captures[i].revolutions_per_second, end=", ") + print() + + # Increment the print count, and wrap it + print_count = (print_count + 1) % PRINT_DIVIDER + + update += 1 # 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 + if sequence == 0: + drive_forward(1.0) + elif sequence == 1: + drive_forward(-1.0) + elif sequence == 2: + turn_right(1.0) + elif sequence == 3: + turn_right(-1.0) + elif sequence == 4: + strafe_right(1.0) + elif sequence == 5: + strafe_right(-1.0) + elif sequence == 6: + stop() + + time.sleep(UPDATE_RATE) + +# Stop all the motors +for m in motors: + m.disable() diff --git a/micropython/examples/motor2040/read_sensors.py b/micropython/examples/motor2040/read_sensors.py index e6c64c66..15dd6335 100644 --- a/micropython/examples/motor2040/read_sensors.py +++ b/micropython/examples/motor2040/read_sensors.py @@ -1,32 +1,35 @@ import time from machine import Pin from pimoroni import Analog, AnalogMux, Button -from servo import servo2040 +from motor import motor2040 """ -Shows how to initialise and read the 6 external -and 2 internal sensors of Servo 2040. +Shows how to initialise and read the 2 external +and 6 internal sensors of Motor 2040. Press "Boot" to exit the program. """ # Set up the shared analog inputs -sen_adc = Analog(servo2040.SHARED_ADC) -vol_adc = Analog(servo2040.SHARED_ADC, servo2040.VOLTAGE_GAIN) -cur_adc = Analog(servo2040.SHARED_ADC, servo2040.CURRENT_GAIN, - servo2040.SHUNT_RESISTOR, servo2040.CURRENT_OFFSET) +sen_adc = Analog(motor2040.SHARED_ADC) +vol_adc = Analog(motor2040.SHARED_ADC, motor2040.VOLTAGE_GAIN) +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 -mux = AnalogMux(servo2040.ADC_ADDR_0, servo2040.ADC_ADDR_1, servo2040.ADC_ADDR_2, - muxed_pin=Pin(servo2040.SHARED_ADC)) +mux = AnalogMux(motor2040.ADC_ADDR_0, motor2040.ADC_ADDR_1, motor2040.ADC_ADDR_2, + muxed_pin=Pin(motor2040.SHARED_ADC)) # Set up the sensor addresses and have them pulled down by default -sensor_addrs = list(range(servo2040.SENSOR_1_ADDR, servo2040.SENSOR_6_ADDR + 1)) +sensor_addrs = list(range(motor2040.SENSOR_1_ADDR, motor2040.SENSOR_2_ADDR + 1)) for addr in sensor_addrs: mux.configure_pull(addr, Pin.PULL_DOWN) +# Set up the pull-up for the fault sense +mux.configure_pull(motor2040.FAULT_SENSE_ADDR, Pin.PULL_UP) + # Create the user button -user_sw = Button(servo2040.USER_SW) +user_sw = Button(motor2040.USER_SW) # Read sensors until the user button is pressed @@ -38,11 +41,16 @@ while user_sw.raw() is not True: print("S", i + 1, " = ", round(sen_adc.read_voltage(), 3), sep="", end=", ") # Read the voltage sense and print the value - mux.select(servo2040.VOLTAGE_SENSE_ADDR) + mux.select(motor2040.VOLTAGE_SENSE_ADDR) print("Voltage =", round(vol_adc.read_voltage(), 4), end=", ") - # Read the current sense and print the value - mux.select(servo2040.CURRENT_SENSE_ADDR) - print("Current =", round(cur_adc.read_current(), 4)) + # Read the current sense's of each motor and print the value + for i in range(motor2040.NUM_MOTORS): + mux.select(i + motor2040.CURRENT_SENSE_A_ADDR) + print("C", i + 1, "=", cur_adc.read_current(), sep="", end=", ") + + # Read the fault sense and print the value + mux.select(motor2040.FAULT_SENSE_ADDR) + print("Fault =", not mux.read()) time.sleep(0.5) diff --git a/micropython/examples/motor2040/sensor_feedback.py b/micropython/examples/motor2040/sensor_feedback.py deleted file mode 100644 index c781cc92..00000000 --- a/micropython/examples/motor2040/sensor_feedback.py +++ /dev/null @@ -1,58 +0,0 @@ -import time -from machine import Pin -from pimoroni import Analog, AnalogMux, Button -from plasma import WS2812 -from servo import servo2040 - -""" -Show how to read the 6 external sensors and -display their values on the neighbouring LEDs. - -Press "Boot" to exit the program. - -NOTE: Plasma WS2812 uses the RP2040's PIO system, and as -such may have problems when running code multiple times. -If you encounter issues, try resetting your board. -""" - -BRIGHTNESS = 0.4 # The brightness of the LEDs -UPDATES = 50 # How many times to update LEDs and Servos per second - -# Set up the shared analog inputs -sen_adc = Analog(servo2040.SHARED_ADC) - -# Set up the analog multiplexer, including the pin for controlling pull-up/pull-down -mux = AnalogMux(servo2040.ADC_ADDR_0, servo2040.ADC_ADDR_1, servo2040.ADC_ADDR_2, - muxed_pin=Pin(servo2040.SHARED_ADC)) - -# Set up the sensor addresses and have them pulled down by default -sensor_addrs = list(range(servo2040.SENSOR_1_ADDR, servo2040.SENSOR_6_ADDR + 1)) -for addr in sensor_addrs: - mux.configure_pull(addr, Pin.PULL_DOWN) - -# Create the LED bar, using PIO 1 and State Machine 0 -led_bar = WS2812(servo2040.NUM_LEDS, 1, 0, servo2040.LED_DATA) - -# Create the user button -user_sw = Button(servo2040.USER_SW) - -# Start updating the LED bar -led_bar.start() - - -# Read sensors until the user button is pressed -while user_sw.raw() is not True: - - # Read each sensor in turn and print its voltage - for i in range(len(sensor_addrs)): - mux.select(sensor_addrs[i]) - sensor_voltage = sen_adc.read_voltage() - - # Calculate the LED's hue, with Green for high voltages and Blue for low - hue = (2.0 - (sensor_voltage / 3.3)) * 0.333 - led_bar.set_hsv(i, hue, 1.0, BRIGHTNESS) - - print("S", i + 1, " = ", round(sensor_voltage, 3), sep="", end=", ") - print() - - time.sleep(1.0 / UPDATES) diff --git a/micropython/modules/motor/motor.c b/micropython/modules/motor/motor.c index 715984bd..d7881712 100644 --- a/micropython/modules/motor/motor.c +++ b/micropython/modules/motor/motor.c @@ -177,10 +177,9 @@ typedef struct _mp_obj_float_t { mp_obj_base_t base; mp_float_t value; } mp_obj_float_t; -//TODO confirm below numbers are correct mp_obj_float_t motor2040_shunt_resistor = {{&mp_type_float}, 0.47f}; mp_obj_float_t motor2040_voltage_gain = {{&mp_type_float}, 3.9f / 13.9f}; -mp_obj_float_t motor2040_current_offset = {{&mp_type_float}, -0.02f}; +mp_obj_float_t motor2040_current_offset = {{&mp_type_float}, -0.005f}; /***** Globals Table *****/ @@ -231,7 +230,7 @@ STATIC const mp_rom_map_elem_t motor2040_globals_table[] = { { MP_ROM_QSTR(MP_QSTR_SENSOR_2_ADDR), MP_ROM_INT(0b111) }, { MP_ROM_QSTR(MP_QSTR_NUM_SENSORS), MP_ROM_INT(2) }, { MP_ROM_QSTR(MP_QSTR_SHUNT_RESISTOR), MP_ROM_PTR(&motor2040_shunt_resistor) }, - { MP_ROM_QSTR(MP_QSTR_CURRENT_GAIN), MP_ROM_INT(5) }, + { MP_ROM_QSTR(MP_QSTR_CURRENT_GAIN), MP_ROM_INT(1) }, { MP_ROM_QSTR(MP_QSTR_VOLTAGE_GAIN), MP_ROM_PTR(&motor2040_voltage_gain) }, { MP_ROM_QSTR(MP_QSTR_CURRENT_OFFSET), MP_ROM_PTR(&motor2040_current_offset) }, }; diff --git a/micropython/modules_py/pimoroni.py b/micropython/modules_py/pimoroni.py index f833329a..b8c9e739 100644 --- a/micropython/modules_py/pimoroni.py +++ b/micropython/modules_py/pimoroni.py @@ -79,6 +79,12 @@ class AnalogMux: else: self.pulls[address] = pull + def read(self): + if self.muxed_pin is not None: + return self.muxed_pin.value() + else: + raise RuntimeError("there is no muxed pin assigned to this mux") + class Button: def __init__(self, button, invert=True, repeat_time=200, hold_time=1000): From 415a1e559abedbdb153f59606cba8b76cd70e127 Mon Sep 17 00:00:00 2001 From: ZodiusInfuser Date: Tue, 26 Apr 2022 23:15:13 +0100 Subject: [PATCH 37/60] Setup and partial implementations of C++ examples --- drivers/CMakeLists.txt | 1 + drivers/analogmux/analogmux.cpp | 7 + drivers/analogmux/analogmux.hpp | 1 + drivers/encoder/encoder.hpp | 2 + drivers/pid/CMakeLists.txt | 1 + drivers/pid/pid.cmake | 11 ++ drivers/pid/pid.cpp | 25 ++++ drivers/pid/pid.hpp | 27 ++++ examples/motor2040/CMakeLists.txt | 27 ++-- examples/motor2040/README.md | 135 ++++++++++++++++++ .../motor2040/motor2040_led_rainbow.cmake | 13 ++ examples/motor2040/motor2040_led_rainbow.cpp | 56 ++++++++ .../motor2040/motor2040_motor_cluster.cmake | 13 ++ .../motor2040/motor2040_motor_cluster.cpp | 79 ++++++++++ .../motor2040/motor2040_motor_profiler.cmake | 12 ++ .../motor2040/motor2040_motor_profiler.cpp | 73 ++++++++++ examples/motor2040/motor2040_motor_wave.cmake | 13 ++ examples/motor2040/motor2040_motor_wave.cpp | 82 +++++++++++ .../motor2040/motor2040_multiple_motors.cmake | 12 ++ .../motor2040/motor2040_multiple_motors.cpp | 100 +++++++++++++ .../motor2040_position_control.cmake | 14 ++ .../motor2040/motor2040_position_control.cpp | 75 ++++++++++ ...tor2040_position_on_velocity_control.cmake | 14 ++ ...motor2040_position_on_velocity_control.cpp | 75 ++++++++++ ...otor2040_position_on_velocity_tuning.cmake | 14 ++ .../motor2040_position_on_velocity_tuning.cpp | 77 ++++++++++ .../motor2040/motor2040_position_tuning.cmake | 14 ++ .../motor2040/motor2040_position_tuning.cpp | 131 +++++++++++++++++ .../motor2040_quad_position_wave.cmake | 14 ++ .../motor2040_quad_position_wave.cpp | 75 ++++++++++ .../motor2040_quad_velocity_sequence.cmake | 14 ++ .../motor2040_quad_velocity_sequence.cpp | 75 ++++++++++ .../motor2040_reactive_encoder.cmake | 12 ++ .../motor2040/motor2040_reactive_encoder.cpp | 76 ++++++++++ .../motor2040/motor2040_read_encoders.cmake | 13 ++ .../motor2040/motor2040_read_encoders.cpp | 60 ++++++++ .../motor2040/motor2040_read_sensors.cmake | 15 ++ examples/motor2040/motor2040_read_sensors.cpp | 68 +++++++++ examples/motor2040/motor2040_single_motor.cpp | 33 +++-- .../motor2040_velocity_control.cmake | 12 ++ .../motor2040/motor2040_velocity_control.cpp | 75 ++++++++++ .../motor2040/motor2040_velocity_tuning.cmake | 13 ++ .../motor2040/motor2040_velocity_tuning.cpp | 77 ++++++++++ libraries/motor2040/motor2040.cmake | 2 +- libraries/motor2040/motor2040.hpp | 1 + micropython/examples/motor2040/led_rainbow.py | 6 +- micropython/examples/motor2040/motor_wave.py | 4 +- .../examples/motor2040/multiple_motors.py | 2 +- .../examples/motor2040/position_tuning.py | 8 +- 49 files changed, 1724 insertions(+), 35 deletions(-) create mode 100644 drivers/pid/CMakeLists.txt create mode 100644 drivers/pid/pid.cmake create mode 100644 drivers/pid/pid.cpp create mode 100644 drivers/pid/pid.hpp create mode 100644 examples/motor2040/README.md create mode 100644 examples/motor2040/motor2040_led_rainbow.cmake create mode 100644 examples/motor2040/motor2040_led_rainbow.cpp create mode 100644 examples/motor2040/motor2040_motor_cluster.cmake create mode 100644 examples/motor2040/motor2040_motor_cluster.cpp create mode 100644 examples/motor2040/motor2040_motor_profiler.cmake create mode 100644 examples/motor2040/motor2040_motor_profiler.cpp create mode 100644 examples/motor2040/motor2040_motor_wave.cmake create mode 100644 examples/motor2040/motor2040_motor_wave.cpp create mode 100644 examples/motor2040/motor2040_multiple_motors.cmake create mode 100644 examples/motor2040/motor2040_multiple_motors.cpp create mode 100644 examples/motor2040/motor2040_position_control.cmake create mode 100644 examples/motor2040/motor2040_position_control.cpp create mode 100644 examples/motor2040/motor2040_position_on_velocity_control.cmake create mode 100644 examples/motor2040/motor2040_position_on_velocity_control.cpp create mode 100644 examples/motor2040/motor2040_position_on_velocity_tuning.cmake create mode 100644 examples/motor2040/motor2040_position_on_velocity_tuning.cpp create mode 100644 examples/motor2040/motor2040_position_tuning.cmake create mode 100644 examples/motor2040/motor2040_position_tuning.cpp create mode 100644 examples/motor2040/motor2040_quad_position_wave.cmake create mode 100644 examples/motor2040/motor2040_quad_position_wave.cpp create mode 100644 examples/motor2040/motor2040_quad_velocity_sequence.cmake create mode 100644 examples/motor2040/motor2040_quad_velocity_sequence.cpp create mode 100644 examples/motor2040/motor2040_reactive_encoder.cmake create mode 100644 examples/motor2040/motor2040_reactive_encoder.cpp create mode 100644 examples/motor2040/motor2040_read_encoders.cmake create mode 100644 examples/motor2040/motor2040_read_encoders.cpp create mode 100644 examples/motor2040/motor2040_read_sensors.cmake create mode 100644 examples/motor2040/motor2040_read_sensors.cpp create mode 100644 examples/motor2040/motor2040_velocity_control.cmake create mode 100644 examples/motor2040/motor2040_velocity_control.cpp create mode 100644 examples/motor2040/motor2040_velocity_tuning.cmake create mode 100644 examples/motor2040/motor2040_velocity_tuning.cpp diff --git a/drivers/CMakeLists.txt b/drivers/CMakeLists.txt index 9fc45193..c76e13fa 100644 --- a/drivers/CMakeLists.txt +++ b/drivers/CMakeLists.txt @@ -21,6 +21,7 @@ add_subdirectory(bme68x) add_subdirectory(bmp280) add_subdirectory(bme280) add_subdirectory(button) +add_subdirectory(pid) add_subdirectory(plasma) add_subdirectory(rgbled) add_subdirectory(icp10125) diff --git a/drivers/analogmux/analogmux.cpp b/drivers/analogmux/analogmux.cpp index c1e36094..3898df0a 100644 --- a/drivers/analogmux/analogmux.cpp +++ b/drivers/analogmux/analogmux.cpp @@ -77,4 +77,11 @@ namespace pimoroni { pull_downs &= ~(1u << address); } } + + bool AnalogMux::read() { + if(muxed_pin != PIN_UNUSED) { + return gpio_get(muxed_pin); + } + return false; + } } \ No newline at end of file diff --git a/drivers/analogmux/analogmux.hpp b/drivers/analogmux/analogmux.hpp index 63236848..66011e37 100644 --- a/drivers/analogmux/analogmux.hpp +++ b/drivers/analogmux/analogmux.hpp @@ -14,6 +14,7 @@ namespace pimoroni { void select(uint8_t address); void disable(); void configure_pulls(uint8_t address, bool pullup, bool pulldown); + bool read(); private: uint addr0_pin; diff --git a/drivers/encoder/encoder.hpp b/drivers/encoder/encoder.hpp index 45441ef3..e28e73b5 100644 --- a/drivers/encoder/encoder.hpp +++ b/drivers/encoder/encoder.hpp @@ -6,6 +6,8 @@ using namespace pimoroni; namespace encoder { + static constexpr float MMME_CPR = 12.0f; + static constexpr float ROTARY_CPR = 24.0f; class Encoder { //-------------------------------------------------- diff --git a/drivers/pid/CMakeLists.txt b/drivers/pid/CMakeLists.txt new file mode 100644 index 00000000..b8b8971d --- /dev/null +++ b/drivers/pid/CMakeLists.txt @@ -0,0 +1 @@ +include(pid.cmake) \ No newline at end of file diff --git a/drivers/pid/pid.cmake b/drivers/pid/pid.cmake new file mode 100644 index 00000000..3b9f6720 --- /dev/null +++ b/drivers/pid/pid.cmake @@ -0,0 +1,11 @@ +set(DRIVER_NAME pid) +add_library(${DRIVER_NAME} INTERFACE) + +target_sources(${DRIVER_NAME} INTERFACE + ${CMAKE_CURRENT_LIST_DIR}/${DRIVER_NAME}.cpp +) + +target_include_directories(${DRIVER_NAME} INTERFACE ${CMAKE_CURRENT_LIST_DIR}) + +# Pull in pico libraries that we need +target_link_libraries(${DRIVER_NAME} INTERFACE pico_stdlib) \ No newline at end of file diff --git a/drivers/pid/pid.cpp b/drivers/pid/pid.cpp new file mode 100644 index 00000000..9134e5d7 --- /dev/null +++ b/drivers/pid/pid.cpp @@ -0,0 +1,25 @@ +#include "pid.hpp" + +namespace pimoroni { + PID::PID(float kp, float ki, float kd, float sample_rate) + : kp(kp), ki(ki), kd(kd), setpoint(0.0f) + , error_sum(0.0f), last_value(0.0f), sample_rate(sample_rate) { + } + + float PID::calculate(float value) { + float error = setpoint - value; + error_sum += error * sample_rate; + float rate_error = (value - last_value) / sample_rate; + last_value = value; + + return (error * kp) + (error_sum * ki) - (rate_error * kd); + } + + float PID::calculate(float value, float value_change) { + float error = setpoint - value; + error_sum += error * sample_rate; + last_value = value; + + return (error * kp) + (error_sum * ki) - (value_change * kd); + } +} \ No newline at end of file diff --git a/drivers/pid/pid.hpp b/drivers/pid/pid.hpp new file mode 100644 index 00000000..7dd17c8a --- /dev/null +++ b/drivers/pid/pid.hpp @@ -0,0 +1,27 @@ +#pragma once + +//#include +#include "pico/stdlib.h" +//include "common/pimoroni_common.hpp" + +namespace pimoroni { + + class PID { + public: + PID(float kp, float ki, float kd, float sample_rate); + + float calculate(float value); + float calculate(float value, float value_change); + + public: + float kp; + float ki; + float kd; + float setpoint; + private: + float error_sum; + float last_value; + float sample_rate; + }; + +} \ No newline at end of file diff --git a/examples/motor2040/CMakeLists.txt b/examples/motor2040/CMakeLists.txt index d2009f34..30694b51 100644 --- a/examples/motor2040/CMakeLists.txt +++ b/examples/motor2040/CMakeLists.txt @@ -1,10 +1,17 @@ -#include(servo2040_calibration.cmake) -#include(servo2040_current_meter.cmake) -#include(servo2040_led_rainbow.cmake) -#include(servo2040_multiple_servos.cmake) -#include(servo2040_read_sensors.cmake) -#include(servo2040_sensor_feedback.cmake) -#include(servo2040_servo_cluster.cmake) -#include(servo2040_servo_wave.cmake) -#include(servo2040_simple_easing.cmake) -include(motor2040_single_motor.cmake) \ No newline at end of file +include(motor2040_led_rainbow.cmake) +include(motor2040_motor_cluster.cmake) +include(motor2040_motor_profiler.cmake) +include(motor2040_motor_wave.cmake) +include(motor2040_multiple_motors.cmake) +include(motor2040_position_control.cmake) +include(motor2040_position_tuning.cmake) +include(motor2040_position_on_velocity_control.cmake) +include(motor2040_position_on_velocity_tuning.cmake) +include(motor2040_quad_position_wave.cmake) +include(motor2040_quad_velocity_sequence.cmake) +include(motor2040_reactive_encoder.cmake) +include(motor2040_read_encoders.cmake) +include(motor2040_read_sensors.cmake) +include(motor2040_single_motor.cmake) +include(motor2040_velocity_control.cmake) +include(motor2040_velocity_tuning.cmake) \ No newline at end of file diff --git a/examples/motor2040/README.md b/examples/motor2040/README.md new file mode 100644 index 00000000..ad152b7b --- /dev/null +++ b/examples/motor2040/README.md @@ -0,0 +1,135 @@ +# Motor 2040 C++ Examples + +- [Motor Examples](#motor-examples) + - [Single Motor](#single-motor) + - [Multiple Motors](#multiple-motors) + - [Motor Cluster](#motor-cluster) + - [Motor Wave](#motor-wave) + - [Stop Motors](#stop-motors) +- [Function Examples](#function-examples) + - [Read Sensors](#read-sensors) + - [Read Encoders](#read-encoders) + - [Motor Profiler](#motor-profiler) + - [LED Rainbow](#led-rainbow) + - [Turn Off LEDs](#turn-off-leds) +- [Control Examples](#control-examples) + - [Position Control](#position-control) + - [Velocity Control](#velocity-control) + - [Position on Velocity Control](#position-on-velocity-control) + - [Reactive Encoder](#reactive-encoder) + - [Quad Position Wave](#quad-position-wave) + - [Quad Velocity Sequence](#quad-velocity-sequence) +- [Tuning Examples](#tuning-examples) + - [Position Tuning](#position-tuning) + - [Velocity Tuning](#velocity-tuning) + - [Position on Velocity Tuning](#position-on-velocity-tuning) + + +## Motor Examples + +### Single Motor +[motor2040_single_motor.py](motor2040_single_motor.py) + +Demonstrates how to create a Motor object and control it. + + +### Multiple Motors +[motor2040_multiple_motors.py](motor2040_multiple_motors.py) + +Demonstrates how to create multiple Motor objects and control them together. + + +### Motor Cluster +[motor2040_motor_cluster.py](motor2040_motor_cluster.py) + +Demonstrates how to create a MotorCluster object to control multiple motors at once. + + +### Motor Wave +[motor2040_motor_wave.py](motor2040_motor_wave.py) + +An example of applying a wave pattern to a group of motors and the LEDs. + + +## Function Examples + +### Read Sensors +[motor2040_read_sensors.py](motor2040_read_sensors.py) + +Shows how to initialise and read the 2 external and 6 internal sensors of Motor 2040. + + +### Read Encoders +[motor2040_read_encoders.py](motor2040_read_encoders.py) + +Demonstrates how to read the angles of Motor 2040's four encoders. + + +### Motor Profiler +[motor2040_motor_profiler.py](motor2040_motor_profiler.py) + +A program that profiles the speed of a motor across its PWM +duty cycle range using the attached encoder for feedback. + + +### LED Rainbow +[motor2040_led_rainbow.py](motor2040_led_rainbow.py) + +Displays a rotating rainbow pattern on the Motor 2040's onboard LED. + + +## Control Examples + +### Position Control +[motor2040_position_control.py](motor2040_position_control.py) + +An example of how to move a motor smoothly between random positions, with the help of it's attached encoder and PID control. + + +### Velocity Control +[motor2040_velocity_control.py](motor2040_velocity_control.py) + +An example of how to drive a motor smoothly between random speeds, with the help of it's attached encoder and PID control. + + +### Position on Velocity Control +[motor2040_position_on_velocity_control.py](motor2040_position_on_velocity_control.py) + +An example of how to move a motor smoothly between random positions, with velocity limits, with the help of it's attached encoder and PID control. + + +### Reactive Encoder +[motor2040_reactive_encoder.py](motor2040_reactive_encoder.py) + +A demonstration of how a motor with an encoder can be used as a programmable rotary encoder for user input, with force-feedback for arbitrary detents and end stops. + + +### Quad Position Wave +[motor2040_quad_position_wave.py](motor2040_quad_position_wave.py) + +A demonstration of driving all four of Motor 2040's motor outputs between positions, with the help of their attached encoders and PID control. + +### Quad Velocity Sequence +[motor2040_quad_velocity_sequence.py](motor2040_quad_velocity_sequence.py) + +A demonstration of driving all four of Motor 2040's motor outputs through a sequence of velocities, with the help of their attached encoders and PID control. + + +## Tuning Examples + +### Position Tuning +[motor2040_position_tuning.py](motor2040_position_tuning.py) + +A program to aid in the discovery and tuning of motor PID values for position control. It does this by commanding the motor to move repeatedly between two setpoint angles and plots the measured response. + + +### Velocity Tuning +[motor2040_velocity_tuning.py](motor2040_velocity_tuning.py) + +A program to aid in the discovery and tuning of motor PID values for velocity control. It does this by commanding the motor to drive repeatedly between two setpoint speeds and plots the measured response. + + +### Position on Velocity Tuning +[motor2040_position_on_velocity_tuning.py](motor2040_position_on_velocity_tuning.py) + +A program to aid in the discovery and tuning of motor PID values for position on velocity control. It does this by commanding the motor to move repeatedly between two setpoint angles and plots the measured response. diff --git a/examples/motor2040/motor2040_led_rainbow.cmake b/examples/motor2040/motor2040_led_rainbow.cmake new file mode 100644 index 00000000..938d0216 --- /dev/null +++ b/examples/motor2040/motor2040_led_rainbow.cmake @@ -0,0 +1,13 @@ +set(OUTPUT_NAME motor2040_led_rainbow) +add_executable(${OUTPUT_NAME} motor2040_led_rainbow.cpp) + +target_link_libraries(${OUTPUT_NAME} + pico_stdlib + motor2040 + button + ) + +# enable usb output +pico_enable_stdio_usb(${OUTPUT_NAME} 1) + +pico_add_extra_outputs(${OUTPUT_NAME}) diff --git a/examples/motor2040/motor2040_led_rainbow.cpp b/examples/motor2040/motor2040_led_rainbow.cpp new file mode 100644 index 00000000..b46e2344 --- /dev/null +++ b/examples/motor2040/motor2040_led_rainbow.cpp @@ -0,0 +1,56 @@ +#include "pico/stdlib.h" + +#include "motor2040.hpp" +#include "button.hpp" + +/* +Displays a rotating rainbow pattern on the Motor 2040's onboard LED. + +Press "Boot" to exit the program. +*/ + +using namespace plasma; +using namespace motor; + +// The speed that the LED will cycle at +const uint SPEED = 5; + +// The brightness of the LED +constexpr float BRIGHTNESS = 0.4f; + +// How many times the LED will be updated per second +const uint UPDATES = 50; + + +// Create the LED, using PIO 1 and State Machine 0 +WS2812 led(motor2040::NUM_LEDS, pio1, 0, motor2040::LED_DATA); + +// Create the user button +Button user_sw(motor2040::USER_SW); + + +int main() { + stdio_init_all(); + + // Start updating the LED + led.start(); + + float hue = 0.0f; + + // Make rainbows until the user button is pressed + while(!user_sw.raw()) { + + hue += (float)SPEED / 1000.0f; + + // Update the LED + led.set_hsv(0, hue, 1.0f, BRIGHTNESS); + + sleep_ms(1000 / UPDATES); + } + + // Turn off the LED + led.clear(); + + // Sleep a short time so the clear takes effect + sleep_ms(100); +} diff --git a/examples/motor2040/motor2040_motor_cluster.cmake b/examples/motor2040/motor2040_motor_cluster.cmake new file mode 100644 index 00000000..efe9b5f6 --- /dev/null +++ b/examples/motor2040/motor2040_motor_cluster.cmake @@ -0,0 +1,13 @@ +set(OUTPUT_NAME motor2040_motor_cluster) +add_executable(${OUTPUT_NAME} motor2040_motor_cluster.cpp) + +target_link_libraries(${OUTPUT_NAME} + pico_stdlib + motor2040 + button + ) + +# enable usb output +pico_enable_stdio_usb(${OUTPUT_NAME} 1) + +pico_add_extra_outputs(${OUTPUT_NAME}) diff --git a/examples/motor2040/motor2040_motor_cluster.cpp b/examples/motor2040/motor2040_motor_cluster.cpp new file mode 100644 index 00000000..804b77ca --- /dev/null +++ b/examples/motor2040/motor2040_motor_cluster.cpp @@ -0,0 +1,79 @@ +#include "pico/stdlib.h" + +#include "motor2040.hpp" + +/* +Demonstrates how to create multiple Motor objects and control them together. +*/ + +using namespace motor; + +// How many sweeps of the motors to perform +const uint SWEEPS = 2; + +// The number of discrete sweep steps +const uint STEPS = 10; + +// The time in milliseconds between each step of the sequence +const uint STEPS_INTERVAL_MS = 500; + +// How far from zero to drive the motors when sweeping +constexpr float SPEED_EXTENT = 1.0f; + +// Create a motor cluster, using PIO 0 and State Machine 0 +const pin_pair motor_pins[] = {motor2040::MOTOR_A, motor2040::MOTOR_B, + motor2040::MOTOR_C, motor2040::MOTOR_D}; +const uint NUM_MOTORS = count_of(motor_pins); +MotorCluster motors = MotorCluster(pio0, 0, motor_pins, NUM_MOTORS); + + +int main() { + stdio_init_all(); + + // Initialise the motor cluster + motors.init(); + + // Enable all motors + motors.enable_all(); + sleep_ms(2000); + + // Drive at full positive + motors.all_full_positive(); + sleep_ms(2000); + + // Stop all moving + motors.stop_all(); + sleep_ms(2000); + + // Drive at full negative + motors.all_full_negative(); + sleep_ms(2000); + + // Coast all to a gradual stop + motors.coast_all(); + sleep_ms(2000); + + // Do a sine speed sweep + for(auto j = 0u; j < SWEEPS; j++) { + for(auto i = 0u; i < 360; i++) { + float speed = sin(((float)i * (float)M_PI) / 180.0f) * SPEED_EXTENT; + motors.all_to_speed(speed); + sleep_ms(20); + } + } + + // Do a stepped speed sweep + for(auto j = 0u; j < SWEEPS; j++) { + for(auto i = 0u; i < STEPS; i++) { + motors.all_to_percent(i, 0, STEPS, 0.0 - SPEED_EXTENT, SPEED_EXTENT); + sleep_ms(STEPS_INTERVAL_MS); + } + for(auto i = 0u; i < STEPS; i++) { + motors.all_to_percent(i, STEPS, 0, 0.0 - SPEED_EXTENT, SPEED_EXTENT); + sleep_ms(STEPS_INTERVAL_MS); + } + } + + // Disable the motors + motors.disable_all(); +} diff --git a/examples/motor2040/motor2040_motor_profiler.cmake b/examples/motor2040/motor2040_motor_profiler.cmake new file mode 100644 index 00000000..39c19738 --- /dev/null +++ b/examples/motor2040/motor2040_motor_profiler.cmake @@ -0,0 +1,12 @@ +set(OUTPUT_NAME motor2040_motor_profiler) +add_executable(${OUTPUT_NAME} motor2040_motor_profiler.cpp) + +target_link_libraries(${OUTPUT_NAME} + pico_stdlib + motor2040 + ) + +# enable usb output +pico_enable_stdio_usb(${OUTPUT_NAME} 1) + +pico_add_extra_outputs(${OUTPUT_NAME}) diff --git a/examples/motor2040/motor2040_motor_profiler.cpp b/examples/motor2040/motor2040_motor_profiler.cpp new file mode 100644 index 00000000..a257c6f4 --- /dev/null +++ b/examples/motor2040/motor2040_motor_profiler.cpp @@ -0,0 +1,73 @@ +#include "pico/stdlib.h" + +#include "motor2040.hpp" + +/* +A program that profiles the speed of a motor across its PWM +duty cycle range using the attached encoder for feedback. +*/ + +using namespace motor; + +// How many sweeps of the motor to perform +const uint SWEEPS = 3; + +// The number of discrete sweep steps +const uint STEPS = 10; + +// The time in milliseconds between each step of the sequence +const uint STEPS_INTERVAL_MS = 500; + +// How far from zero to move the motor when sweeping +constexpr float SWEEP_EXTENT = 90.0f; + + +// Create a motor on pin 0 and 1 +Motor m = Motor(motor2040::MOTOR_A); + + +int main() { + stdio_init_all(); + + // Initialise the motor + m.init(); + + // Enable the motor + m.enable(); + sleep_ms(2000); + + // Go at full neative + m.full_negative(); + sleep_ms(2000); + + // Go at full positive + m.full_positive(); + sleep_ms(2000); + + // Stop the motor + m.stop(); + sleep_ms(2000); + + // Do a sine sweep + for(auto j = 0u; j < SWEEPS; j++) { + for(auto i = 0u; i < 360; i++) { + m.speed(sin(((float)i * (float)M_PI) / 180.0f) * SWEEP_EXTENT); + sleep_ms(20); + } + } + + // Do a stepped sweep + for(auto j = 0u; j < SWEEPS; j++) { + for(auto i = 0u; i < STEPS; i++) { + m.to_percent(i, 0, STEPS, 0.0 - SWEEP_EXTENT, SWEEP_EXTENT); + sleep_ms(STEPS_INTERVAL_MS); + } + for(auto i = 0u; i < STEPS; i++) { + m.to_percent(i, STEPS, 0, 0.0 - SWEEP_EXTENT, SWEEP_EXTENT); + sleep_ms(STEPS_INTERVAL_MS); + } + } + + // Disable the motor + m.disable(); +} diff --git a/examples/motor2040/motor2040_motor_wave.cmake b/examples/motor2040/motor2040_motor_wave.cmake new file mode 100644 index 00000000..68719ef2 --- /dev/null +++ b/examples/motor2040/motor2040_motor_wave.cmake @@ -0,0 +1,13 @@ +set(OUTPUT_NAME motor2040_motor_wave) +add_executable(${OUTPUT_NAME} motor2040_motor_wave.cpp) + +target_link_libraries(${OUTPUT_NAME} + pico_stdlib + motor2040 + button + ) + +# enable usb output +pico_enable_stdio_usb(${OUTPUT_NAME} 1) + +pico_add_extra_outputs(${OUTPUT_NAME}) diff --git a/examples/motor2040/motor2040_motor_wave.cpp b/examples/motor2040/motor2040_motor_wave.cpp new file mode 100644 index 00000000..a66a5713 --- /dev/null +++ b/examples/motor2040/motor2040_motor_wave.cpp @@ -0,0 +1,82 @@ +#include "pico/stdlib.h" + +#include "motor2040.hpp" +#include "button.hpp" + +/* +An example of applying a wave pattern to a group of motors and the LED. + +Press "Boot" to exit the program. +*/ + +using namespace plasma; +using namespace motor; + +// The speed that the LEDs and motors will cycle at +const uint SPEED = 5; + +// The brightness of the LEDs +constexpr float BRIGHTNESS = 0.4; + +// How many times to update LEDs and motors per second +const uint UPDATES = 50; + +// How far from zero to drive the motors +constexpr float SPEED_EXTENT = 1.0f; + + +// Create an array of motor pointers +const pin_pair motor_pins[] = {motor2040::MOTOR_A, motor2040::MOTOR_B, + motor2040::MOTOR_C, motor2040::MOTOR_D}; +const uint NUM_MOTORS = count_of(motor_pins); +Motor *motors[NUM_MOTORS]; + +// Create the LED, using PIO 1 and State Machine 0 +WS2812 led(motor2040::NUM_LEDS, pio1, 0, motor2040::LED_DATA); + +// Create the user button +Button user_sw(motor2040::USER_SW); + + +int main() { + stdio_init_all(); + + // Fill the array of motors, and initialise them. Up to 8 motors can be created + for(auto m = 0u; m < NUM_MOTORS; m++) { + motors[m] = new Motor(motor_pins[m]); + motors[m]->init(); + } + + // Start updating the LED + led.start(); + + float offset = 0.0f; + + // Make rainbows until the user button is pressed + while(!user_sw.raw()) { + + offset += (float)SPEED / 1000.0f; + + // Update the LED + led.set_hsv(0, offset / 2.0f, 1.0f, BRIGHTNESS); + + // Update all the motors + for(auto m = 0u; m < NUM_MOTORS; m++) { + float angle = (((float)m / (float)NUM_MOTORS) + offset) * (float)M_TWOPI; + motors[m]->speed(sin(angle) * SPEED_EXTENT); + } + + sleep_ms(1000 / UPDATES); + } + + // Stop all the motors + for(auto m = 0u; m < NUM_MOTORS; m++) { + motors[m]->disable(); + } + + // Turn off the LED + led.clear(); + + // Sleep a short time so the clear takes effect + sleep_ms(100); +} diff --git a/examples/motor2040/motor2040_multiple_motors.cmake b/examples/motor2040/motor2040_multiple_motors.cmake new file mode 100644 index 00000000..43efe984 --- /dev/null +++ b/examples/motor2040/motor2040_multiple_motors.cmake @@ -0,0 +1,12 @@ +set(OUTPUT_NAME motor2040_multiple_motors) +add_executable(${OUTPUT_NAME} motor2040_multiple_motors.cpp) + +target_link_libraries(${OUTPUT_NAME} + pico_stdlib + motor2040 + ) + +# enable usb output +pico_enable_stdio_usb(${OUTPUT_NAME} 1) + +pico_add_extra_outputs(${OUTPUT_NAME}) diff --git a/examples/motor2040/motor2040_multiple_motors.cpp b/examples/motor2040/motor2040_multiple_motors.cpp new file mode 100644 index 00000000..ec68548f --- /dev/null +++ b/examples/motor2040/motor2040_multiple_motors.cpp @@ -0,0 +1,100 @@ +#include "pico/stdlib.h" + +#include "motor2040.hpp" + +/* +Demonstrates how to create multiple Motor objects and control them together. +*/ + +using namespace motor; + +// How many sweeps of the motors to perform +const uint SWEEPS = 2; + +// The number of discrete sweep steps +const uint STEPS = 10; + +// The time in milliseconds between each step of the sequence +const uint STEPS_INTERVAL_MS = 500; + +// How far from zero to drive the motors when sweeping +constexpr float SPEED_EXTENT = 1.0f; + +// Create an array of motor pointers +const pin_pair motor_pins[] = {motor2040::MOTOR_A, motor2040::MOTOR_B, + motor2040::MOTOR_C, motor2040::MOTOR_D}; +const uint NUM_MOTORS = count_of(motor_pins); +Motor *motors[NUM_MOTORS]; + + +int main() { + stdio_init_all(); + + // Fill the array of motors, and initialise them. Up to 8 motors can be created + for(auto m = 0u; m < NUM_MOTORS; m++) { + motors[m] = new Motor(motor_pins[m]); + motors[m]->init(); + } + + // Enable all motors + for(auto m = 0u; m < NUM_MOTORS; m++) { + motors[m]->enable(); + } + sleep_ms(2000); + + // Drive at full positive + for(auto m = 0u; m < NUM_MOTORS; m++) { + motors[m]->full_positive(); + } + sleep_ms(2000); + + // Stop all moving + for(auto m = 0u; m < NUM_MOTORS; m++) { + motors[m]->stop(); + } + sleep_ms(2000); + + // Drive at full negative + for(auto m = 0u; m < NUM_MOTORS; m++) { + motors[m]->full_negative(); + } + sleep_ms(2000); + + // Coast all to a gradual stop + for(auto m = 0u; m < NUM_MOTORS; m++) { + motors[m]->coast(); + } + sleep_ms(2000); + + // Do a sine speed sweep + for(auto j = 0u; j < SWEEPS; j++) { + for(auto i = 0u; i < 360; i++) { + float speed = sin(((float)i * (float)M_PI) / 180.0f) * SPEED_EXTENT; + for(auto m = 0u; m < NUM_MOTORS; m++) { + motors[m]->speed(speed); + } + sleep_ms(20); + } + } + + // Do a stepped speed sweep + for(auto j = 0u; j < SWEEPS; j++) { + for(auto i = 0u; i < STEPS; i++) { + for(auto m = 0u; m < NUM_MOTORS; m++) { + motors[m]->to_percent(i, 0, STEPS, 0.0 - SPEED_EXTENT, SPEED_EXTENT); + } + sleep_ms(STEPS_INTERVAL_MS); + } + for(auto i = 0u; i < STEPS; i++) { + for(auto m = 0u; m < NUM_MOTORS; m++) { + motors[m]->to_percent(i, STEPS, 0, 0.0 - SPEED_EXTENT, SPEED_EXTENT); + } + sleep_ms(STEPS_INTERVAL_MS); + } + } + + // Disable the motors + for(auto m = 0u; m < NUM_MOTORS; m++) { + motors[m]->disable(); + } +} diff --git a/examples/motor2040/motor2040_position_control.cmake b/examples/motor2040/motor2040_position_control.cmake new file mode 100644 index 00000000..48a2d567 --- /dev/null +++ b/examples/motor2040/motor2040_position_control.cmake @@ -0,0 +1,14 @@ +set(OUTPUT_NAME motor2040_position_control) +add_executable(${OUTPUT_NAME} motor2040_position_control.cpp) + +target_link_libraries(${OUTPUT_NAME} + pico_stdlib + motor2040 + button + pid + ) + +# enable usb output +pico_enable_stdio_usb(${OUTPUT_NAME} 1) + +pico_add_extra_outputs(${OUTPUT_NAME}) diff --git a/examples/motor2040/motor2040_position_control.cpp b/examples/motor2040/motor2040_position_control.cpp new file mode 100644 index 00000000..ee6f4133 --- /dev/null +++ b/examples/motor2040/motor2040_position_control.cpp @@ -0,0 +1,75 @@ +#include "pico/stdlib.h" + +#include "motor2040.hpp" + +/* +An example of how to move a motor smoothly between random positions, +with the help of it's attached encoder and PID control. + +Press "Boot" to exit the program. +*/ + +using namespace motor; + +// How many sweeps of the motor to perform +const uint SWEEPS = 3; + +// The number of discrete sweep steps +const uint STEPS = 10; + +// The time in milliseconds between each step of the sequence +const uint STEPS_INTERVAL_MS = 500; + +// How far from zero to move the motor when sweeping +constexpr float SWEEP_EXTENT = 90.0f; + + +// Create a motor on pin 0 and 1 +Motor m = Motor(motor2040::MOTOR_A); + + +int main() { + stdio_init_all(); + + // Initialise the motor + m.init(); + + // Enable the motor + m.enable(); + sleep_ms(2000); + + // Go at full neative + m.full_negative(); + sleep_ms(2000); + + // Go at full positive + m.full_positive(); + sleep_ms(2000); + + // Stop the motor + m.stop(); + sleep_ms(2000); + + // Do a sine sweep + for(auto j = 0u; j < SWEEPS; j++) { + for(auto i = 0u; i < 360; i++) { + m.speed(sin(((float)i * (float)M_PI) / 180.0f) * SWEEP_EXTENT); + sleep_ms(20); + } + } + + // Do a stepped sweep + for(auto j = 0u; j < SWEEPS; j++) { + for(auto i = 0u; i < STEPS; i++) { + m.to_percent(i, 0, STEPS, 0.0 - SWEEP_EXTENT, SWEEP_EXTENT); + sleep_ms(STEPS_INTERVAL_MS); + } + for(auto i = 0u; i < STEPS; i++) { + m.to_percent(i, STEPS, 0, 0.0 - SWEEP_EXTENT, SWEEP_EXTENT); + sleep_ms(STEPS_INTERVAL_MS); + } + } + + // Disable the motor + m.disable(); +} diff --git a/examples/motor2040/motor2040_position_on_velocity_control.cmake b/examples/motor2040/motor2040_position_on_velocity_control.cmake new file mode 100644 index 00000000..f3a54587 --- /dev/null +++ b/examples/motor2040/motor2040_position_on_velocity_control.cmake @@ -0,0 +1,14 @@ +set(OUTPUT_NAME motor2040_position_on_velocity_control) +add_executable(${OUTPUT_NAME} motor2040_position_on_velocity_control.cpp) + +target_link_libraries(${OUTPUT_NAME} + pico_stdlib + motor2040 + button + pid + ) + +# enable usb output +pico_enable_stdio_usb(${OUTPUT_NAME} 1) + +pico_add_extra_outputs(${OUTPUT_NAME}) diff --git a/examples/motor2040/motor2040_position_on_velocity_control.cpp b/examples/motor2040/motor2040_position_on_velocity_control.cpp new file mode 100644 index 00000000..3463e37c --- /dev/null +++ b/examples/motor2040/motor2040_position_on_velocity_control.cpp @@ -0,0 +1,75 @@ +#include "pico/stdlib.h" + +#include "motor2040.hpp" + +/* +An example of how to move a motor smoothly between random positions, +with velocity limits, with the help of it's attached encoder and PID control. + +Press "Boot" to exit the program. +*/ + +using namespace motor; + +// How many sweeps of the motor to perform +const uint SWEEPS = 3; + +// The number of discrete sweep steps +const uint STEPS = 10; + +// The time in milliseconds between each step of the sequence +const uint STEPS_INTERVAL_MS = 500; + +// How far from zero to move the motor when sweeping +constexpr float SWEEP_EXTENT = 90.0f; + + +// Create a motor on pin 0 and 1 +Motor m = Motor(motor2040::MOTOR_A); + + +int main() { + stdio_init_all(); + + // Initialise the motor + m.init(); + + // Enable the motor + m.enable(); + sleep_ms(2000); + + // Go at full neative + m.full_negative(); + sleep_ms(2000); + + // Go at full positive + m.full_positive(); + sleep_ms(2000); + + // Stop the motor + m.stop(); + sleep_ms(2000); + + // Do a sine sweep + for(auto j = 0u; j < SWEEPS; j++) { + for(auto i = 0u; i < 360; i++) { + m.speed(sin(((float)i * (float)M_PI) / 180.0f) * SWEEP_EXTENT); + sleep_ms(20); + } + } + + // Do a stepped sweep + for(auto j = 0u; j < SWEEPS; j++) { + for(auto i = 0u; i < STEPS; i++) { + m.to_percent(i, 0, STEPS, 0.0 - SWEEP_EXTENT, SWEEP_EXTENT); + sleep_ms(STEPS_INTERVAL_MS); + } + for(auto i = 0u; i < STEPS; i++) { + m.to_percent(i, STEPS, 0, 0.0 - SWEEP_EXTENT, SWEEP_EXTENT); + sleep_ms(STEPS_INTERVAL_MS); + } + } + + // Disable the motor + m.disable(); +} diff --git a/examples/motor2040/motor2040_position_on_velocity_tuning.cmake b/examples/motor2040/motor2040_position_on_velocity_tuning.cmake new file mode 100644 index 00000000..43f948e2 --- /dev/null +++ b/examples/motor2040/motor2040_position_on_velocity_tuning.cmake @@ -0,0 +1,14 @@ +set(OUTPUT_NAME motor2040_position_on_velocity_tuning) +add_executable(${OUTPUT_NAME} motor2040_position_on_velocity_tuning.cpp) + +target_link_libraries(${OUTPUT_NAME} + pico_stdlib + motor2040 + button + pid + ) + +# enable usb output +pico_enable_stdio_usb(${OUTPUT_NAME} 1) + +pico_add_extra_outputs(${OUTPUT_NAME}) diff --git a/examples/motor2040/motor2040_position_on_velocity_tuning.cpp b/examples/motor2040/motor2040_position_on_velocity_tuning.cpp new file mode 100644 index 00000000..51847ea4 --- /dev/null +++ b/examples/motor2040/motor2040_position_on_velocity_tuning.cpp @@ -0,0 +1,77 @@ +#include "pico/stdlib.h" + +#include "motor2040.hpp" + +/* +A program to aid in the discovery and tuning of motor PID +values for position on velocity control. It does this by +commanding the motor to move repeatedly between two setpoint +angles and plots the measured response. + +Press "Boot" to exit the program. +*/ + +using namespace motor; + +// How many sweeps of the motor to perform +const uint SWEEPS = 3; + +// The number of discrete sweep steps +const uint STEPS = 10; + +// The time in milliseconds between each step of the sequence +const uint STEPS_INTERVAL_MS = 500; + +// How far from zero to move the motor when sweeping +constexpr float SWEEP_EXTENT = 90.0f; + + +// Create a motor on pin 0 and 1 +Motor m = Motor(motor2040::MOTOR_A); + + +int main() { + stdio_init_all(); + + // Initialise the motor + m.init(); + + // Enable the motor + m.enable(); + sleep_ms(2000); + + // Go at full neative + m.full_negative(); + sleep_ms(2000); + + // Go at full positive + m.full_positive(); + sleep_ms(2000); + + // Stop the motor + m.stop(); + sleep_ms(2000); + + // Do a sine sweep + for(auto j = 0u; j < SWEEPS; j++) { + for(auto i = 0u; i < 360; i++) { + m.speed(sin(((float)i * (float)M_PI) / 180.0f) * SWEEP_EXTENT); + sleep_ms(20); + } + } + + // Do a stepped sweep + for(auto j = 0u; j < SWEEPS; j++) { + for(auto i = 0u; i < STEPS; i++) { + m.to_percent(i, 0, STEPS, 0.0 - SWEEP_EXTENT, SWEEP_EXTENT); + sleep_ms(STEPS_INTERVAL_MS); + } + for(auto i = 0u; i < STEPS; i++) { + m.to_percent(i, STEPS, 0, 0.0 - SWEEP_EXTENT, SWEEP_EXTENT); + sleep_ms(STEPS_INTERVAL_MS); + } + } + + // Disable the motor + m.disable(); +} diff --git a/examples/motor2040/motor2040_position_tuning.cmake b/examples/motor2040/motor2040_position_tuning.cmake new file mode 100644 index 00000000..9b8feaea --- /dev/null +++ b/examples/motor2040/motor2040_position_tuning.cmake @@ -0,0 +1,14 @@ +set(OUTPUT_NAME motor2040_position_tuning) +add_executable(${OUTPUT_NAME} motor2040_position_tuning.cpp) + +target_link_libraries(${OUTPUT_NAME} + pico_stdlib + motor2040 + button + pid + ) + +# enable usb output +pico_enable_stdio_usb(${OUTPUT_NAME} 1) + +pico_add_extra_outputs(${OUTPUT_NAME}) diff --git a/examples/motor2040/motor2040_position_tuning.cpp b/examples/motor2040/motor2040_position_tuning.cpp new file mode 100644 index 00000000..2c20beea --- /dev/null +++ b/examples/motor2040/motor2040_position_tuning.cpp @@ -0,0 +1,131 @@ +#include +#include "pico/stdlib.h" + +#include "motor2040.hpp" +#include "button.hpp" +#include "pid.hpp" + +/* +A program to aid in the discovery and tuning of motor PID +values for position control. It does this by commanding the +motor to move repeatedly between two setpoint angles and +plots the measured response. + +Press "Boot" to exit the program. +*/ + +using namespace motor; +using namespace encoder; + +// The pins of the motor being profiled +const pin_pair MOTOR_PINS = motor2040::MOTOR_A; + +// The pins of the encoder attached to the profiled motor +const pin_pair ENCODER_PINS = motor2040::ENCODER_A; + +// The gear ratio of the motor +constexpr float GEAR_RATIO = 50.0f; + +// The counts per revolution of the motor's output shaft +constexpr float COUNTS_PER_REV = MMME_CPR * GEAR_RATIO; + +// The direction to spin the motor in. NORMAL_DIR (0), REVERSED_DIR (1) +const Direction DIRECTION = NORMAL_DIR; + +// The scaling to apply to the motor's speed to match its real-world speed +float SPEED_SCALE = 5.4f; + +// How many times to update the motor per second +const uint UPDATES = 100; +constexpr float UPDATE_RATE = 1.0f / (float)UPDATES; + +// The time (in seconds) after a new setpoint, to display print out motor values +constexpr float PRINT_WINDOW = 0.25f; + +// The time (in seconds) between each new setpoint being set +constexpr float MOVEMENT_WINDOW = 2.0f; + +// How many of the updates should be printed (i.e. 2 would be every other update) +const uint PRINT_DIVIDER = 1; + +// Multipliers for the different printed values, so they appear nicely on the Thonny plotter +constexpr float SPD_PRINT_SCALE = 10.0f; // Driving Speed multipler + +// How far from zero to move the motor, in degrees +constexpr float POSITION_EXTENT = 90.0f; + +// PID values +constexpr float POS_KP = 0.14; // Position proportional (P) gain +constexpr float POS_KI = 0.0; // Position integral (I) gain +constexpr float POS_KD = 0.0022; // Position derivative (D) gain + + +// Create a motor and set its direction and speed scale +Motor m = Motor(MOTOR_PINS, DIRECTION, SPEED_SCALE); + +// Create an encoder and set its direction and counts per rev, using PIO 0 and State Machine 0 +Encoder enc = Encoder(pio0, 0, ENCODER_PINS, PIN_UNUSED, DIRECTION, COUNTS_PER_REV, true); + +// Create the user button +Button user_sw(motor2040::USER_SW); + +// Create PID object for position control +PID pos_pid = PID(POS_KP, POS_KI, POS_KD, UPDATE_RATE); + + +int main() { + stdio_init_all(); + + // Initialise the motor and encoder + m.init(); + enc.init(); + + // Enable the motor + m.enable(); + + // Set the initial setpoint position + pos_pid.setpoint = POSITION_EXTENT; + + + uint update = 0; + uint print_count = 0; + + // Continually move the motor until the user button is pressed + while(!user_sw.raw()) { + + // Capture the state of the encoder + Encoder::Capture capture = enc.capture(); + + // Calculate the velocity to move the motor closer to the position setpoint + float vel = pos_pid.calculate(capture.degrees(), capture.degrees_per_second()); + + // Set the new motor driving speed + m.speed(vel); + + // Print out the current motor values and their setpoints, + // but only for the first few updates and only every multiple + if(update < (PRINT_WINDOW * UPDATES) && print_count == 0) { + printf("Pos = %f, ", capture.degrees()); + printf("Pos SP = %f, ", pos_pid.setpoint); + printf("Speed = %f\n", m.speed() * SPD_PRINT_SCALE); + } + + // Increment the print count, and wrap it + print_count = (print_count + 1) % PRINT_DIVIDER; + + update++; // Move along in time + + // Have we reached the end of this time window? + if(update >= (MOVEMENT_WINDOW * UPDATES)){ + update = 0; // Reset the counter + + // Set the new position setpoint to be the inverse of the current setpoint + pos_pid.setpoint = 0.0 - pos_pid.setpoint; + } + + sleep_ms(UPDATE_RATE * 1000.0f); + } + + // Disable the motor + m.disable(); +} diff --git a/examples/motor2040/motor2040_quad_position_wave.cmake b/examples/motor2040/motor2040_quad_position_wave.cmake new file mode 100644 index 00000000..84f7a15b --- /dev/null +++ b/examples/motor2040/motor2040_quad_position_wave.cmake @@ -0,0 +1,14 @@ +set(OUTPUT_NAME motor2040_quad_position_wave) +add_executable(${OUTPUT_NAME} motor2040_quad_position_wave.cpp) + +target_link_libraries(${OUTPUT_NAME} + pico_stdlib + motor2040 + button + pid + ) + +# enable usb output +pico_enable_stdio_usb(${OUTPUT_NAME} 1) + +pico_add_extra_outputs(${OUTPUT_NAME}) diff --git a/examples/motor2040/motor2040_quad_position_wave.cpp b/examples/motor2040/motor2040_quad_position_wave.cpp new file mode 100644 index 00000000..240cc936 --- /dev/null +++ b/examples/motor2040/motor2040_quad_position_wave.cpp @@ -0,0 +1,75 @@ +#include "pico/stdlib.h" + +#include "motor2040.hpp" + +/* +A demonstration of driving all four of Motor 2040's motor outputs between +positions, with the help of their attached encoders and PID control. + +Press "Boot" to exit the program. +*/ + +using namespace motor; + +// How many sweeps of the motor to perform +const uint SWEEPS = 3; + +// The number of discrete sweep steps +const uint STEPS = 10; + +// The time in milliseconds between each step of the sequence +const uint STEPS_INTERVAL_MS = 500; + +// How far from zero to move the motor when sweeping +constexpr float SWEEP_EXTENT = 90.0f; + + +// Create a motor on pin 0 and 1 +Motor m = Motor(motor2040::MOTOR_A); + + +int main() { + stdio_init_all(); + + // Initialise the motor + m.init(); + + // Enable the motor + m.enable(); + sleep_ms(2000); + + // Go at full neative + m.full_negative(); + sleep_ms(2000); + + // Go at full positive + m.full_positive(); + sleep_ms(2000); + + // Stop the motor + m.stop(); + sleep_ms(2000); + + // Do a sine sweep + for(auto j = 0u; j < SWEEPS; j++) { + for(auto i = 0u; i < 360; i++) { + m.speed(sin(((float)i * (float)M_PI) / 180.0f) * SWEEP_EXTENT); + sleep_ms(20); + } + } + + // Do a stepped sweep + for(auto j = 0u; j < SWEEPS; j++) { + for(auto i = 0u; i < STEPS; i++) { + m.to_percent(i, 0, STEPS, 0.0 - SWEEP_EXTENT, SWEEP_EXTENT); + sleep_ms(STEPS_INTERVAL_MS); + } + for(auto i = 0u; i < STEPS; i++) { + m.to_percent(i, STEPS, 0, 0.0 - SWEEP_EXTENT, SWEEP_EXTENT); + sleep_ms(STEPS_INTERVAL_MS); + } + } + + // Disable the motor + m.disable(); +} diff --git a/examples/motor2040/motor2040_quad_velocity_sequence.cmake b/examples/motor2040/motor2040_quad_velocity_sequence.cmake new file mode 100644 index 00000000..f9e32c49 --- /dev/null +++ b/examples/motor2040/motor2040_quad_velocity_sequence.cmake @@ -0,0 +1,14 @@ +set(OUTPUT_NAME motor2040_quad_velocity_sequence) +add_executable(${OUTPUT_NAME} motor2040_quad_velocity_sequence.cpp) + +target_link_libraries(${OUTPUT_NAME} + pico_stdlib + motor2040 + button + pid + ) + +# enable usb output +pico_enable_stdio_usb(${OUTPUT_NAME} 1) + +pico_add_extra_outputs(${OUTPUT_NAME}) diff --git a/examples/motor2040/motor2040_quad_velocity_sequence.cpp b/examples/motor2040/motor2040_quad_velocity_sequence.cpp new file mode 100644 index 00000000..e612f655 --- /dev/null +++ b/examples/motor2040/motor2040_quad_velocity_sequence.cpp @@ -0,0 +1,75 @@ +#include "pico/stdlib.h" + +#include "motor2040.hpp" + +/* +A demonstration of driving all four of Motor 2040's motor outputs through a +sequence of velocities, with the help of their attached encoders and PID control. + +Press "Boot" to exit the program. +*/ + +using namespace motor; + +// How many sweeps of the motor to perform +const uint SWEEPS = 3; + +// The number of discrete sweep steps +const uint STEPS = 10; + +// The time in milliseconds between each step of the sequence +const uint STEPS_INTERVAL_MS = 500; + +// How far from zero to move the motor when sweeping +constexpr float SWEEP_EXTENT = 90.0f; + + +// Create a motor on pin 0 and 1 +Motor m = Motor(motor2040::MOTOR_A); + + +int main() { + stdio_init_all(); + + // Initialise the motor + m.init(); + + // Enable the motor + m.enable(); + sleep_ms(2000); + + // Go at full neative + m.full_negative(); + sleep_ms(2000); + + // Go at full positive + m.full_positive(); + sleep_ms(2000); + + // Stop the motor + m.stop(); + sleep_ms(2000); + + // Do a sine sweep + for(auto j = 0u; j < SWEEPS; j++) { + for(auto i = 0u; i < 360; i++) { + m.speed(sin(((float)i * (float)M_PI) / 180.0f) * SWEEP_EXTENT); + sleep_ms(20); + } + } + + // Do a stepped sweep + for(auto j = 0u; j < SWEEPS; j++) { + for(auto i = 0u; i < STEPS; i++) { + m.to_percent(i, 0, STEPS, 0.0 - SWEEP_EXTENT, SWEEP_EXTENT); + sleep_ms(STEPS_INTERVAL_MS); + } + for(auto i = 0u; i < STEPS; i++) { + m.to_percent(i, STEPS, 0, 0.0 - SWEEP_EXTENT, SWEEP_EXTENT); + sleep_ms(STEPS_INTERVAL_MS); + } + } + + // Disable the motor + m.disable(); +} diff --git a/examples/motor2040/motor2040_reactive_encoder.cmake b/examples/motor2040/motor2040_reactive_encoder.cmake new file mode 100644 index 00000000..c5173529 --- /dev/null +++ b/examples/motor2040/motor2040_reactive_encoder.cmake @@ -0,0 +1,12 @@ +set(OUTPUT_NAME motor2040_reactive_encoder) +add_executable(${OUTPUT_NAME} motor2040_reactive_encoder.cpp) + +target_link_libraries(${OUTPUT_NAME} + pico_stdlib + motor2040 + ) + +# enable usb output +pico_enable_stdio_usb(${OUTPUT_NAME} 1) + +pico_add_extra_outputs(${OUTPUT_NAME}) diff --git a/examples/motor2040/motor2040_reactive_encoder.cpp b/examples/motor2040/motor2040_reactive_encoder.cpp new file mode 100644 index 00000000..bde6948b --- /dev/null +++ b/examples/motor2040/motor2040_reactive_encoder.cpp @@ -0,0 +1,76 @@ +#include "pico/stdlib.h" + +#include "motor2040.hpp" + +/* +A demonstration of how a motor with an encoder can be used +as a programmable rotary encoder for user input, with +force-feedback for arbitrary detents and end stops. + +Press "Boot" to exit the program. +*/ + +using namespace motor; + +// How many sweeps of the motor to perform +const uint SWEEPS = 3; + +// The number of discrete sweep steps +const uint STEPS = 10; + +// The time in milliseconds between each step of the sequence +const uint STEPS_INTERVAL_MS = 500; + +// How far from zero to move the motor when sweeping +constexpr float SWEEP_EXTENT = 90.0f; + + +// Create a motor on pin 0 and 1 +Motor m = Motor(motor2040::MOTOR_A); + + +int main() { + stdio_init_all(); + + // Initialise the motor + m.init(); + + // Enable the motor + m.enable(); + sleep_ms(2000); + + // Go at full neative + m.full_negative(); + sleep_ms(2000); + + // Go at full positive + m.full_positive(); + sleep_ms(2000); + + // Stop the motor + m.stop(); + sleep_ms(2000); + + // Do a sine sweep + for(auto j = 0u; j < SWEEPS; j++) { + for(auto i = 0u; i < 360; i++) { + m.speed(sin(((float)i * (float)M_PI) / 180.0f) * SWEEP_EXTENT); + sleep_ms(20); + } + } + + // Do a stepped sweep + for(auto j = 0u; j < SWEEPS; j++) { + for(auto i = 0u; i < STEPS; i++) { + m.to_percent(i, 0, STEPS, 0.0 - SWEEP_EXTENT, SWEEP_EXTENT); + sleep_ms(STEPS_INTERVAL_MS); + } + for(auto i = 0u; i < STEPS; i++) { + m.to_percent(i, STEPS, 0, 0.0 - SWEEP_EXTENT, SWEEP_EXTENT); + sleep_ms(STEPS_INTERVAL_MS); + } + } + + // Disable the motor + m.disable(); +} diff --git a/examples/motor2040/motor2040_read_encoders.cmake b/examples/motor2040/motor2040_read_encoders.cmake new file mode 100644 index 00000000..7d3e586b --- /dev/null +++ b/examples/motor2040/motor2040_read_encoders.cmake @@ -0,0 +1,13 @@ +set(OUTPUT_NAME motor2040_read_encoders) +add_executable(${OUTPUT_NAME} motor2040_read_encoders.cpp) + +target_link_libraries(${OUTPUT_NAME} + pico_stdlib + motor2040 + button + ) + +# enable usb output +pico_enable_stdio_usb(${OUTPUT_NAME} 1) + +pico_add_extra_outputs(${OUTPUT_NAME}) diff --git a/examples/motor2040/motor2040_read_encoders.cpp b/examples/motor2040/motor2040_read_encoders.cpp new file mode 100644 index 00000000..01c61305 --- /dev/null +++ b/examples/motor2040/motor2040_read_encoders.cpp @@ -0,0 +1,60 @@ +#include +#include "pico/stdlib.h" + +#include "motor2040.hpp" +#include "button.hpp" + +/* +Demonstrates how to read the angles of Motor 2040's four encoders. + +Press "Boot" to exit the program. +*/ + +using namespace motor; +using namespace encoder; + +// The gear ratio of the motor +constexpr float GEAR_RATIO = 50.0f; + +// The counts per revolution of the motor's output shaft +constexpr float COUNTS_PER_REV = MMME_CPR * GEAR_RATIO; + + +// Create an array of encoder pointers +const pin_pair encoder_pins[] = {motor2040::ENCODER_A, motor2040::ENCODER_B, + motor2040::ENCODER_C, motor2040::ENCODER_D}; +const char* ENCODER_NAMES[] = {"A", "B", "C", "D"}; +const uint NUM_ENCODERS = count_of(encoder_pins); +Encoder *encoders[NUM_ENCODERS]; + +// Create the user button +Button user_sw(motor2040::USER_SW); + + +int main() { + stdio_init_all(); + + // Fill the array of motors, and initialise them. Up to 8 motors can be created + for(auto e = 0u; e < NUM_ENCODERS; e++) { + encoders[e] = new Encoder(pio0, e, encoder_pins[e], PIN_UNUSED, NORMAL_DIR, COUNTS_PER_REV, true); + encoders[e]->init(); + } + + // Uncomment the below lines to reverse + // the counting direction of an encoder + // encoders[0].direction(REVERSED_DIR); + // encoders[1].direction(REVERSED_DIR); + // encoders[2].direction(REVERSED_DIR); + // encoders[3].direction(REVERSED_DIR); + + // Read the encoders until the user button is pressed + while(!user_sw.raw()) { + // Print out the angle of each encoder + for(auto e = 0u; e < NUM_ENCODERS; e++) { + printf("%s = %f, ", ENCODER_NAMES[e], encoders[e]->degrees()); + } + printf("\n"); + + sleep_ms(100); + } +} diff --git a/examples/motor2040/motor2040_read_sensors.cmake b/examples/motor2040/motor2040_read_sensors.cmake new file mode 100644 index 00000000..837451a6 --- /dev/null +++ b/examples/motor2040/motor2040_read_sensors.cmake @@ -0,0 +1,15 @@ +set(OUTPUT_NAME motor2040_read_sensors) +add_executable(${OUTPUT_NAME} motor2040_read_sensors.cpp) + +target_link_libraries(${OUTPUT_NAME} + pico_stdlib + motor2040 + analogmux + analog + button + ) + +# enable usb output +pico_enable_stdio_usb(${OUTPUT_NAME} 1) + +pico_add_extra_outputs(${OUTPUT_NAME}) diff --git a/examples/motor2040/motor2040_read_sensors.cpp b/examples/motor2040/motor2040_read_sensors.cpp new file mode 100644 index 00000000..7cf775b8 --- /dev/null +++ b/examples/motor2040/motor2040_read_sensors.cpp @@ -0,0 +1,68 @@ +#include +#include "pico/stdlib.h" + +#include "motor2040.hpp" +#include "analogmux.hpp" +#include "analog.hpp" +#include "button.hpp" + +/* +Shows how to initialise and read the 2 external +and 6 internal sensors of Motor 2040. + +Press "Boot" to exit the program. +*/ + +using namespace motor; + +// Set up the shared analog inputs +Analog sen_adc = Analog(motor2040::SHARED_ADC); +Analog vol_adc = Analog(motor2040::SHARED_ADC, motor2040::VOLTAGE_GAIN); +Analog cur_adc = Analog(motor2040::SHARED_ADC, motor2040::CURRENT_GAIN, + motor2040::SHUNT_RESISTOR, motor2040::CURRENT_OFFSET); + +// Set up the analog multiplexer, including the pin for controlling pull-up/pull-down +AnalogMux mux = AnalogMux(motor2040::ADC_ADDR_0, motor2040::ADC_ADDR_1, motor2040::ADC_ADDR_2, + PIN_UNUSED, motor2040::SHARED_ADC); + +// Create the user button +Button user_sw(motor2040::USER_SW); + + +int main() { + stdio_init_all(); + + // Set up the sensor addresses with pull downs + for(auto i = 0u; i < motor2040::NUM_SENSORS; i++) { + mux.configure_pulls(motor2040::SENSOR_1_ADDR + i, false, true); + } + + // Set up the pull-up for the fault sense + mux.configure_pulls(motor2040::FAULT_SENSE_ADDR, true, false); + + // Read sensors until the user button is pressed + while(!user_sw.raw()) { + + // Read each sensor in turn and print its voltage + for(auto i = 0u; i < motor2040::NUM_SENSORS; i++) { + mux.select(motor2040::SENSOR_1_ADDR + i); + printf("S%d = %f, ", i + 1, sen_adc.read_voltage()); + } + + // Read the voltage sense and print the value + mux.select(motor2040::VOLTAGE_SENSE_ADDR); + printf("Voltage = %f, ", vol_adc.read_voltage()); + + // Read the current sense's of each motor and print the value + for(auto i = 0u; i < motor2040::NUM_MOTORS; i++) { + mux.select(motor2040::CURRENT_SENSE_A_ADDR + i); + printf("C%d = %f, ", i + 1, cur_adc.read_current()); + } + + // Read the fault sense and print the value + mux.select(motor2040::FAULT_SENSE_ADDR); + printf("Fault = %s\n", mux.read() ? "false" : "true"); + + sleep_ms(500); + } +} diff --git a/examples/motor2040/motor2040_single_motor.cpp b/examples/motor2040/motor2040_single_motor.cpp index d08767a1..bafe03a8 100644 --- a/examples/motor2040/motor2040_single_motor.cpp +++ b/examples/motor2040/motor2040_single_motor.cpp @@ -9,7 +9,7 @@ Demonstrates how to create a Motor object and control it. using namespace motor; // How many sweeps of the motor to perform -const uint SWEEPS = 3; +const uint SWEEPS = 2; // The number of discrete sweep steps const uint STEPS = 10; @@ -18,10 +18,10 @@ const uint STEPS = 10; const uint STEPS_INTERVAL_MS = 500; // How far from zero to move the motor when sweeping -constexpr float SWEEP_EXTENT = 90.0f; +constexpr float SPEED_EXTENT = 10.0f; -// Create a motor on pin 0 and 1 +// Create a motor Motor m = Motor(motor2040::MOTOR_A); @@ -35,34 +35,39 @@ int main() { m.enable(); sleep_ms(2000); - // Go at full neative - m.full_negative(); - sleep_ms(2000); - - // Go at full positive + // Drive at full positive m.full_positive(); sleep_ms(2000); - // Stop the motor + // Stop moving m.stop(); sleep_ms(2000); - // Do a sine sweep + // Drive at full negative + m.full_negative(); + sleep_ms(2000); + + // Coast to a gradual stop + m.coast(); + sleep_ms(2000); + + + // Do a sine speed sweep for(auto j = 0u; j < SWEEPS; j++) { for(auto i = 0u; i < 360; i++) { - m.speed(sin(((float)i * (float)M_PI) / 180.0f) * SWEEP_EXTENT); + m.speed(sin(((float)i * (float)M_PI) / 180.0f) * SPEED_EXTENT); sleep_ms(20); } } - // Do a stepped sweep + // Do a stepped speed sweep for(auto j = 0u; j < SWEEPS; j++) { for(auto i = 0u; i < STEPS; i++) { - m.to_percent(i, 0, STEPS, 0.0 - SWEEP_EXTENT, SWEEP_EXTENT); + m.to_percent(i, 0, STEPS, 0.0 - SPEED_EXTENT, SPEED_EXTENT); sleep_ms(STEPS_INTERVAL_MS); } for(auto i = 0u; i < STEPS; i++) { - m.to_percent(i, STEPS, 0, 0.0 - SWEEP_EXTENT, SWEEP_EXTENT); + m.to_percent(i, STEPS, 0, 0.0 - SPEED_EXTENT, SPEED_EXTENT); sleep_ms(STEPS_INTERVAL_MS); } } diff --git a/examples/motor2040/motor2040_velocity_control.cmake b/examples/motor2040/motor2040_velocity_control.cmake new file mode 100644 index 00000000..be841677 --- /dev/null +++ b/examples/motor2040/motor2040_velocity_control.cmake @@ -0,0 +1,12 @@ +set(OUTPUT_NAME motor2040_velocity_control) +add_executable(${OUTPUT_NAME} motor2040_velocity_control.cpp) + +target_link_libraries(${OUTPUT_NAME} + pico_stdlib + motor2040 + ) + +# enable usb output +pico_enable_stdio_usb(${OUTPUT_NAME} 1) + +pico_add_extra_outputs(${OUTPUT_NAME}) diff --git a/examples/motor2040/motor2040_velocity_control.cpp b/examples/motor2040/motor2040_velocity_control.cpp new file mode 100644 index 00000000..722c7639 --- /dev/null +++ b/examples/motor2040/motor2040_velocity_control.cpp @@ -0,0 +1,75 @@ +#include "pico/stdlib.h" + +#include "motor2040.hpp" + +/* +An example of how to drive a motor smoothly between random speeds, +with the help of it's attached encoder and PID control. + +Press "Boot" to exit the program. +*/ + +using namespace motor; + +// How many sweeps of the motor to perform +const uint SWEEPS = 3; + +// The number of discrete sweep steps +const uint STEPS = 10; + +// The time in milliseconds between each step of the sequence +const uint STEPS_INTERVAL_MS = 500; + +// How far from zero to move the motor when sweeping +constexpr float SWEEP_EXTENT = 90.0f; + + +// Create a motor on pin 0 and 1 +Motor m = Motor(motor2040::MOTOR_A); + + +int main() { + stdio_init_all(); + + // Initialise the motor + m.init(); + + // Enable the motor + m.enable(); + sleep_ms(2000); + + // Go at full neative + m.full_negative(); + sleep_ms(2000); + + // Go at full positive + m.full_positive(); + sleep_ms(2000); + + // Stop the motor + m.stop(); + sleep_ms(2000); + + // Do a sine sweep + for(auto j = 0u; j < SWEEPS; j++) { + for(auto i = 0u; i < 360; i++) { + m.speed(sin(((float)i * (float)M_PI) / 180.0f) * SWEEP_EXTENT); + sleep_ms(20); + } + } + + // Do a stepped sweep + for(auto j = 0u; j < SWEEPS; j++) { + for(auto i = 0u; i < STEPS; i++) { + m.to_percent(i, 0, STEPS, 0.0 - SWEEP_EXTENT, SWEEP_EXTENT); + sleep_ms(STEPS_INTERVAL_MS); + } + for(auto i = 0u; i < STEPS; i++) { + m.to_percent(i, STEPS, 0, 0.0 - SWEEP_EXTENT, SWEEP_EXTENT); + sleep_ms(STEPS_INTERVAL_MS); + } + } + + // Disable the motor + m.disable(); +} diff --git a/examples/motor2040/motor2040_velocity_tuning.cmake b/examples/motor2040/motor2040_velocity_tuning.cmake new file mode 100644 index 00000000..076cfc92 --- /dev/null +++ b/examples/motor2040/motor2040_velocity_tuning.cmake @@ -0,0 +1,13 @@ +set(OUTPUT_NAME motor2040_velocity_tuning) +add_executable(${OUTPUT_NAME} motor2040_velocity_tuning.cpp) + +target_link_libraries(${OUTPUT_NAME} + pico_stdlib + motor2040 + button + ) + +# enable usb output +pico_enable_stdio_usb(${OUTPUT_NAME} 1) + +pico_add_extra_outputs(${OUTPUT_NAME}) diff --git a/examples/motor2040/motor2040_velocity_tuning.cpp b/examples/motor2040/motor2040_velocity_tuning.cpp new file mode 100644 index 00000000..7ee2fa93 --- /dev/null +++ b/examples/motor2040/motor2040_velocity_tuning.cpp @@ -0,0 +1,77 @@ +#include "pico/stdlib.h" + +#include "motor2040.hpp" + +/* +A program to aid in the discovery and tuning of motor PID +values for velocity control. It does this by commanding the +motor to drive repeatedly between two setpoint speeds and +plots the measured response. + +Press "Boot" to exit the program. +*/ + +using namespace motor; + +// How many sweeps of the motor to perform +const uint SWEEPS = 2; + +// The number of discrete sweep steps +const uint STEPS = 10; + +// The time in milliseconds between each step of the sequence +const uint STEPS_INTERVAL_MS = 500; + +// How far from zero to drive the motor when sweeping +constexpr float SWEEP_EXTENT = 90.0f; + + +// Create a motor on pin 0 and 1 +Motor m = Motor(motor2040::MOTOR_A); + + +int main() { + stdio_init_all(); + + // Initialise the motor + m.init(); + + // Enable the motor + m.enable(); + sleep_ms(2000); + + // Go at full neative + m.full_negative(); + sleep_ms(2000); + + // Go at full positive + m.full_positive(); + sleep_ms(2000); + + // Stop the motor + m.stop(); + sleep_ms(2000); + + // Do a sine sweep + for(auto j = 0u; j < SWEEPS; j++) { + for(auto i = 0u; i < 360; i++) { + m.speed(sin(((float)i * (float)M_PI) / 180.0f) * SWEEP_EXTENT); + sleep_ms(20); + } + } + + // Do a stepped sweep + for(auto j = 0u; j < SWEEPS; j++) { + for(auto i = 0u; i < STEPS; i++) { + m.to_percent(i, 0, STEPS, 0.0 - SWEEP_EXTENT, SWEEP_EXTENT); + sleep_ms(STEPS_INTERVAL_MS); + } + for(auto i = 0u; i < STEPS; i++) { + m.to_percent(i, STEPS, 0, 0.0 - SWEEP_EXTENT, SWEEP_EXTENT); + sleep_ms(STEPS_INTERVAL_MS); + } + } + + // Disable the motor + m.disable(); +} diff --git a/libraries/motor2040/motor2040.cmake b/libraries/motor2040/motor2040.cmake index 73ae2487..647c53c7 100644 --- a/libraries/motor2040/motor2040.cmake +++ b/libraries/motor2040/motor2040.cmake @@ -3,4 +3,4 @@ add_library(motor2040 INTERFACE) target_include_directories(motor2040 INTERFACE ${CMAKE_CURRENT_LIST_DIR}) # Pull in pico libraries that we need -target_link_libraries(motor2040 INTERFACE pico_stdlib plasma motor motor_cluster) \ No newline at end of file +target_link_libraries(motor2040 INTERFACE pico_stdlib plasma motor motor_cluster encoder) \ No newline at end of file diff --git a/libraries/motor2040/motor2040.hpp b/libraries/motor2040/motor2040.hpp index a89fd549..7ee5a688 100644 --- a/libraries/motor2040/motor2040.hpp +++ b/libraries/motor2040/motor2040.hpp @@ -4,6 +4,7 @@ #include "ws2812.hpp" #include "motor.hpp" #include "motor_cluster.hpp" +#include "encoder.hpp" namespace motor { namespace motor2040 { diff --git a/micropython/examples/motor2040/led_rainbow.py b/micropython/examples/motor2040/led_rainbow.py index bb3db6b6..e20f1dd6 100644 --- a/micropython/examples/motor2040/led_rainbow.py +++ b/micropython/examples/motor2040/led_rainbow.py @@ -13,9 +13,9 @@ such may have problems when running code multiple times. If you encounter issues, try resetting your board. """ -SPEED = 5 # The speed that the LEDs will cycle at -BRIGHTNESS = 0.4 # The brightness of the LEDs -UPDATES = 50 # How many times the LEDs will be updated per second +SPEED = 5 # The speed that the LED will cycle at +BRIGHTNESS = 0.4 # The brightness of the LED +UPDATES = 50 # How many times the LED will be updated per second # Create the LED, using PIO 1 and State Machine 0 led = WS2812(motor2040.NUM_LEDS, 1, 0, motor2040.LED_DATA) diff --git a/micropython/examples/motor2040/motor_wave.py b/micropython/examples/motor2040/motor_wave.py index 369f326c..7aab108b 100644 --- a/micropython/examples/motor2040/motor_wave.py +++ b/micropython/examples/motor2040/motor_wave.py @@ -47,7 +47,7 @@ while user_sw.raw() is not True: # Update the LED led.set_hsv(0, offset / 2, 1.0, BRIGHTNESS) - # Update all the MOTORs + # Update all the motors for i in range(len(motors)): angle = ((i / len(motors)) + offset) * math.pi motors[i].speed(math.sin(angle) * SPEED_EXTENT) @@ -58,5 +58,5 @@ while user_sw.raw() is not True: for m in motors: m.disable() -# Turn off the LED bar +# Turn off the LED led.clear() diff --git a/micropython/examples/motor2040/multiple_motors.py b/micropython/examples/motor2040/multiple_motors.py index 19ebacd5..008e963b 100644 --- a/micropython/examples/motor2040/multiple_motors.py +++ b/micropython/examples/motor2040/multiple_motors.py @@ -36,7 +36,7 @@ for m in motors: time.sleep(2) -SWEEPS = 2 # How many speed sweeps of the motor to perform +SWEEPS = 2 # How many speed sweeps of the motors to perform STEPS = 10 # The number of discrete sweep steps STEPS_INTERVAL = 0.5 # The time in seconds between each step of the sequence SPEED_EXTENT = 1.0 # How far from zero to drive the motors when sweeping diff --git a/micropython/examples/motor2040/position_tuning.py b/micropython/examples/motor2040/position_tuning.py index 9ecfd13e..69e0df42 100644 --- a/micropython/examples/motor2040/position_tuning.py +++ b/micropython/examples/motor2040/position_tuning.py @@ -41,10 +41,10 @@ POS_KD = 0.0022 # Position derivative (D) gain # Free up hardware resources ahead of creating a new Encoder gc.collect() -# Create a motor and set its speed scale -m = Motor(MOTOR_PINS, direction=DIRECTION, speed_scale=SPEED_SCALE, deadzone=0.0) +# Create a motor and set its direction and speed scale +m = Motor(MOTOR_PINS, direction=DIRECTION, speed_scale=SPEED_SCALE) -# Create an encoder, using PIO 0 and State Machine 0 +# Create an encoder and set its direction and counts per rev, using PIO 0 and State Machine 0 enc = Encoder(0, 0, ENCODER_PINS, direction=DIRECTION, counts_per_rev=COUNTS_PER_REV, count_microsteps=True) # Create the user button @@ -96,5 +96,5 @@ while user_sw.raw() is not True: time.sleep(UPDATE_RATE) -# Disable the servo +# Disable the motor m.disable() From 15e5eaa89043d3a3a433ebeee4bf8286aab5c7e1 Mon Sep 17 00:00:00 2001 From: ZodiusInfuser Date: Thu, 28 Apr 2022 21:23:18 +0100 Subject: [PATCH 38/60] Finished C++ examples and tweaked MP examples --- drivers/analogmux/analogmux.cpp | 4 + drivers/pid/pid.cpp | 5 + drivers/pid/pid.hpp | 1 + .../motor2040/motor2040_motor_cluster.cpp | 3 + .../motor2040/motor2040_motor_profiler.cpp | 136 +++++++---- examples/motor2040/motor2040_motor_wave.cpp | 2 +- .../motor2040/motor2040_position_control.cpp | 147 +++++++++--- ...motor2040_position_on_velocity_control.cpp | 164 ++++++++++--- .../motor2040_position_on_velocity_tuning.cpp | 144 ++++++++--- .../motor2040/motor2040_position_tuning.cpp | 12 +- .../motor2040_quad_position_wave.cpp | 184 ++++++++++---- .../motor2040_quad_velocity_sequence.cpp | 224 ++++++++++++++---- .../motor2040_reactive_encoder.cmake | 2 + .../motor2040/motor2040_reactive_encoder.cpp | 161 ++++++++++--- examples/motor2040/motor2040_single_motor.cpp | 2 +- .../motor2040_velocity_control.cmake | 2 + .../motor2040/motor2040_velocity_control.cpp | 147 +++++++++--- .../motor2040/motor2040_velocity_tuning.cmake | 1 + .../motor2040/motor2040_velocity_tuning.cpp | 127 +++++++--- libraries/motor2040/motor2040.hpp | 4 +- .../examples/motor2040/motor_profiler.py | 4 +- .../examples/motor2040/position_control.py | 2 +- .../motor2040/position_on_velocity_control.py | 3 - .../motor2040/position_on_velocity_tuning.py | 2 +- .../examples/motor2040/quad_position_wave.py | 8 +- .../motor2040/quad_velocity_sequence.py | 46 ++-- .../examples/motor2040/reactive_encoder.py | 4 +- .../examples/motor2040/read_sensors.py | 2 +- .../examples/motor2040/velocity_control.py | 5 +- .../examples/motor2040/velocity_tuning.py | 6 +- 30 files changed, 1156 insertions(+), 398 deletions(-) diff --git a/drivers/analogmux/analogmux.cpp b/drivers/analogmux/analogmux.cpp index 3898df0a..916d3d77 100644 --- a/drivers/analogmux/analogmux.cpp +++ b/drivers/analogmux/analogmux.cpp @@ -27,6 +27,10 @@ namespace pimoroni { gpio_init(en_pin); gpio_set_dir(en_pin, GPIO_OUT); } + + if(muxed_pin != PIN_UNUSED) { + gpio_set_input_enabled(muxed_pin, true); + } } void AnalogMux::select(uint8_t address) { diff --git a/drivers/pid/pid.cpp b/drivers/pid/pid.cpp index 9134e5d7..0d8cdb4f 100644 --- a/drivers/pid/pid.cpp +++ b/drivers/pid/pid.cpp @@ -1,6 +1,11 @@ #include "pid.hpp" namespace pimoroni { + PID::PID() + : kp(0.0f), ki(0.0f), kd(0.0f), setpoint(0.0f) + , error_sum(0.0f), last_value(0.0f), sample_rate(1.0f) { + } + PID::PID(float kp, float ki, float kd, float sample_rate) : kp(kp), ki(ki), kd(kd), setpoint(0.0f) , error_sum(0.0f), last_value(0.0f), sample_rate(sample_rate) { diff --git a/drivers/pid/pid.hpp b/drivers/pid/pid.hpp index 7dd17c8a..ef603941 100644 --- a/drivers/pid/pid.hpp +++ b/drivers/pid/pid.hpp @@ -8,6 +8,7 @@ namespace pimoroni { class PID { public: + PID(); PID(float kp, float ki, float kd, float sample_rate); float calculate(float value); diff --git a/examples/motor2040/motor2040_motor_cluster.cpp b/examples/motor2040/motor2040_motor_cluster.cpp index 804b77ca..27fc16e5 100644 --- a/examples/motor2040/motor2040_motor_cluster.cpp +++ b/examples/motor2040/motor2040_motor_cluster.cpp @@ -76,4 +76,7 @@ int main() { // Disable the motors motors.disable_all(); + + // Sleep a short time so the disable takes effect + sleep_ms(100); } diff --git a/examples/motor2040/motor2040_motor_profiler.cpp b/examples/motor2040/motor2040_motor_profiler.cpp index a257c6f4..55fa3ce1 100644 --- a/examples/motor2040/motor2040_motor_profiler.cpp +++ b/examples/motor2040/motor2040_motor_profiler.cpp @@ -1,3 +1,4 @@ +#include #include "pico/stdlib.h" #include "motor2040.hpp" @@ -8,66 +9,119 @@ duty cycle range using the attached encoder for feedback. */ using namespace motor; +using namespace encoder; -// How many sweeps of the motor to perform -const uint SWEEPS = 3; +// The pins of the motor being profiled +const pin_pair MOTOR_PINS = motor2040::MOTOR_A; -// The number of discrete sweep steps -const uint STEPS = 10; +// The pins of the encoder attached to the profiled motor +const pin_pair ENCODER_PINS = motor2040::ENCODER_A; -// The time in milliseconds between each step of the sequence -const uint STEPS_INTERVAL_MS = 500; +// The gear ratio of the motor +constexpr float GEAR_RATIO = 50; -// How far from zero to move the motor when sweeping -constexpr float SWEEP_EXTENT = 90.0f; +// The counts per revolution of the motor's output shaft +constexpr float COUNTS_PER_REV = MMME_CPR * GEAR_RATIO; + +// The direction to spin the motor in. NORMAL_DIR (0), REVERSED_DIR (1) +const Direction DIRECTION = NORMAL_DIR; + +// The scaling to apply to the motor's speed. Set this to the maximum measured speed +constexpr float SPEED_SCALE = 5.4f; + +// The duty cycle that corresponds with zero speed when plotting the motor's speed as a straight line +constexpr float ZERO_POINT = 0.0f; + +// The duty cycle below which the motor's friction prevents it from moving +constexpr float DEAD_ZONE = 0.0f; + +// How many duty cycle steps to sample the speed of +const uint DUTY_STEPS = 100; + +// How long to wait after changing motor duty cycle +const uint SETTLE_TIME = 100; + +// How long to capture the motor's speed at each step +const uint CAPTURE_TIME = 200; -// Create a motor on pin 0 and 1 -Motor m = Motor(motor2040::MOTOR_A); +// Create a motor and set its direction, speed scale, zero point, and dead zone +Motor m = Motor(MOTOR_PINS, DIRECTION, SPEED_SCALE, ZERO_POINT, DEAD_ZONE); + +// Create an encoder and set its direction and counts per rev, using PIO 0 and State Machine 0 +Encoder enc = Encoder(pio0, 0, ENCODER_PINS, PIN_UNUSED, DIRECTION, COUNTS_PER_REV, true); + + +// Function that performs a single profiling step +void profile_at_duty(float duty) { + // Set the motor to a new duty cycle and wait for it to settle + if(DIRECTION == REVERSED_DIR) + m.duty(0.0 - duty); + else + m.duty(duty); + sleep_ms(SETTLE_TIME); + + // Perform a dummy capture to clear the encoder + enc.capture(); + + // Wait for the capture time to pass + sleep_ms(CAPTURE_TIME); + + // Perform a capture and read the measured speed + Encoder::Capture capture = enc.capture(); + float measured_speed = capture.revolutions_per_second(); + + // These are some alternate speed measurements from the encoder + // float measured_speed = capture.revolutions_per_minute(); + // float measured_speed = capture.degrees_per_second(); + // float measured_speed = capture.radians_per_second(); + + // Print out the expected and measured speeds, as well as their difference + printf("Duty = %f, Expected = %f, Measured = %f, Diff = %f\n", + m.duty(), m.speed(), measured_speed, m.speed() - measured_speed); +} int main() { stdio_init_all(); - // Initialise the motor + // Give some time to connect up a serial terminal + sleep_ms(10000); + + // Initialise the motor and enable it m.init(); - - // Enable the motor m.enable(); - sleep_ms(2000); - // Go at full neative - m.full_negative(); - sleep_ms(2000); + // Initialise the encoder + enc.init(); - // Go at full positive - m.full_positive(); - sleep_ms(2000); + printf("Profiler Starting...\n"); - // Stop the motor - m.stop(); - sleep_ms(2000); - - // Do a sine sweep - for(auto j = 0u; j < SWEEPS; j++) { - for(auto i = 0u; i < 360; i++) { - m.speed(sin(((float)i * (float)M_PI) / 180.0f) * SWEEP_EXTENT); - sleep_ms(20); - } + // Profile from 0% up to one step below 100% + for(uint i = 0; i < DUTY_STEPS; i++) { + profile_at_duty((float)i / (float)DUTY_STEPS); } - // Do a stepped sweep - for(auto j = 0u; j < SWEEPS; j++) { - for(auto i = 0u; i < STEPS; i++) { - m.to_percent(i, 0, STEPS, 0.0 - SWEEP_EXTENT, SWEEP_EXTENT); - sleep_ms(STEPS_INTERVAL_MS); - } - for(auto i = 0u; i < STEPS; i++) { - m.to_percent(i, STEPS, 0, 0.0 - SWEEP_EXTENT, SWEEP_EXTENT); - sleep_ms(STEPS_INTERVAL_MS); - } + // Profile from 100% down to one step above 0% + for(uint i = 0; i < DUTY_STEPS; i++) { + profile_at_duty((float)(DUTY_STEPS - i) / (float)DUTY_STEPS); } - // Disable the motor + // Profile from 0% down to one step above -100% + for(uint i = 0; i < DUTY_STEPS; i++) { + profile_at_duty(-(float)i / (float)DUTY_STEPS); + } + + // Profile from -100% up to one step below 0% + for(uint i = 0; i < DUTY_STEPS; i++) { + profile_at_duty(-(float)(DUTY_STEPS - i) / (float)DUTY_STEPS); + } + + // Profile 0% again + profile_at_duty(0.0f); + + printf("Profiler Finished...\n"); + + // Disable the motor now the profiler has finished m.disable(); } diff --git a/examples/motor2040/motor2040_motor_wave.cpp b/examples/motor2040/motor2040_motor_wave.cpp index a66a5713..14ab9cbd 100644 --- a/examples/motor2040/motor2040_motor_wave.cpp +++ b/examples/motor2040/motor2040_motor_wave.cpp @@ -62,7 +62,7 @@ int main() { // Update all the motors for(auto m = 0u; m < NUM_MOTORS; m++) { - float angle = (((float)m / (float)NUM_MOTORS) + offset) * (float)M_TWOPI; + float angle = (((float)m / (float)NUM_MOTORS) + offset) * (float)M_PI; motors[m]->speed(sin(angle) * SPEED_EXTENT); } diff --git a/examples/motor2040/motor2040_position_control.cpp b/examples/motor2040/motor2040_position_control.cpp index ee6f4133..3eac307d 100644 --- a/examples/motor2040/motor2040_position_control.cpp +++ b/examples/motor2040/motor2040_position_control.cpp @@ -1,6 +1,9 @@ +#include #include "pico/stdlib.h" #include "motor2040.hpp" +#include "button.hpp" +#include "pid.hpp" /* An example of how to move a motor smoothly between random positions, @@ -10,64 +13,138 @@ Press "Boot" to exit the program. */ using namespace motor; +using namespace encoder; -// How many sweeps of the motor to perform -const uint SWEEPS = 3; +// The pins of the motor being profiled +const pin_pair MOTOR_PINS = motor2040::MOTOR_A; -// The number of discrete sweep steps -const uint STEPS = 10; +// The pins of the encoder attached to the profiled motor +const pin_pair ENCODER_PINS = motor2040::ENCODER_A; -// The time in milliseconds between each step of the sequence -const uint STEPS_INTERVAL_MS = 500; +// The gear ratio of the motor +constexpr float GEAR_RATIO = 50.0f; -// How far from zero to move the motor when sweeping -constexpr float SWEEP_EXTENT = 90.0f; +// The counts per revolution of the motor's output shaft +constexpr float COUNTS_PER_REV = MMME_CPR * GEAR_RATIO; + +// The direction to spin the motor in. NORMAL_DIR (0), REVERSED_DIR (1) +const Direction DIRECTION = NORMAL_DIR; + +// The scaling to apply to the motor's speed to match its real-world speed +float SPEED_SCALE = 5.4f; + +// How many times to update the motor per second +const uint UPDATES = 100; +constexpr float UPDATE_RATE = 1.0f / (float)UPDATES; + +// The time to travel between each random value +constexpr float TIME_FOR_EACH_MOVE = 1.0f; +const uint UPDATES_PER_MOVE = TIME_FOR_EACH_MOVE * UPDATES; + +// How many of the updates should be printed (i.e. 2 would be every other update) +const uint PRINT_DIVIDER = 4; + +// Multipliers for the different printed values, so they appear nicely on the Thonny plotter +constexpr float SPD_PRINT_SCALE = 20.0f; // Driving Speed multipler + +// How far from zero to move the motor, in degrees +constexpr float POSITION_EXTENT = 180.0f; + +// The interpolating mode between setpoints. STEP (0), LINEAR (1), COSINE (2) +const uint INTERP_MODE = 2; -// Create a motor on pin 0 and 1 -Motor m = Motor(motor2040::MOTOR_A); +// PID values +constexpr float POS_KP = 0.14f; // Position proportional (P) gain +constexpr float POS_KI = 0.0f; // Position integral (I) gain +constexpr float POS_KD = 0.002f; // Position derivative (D) gain + + +// Create a motor and set its direction and speed scale +Motor m = Motor(MOTOR_PINS, DIRECTION, SPEED_SCALE); + +// Create an encoder and set its direction and counts per rev, using PIO 0 and State Machine 0 +Encoder enc = Encoder(pio0, 0, ENCODER_PINS, PIN_UNUSED, DIRECTION, COUNTS_PER_REV, true); + +// Create the user button +Button user_sw(motor2040::USER_SW); + +// Create PID object for position control +PID pos_pid = PID(POS_KP, POS_KI, POS_KD, UPDATE_RATE); int main() { stdio_init_all(); - // Initialise the motor + // Initialise the motor and encoder m.init(); + enc.init(); // Enable the motor m.enable(); - sleep_ms(2000); - // Go at full neative - m.full_negative(); - sleep_ms(2000); - // Go at full positive - m.full_positive(); - sleep_ms(2000); + uint update = 0; + uint print_count = 0; - // Stop the motor - m.stop(); - sleep_ms(2000); + // Set the initial value and create a random end value between the extents + float start_value = 0.0f; + float end_value = (((float)rand() / (float)RAND_MAX) * (POSITION_EXTENT * 2.0f)) - POSITION_EXTENT; - // Do a sine sweep - for(auto j = 0u; j < SWEEPS; j++) { - for(auto i = 0u; i < 360; i++) { - m.speed(sin(((float)i * (float)M_PI) / 180.0f) * SWEEP_EXTENT); - sleep_ms(20); + // Continually move the motor until the user button is pressed + while(!user_sw.raw()) { + + // Capture the state of the encoder + Encoder::Capture capture = enc.capture(); + + // Calculate how far along this movement to be + float percent_along = (float)update / (float)UPDATES_PER_MOVE; + + switch(INTERP_MODE) { + case 0: + // Move the motor instantly to the end value + pos_pid.setpoint = end_value; + break; + + case 2: + // Move the motor between values using cosine + pos_pid.setpoint = (((-cosf(percent_along * (float)M_PI) + 1.0) / 2.0) * (end_value - start_value)) + start_value; + break; + + case 1: + default: + // Move the motor linearly between values + pos_pid.setpoint = (percent_along * (end_value - start_value)) + start_value; } - } - // Do a stepped sweep - for(auto j = 0u; j < SWEEPS; j++) { - for(auto i = 0u; i < STEPS; i++) { - m.to_percent(i, 0, STEPS, 0.0 - SWEEP_EXTENT, SWEEP_EXTENT); - sleep_ms(STEPS_INTERVAL_MS); + // Calculate the velocity to move the motor closer to the position setpoint + float vel = pos_pid.calculate(capture.degrees(), capture.degrees_per_second()); + + // Set the new motor driving speed + m.speed(vel); + + // Print out the current motor values and their setpoints, but only on every multiple + if(print_count == 0) { + printf("Pos = %f, ", capture.degrees()); + printf("Pos SP = %f, ", pos_pid.setpoint); + printf("Speed = %f\n", m.speed() * SPD_PRINT_SCALE); } - for(auto i = 0u; i < STEPS; i++) { - m.to_percent(i, STEPS, 0, 0.0 - SWEEP_EXTENT, SWEEP_EXTENT); - sleep_ms(STEPS_INTERVAL_MS); + + // Increment the print count, and wrap it + print_count = (print_count + 1) % PRINT_DIVIDER; + + update++; // Move along in time + + // Have we reached the end of this movement? + if(update >= UPDATES_PER_MOVE) { + update = 0; // Reset the counter + + // Set the start as the last end and create a new random end value + start_value = end_value; + end_value = (((float)rand() / (float)RAND_MAX) * (POSITION_EXTENT * 2.0f)) - POSITION_EXTENT; } + + sleep_ms(UPDATE_RATE * 1000.0f); } // Disable the motor diff --git a/examples/motor2040/motor2040_position_on_velocity_control.cpp b/examples/motor2040/motor2040_position_on_velocity_control.cpp index 3463e37c..f53c91e9 100644 --- a/examples/motor2040/motor2040_position_on_velocity_control.cpp +++ b/examples/motor2040/motor2040_position_on_velocity_control.cpp @@ -1,6 +1,9 @@ +#include #include "pico/stdlib.h" #include "motor2040.hpp" +#include "button.hpp" +#include "pid.hpp" /* An example of how to move a motor smoothly between random positions, @@ -10,64 +13,155 @@ Press "Boot" to exit the program. */ using namespace motor; +using namespace encoder; -// How many sweeps of the motor to perform -const uint SWEEPS = 3; +// The pins of the motor being profiled +const pin_pair MOTOR_PINS = motor2040::MOTOR_A; -// The number of discrete sweep steps -const uint STEPS = 10; +// The pins of the encoder attached to the profiled motor +const pin_pair ENCODER_PINS = motor2040::ENCODER_A; -// The time in milliseconds between each step of the sequence -const uint STEPS_INTERVAL_MS = 500; +// The gear ratio of the motor +constexpr float GEAR_RATIO = 50.0f; -// How far from zero to move the motor when sweeping -constexpr float SWEEP_EXTENT = 90.0f; +// The counts per revolution of the motor's output shaft +constexpr float COUNTS_PER_REV = MMME_CPR * GEAR_RATIO; + +// The direction to spin the motor in. NORMAL_DIR (0), REVERSED_DIR (1) +const Direction DIRECTION = NORMAL_DIR; + +// The scaling to apply to the motor's speed to match its real-world speed +float SPEED_SCALE = 5.4f; + +// How many times to update the motor per second +const uint UPDATES = 100; +constexpr float UPDATE_RATE = 1.0f / (float)UPDATES; + +// The time to travel between each random value, in seconds +constexpr float TIME_FOR_EACH_MOVE = 1.0f; +const uint UPDATES_PER_MOVE = TIME_FOR_EACH_MOVE * UPDATES; + +// How many of the updates should be printed (i.e. 2 would be every other update) +const uint PRINT_DIVIDER = 4; + +// Multipliers for the different printed values, so they appear nicely on the Thonny plotter +constexpr float ACC_PRINT_SCALE = 2.0f; // Acceleration multiplier +constexpr float SPD_PRINT_SCALE = 40.0f; // Driving Speed multipler + +// How far from zero to move the motor, in degrees +constexpr float POSITION_EXTENT = 180.0f; + +// The maximum speed to move the motor at, in revolutions per second +constexpr float MAX_SPEED = 1.0f; + +// The interpolating mode between setpoints. STEP (0), LINEAR (1), COSINE (2) +const uint INTERP_MODE = 0; + +// PID values +constexpr float POS_KP = 0.025f; // Position proportional (P) gain +constexpr float POS_KI = 0.0f; // Position integral (I) gain +constexpr float POS_KD = 0.0f; // Position derivative (D) gain + +constexpr float VEL_KP = 30.0f; // Velocity proportional (P) gain +constexpr float VEL_KI = 0.0f; // Velocity integral (I) gain +constexpr float VEL_KD = 0.4f; // Velocity derivative (D) gain -// Create a motor on pin 0 and 1 -Motor m = Motor(motor2040::MOTOR_A); +// Create a motor and set its direction and speed scale +Motor m = Motor(MOTOR_PINS, DIRECTION, SPEED_SCALE); + +// Create an encoder and set its direction and counts per rev, using PIO 0 and State Machine 0 +Encoder enc = Encoder(pio0, 0, ENCODER_PINS, PIN_UNUSED, DIRECTION, COUNTS_PER_REV, true); + +// Create the user button +Button user_sw(motor2040::USER_SW); + +// Create PID object for both position and velocity control +PID pos_pid = PID(POS_KP, POS_KI, POS_KD, UPDATE_RATE); +PID vel_pid = PID(VEL_KP, VEL_KI, VEL_KD, UPDATE_RATE); int main() { stdio_init_all(); - // Initialise the motor + // Initialise the motor and encoder m.init(); + enc.init(); // Enable the motor m.enable(); - sleep_ms(2000); - // Go at full neative - m.full_negative(); - sleep_ms(2000); - // Go at full positive - m.full_positive(); - sleep_ms(2000); + uint update = 0; + uint print_count = 0; - // Stop the motor - m.stop(); - sleep_ms(2000); + // Set the initial value and create a random end value between the extents + float start_value = 0.0f; + float end_value = (((float)rand() / (float)RAND_MAX) * (POSITION_EXTENT * 2.0f)) - POSITION_EXTENT; - // Do a sine sweep - for(auto j = 0u; j < SWEEPS; j++) { - for(auto i = 0u; i < 360; i++) { - m.speed(sin(((float)i * (float)M_PI) / 180.0f) * SWEEP_EXTENT); - sleep_ms(20); + // Continually move the motor until the user button is pressed + while(!user_sw.raw()) { + + // Capture the state of the encoder + Encoder::Capture capture = enc.capture(); + + // Calculate how far along this movement to be + float percent_along = (float)update / (float)UPDATES_PER_MOVE; + + switch(INTERP_MODE) { + case 0: + // Move the motor instantly to the end value + pos_pid.setpoint = end_value; + break; + + case 2: + // Move the motor between values using cosine + pos_pid.setpoint = (((-cosf(percent_along * (float)M_PI) + 1.0) / 2.0) * (end_value - start_value)) + start_value; + break; + + case 1: + default: + // Move the motor linearly between values + pos_pid.setpoint = (percent_along * (end_value - start_value)) + start_value; } - } - // Do a stepped sweep - for(auto j = 0u; j < SWEEPS; j++) { - for(auto i = 0u; i < STEPS; i++) { - m.to_percent(i, 0, STEPS, 0.0 - SWEEP_EXTENT, SWEEP_EXTENT); - sleep_ms(STEPS_INTERVAL_MS); + // Calculate the velocity to move the motor closer to the position setpoint + float vel = pos_pid.calculate(capture.degrees(), capture.degrees_per_second()); + + // Limit the velocity between user defined limits, and set it as the new setpoint of the velocity PID + vel_pid.setpoint = CLAMP(vel, -MAX_SPEED, MAX_SPEED); + + // Calculate the acceleration to apply to the motor to move it closer to the velocity setpoint + float accel = vel_pid.calculate(capture.revolutions_per_second()); + + // Accelerate or decelerate the motor + m.speed(m.speed() + (accel * UPDATE_RATE)); + + // Print out the current motor values and their setpoints, but only on every multiple + if(print_count == 0) { + printf("Pos = %f, ", capture.degrees()); + printf("Pos SP = %f, ", pos_pid.setpoint); + printf("Vel = %f, ", capture.revolutions_per_second() * SPD_PRINT_SCALE); + printf("Vel SP = %f, ", vel_pid.setpoint * SPD_PRINT_SCALE); + printf("Accel = %f, ", accel * ACC_PRINT_SCALE); + printf("Speed = %f\n", m.speed()); } - for(auto i = 0u; i < STEPS; i++) { - m.to_percent(i, STEPS, 0, 0.0 - SWEEP_EXTENT, SWEEP_EXTENT); - sleep_ms(STEPS_INTERVAL_MS); + + // Increment the print count, and wrap it + print_count = (print_count + 1) % PRINT_DIVIDER; + + update++; // Move along in time + + // Have we reached the end of this movement? + if(update >= UPDATES_PER_MOVE) { + update = 0; // Reset the counter + + // Set the start as the last end and create a new random end value + start_value = end_value; + end_value = (((float)rand() / (float)RAND_MAX) * (POSITION_EXTENT * 2.0f)) - POSITION_EXTENT; } + + sleep_ms(UPDATE_RATE * 1000.0f); } // Disable the motor diff --git a/examples/motor2040/motor2040_position_on_velocity_tuning.cpp b/examples/motor2040/motor2040_position_on_velocity_tuning.cpp index 51847ea4..c97c4bf6 100644 --- a/examples/motor2040/motor2040_position_on_velocity_tuning.cpp +++ b/examples/motor2040/motor2040_position_on_velocity_tuning.cpp @@ -1,6 +1,9 @@ +#include #include "pico/stdlib.h" #include "motor2040.hpp" +#include "button.hpp" +#include "pid.hpp" /* A program to aid in the discovery and tuning of motor PID @@ -12,64 +15,133 @@ Press "Boot" to exit the program. */ using namespace motor; +using namespace encoder; -// How many sweeps of the motor to perform -const uint SWEEPS = 3; +// The pins of the motor being profiled +const pin_pair MOTOR_PINS = motor2040::MOTOR_A; -// The number of discrete sweep steps -const uint STEPS = 10; +// The pins of the encoder attached to the profiled motor +const pin_pair ENCODER_PINS = motor2040::ENCODER_A; -// The time in milliseconds between each step of the sequence -const uint STEPS_INTERVAL_MS = 500; +// The gear ratio of the motor +constexpr float GEAR_RATIO = 50.0f; -// How far from zero to move the motor when sweeping -constexpr float SWEEP_EXTENT = 90.0f; +// The counts per revolution of the motor's output shaft +constexpr float COUNTS_PER_REV = MMME_CPR * GEAR_RATIO; + +// The direction to spin the motor in. NORMAL_DIR (0), REVERSED_DIR (1) +const Direction DIRECTION = NORMAL_DIR; + +// The scaling to apply to the motor's speed to match its real-world speed +float SPEED_SCALE = 5.4f; + +// How many times to update the motor per second +const uint UPDATES = 100; +constexpr float UPDATE_RATE = 1.0f / (float)UPDATES; + +// The time (in seconds) after a new setpoint, to display print out motor values +constexpr float PRINT_WINDOW = 1.0f; + +// The time (in seconds) between each new setpoint being set +constexpr float MOVEMENT_WINDOW = 2.0f; + +// How many of the updates should be printed (i.e. 2 would be every other update) +const uint PRINT_DIVIDER = 4; + +// Multipliers for the different printed values, so they appear nicely on the Thonny plotter +constexpr float ACC_PRINT_SCALE = 1.0f; // Acceleration multiplier +constexpr float SPD_PRINT_SCALE = 20.0f; // Driving Speed multipler + +// How far from zero to move the motor, in degrees +constexpr float POSITION_EXTENT = 180.0f; + +// The maximum speed to move the motor at, in revolutions per second +constexpr float MAX_SPEED = 2.0f; + +// PID values +constexpr float POS_KP = 0.025f; // Position proportional (P) gain +constexpr float POS_KI = 0.0f; // Position integral (I) gain +constexpr float POS_KD = 0.0f; // Position derivative (D) gain + +constexpr float VEL_KP = 30.0f; // Velocity proportional (P) gain +constexpr float VEL_KI = 0.0f; // Velocity integral (I) gain +constexpr float VEL_KD = 0.4f; // Velocity derivative (D) gain -// Create a motor on pin 0 and 1 -Motor m = Motor(motor2040::MOTOR_A); +// Create a motor and set its direction and speed scale +Motor m = Motor(MOTOR_PINS, DIRECTION, SPEED_SCALE); + +// Create an encoder and set its direction and counts per rev, using PIO 0 and State Machine 0 +Encoder enc = Encoder(pio0, 0, ENCODER_PINS, PIN_UNUSED, DIRECTION, COUNTS_PER_REV, true); + +// Create the user button +Button user_sw(motor2040::USER_SW); + +// Create PID object for both position and velocity control +PID pos_pid = PID(POS_KP, POS_KI, POS_KD, UPDATE_RATE); +PID vel_pid = PID(VEL_KP, VEL_KI, VEL_KD, UPDATE_RATE); int main() { stdio_init_all(); - // Initialise the motor + // Initialise the motor and encoder m.init(); + enc.init(); // Enable the motor m.enable(); - sleep_ms(2000); - // Go at full neative - m.full_negative(); - sleep_ms(2000); + // Set the initial setpoint position + pos_pid.setpoint = POSITION_EXTENT; - // Go at full positive - m.full_positive(); - sleep_ms(2000); - // Stop the motor - m.stop(); - sleep_ms(2000); + uint update = 0; + uint print_count = 0; - // Do a sine sweep - for(auto j = 0u; j < SWEEPS; j++) { - for(auto i = 0u; i < 360; i++) { - m.speed(sin(((float)i * (float)M_PI) / 180.0f) * SWEEP_EXTENT); - sleep_ms(20); + // Continually move the motor until the user button is pressed + while(!user_sw.raw()) { + + // Capture the state of the encoder + Encoder::Capture capture = enc.capture(); + + // Calculate the velocity to move the motor closer to the position setpoint + float vel = pos_pid.calculate(capture.degrees(), capture.degrees_per_second()); + + // Limit the velocity between user defined limits, and set it as the new setpoint of the velocity PID + vel_pid.setpoint = CLAMP(vel, -MAX_SPEED, MAX_SPEED); + + // Calculate the acceleration to apply to the motor to move it closer to the velocity setpoint + float accel = vel_pid.calculate(capture.revolutions_per_second()); + + // Accelerate or decelerate the motor + m.speed(m.speed() + (accel * UPDATE_RATE)); + + // Print out the current motor values and their setpoints, + // but only for the first few updates and only every multiple + if(update < (uint)(PRINT_WINDOW * UPDATES) && print_count == 0) { + printf("Pos = %f, ", capture.degrees()); + printf("Pos SP = %f, ", pos_pid.setpoint); + printf("Vel = %f, ", capture.revolutions_per_second() * SPD_PRINT_SCALE); + printf("Vel SP = %f, ", vel_pid.setpoint * SPD_PRINT_SCALE); + printf("Accel = %f, ", accel * ACC_PRINT_SCALE); + printf("Speed = %f\n", m.speed()); } - } - // Do a stepped sweep - for(auto j = 0u; j < SWEEPS; j++) { - for(auto i = 0u; i < STEPS; i++) { - m.to_percent(i, 0, STEPS, 0.0 - SWEEP_EXTENT, SWEEP_EXTENT); - sleep_ms(STEPS_INTERVAL_MS); - } - for(auto i = 0u; i < STEPS; i++) { - m.to_percent(i, STEPS, 0, 0.0 - SWEEP_EXTENT, SWEEP_EXTENT); - sleep_ms(STEPS_INTERVAL_MS); + // Increment the print count, and wrap it + print_count = (print_count + 1) % PRINT_DIVIDER; + + update++; // Move along in time + + // Have we reached the end of this time window? + if(update >= (uint)(MOVEMENT_WINDOW * UPDATES)) { + update = 0; // Reset the counter + + // Set the new position setpoint to be the inverse of the current setpoint + pos_pid.setpoint = 0.0 - pos_pid.setpoint; } + + sleep_ms(UPDATE_RATE * 1000.0f); } // Disable the motor diff --git a/examples/motor2040/motor2040_position_tuning.cpp b/examples/motor2040/motor2040_position_tuning.cpp index 2c20beea..4710a359 100644 --- a/examples/motor2040/motor2040_position_tuning.cpp +++ b/examples/motor2040/motor2040_position_tuning.cpp @@ -55,9 +55,9 @@ constexpr float SPD_PRINT_SCALE = 10.0f; // Driving Speed multipler constexpr float POSITION_EXTENT = 90.0f; // PID values -constexpr float POS_KP = 0.14; // Position proportional (P) gain -constexpr float POS_KI = 0.0; // Position integral (I) gain -constexpr float POS_KD = 0.0022; // Position derivative (D) gain +constexpr float POS_KP = 0.14f; // Position proportional (P) gain +constexpr float POS_KI = 0.0f; // Position integral (I) gain +constexpr float POS_KD = 0.002f; // Position derivative (D) gain // Create a motor and set its direction and speed scale @@ -104,7 +104,7 @@ int main() { // Print out the current motor values and their setpoints, // but only for the first few updates and only every multiple - if(update < (PRINT_WINDOW * UPDATES) && print_count == 0) { + if(update < (uint)(PRINT_WINDOW * UPDATES) && print_count == 0) { printf("Pos = %f, ", capture.degrees()); printf("Pos SP = %f, ", pos_pid.setpoint); printf("Speed = %f\n", m.speed() * SPD_PRINT_SCALE); @@ -116,11 +116,11 @@ int main() { update++; // Move along in time // Have we reached the end of this time window? - if(update >= (MOVEMENT_WINDOW * UPDATES)){ + if(update >= (uint)(MOVEMENT_WINDOW * UPDATES)) { update = 0; // Reset the counter // Set the new position setpoint to be the inverse of the current setpoint - pos_pid.setpoint = 0.0 - pos_pid.setpoint; + pos_pid.setpoint = 0.0f - pos_pid.setpoint; } sleep_ms(UPDATE_RATE * 1000.0f); diff --git a/examples/motor2040/motor2040_quad_position_wave.cpp b/examples/motor2040/motor2040_quad_position_wave.cpp index 240cc936..67895d2c 100644 --- a/examples/motor2040/motor2040_quad_position_wave.cpp +++ b/examples/motor2040/motor2040_quad_position_wave.cpp @@ -1,6 +1,9 @@ +#include #include "pico/stdlib.h" #include "motor2040.hpp" +#include "button.hpp" +#include "pid.hpp" /* A demonstration of driving all four of Motor 2040's motor outputs between @@ -9,67 +12,158 @@ positions, with the help of their attached encoders and PID control. Press "Boot" to exit the program. */ +using namespace plasma; using namespace motor; +using namespace encoder; -// How many sweeps of the motor to perform -const uint SWEEPS = 3; +// The gear ratio of the motor +constexpr float GEAR_RATIO = 50.0f; -// The number of discrete sweep steps -const uint STEPS = 10; +// The counts per revolution of the motor's output shaft +constexpr float COUNTS_PER_REV = MMME_CPR * GEAR_RATIO; -// The time in milliseconds between each step of the sequence -const uint STEPS_INTERVAL_MS = 500; +// The scaling to apply to the motor's speed to match its real-world speed +float SPEED_SCALE = 5.4f; -// How far from zero to move the motor when sweeping -constexpr float SWEEP_EXTENT = 90.0f; +// How many times to update the motor per second +const uint UPDATES = 100; +constexpr float UPDATE_RATE = 1.0f / (float)UPDATES; + +// The time to travel between each random value +constexpr float TIME_FOR_EACH_MOVE = 2.0f; +const uint UPDATES_PER_MOVE = TIME_FOR_EACH_MOVE * UPDATES; + +// How many of the updates should be printed (i.e. 2 would be every other update) +const uint PRINT_DIVIDER = 4; + +// The brightness of the RGB LED +constexpr float BRIGHTNESS = 0.4f; + +// PID values +constexpr float POS_KP = 0.14f; // Position proportional (P) gain +constexpr float POS_KI = 0.0f; // Position integral (I) gain +constexpr float POS_KD = 0.002f; // Position derivative (D) gain -// Create a motor on pin 0 and 1 -Motor m = Motor(motor2040::MOTOR_A); +// Create an array of motor pointers +const pin_pair motor_pins[] = {motor2040::MOTOR_A, motor2040::MOTOR_B, + motor2040::MOTOR_C, motor2040::MOTOR_D}; +const uint NUM_MOTORS = count_of(motor_pins); +Motor *motors[NUM_MOTORS]; + +// Create an array of encoder pointers +const pin_pair encoder_pins[] = {motor2040::ENCODER_A, motor2040::ENCODER_B, + motor2040::ENCODER_C, motor2040::ENCODER_D}; +const char* ENCODER_NAMES[] = {"A", "B", "C", "D"}; +const uint NUM_ENCODERS = count_of(encoder_pins); +Encoder *encoders[NUM_ENCODERS]; + +// Create the LED, using PIO 1 and State Machine 0 +WS2812 led(motor2040::NUM_LEDS, pio1, 0, motor2040::LED_DATA); + +// Create the user button +Button user_sw(motor2040::USER_SW); + +// Create an array of PID pointers +PID pos_pids[NUM_MOTORS]; int main() { stdio_init_all(); - // Initialise the motor - m.init(); + // Fill the arrays of motors, encoders, and pids, and initialise them + for(auto i = 0u; i < NUM_MOTORS; i++) { + motors[i] = new Motor(motor_pins[i], NORMAL_DIR, SPEED_SCALE); + motors[i]->init(); - // Enable the motor - m.enable(); - sleep_ms(2000); + encoders[i] = new Encoder(pio0, i, encoder_pins[i], PIN_UNUSED, NORMAL_DIR, COUNTS_PER_REV, true); + encoders[i]->init(); - // Go at full neative - m.full_negative(); - sleep_ms(2000); - - // Go at full positive - m.full_positive(); - sleep_ms(2000); - - // Stop the motor - m.stop(); - sleep_ms(2000); - - // Do a sine sweep - for(auto j = 0u; j < SWEEPS; j++) { - for(auto i = 0u; i < 360; i++) { - m.speed(sin(((float)i * (float)M_PI) / 180.0f) * SWEEP_EXTENT); - sleep_ms(20); - } + pos_pids[i] = PID(POS_KP, POS_KI, POS_KD, UPDATE_RATE); } - // Do a stepped sweep - for(auto j = 0u; j < SWEEPS; j++) { - for(auto i = 0u; i < STEPS; i++) { - m.to_percent(i, 0, STEPS, 0.0 - SWEEP_EXTENT, SWEEP_EXTENT); - sleep_ms(STEPS_INTERVAL_MS); - } - for(auto i = 0u; i < STEPS; i++) { - m.to_percent(i, STEPS, 0, 0.0 - SWEEP_EXTENT, SWEEP_EXTENT); - sleep_ms(STEPS_INTERVAL_MS); - } + // Reverse the direction of the B and D motors and encoders + motors[1]->direction(REVERSED_DIR); + motors[3]->direction(REVERSED_DIR); + encoders[1]->direction(REVERSED_DIR); + encoders[3]->direction(REVERSED_DIR); + + // Start updating the LED + led.start(); + + // Enable all motors + for(auto i = 0u; i < NUM_MOTORS; i++) { + motors[i]->enable(); } - // Disable the motor - m.disable(); + uint update = 0; + uint print_count = 0; + + // Set the initial and end values + float start_value = 0.0f; + float end_value = 270.0f; + + Encoder::Capture captures[NUM_MOTORS]; + + // Continually move the motor until the user button is pressed + while(!user_sw.raw()) { + + // Capture the state of all the encoders + for(auto i = 0u; i < NUM_MOTORS; i++) { + captures[i] = encoders[i]->capture(); + } + + // Calculate how far along this movement to be + float percent_along = (float)update / (float)UPDATES_PER_MOVE; + + for(auto i = 0u; i < NUM_MOTORS; i++) { + // Move the motor between values using cosine + pos_pids[i].setpoint = (((-cosf(percent_along * (float)M_PI) + 1.0) / 2.0) * (end_value - start_value)) + start_value; + + // Calculate the velocity to move the motor closer to the position setpoint + float vel = pos_pids[i].calculate(captures[i].degrees(), captures[i].degrees_per_second()); + + // Set the new motor driving speed + motors[i]->speed(vel); + } + + // Update the LED + led.set_hsv(0, percent_along, 1.0f, BRIGHTNESS); + + // Print out the current motor values and their setpoints, but only on every multiple + if(print_count == 0) { + for(auto i = 0u; i < NUM_ENCODERS; i++) { + printf("%s = %f, ", ENCODER_NAMES[i], captures[i].degrees()); + } + printf("\n"); + } + + // Increment the print count, and wrap it + print_count = (print_count + 1) % PRINT_DIVIDER; + + update++; // Move along in time + + // Have we reached the end of this movement? + if(update >= UPDATES_PER_MOVE) { + update = 0; // Reset the counter + + // Swap the start and end values + float temp = start_value; + start_value = end_value; + end_value = temp; + } + + sleep_ms(UPDATE_RATE * 1000.0f); + } + + // Stop all the motors + for(auto m = 0u; m < NUM_MOTORS; m++) { + motors[m]->disable(); + } + + // Turn off the LED + led.clear(); + + // Sleep a short time so the clear takes effect + sleep_ms(100); } diff --git a/examples/motor2040/motor2040_quad_velocity_sequence.cpp b/examples/motor2040/motor2040_quad_velocity_sequence.cpp index e612f655..5b3825f0 100644 --- a/examples/motor2040/motor2040_quad_velocity_sequence.cpp +++ b/examples/motor2040/motor2040_quad_velocity_sequence.cpp @@ -1,6 +1,9 @@ +#include #include "pico/stdlib.h" #include "motor2040.hpp" +#include "button.hpp" +#include "pid.hpp" /* A demonstration of driving all four of Motor 2040's motor outputs through a @@ -10,66 +13,197 @@ Press "Boot" to exit the program. */ using namespace motor; +using namespace encoder; -// How many sweeps of the motor to perform -const uint SWEEPS = 3; +enum Wheels { + FL = 2, + FR = 3, + RL = 1, + RR = 0, +}; -// The number of discrete sweep steps -const uint STEPS = 10; +// The gear ratio of the motor +constexpr float GEAR_RATIO = 50.0f; -// The time in milliseconds between each step of the sequence -const uint STEPS_INTERVAL_MS = 500; +// The counts per revolution of the motor's output shaft +constexpr float COUNTS_PER_REV = MMME_CPR * GEAR_RATIO; -// How far from zero to move the motor when sweeping -constexpr float SWEEP_EXTENT = 90.0f; +// The scaling to apply to the motor's speed to match its real-world speed +float SPEED_SCALE = 5.4f; + +// How many times to update the motor per second +const uint UPDATES = 100; +constexpr float UPDATE_RATE = 1.0f / (float)UPDATES; + +// The time to travel between each random value +constexpr float TIME_FOR_EACH_MOVE = 2.0f; +const uint UPDATES_PER_MOVE = TIME_FOR_EACH_MOVE * UPDATES; + +// How many of the updates should be printed (i.e. 2 would be every other update) +const uint PRINT_DIVIDER = 4; + +// The speed to drive the wheels at +constexpr float DRIVING_SPEED = 1.0f; + +// PID values +constexpr float VEL_KP = 30.0f; // Velocity proportional (P) gain +constexpr float VEL_KI = 0.0f; // Velocity integral (I) gain +constexpr float VEL_KD = 0.4f; // Velocity derivative (D) gain -// Create a motor on pin 0 and 1 -Motor m = Motor(motor2040::MOTOR_A); +// Create an array of motor pointers +const pin_pair motor_pins[] = {motor2040::MOTOR_A, motor2040::MOTOR_B, + motor2040::MOTOR_C, motor2040::MOTOR_D}; +const uint NUM_MOTORS = count_of(motor_pins); +Motor *motors[NUM_MOTORS]; + +// Create an array of encoder pointers +const pin_pair encoder_pins[] = {motor2040::ENCODER_A, motor2040::ENCODER_B, + motor2040::ENCODER_C, motor2040::ENCODER_D}; +const char* ENCODER_NAMES[] = {"RR", "RL", "FL", "FR"}; +const uint NUM_ENCODERS = count_of(encoder_pins); +Encoder *encoders[NUM_ENCODERS]; + +// Create the user button +Button user_sw(motor2040::USER_SW); + +// Create an array of PID pointers +PID vel_pids[NUM_MOTORS]; + + +// Helper functions for driving in common directions +void drive_forward(float speed) { + vel_pids[FL].setpoint = speed; + vel_pids[FR].setpoint = speed; + vel_pids[RL].setpoint = speed; + vel_pids[RR].setpoint = speed; +} + +void turn_right(float speed) { + vel_pids[FL].setpoint = speed; + vel_pids[FR].setpoint = -speed; + vel_pids[RL].setpoint = speed; + vel_pids[RR].setpoint = -speed; +} + +void strafe_right(float speed) { + vel_pids[FL].setpoint = speed; + vel_pids[FR].setpoint = -speed; + vel_pids[RL].setpoint = -speed; + vel_pids[RR].setpoint = speed; +} + +void stop() { + vel_pids[FL].setpoint = 0.0f; + vel_pids[FR].setpoint = 0.0f; + vel_pids[RL].setpoint = 0.0f; + vel_pids[RR].setpoint = 0.0f; +} int main() { stdio_init_all(); - // Initialise the motor - m.init(); + // Fill the arrays of motors, encoders, and pids, and initialise them + for(auto i = 0u; i < NUM_MOTORS; i++) { + motors[i] = new Motor(motor_pins[i], NORMAL_DIR, SPEED_SCALE); + motors[i]->init(); - // Enable the motor - m.enable(); - sleep_ms(2000); + encoders[i] = new Encoder(pio0, i, encoder_pins[i], PIN_UNUSED, NORMAL_DIR, COUNTS_PER_REV, true); + encoders[i]->init(); - // Go at full neative - m.full_negative(); - sleep_ms(2000); - - // Go at full positive - m.full_positive(); - sleep_ms(2000); - - // Stop the motor - m.stop(); - sleep_ms(2000); - - // Do a sine sweep - for(auto j = 0u; j < SWEEPS; j++) { - for(auto i = 0u; i < 360; i++) { - m.speed(sin(((float)i * (float)M_PI) / 180.0f) * SWEEP_EXTENT); - sleep_ms(20); - } + vel_pids[i] = PID(VEL_KP, VEL_KI, VEL_KD, UPDATE_RATE); } - // Do a stepped sweep - for(auto j = 0u; j < SWEEPS; j++) { - for(auto i = 0u; i < STEPS; i++) { - m.to_percent(i, 0, STEPS, 0.0 - SWEEP_EXTENT, SWEEP_EXTENT); - sleep_ms(STEPS_INTERVAL_MS); - } - for(auto i = 0u; i < STEPS; i++) { - m.to_percent(i, STEPS, 0, 0.0 - SWEEP_EXTENT, SWEEP_EXTENT); - sleep_ms(STEPS_INTERVAL_MS); - } + // Reverse the direction of the B and D motors and encoders + motors[FL]->direction(REVERSED_DIR); + motors[RL]->direction(REVERSED_DIR); + encoders[FL]->direction(REVERSED_DIR); + encoders[RL]->direction(REVERSED_DIR); + + // Enable all motors + for(auto i = 0u; i < NUM_MOTORS; i++) { + motors[i]->enable(); } - // Disable the motor - m.disable(); + uint update = 0; + uint print_count = 0; + uint sequence = 0; + + Encoder::Capture captures[NUM_MOTORS]; + + // Continually move the motor until the user button is pressed + while(!user_sw.raw()) { + + // Capture the state of all the encoders + for(auto i = 0u; i < NUM_MOTORS; i++) { + captures[i] = encoders[i]->capture(); + } + + for(auto i = 0u; i < NUM_MOTORS; i++) { + // Calculate the acceleration to apply to the motor to move it closer to the velocity setpoint + float accel = vel_pids[i].calculate(captures[i].revolutions_per_second()); + + // Accelerate or decelerate the motor + motors[i]->speed(motors[i]->speed() + (accel * UPDATE_RATE)); + } + + // Print out the current motor values and their setpoints, but only on every multiple + if(print_count == 0) { + for(auto i = 0u; i < NUM_ENCODERS; i++) { + printf("%s = %f, ", ENCODER_NAMES[i], captures[i].revolutions_per_second()); + } + printf("\n"); + } + + // Increment the print count, and wrap it + print_count = (print_count + 1) % PRINT_DIVIDER; + + update++; // Move along in time + + // Have we reached the end of this movement? + if(update >= UPDATES_PER_MOVE) { + update = 0; // Reset the counter + + // Move on to the next part of the sequence + sequence += 1; + + // Loop the sequence back around + if(sequence >= 7) { + sequence = 0; + } + } + + // Set the motor speeds, based on the sequence + switch(sequence) { + case 0: + drive_forward(DRIVING_SPEED); + break; + case 1: + drive_forward(-DRIVING_SPEED); + break; + case 2: + turn_right(DRIVING_SPEED); + break; + case 3: + turn_right(-DRIVING_SPEED); + break; + case 4: + strafe_right(DRIVING_SPEED); + break; + case 5: + strafe_right(-DRIVING_SPEED); + break; + default: + stop(); + break; + } + + sleep_ms(UPDATE_RATE * 1000.0f); + } + + // Stop all the motors + for(auto m = 0u; m < NUM_MOTORS; m++) { + motors[m]->disable(); + } } diff --git a/examples/motor2040/motor2040_reactive_encoder.cmake b/examples/motor2040/motor2040_reactive_encoder.cmake index c5173529..1c943253 100644 --- a/examples/motor2040/motor2040_reactive_encoder.cmake +++ b/examples/motor2040/motor2040_reactive_encoder.cmake @@ -4,6 +4,8 @@ add_executable(${OUTPUT_NAME} motor2040_reactive_encoder.cpp) target_link_libraries(${OUTPUT_NAME} pico_stdlib motor2040 + button + pid ) # enable usb output diff --git a/examples/motor2040/motor2040_reactive_encoder.cpp b/examples/motor2040/motor2040_reactive_encoder.cpp index bde6948b..8a2317f2 100644 --- a/examples/motor2040/motor2040_reactive_encoder.cpp +++ b/examples/motor2040/motor2040_reactive_encoder.cpp @@ -1,6 +1,9 @@ +#include #include "pico/stdlib.h" #include "motor2040.hpp" +#include "button.hpp" +#include "pid.hpp" /* A demonstration of how a motor with an encoder can be used @@ -10,67 +13,153 @@ force-feedback for arbitrary detents and end stops. Press "Boot" to exit the program. */ +using namespace plasma; using namespace motor; +using namespace encoder; -// How many sweeps of the motor to perform -const uint SWEEPS = 3; +// The pins of the motor being profiled +const pin_pair MOTOR_PINS = motor2040::MOTOR_A; -// The number of discrete sweep steps -const uint STEPS = 10; +// The pins of the encoder attached to the profiled motor +const pin_pair ENCODER_PINS = motor2040::ENCODER_A; -// The time in milliseconds between each step of the sequence -const uint STEPS_INTERVAL_MS = 500; +// The gear ratio of the motor +constexpr float GEAR_RATIO = 50.0f; -// How far from zero to move the motor when sweeping -constexpr float SWEEP_EXTENT = 90.0f; +// The counts per revolution of the motor's output shaft +constexpr float COUNTS_PER_REV = MMME_CPR * GEAR_RATIO; + +// The direction to spin the motor in. NORMAL_DIR (0), REVERSED_DIR (1) +const Direction DIRECTION = NORMAL_DIR; + +// The scaling to apply to the motor's speed to match its real-world speed +float SPEED_SCALE = 5.4f; + +// How many times to update the motor per second +const uint UPDATES = 100; +constexpr float UPDATE_RATE = 1.0f / (float)UPDATES; + +// How many of the updates should be printed (i.e. 2 would be every other update) +const uint PRINT_DIVIDER = 4; + +// Multipliers for the different printed values, so they appear nicely on the Thonny plotter +constexpr float SPD_PRINT_SCALE = 20.0f; // Driving Speed multipler + +// The size (in degrees) of each detent region +constexpr float DETENT_SIZE = 20.0f; + +// The minimum detent that can be counted to +const int MIN_DETENT = -12; + +// The maximum detent that can be counted to +const int MAX_DETENT = +12; + +// The maximum drive force (as a percent) to apply when crossing detents +constexpr float MAX_DRIVE_PERCENT = 0.5f; + +// The brightness of the RGB LED +constexpr float BRIGHTNESS = 0.4f; + +// PID values +constexpr float POS_KP = 0.14f; // Position proportional (P) gain +constexpr float POS_KI = 0.0f; // Position integral (I) gain +constexpr float POS_KD = 0.002f; // Position derivative (D) gain -// Create a motor on pin 0 and 1 -Motor m = Motor(motor2040::MOTOR_A); +// Create a motor and set its direction and speed scale +Motor m = Motor(MOTOR_PINS, DIRECTION, SPEED_SCALE); + +// Create an encoder and set its direction and counts per rev, using PIO 0 and State Machine 0 +Encoder enc = Encoder(pio0, 0, ENCODER_PINS, PIN_UNUSED, DIRECTION, COUNTS_PER_REV, true); + +// Create the user button +Button user_sw(motor2040::USER_SW); + +// Create the LED, using PIO 1 and State Machine 0 +WS2812 led(motor2040::NUM_LEDS, pio1, 0, motor2040::LED_DATA); + +// Create PID object for position control +PID pos_pid = PID(POS_KP, POS_KI, POS_KD, UPDATE_RATE); + +int current_detent = 0; + + +// Function to deal with a detent change +void detent_change(int change) { + // Update the current detent and pid setpoint + current_detent += change; + + // Update the motor position setpoint + pos_pid.setpoint = (current_detent * DETENT_SIZE); + printf("Detent = %d\n", current_detent); + + // Convert the current detent to a hue and set the onboard led to it + float hue = (float)(current_detent - MIN_DETENT) / (float)(MAX_DETENT - MIN_DETENT); + led.set_hsv(0, hue, 1.0, BRIGHTNESS); +} int main() { stdio_init_all(); - // Initialise the motor + // Initialise the motor and encoder m.init(); + enc.init(); + + // Start updating the LED + led.start(); // Enable the motor m.enable(); - sleep_ms(2000); - // Go at full neative - m.full_negative(); - sleep_ms(2000); + // Call the function once to set the setpoint and print the value + detent_change(0); - // Go at full positive - m.full_positive(); - sleep_ms(2000); + // Continually move the motor until the user button is pressed + while(!user_sw.raw()) { - // Stop the motor - m.stop(); - sleep_ms(2000); + // Capture the state of the encoder + Encoder::Capture capture = enc.capture(); - // Do a sine sweep - for(auto j = 0u; j < SWEEPS; j++) { - for(auto i = 0u; i < 360; i++) { - m.speed(sin(((float)i * (float)M_PI) / 180.0f) * SWEEP_EXTENT); - sleep_ms(20); + // Get the current detent's centre angle + float detent_angle = ((float)current_detent * DETENT_SIZE); + + // Is the current angle above the region of this detent? + if(capture.degrees() > detent_angle + (DETENT_SIZE / 2)) { + // Is there another detent we can move to? + if(current_detent < MAX_DETENT) { + detent_change(1); // Increment to the next detent + } + } + // Is the current angle below the region of this detent? + else if(capture.degrees() < detent_angle - (DETENT_SIZE / 2)) { + // Is there another detent we can move to? + if(current_detent > MIN_DETENT) { + detent_change(-1); // Decrement to the next detent + } } - } - // Do a stepped sweep - for(auto j = 0u; j < SWEEPS; j++) { - for(auto i = 0u; i < STEPS; i++) { - m.to_percent(i, 0, STEPS, 0.0 - SWEEP_EXTENT, SWEEP_EXTENT); - sleep_ms(STEPS_INTERVAL_MS); - } - for(auto i = 0u; i < STEPS; i++) { - m.to_percent(i, STEPS, 0, 0.0 - SWEEP_EXTENT, SWEEP_EXTENT); - sleep_ms(STEPS_INTERVAL_MS); + // Calculate the velocity to move the motor closer to the position setpoint + float vel = pos_pid.calculate(capture.degrees(), capture.degrees_per_second()); + + // If the current angle is within the detent range, limit the max vel + // (aka feedback force) that the user will feel when turning the motor between detents + if((capture.degrees() >= MIN_DETENT * DETENT_SIZE) && (capture.degrees() <= MAX_DETENT * DETENT_SIZE)) { + vel = CLAMP(vel, -MAX_DRIVE_PERCENT, MAX_DRIVE_PERCENT); } + + // Set the new motor driving speed + m.speed(vel); + + sleep_ms(UPDATE_RATE * 1000.0f); } // Disable the motor m.disable(); + + // Turn off the LED + led.clear(); + + // Sleep a short time so the clear takes effect + sleep_ms(100); } diff --git a/examples/motor2040/motor2040_single_motor.cpp b/examples/motor2040/motor2040_single_motor.cpp index bafe03a8..bda6ed44 100644 --- a/examples/motor2040/motor2040_single_motor.cpp +++ b/examples/motor2040/motor2040_single_motor.cpp @@ -18,7 +18,7 @@ const uint STEPS = 10; const uint STEPS_INTERVAL_MS = 500; // How far from zero to move the motor when sweeping -constexpr float SPEED_EXTENT = 10.0f; +constexpr float SPEED_EXTENT = 1.0f; // Create a motor diff --git a/examples/motor2040/motor2040_velocity_control.cmake b/examples/motor2040/motor2040_velocity_control.cmake index be841677..d4ea113b 100644 --- a/examples/motor2040/motor2040_velocity_control.cmake +++ b/examples/motor2040/motor2040_velocity_control.cmake @@ -4,6 +4,8 @@ add_executable(${OUTPUT_NAME} motor2040_velocity_control.cpp) target_link_libraries(${OUTPUT_NAME} pico_stdlib motor2040 + button + pid ) # enable usb output diff --git a/examples/motor2040/motor2040_velocity_control.cpp b/examples/motor2040/motor2040_velocity_control.cpp index 722c7639..09e18509 100644 --- a/examples/motor2040/motor2040_velocity_control.cpp +++ b/examples/motor2040/motor2040_velocity_control.cpp @@ -1,6 +1,9 @@ +#include #include "pico/stdlib.h" #include "motor2040.hpp" +#include "button.hpp" +#include "pid.hpp" /* An example of how to drive a motor smoothly between random speeds, @@ -10,64 +13,138 @@ Press "Boot" to exit the program. */ using namespace motor; +using namespace encoder; -// How many sweeps of the motor to perform -const uint SWEEPS = 3; +// The pins of the motor being profiled +const pin_pair MOTOR_PINS = motor2040::MOTOR_A; -// The number of discrete sweep steps -const uint STEPS = 10; +// The pins of the encoder attached to the profiled motor +const pin_pair ENCODER_PINS = motor2040::ENCODER_A; -// The time in milliseconds between each step of the sequence -const uint STEPS_INTERVAL_MS = 500; +// The gear ratio of the motor +constexpr float GEAR_RATIO = 50.0f; -// How far from zero to move the motor when sweeping -constexpr float SWEEP_EXTENT = 90.0f; +// The counts per revolution of the motor's output shaft +constexpr float COUNTS_PER_REV = MMME_CPR * GEAR_RATIO; + +// The direction to spin the motor in. NORMAL_DIR (0), REVERSED_DIR (1) +const Direction DIRECTION = NORMAL_DIR; + +// The scaling to apply to the motor's speed to match its real-world speed +float SPEED_SCALE = 5.4f; + +// How many times to update the motor per second +const uint UPDATES = 100; +constexpr float UPDATE_RATE = 1.0f / (float)UPDATES; + +// The time to travel between each random value, in seconds +constexpr float TIME_FOR_EACH_MOVE = 1.0f; +const uint UPDATES_PER_MOVE = TIME_FOR_EACH_MOVE * UPDATES; + +// How many of the updates should be printed (i.e. 2 would be every other update) +const uint PRINT_DIVIDER = 4; + +// Multipliers for the different printed values, so they appear nicely on the Thonny plotter +constexpr float ACC_PRINT_SCALE = 0.05f; // Acceleration multiplier + +// How far from zero to drive the motor at, in revolutions per second +constexpr float VELOCITY_EXTENT = 3.0f; + +// The interpolating mode between setpoints. STEP (0), LINEAR (1), COSINE (2) +const uint INTERP_MODE = 2; + +// PID values +constexpr float VEL_KP = 30.0f; // Velocity proportional (P) gain +constexpr float VEL_KI = 0.0f; // Velocity integral (I) gain +constexpr float VEL_KD = 0.4f; // Velocity derivative (D) gain -// Create a motor on pin 0 and 1 -Motor m = Motor(motor2040::MOTOR_A); +// Create a motor and set its direction and speed scale +Motor m = Motor(MOTOR_PINS, DIRECTION, SPEED_SCALE); + +// Create an encoder and set its direction and counts per rev, using PIO 0 and State Machine 0 +Encoder enc = Encoder(pio0, 0, ENCODER_PINS, PIN_UNUSED, DIRECTION, COUNTS_PER_REV, true); + +// Create the user button +Button user_sw(motor2040::USER_SW); + +// Create PID object for velocity control +PID vel_pid = PID(VEL_KP, VEL_KI, VEL_KD, UPDATE_RATE); int main() { stdio_init_all(); - // Initialise the motor + // Initialise the motor and encoder m.init(); + enc.init(); // Enable the motor m.enable(); - sleep_ms(2000); - // Go at full neative - m.full_negative(); - sleep_ms(2000); - // Go at full positive - m.full_positive(); - sleep_ms(2000); + uint update = 0; + uint print_count = 0; - // Stop the motor - m.stop(); - sleep_ms(2000); + // Set the initial value and create a random end value between the extents + float start_value = 0.0f; + float end_value = (((float)rand() / (float)RAND_MAX) * (VELOCITY_EXTENT * 2.0f)) - VELOCITY_EXTENT; - // Do a sine sweep - for(auto j = 0u; j < SWEEPS; j++) { - for(auto i = 0u; i < 360; i++) { - m.speed(sin(((float)i * (float)M_PI) / 180.0f) * SWEEP_EXTENT); - sleep_ms(20); + // Continually move the motor until the user button is pressed + while(!user_sw.raw()) { + + // Capture the state of the encoder + Encoder::Capture capture = enc.capture(); + + // Calculate how far along this movement to be + float percent_along = (float)update / (float)UPDATES_PER_MOVE; + + switch(INTERP_MODE) { + case 0: + // Move the motor instantly to the end value + vel_pid.setpoint = end_value; + break; + + case 2: + // Move the motor between values using cosine + vel_pid.setpoint = (((-cosf(percent_along * (float)M_PI) + 1.0) / 2.0) * (end_value - start_value)) + start_value; + break; + + case 1: + default: + // Move the motor linearly between values + vel_pid.setpoint = (percent_along * (end_value - start_value)) + start_value; } - } - // Do a stepped sweep - for(auto j = 0u; j < SWEEPS; j++) { - for(auto i = 0u; i < STEPS; i++) { - m.to_percent(i, 0, STEPS, 0.0 - SWEEP_EXTENT, SWEEP_EXTENT); - sleep_ms(STEPS_INTERVAL_MS); + // Calculate the acceleration to apply to the motor to move it closer to the velocity setpoint + float accel = vel_pid.calculate(capture.revolutions_per_second()); + + // Accelerate or decelerate the motor + m.speed(m.speed() + (accel * UPDATE_RATE)); + + // Print out the current motor values and their setpoints, but only on every multiple + if(print_count == 0) { + printf("Vel = %f, ", capture.revolutions_per_second()); + printf("Vel SP = %f, ", vel_pid.setpoint); + printf("Accel = %f, ", accel * ACC_PRINT_SCALE); + printf("Speed = %f\n", m.speed()); } - for(auto i = 0u; i < STEPS; i++) { - m.to_percent(i, STEPS, 0, 0.0 - SWEEP_EXTENT, SWEEP_EXTENT); - sleep_ms(STEPS_INTERVAL_MS); + + // Increment the print count, and wrap it + print_count = (print_count + 1) % PRINT_DIVIDER; + + update++; // Move along in time + + // Have we reached the end of this movement? + if(update >= UPDATES_PER_MOVE) { + update = 0; // Reset the counter + + // Set the start as the last end and create a new random end value + start_value = end_value; + end_value = (((float)rand() / (float)RAND_MAX) * (VELOCITY_EXTENT * 2.0f)) - VELOCITY_EXTENT; } + + sleep_ms(UPDATE_RATE * 1000.0f); } // Disable the motor diff --git a/examples/motor2040/motor2040_velocity_tuning.cmake b/examples/motor2040/motor2040_velocity_tuning.cmake index 076cfc92..a21dd16e 100644 --- a/examples/motor2040/motor2040_velocity_tuning.cmake +++ b/examples/motor2040/motor2040_velocity_tuning.cmake @@ -5,6 +5,7 @@ target_link_libraries(${OUTPUT_NAME} pico_stdlib motor2040 button + pid ) # enable usb output diff --git a/examples/motor2040/motor2040_velocity_tuning.cpp b/examples/motor2040/motor2040_velocity_tuning.cpp index 7ee2fa93..1348fe54 100644 --- a/examples/motor2040/motor2040_velocity_tuning.cpp +++ b/examples/motor2040/motor2040_velocity_tuning.cpp @@ -1,6 +1,9 @@ +#include #include "pico/stdlib.h" #include "motor2040.hpp" +#include "button.hpp" +#include "pid.hpp" /* A program to aid in the discovery and tuning of motor PID @@ -12,64 +15,116 @@ Press "Boot" to exit the program. */ using namespace motor; +using namespace encoder; -// How many sweeps of the motor to perform -const uint SWEEPS = 2; +// The pins of the motor being profiled +const pin_pair MOTOR_PINS = motor2040::MOTOR_A; -// The number of discrete sweep steps -const uint STEPS = 10; +// The pins of the encoder attached to the profiled motor +const pin_pair ENCODER_PINS = motor2040::ENCODER_A; -// The time in milliseconds between each step of the sequence -const uint STEPS_INTERVAL_MS = 500; +// The gear ratio of the motor +constexpr float GEAR_RATIO = 50.0f; -// How far from zero to drive the motor when sweeping -constexpr float SWEEP_EXTENT = 90.0f; +// The counts per revolution of the motor's output shaft +constexpr float COUNTS_PER_REV = MMME_CPR * GEAR_RATIO; + +// The direction to spin the motor in. NORMAL_DIR (0), REVERSED_DIR (1) +const Direction DIRECTION = NORMAL_DIR; + +// The scaling to apply to the motor's speed to match its real-world speed +float SPEED_SCALE = 5.4f; + +// How many times to update the motor per second +const uint UPDATES = 100; +constexpr float UPDATE_RATE = 1.0f / (float)UPDATES; + +// The time (in seconds) after a new setpoint, to display print out motor values +constexpr float PRINT_WINDOW = 0.25f; + +// The time (in seconds) between each new setpoint being set +constexpr float MOVEMENT_WINDOW = 2.0f; + +// How many of the updates should be printed (i.e. 2 would be every other update) +const uint PRINT_DIVIDER = 1; + +// Multipliers for the different printed values, so they appear nicely on the Thonny plotter +constexpr float ACC_PRINT_SCALE = 0.01f; // Acceleration multiplier + +// How far from zero to drive the motor at, in revolutions per second +constexpr float VELOCITY_EXTENT = 3.0f; + +// PID values +constexpr float VEL_KP = 30.0f; // Velocity proportional (P) gain +constexpr float VEL_KI = 0.0f; // Velocity integral (I) gain +constexpr float VEL_KD = 0.4f; // Velocity derivative (D) gain -// Create a motor on pin 0 and 1 -Motor m = Motor(motor2040::MOTOR_A); +// Create a motor and set its direction and speed scale +Motor m = Motor(MOTOR_PINS, DIRECTION, SPEED_SCALE); + +// Create an encoder and set its direction and counts per rev, using PIO 0 and State Machine 0 +Encoder enc = Encoder(pio0, 0, ENCODER_PINS, PIN_UNUSED, DIRECTION, COUNTS_PER_REV, true); + +// Create the user button +Button user_sw(motor2040::USER_SW); + +// Create PID object for velocity control +PID vel_pid = PID(VEL_KP, VEL_KI, VEL_KD, UPDATE_RATE); int main() { stdio_init_all(); - // Initialise the motor + // Initialise the motor and encoder m.init(); + enc.init(); // Enable the motor m.enable(); - sleep_ms(2000); - // Go at full neative - m.full_negative(); - sleep_ms(2000); + // Set the initial setpoint velocity + vel_pid.setpoint = VELOCITY_EXTENT; - // Go at full positive - m.full_positive(); - sleep_ms(2000); - // Stop the motor - m.stop(); - sleep_ms(2000); + uint update = 0; + uint print_count = 0; - // Do a sine sweep - for(auto j = 0u; j < SWEEPS; j++) { - for(auto i = 0u; i < 360; i++) { - m.speed(sin(((float)i * (float)M_PI) / 180.0f) * SWEEP_EXTENT); - sleep_ms(20); + // Continually move the motor until the user button is pressed + while(!user_sw.raw()) { + + // Capture the state of the encoder + Encoder::Capture capture = enc.capture(); + + // Calculate the acceleration to apply to the motor to move it closer to the velocity setpoint + float accel = vel_pid.calculate(capture.revolutions_per_second()); + + // Accelerate or decelerate the motor + m.speed(m.speed() + (accel * UPDATE_RATE)); + + // Print out the current motor values and their setpoints, + // but only for the first few updates and only every multiple + if(update < (uint)(PRINT_WINDOW * UPDATES) && print_count == 0) { + printf("Vel = %f, ", capture.revolutions_per_second()); + printf("Vel SP = %f, ", vel_pid.setpoint); + printf("Accel = %f, ", accel * ACC_PRINT_SCALE); + printf("Speed = %f\n", m.speed()); } - } - // Do a stepped sweep - for(auto j = 0u; j < SWEEPS; j++) { - for(auto i = 0u; i < STEPS; i++) { - m.to_percent(i, 0, STEPS, 0.0 - SWEEP_EXTENT, SWEEP_EXTENT); - sleep_ms(STEPS_INTERVAL_MS); - } - for(auto i = 0u; i < STEPS; i++) { - m.to_percent(i, STEPS, 0, 0.0 - SWEEP_EXTENT, SWEEP_EXTENT); - sleep_ms(STEPS_INTERVAL_MS); + // Increment the print count, and wrap it + print_count = (print_count + 1) % PRINT_DIVIDER; + + update++; // Move along in time + + // Have we reached the end of this time window? + if(update >= (uint)(MOVEMENT_WINDOW * UPDATES)) { + update = 0; // Reset the counter + + // Set the new velocity setpoint to be the inverse of the current setpoint + vel_pid.setpoint = 0.0f - vel_pid.setpoint; } + + sleep_ms(UPDATE_RATE * 1000.0f); } // Disable the motor diff --git a/libraries/motor2040/motor2040.hpp b/libraries/motor2040/motor2040.hpp index 7ee5a688..0e6180c1 100644 --- a/libraries/motor2040/motor2040.hpp +++ b/libraries/motor2040/motor2040.hpp @@ -70,8 +70,8 @@ namespace motor { const uint NUM_SENSORS = 2; constexpr float SHUNT_RESISTOR = 0.47f; - constexpr float CURRENT_GAIN = 5; + constexpr float CURRENT_GAIN = 1; constexpr float VOLTAGE_GAIN = 3.9f / 13.9f; - constexpr float CURRENT_OFFSET = -0.02f; + constexpr float CURRENT_OFFSET = -0.005f; } } \ No newline at end of file diff --git a/micropython/examples/motor2040/motor_profiler.py b/micropython/examples/motor2040/motor_profiler.py index 30d2896e..6f501629 100644 --- a/micropython/examples/motor2040/motor_profiler.py +++ b/micropython/examples/motor2040/motor_profiler.py @@ -16,6 +16,8 @@ COUNTS_PER_REV = MMME_CPR * GEAR_RATIO # The counts per revolution of the motor DIRECTION = NORMAL_DIR # The direction to spin the motor in. NORMAL_DIR (0), REVERSED_DIR (1) SPEED_SCALE = 5.4 # The scaling to apply to the motor's speed. Set this to the maximum measured speed +ZERO_POINT = 0.0 # The duty cycle that corresponds with zero speed when plotting the motor's speed as a straight line +DEAD_ZONE = 0.0 # The duty cycle below which the motor's friction prevents it from moving DUTY_STEPS = 100 # How many duty cycle steps to sample the speed of SETTLE_TIME = 0.1 # How long to wait after changing motor duty cycle @@ -25,7 +27,7 @@ CAPTURE_TIME = 0.2 # How long to capture the motor's speed gc.collect() # Create a motor and set its speed scale, and give it a zero deadzone -m = Motor(MOTOR_PINS, direction=DIRECTION, speed_scale=SPEED_SCALE, zeropoint=0.0, deadzone=0.0) +m = Motor(MOTOR_PINS, direction=DIRECTION, speed_scale=SPEED_SCALE, zeropoint=ZERO_POINT, deadzone=DEAD_ZONE) # Create an encoder, using PIO 0 and State Machine 0 enc = Encoder(0, 0, ENCODER_PINS, direction=DIRECTION, counts_per_rev=COUNTS_PER_REV, count_microsteps=True) diff --git a/micropython/examples/motor2040/position_control.py b/micropython/examples/motor2040/position_control.py index 7d656527..8a514738 100644 --- a/micropython/examples/motor2040/position_control.py +++ b/micropython/examples/motor2040/position_control.py @@ -43,7 +43,7 @@ POS_KD = 0.0022 # Position derivative (D) gain gc.collect() # Create a motor and set its speed scale -m = Motor(MOTOR_PINS, direction=DIRECTION, speed_scale=SPEED_SCALE, deadzone=0.05) +m = Motor(MOTOR_PINS, direction=DIRECTION, speed_scale=SPEED_SCALE) # Create an encoder, using PIO 0 and State Machine 0 enc = Encoder(0, 0, ENCODER_PINS, direction=DIRECTION, counts_per_rev=COUNTS_PER_REV, count_microsteps=True) diff --git a/micropython/examples/motor2040/position_on_velocity_control.py b/micropython/examples/motor2040/position_on_velocity_control.py index 718f6e93..87e5e065 100644 --- a/micropython/examples/motor2040/position_on_velocity_control.py +++ b/micropython/examples/motor2040/position_on_velocity_control.py @@ -64,9 +64,6 @@ vel_pid = PID(VEL_KP, VEL_KI, VEL_KD, UPDATE_RATE) # Enable the motor to get started m.enable() -# Set the initial setpoint position -pos_pid.setpoint = POSITION_EXTENT - update = 0 print_count = 0 diff --git a/micropython/examples/motor2040/position_on_velocity_tuning.py b/micropython/examples/motor2040/position_on_velocity_tuning.py index 62778644..993b248a 100644 --- a/micropython/examples/motor2040/position_on_velocity_tuning.py +++ b/micropython/examples/motor2040/position_on_velocity_tuning.py @@ -48,7 +48,7 @@ VEL_KD = 0.4 # Velocity derivative (D) gain gc.collect() # Create a motor and set its speed scale -m = Motor(MOTOR_PINS, direction=DIRECTION, speed_scale=SPEED_SCALE, deadzone=0.0) +m = Motor(MOTOR_PINS, direction=DIRECTION, speed_scale=SPEED_SCALE) # Create an encoder, using PIO 0 and State Machine 0 enc = Encoder(0, 0, ENCODER_PINS, direction=DIRECTION, counts_per_rev=COUNTS_PER_REV, count_microsteps=True) diff --git a/micropython/examples/motor2040/quad_position_wave.py b/micropython/examples/motor2040/quad_position_wave.py index 3ef04108..c9a0d6d2 100644 --- a/micropython/examples/motor2040/quad_position_wave.py +++ b/micropython/examples/motor2040/quad_position_wave.py @@ -25,7 +25,7 @@ UPDATES_PER_MOVE = TIME_FOR_EACH_MOVE * UPDATES PRINT_DIVIDER = 4 # How many of the updates should be printed (i.e. 2 would be every other update) # LED constant -BRIGHTNESS = 0.4 # The brightness of the LEDs +BRIGHTNESS = 0.4 # The brightness of the RGB LED # PID values POS_KP = 0.14 # Position proportional (P) gain @@ -38,7 +38,7 @@ gc.collect() # Create a list of motors with a given speed scale MOTOR_PINS = [motor2040.MOTOR_A, motor2040.MOTOR_B, motor2040.MOTOR_C, motor2040.MOTOR_D] -motors = [Motor(pins, speed_scale=SPEED_SCALE, deadzone=0.05) for pins in MOTOR_PINS] +motors = [Motor(pins, speed_scale=SPEED_SCALE) for pins in MOTOR_PINS] # Create a list of encoders, using PIO 0, with the given counts per revolution ENCODER_PINS = [motor2040.ENCODER_A, motor2040.ENCODER_B, motor2040.ENCODER_C, motor2040.ENCODER_D] @@ -63,7 +63,7 @@ pos_pids = [PID(POS_KP, POS_KI, POS_KD, UPDATE_RATE) for i in range(motor2040.NU # Start updating the LED led.start() -# Enable the motor to get started +# Enable all motors for m in motors: m.enable() @@ -102,7 +102,7 @@ while user_sw.raw() is not True: # Print out the current motor values and their setpoints, but only on every multiple if print_count == 0: - for i in range(len(motors)): + for i in range(motor2040.NUM_MOTORS): print(ENCODER_NAMES[i], "=", captures[i].degrees, end=", ") print() diff --git a/micropython/examples/motor2040/quad_velocity_sequence.py b/micropython/examples/motor2040/quad_velocity_sequence.py index e9fc46a2..65183356 100644 --- a/micropython/examples/motor2040/quad_velocity_sequence.py +++ b/micropython/examples/motor2040/quad_velocity_sequence.py @@ -11,6 +11,12 @@ sequence of velocities, with the help of their attached encoders and PID control Press "Boot" to exit the program. """ +# Wheel friendly names +FL = 2 +FR = 3 +RL = 1 +RR = 0 + GEAR_RATIO = 50 # The gear ratio of the motors COUNTS_PER_REV = MMME_CPR * GEAR_RATIO # The counts per revolution of each motor's output shaft @@ -22,6 +28,8 @@ TIME_FOR_EACH_MOVE = 2 # The time to travel between each value UPDATES_PER_MOVE = TIME_FOR_EACH_MOVE * UPDATES PRINT_DIVIDER = 4 # How many of the updates should be printed (i.e. 2 would be every other update) +DRIVING_SPEED = 1.0 # The speed to drive the wheels at + # PID values VEL_KP = 30.0 # Velocity proportional (P) gain VEL_KI = 0.0 # Velocity integral (I) gain @@ -40,12 +48,6 @@ ENCODER_PINS = [motor2040.ENCODER_A, motor2040.ENCODER_B, motor2040.ENCODER_C, m ENCODER_NAMES = ["RR", "RL", "FL", "FR"] encoders = [Encoder(0, i, ENCODER_PINS[i], counts_per_rev=COUNTS_PER_REV, count_microsteps=True) for i in range(motor2040.NUM_MOTORS)] -# Wheel friendly names -FL = 2 -FR = 3 -RL = 1 -RR = 0 - # Reverse the direction of the B and D motors and encoders motors[FL].direction(REVERSED_DIR) motors[RL].direction(REVERSED_DIR) @@ -58,14 +60,6 @@ user_sw = Button(motor2040.USER_SW) # Create PID objects for position control vel_pids = [PID(VEL_KP, VEL_KI, VEL_KD, UPDATE_RATE) for i in range(motor2040.NUM_MOTORS)] -# Enable the motor to get started -for m in motors: - m.enable() - - -update = 0 -print_count = 0 - # Helper functions for driving in common directions def drive_forward(speed): @@ -96,6 +90,13 @@ def stop(): vel_pids[RR].setpoint = 0 +# Enable the motor to get started +for m in motors: + m.enable() + + +update = 0 +print_count = 0 sequence = 0 captures = [None] * motor2040.NUM_MOTORS @@ -107,9 +108,6 @@ while user_sw.raw() is not True: for i in range(motor2040.NUM_MOTORS): captures[i] = encoders[i].capture() - # Calculate how far along this movement to be - percent_along = min(update / UPDATES_PER_MOVE, 1.0) - for i in range(motor2040.NUM_MOTORS): # Calculate the acceleration to apply to the motor to move it closer to the velocity setpoint accel = vel_pids[i].calculate(captures[i].revolutions_per_second) @@ -119,7 +117,7 @@ while user_sw.raw() is not True: # Print out the current motor values, but only on every multiple if print_count == 0: - for i in range(len(motors)): + for i in range(motor2040.NUM_MOTORS): print(ENCODER_NAMES[i], "=", captures[i].revolutions_per_second, end=", ") print() @@ -141,17 +139,17 @@ while user_sw.raw() is not True: # Set the motor speeds, based on the sequence if sequence == 0: - drive_forward(1.0) + drive_forward(DRIVING_SPEED) elif sequence == 1: - drive_forward(-1.0) + drive_forward(-DRIVING_SPEED) elif sequence == 2: - turn_right(1.0) + turn_right(DRIVING_SPEED) elif sequence == 3: - turn_right(-1.0) + turn_right(-DRIVING_SPEED) elif sequence == 4: - strafe_right(1.0) + strafe_right(DRIVING_SPEED) elif sequence == 5: - strafe_right(-1.0) + strafe_right(-DRIVING_SPEED) elif sequence == 6: stop() diff --git a/micropython/examples/motor2040/reactive_encoder.py b/micropython/examples/motor2040/reactive_encoder.py index e5a2983b..4f1cb7fd 100644 --- a/micropython/examples/motor2040/reactive_encoder.py +++ b/micropython/examples/motor2040/reactive_encoder.py @@ -37,13 +37,13 @@ MIN_DETENT = -12 # The minimum detent that can be counted MAX_DETENT = +12 # The maximum detent that can be counted to MAX_DRIVE_PERCENT = 0.5 # The maximum drive force (as a percent) to apply when crossing detents +BRIGHTNESS = 0.4 # The brightness of the RGB LED + # PID values POS_KP = 0.14 # Position proportional (P) gain POS_KI = 0.0 # Position integral (I) gain POS_KD = 0.0022 # Position derivative (D) gain -BRIGHTNESS = 0.4 # The brightness of the LEDs - # Free up hardware resources ahead of creating a new Encoder gc.collect() diff --git a/micropython/examples/motor2040/read_sensors.py b/micropython/examples/motor2040/read_sensors.py index 15dd6335..7ca4909c 100644 --- a/micropython/examples/motor2040/read_sensors.py +++ b/micropython/examples/motor2040/read_sensors.py @@ -47,7 +47,7 @@ while user_sw.raw() is not True: # Read the current sense's of each motor and print the value for i in range(motor2040.NUM_MOTORS): mux.select(i + motor2040.CURRENT_SENSE_A_ADDR) - print("C", i + 1, "=", cur_adc.read_current(), sep="", end=", ") + print("C", i + 1, " = ", cur_adc.read_current(), sep="", end=", ") # Read the fault sense and print the value mux.select(motor2040.FAULT_SENSE_ADDR) diff --git a/micropython/examples/motor2040/velocity_control.py b/micropython/examples/motor2040/velocity_control.py index db05fa1b..53d47668 100644 --- a/micropython/examples/motor2040/velocity_control.py +++ b/micropython/examples/motor2040/velocity_control.py @@ -43,7 +43,7 @@ VEL_KD = 0.4 # Velocity derivative (D) gain gc.collect() # Create a motor and set its speed scale -m = Motor(MOTOR_PINS, direction=DIRECTION, speed_scale=SPEED_SCALE, deadzone=0.05) +m = Motor(MOTOR_PINS, direction=DIRECTION, speed_scale=SPEED_SCALE) # Create an encoder, using PIO 0 and State Machine 0 enc = Encoder(0, 0, ENCODER_PINS, direction=DIRECTION, counts_per_rev=COUNTS_PER_REV, count_microsteps=True) @@ -57,9 +57,6 @@ vel_pid = PID(VEL_KP, VEL_KI, VEL_KD, UPDATE_RATE) # Enable the motor to get started m.enable() -# Set the initial setpoint velocity -vel_pid.setpoint = VELOCITY_EXTENT - update = 0 print_count = 0 diff --git a/micropython/examples/motor2040/velocity_tuning.py b/micropython/examples/motor2040/velocity_tuning.py index e913e7ea..6f275f3b 100644 --- a/micropython/examples/motor2040/velocity_tuning.py +++ b/micropython/examples/motor2040/velocity_tuning.py @@ -23,9 +23,9 @@ SPEED_SCALE = 5.4 # The scaling to apply to the motor's sp UPDATES = 100 # How many times to update the motor per second UPDATE_RATE = 1 / UPDATES -PRINT_WINDOW = 1.0 # The time (in seconds) after a new setpoint, to display print out motor values +PRINT_WINDOW = 0.25 # The time (in seconds) after a new setpoint, to display print out motor values MOVEMENT_WINDOW = 2.0 # The time (in seconds) between each new setpoint being set -PRINT_DIVIDER = 4 # How many of the updates should be printed (i.e. 2 would be every other update) +PRINT_DIVIDER = 1 # How many of the updates should be printed (i.e. 2 would be every other update) # Multipliers for the different printed values, so they appear nicely on the Thonny plotter ACC_PRINT_SCALE = 0.01 # Acceleration multiplier @@ -42,7 +42,7 @@ VEL_KD = 0.4 # Velocity derivative (D) gain gc.collect() # Create a motor and set its speed scale -m = Motor(MOTOR_PINS, direction=DIRECTION, speed_scale=SPEED_SCALE, deadzone=0.0) +m = Motor(MOTOR_PINS, direction=DIRECTION, speed_scale=SPEED_SCALE) # Create an encoder, using PIO 0 and State Machine 0 enc = Encoder(0, 0, ENCODER_PINS, direction=DIRECTION, counts_per_rev=COUNTS_PER_REV, count_microsteps=True) From 898e4bace4014ea19af73dc93d65f301554b0317 Mon Sep 17 00:00:00 2001 From: ZodiusInfuser Date: Fri, 29 Apr 2022 00:09:19 +0100 Subject: [PATCH 39/60] Finalised motor cluster frequency, and disabled loading zone --- drivers/motor/motor_cluster.cpp | 8 +++--- drivers/motor/motor_state.hpp | 2 +- drivers/pwm/pwm_cluster.cpp | 44 +++++++++++++++++------------ drivers/pwm/pwm_cluster.hpp | 24 ++++++++-------- micropython/modules/motor/motor.cpp | 5 ++-- 5 files changed, 46 insertions(+), 37 deletions(-) diff --git a/drivers/motor/motor_cluster.cpp b/drivers/motor/motor_cluster.cpp index 736d16d7..92d3981c 100644 --- a/drivers/motor/motor_cluster.cpp +++ b/drivers/motor/motor_cluster.cpp @@ -10,21 +10,21 @@ 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), pwm_frequency(freq) { + : 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), pwm_frequency(freq) { + : 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_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), pwm_frequency(freq) { + : pwms(pio, sm, pin_pairs, seq_buffer, dat_buffer, false), pwm_frequency(freq) { create_motor_states(direction, speed_scale, zeropoint, deadzone, mode, auto_phase); } @@ -289,7 +289,7 @@ namespace motor { success = true; } - } + //} return success; } diff --git a/drivers/motor/motor_state.hpp b/drivers/motor/motor_state.hpp index 1a7f9cba..e182fe77 100644 --- a/drivers/motor/motor_state.hpp +++ b/drivers/motor/motor_state.hpp @@ -24,7 +24,7 @@ namespace motor { 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 = 50000.0f; + static constexpr float MAX_FREQUENCY = 400000.0f; static constexpr float ZERO_PERCENT = 0.0f; static constexpr float ONEHUNDRED_PERCENT = 1.0f; diff --git a/drivers/pwm/pwm_cluster.cpp b/drivers/pwm/pwm_cluster.cpp index c691d039..0765b46c 100644 --- a/drivers/pwm/pwm_cluster.cpp +++ b/drivers/pwm/pwm_cluster.cpp @@ -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 pins, Sequence *seq_buffer, TransitionData *dat_buffer) +PWMCluster::PWMCluster(PIO pio, uint sm, std::initializer_list 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,13 +106,14 @@ PWMCluster::PWMCluster(PIO pio, uint sm, std::initializer_list 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) +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) { +, wrap_level(0) +, loading_zone(loading_zone) { // Create the pin mask and channel mapping for(uint i = 0; i < length; i++) { @@ -127,13 +132,14 @@ PWMCluster::PWMCluster(PIO pio, uint sm, const pin_pair *pin_pairs, uint32_t len constructor_common(seq_buffer, dat_buffer); } -PWMCluster::PWMCluster(PIO pio, uint sm, std::initializer_list pin_pairs, Sequence *seq_buffer, TransitionData *dat_buffer) +PWMCluster::PWMCluster(PIO pio, uint sm, std::initializer_list 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) { +, wrap_level(0) +, loading_zone(loading_zone) { // Create the pin mask and channel mapping for(auto pair : pin_pairs) { @@ -535,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 diff --git a/drivers/pwm/pwm_cluster.hpp b/drivers/pwm/pwm_cluster.hpp index e60f2969..0d0cc403 100644 --- a/drivers/pwm/pwm_cluster.hpp +++ b/drivers/pwm/pwm_cluster.hpp @@ -15,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; @@ -119,6 +120,7 @@ namespace pimoroni { volatile uint last_written_index = 0; bool initialised = false; + bool loading_zone = true; //-------------------------------------------------- @@ -134,13 +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 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 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); - PWMCluster(PIO pio, uint sm, std::initializer_list pin_pairs, Sequence *seq_buffer = nullptr, TransitionData *dat_buffer = nullptr); + 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_pairs, Sequence *seq_buffer = nullptr, TransitionData *dat_buffer = nullptr, bool loading_zone = DEFAULT_USE_LOADING_ZONE); ~PWMCluster(); private: diff --git a/micropython/modules/motor/motor.cpp b/micropython/modules/motor/motor.cpp index 8a7f5f6f..8f2db642 100644 --- a/micropython/modules/motor/motor.cpp +++ b/micropython/modules/motor/motor.cpp @@ -302,9 +302,8 @@ extern mp_obj_t Motor_frequency(size_t n_args, const mp_obj_t *pos_args, mp_map_ float freq = mp_obj_get_float(args[ARG_freq].u_obj); - // TODO confirm frequency range if(!self->motor->frequency(freq)) { - mp_raise_ValueError("freq out of range. Expected 10Hz to 350Hz"); //TODO + mp_raise_ValueError("freq out of range. Expected 10Hz to 400KHz"); } return mp_const_none; } @@ -1442,7 +1441,7 @@ extern mp_obj_t MotorCluster_frequency(size_t n_args, const mp_obj_t *pos_args, float freq = mp_obj_get_float(args[ARG_freq].u_obj); if(!self->cluster->frequency(freq)) - mp_raise_ValueError("freq out of range. Expected 10Hz to 350Hz"); + mp_raise_ValueError("freq out of range. Expected 10Hz to 400KHz"); else return mp_const_none; } From 24296af3d5fb704ab78c1966e4709832854b2941 Mon Sep 17 00:00:00 2001 From: ZodiusInfuser Date: Fri, 29 Apr 2022 00:09:58 +0100 Subject: [PATCH 40/60] Indentation fixes --- examples/motor2040/motor2040_motor_wave.cpp | 6 +- .../motor2040/motor2040_position_control.cpp | 38 ++++++------- ...motor2040_position_on_velocity_control.cpp | 32 +++++------ .../motor2040_position_on_velocity_tuning.cpp | 18 +++--- .../motor2040/motor2040_position_tuning.cpp | 12 ++-- .../motor2040_quad_position_wave.cpp | 10 ++-- .../motor2040_quad_velocity_sequence.cpp | 56 +++++++++---------- 7 files changed, 86 insertions(+), 86 deletions(-) diff --git a/examples/motor2040/motor2040_motor_wave.cpp b/examples/motor2040/motor2040_motor_wave.cpp index 14ab9cbd..0ba4e4be 100644 --- a/examples/motor2040/motor2040_motor_wave.cpp +++ b/examples/motor2040/motor2040_motor_wave.cpp @@ -61,9 +61,9 @@ int main() { 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); + 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); diff --git a/examples/motor2040/motor2040_position_control.cpp b/examples/motor2040/motor2040_position_control.cpp index 3eac307d..99e94a0c 100644 --- a/examples/motor2040/motor2040_position_control.cpp +++ b/examples/motor2040/motor2040_position_control.cpp @@ -101,20 +101,20 @@ int main() { 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 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 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; + 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 @@ -125,9 +125,9 @@ int main() { // 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); + 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 @@ -137,11 +137,11 @@ int main() { // Have we reached the end of this movement? if(update >= UPDATES_PER_MOVE) { - update = 0; // Reset the counter + 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; + // 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); diff --git a/examples/motor2040/motor2040_position_on_velocity_control.cpp b/examples/motor2040/motor2040_position_on_velocity_control.cpp index f53c91e9..43c76a6b 100644 --- a/examples/motor2040/motor2040_position_on_velocity_control.cpp +++ b/examples/motor2040/motor2040_position_on_velocity_control.cpp @@ -109,20 +109,20 @@ int main() { 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 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 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; + 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 @@ -154,11 +154,11 @@ int main() { // Have we reached the end of this movement? if(update >= UPDATES_PER_MOVE) { - update = 0; // Reset the counter + 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; + // 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); diff --git a/examples/motor2040/motor2040_position_on_velocity_tuning.cpp b/examples/motor2040/motor2040_position_on_velocity_tuning.cpp index c97c4bf6..c2db35a4 100644 --- a/examples/motor2040/motor2040_position_on_velocity_tuning.cpp +++ b/examples/motor2040/motor2040_position_on_velocity_tuning.cpp @@ -120,12 +120,12 @@ int main() { // 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()); + 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 @@ -135,10 +135,10 @@ int main() { // Have we reached the end of this time window? if(update >= (uint)(MOVEMENT_WINDOW * UPDATES)) { - update = 0; // Reset the counter + 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; + // 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); diff --git a/examples/motor2040/motor2040_position_tuning.cpp b/examples/motor2040/motor2040_position_tuning.cpp index 4710a359..28358262 100644 --- a/examples/motor2040/motor2040_position_tuning.cpp +++ b/examples/motor2040/motor2040_position_tuning.cpp @@ -105,9 +105,9 @@ int main() { // 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); + 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 @@ -117,10 +117,10 @@ int main() { // Have we reached the end of this time window? if(update >= (uint)(MOVEMENT_WINDOW * UPDATES)) { - update = 0; // Reset the counter + 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; + // 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); diff --git a/examples/motor2040/motor2040_quad_position_wave.cpp b/examples/motor2040/motor2040_quad_position_wave.cpp index 67895d2c..8fa2428c 100644 --- a/examples/motor2040/motor2040_quad_position_wave.cpp +++ b/examples/motor2040/motor2040_quad_position_wave.cpp @@ -145,12 +145,12 @@ int main() { // Have we reached the end of this movement? if(update >= UPDATES_PER_MOVE) { - update = 0; // Reset the counter + update = 0; // Reset the counter - // Swap the start and end values - float temp = start_value; - start_value = end_value; - end_value = temp; + // Swap the start and end values + float temp = start_value; + start_value = end_value; + end_value = temp; } sleep_ms(UPDATE_RATE * 1000.0f); diff --git a/examples/motor2040/motor2040_quad_velocity_sequence.cpp b/examples/motor2040/motor2040_quad_velocity_sequence.cpp index 5b3825f0..dd4598c3 100644 --- a/examples/motor2040/motor2040_quad_velocity_sequence.cpp +++ b/examples/motor2040/motor2040_quad_velocity_sequence.cpp @@ -163,40 +163,40 @@ int main() { // Have we reached the end of this movement? if(update >= UPDATES_PER_MOVE) { - update = 0; // Reset the counter + update = 0; // Reset the counter - // Move on to the next part of the sequence - sequence += 1; + // Move on to the next part of the sequence + sequence += 1; - // Loop the sequence back around - if(sequence >= 7) { - sequence = 0; - } + // 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; + 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); From 22f29743c1cfa430f5d92cd475c66ddd4546697f Mon Sep 17 00:00:00 2001 From: ZodiusInfuser Date: Fri, 29 Apr 2022 00:27:27 +0100 Subject: [PATCH 41/60] Missing brace --- drivers/motor/motor_cluster.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/drivers/motor/motor_cluster.cpp b/drivers/motor/motor_cluster.cpp index 92d3981c..7fc5e29a 100644 --- a/drivers/motor/motor_cluster.cpp +++ b/drivers/motor/motor_cluster.cpp @@ -289,7 +289,7 @@ namespace motor { success = true; } - //} + } return success; } From 71a7a802185360af3c9af4123878550104826f67 Mon Sep 17 00:00:00 2001 From: ZodiusInfuser Date: Fri, 29 Apr 2022 13:02:00 +0100 Subject: [PATCH 42/60] Finalise motor API --- drivers/motor/motor_cluster.cpp | 10 +- drivers/motor/motor_cluster.hpp | 13 +-- drivers/motor/motor_state.cpp | 61 ++++++------ drivers/motor/motor_state.hpp | 5 +- micropython/modules/motor/motor.c | 22 +++-- micropython/modules/motor/motor.cpp | 143 ++++++++++++++++++++++++++-- micropython/modules/motor/motor.h | 10 +- 7 files changed, 203 insertions(+), 61 deletions(-) diff --git a/drivers/motor/motor_cluster.cpp b/drivers/motor/motor_cluster.cpp index 7fc5e29a..2900fe73 100644 --- a/drivers/motor/motor_cluster.cpp +++ b/drivers/motor/motor_cluster.cpp @@ -544,7 +544,7 @@ namespace motor { } } - void MotorCluster::all_to_direction(Direction 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); @@ -574,7 +574,7 @@ namespace motor { } } - void MotorCluster::all_to_speed_scale(float 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); @@ -604,7 +604,7 @@ namespace motor { } } - void MotorCluster::all_to_zeropoint(float zeropoint, bool load) { + 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); @@ -635,7 +635,7 @@ namespace motor { } } - void MotorCluster::all_to_deadzone(float deadzone, bool load) { + 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); @@ -666,7 +666,7 @@ namespace motor { } } - void MotorCluster::all_to_decay_mode(DecayMode mode, bool load) { + 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); diff --git a/drivers/motor/motor_cluster.hpp b/drivers/motor/motor_cluster.hpp index 785d9c15..c1147692 100644 --- a/drivers/motor/motor_cluster.hpp +++ b/drivers/motor/motor_cluster.hpp @@ -132,36 +132,37 @@ namespace motor { void direction(uint8_t motor, Direction direction); void direction(const uint8_t *motors, uint8_t length, Direction direction); void direction(std::initializer_list motors, Direction direction); - void all_to_direction(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 motors, float speed_scale); - void all_to_speed_scale(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 motors, float zeropoint, bool load = true); - void all_to_zeropoint(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 motors, float deadzone, bool load = true); - void all_to_deadzone(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 motors, DecayMode mode, bool load = true); - void all_to_decay_mode(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); + void create_motor_states(Direction direction, float speed_scale, float zeropoint, + float deadzone, DecayMode mode, bool auto_phase); }; } \ No newline at end of file diff --git a/drivers/motor/motor_state.cpp b/drivers/motor/motor_state.cpp index 4eeca27e..6d0ad992 100644 --- a/drivers/motor/motor_state.cpp +++ b/drivers/motor/motor_state.cpp @@ -5,13 +5,15 @@ 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){ + : 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)) { + : 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() { @@ -48,14 +50,8 @@ namespace motor { // Clamp the duty between the hard limits last_enabled_duty = CLAMP(duty, -1.0f, 1.0f); - if(last_enabled_duty > motor_zeropoint) - motor_speed = map_float(last_enabled_duty, motor_zeropoint, 1.0f, 0.0f, motor_scale); - else if(last_enabled_duty < -motor_zeropoint) - motor_speed = map_float(last_enabled_duty, -motor_zeropoint, -1.0f, 0.0f, -motor_scale); - else - motor_speed = 0.0f; - - //motor_speed = last_enabled_duty * motor_scale; + // Calculate the corresponding speed + motor_speed = duty_to_speed(last_enabled_duty, motor_zeropoint, motor_scale); return enable_with_return(); } @@ -72,13 +68,8 @@ namespace motor { // Clamp the speed between the hard limits motor_speed = CLAMP(speed, 0.0f - motor_scale, motor_scale); - if(motor_speed > 0.0f) - last_enabled_duty = map_float(motor_speed / motor_scale, 0.0f, 1.0f, motor_zeropoint, 1.0f); - else if(motor_speed < 0.0f) - last_enabled_duty = map_float(motor_speed / motor_scale, 0.0f, 1.0f, motor_zeropoint, 1.0f); - else - last_enabled_duty = 0.0f; - //last_enabled_duty = motor_speed / motor_scale; + // Calculate the corresponding duty cycle + last_enabled_duty = speed_to_duty(motor_speed, motor_zeropoint, motor_scale); return enable_with_return(); } @@ -119,7 +110,7 @@ namespace motor { void MotorState::set_speed_scale(float speed_scale) { motor_scale = MAX(speed_scale, FLT_EPSILON); - motor_speed = last_enabled_duty * motor_scale; + motor_speed = duty_to_speed(last_enabled_duty, motor_zeropoint, motor_scale); } float MotorState::get_zeropoint() const { @@ -128,13 +119,7 @@ namespace motor { void MotorState::set_zeropoint(float zeropoint) { motor_zeropoint = CLAMP(zeropoint, 0.0f, 1.0f - FLT_EPSILON); - - if(last_enabled_duty > motor_zeropoint) - motor_speed = map_float(last_enabled_duty, motor_zeropoint, 1.0f, 0.0f, motor_scale); - else if(last_enabled_duty < -motor_zeropoint) - motor_speed = map_float(last_enabled_duty, -motor_zeropoint, -1.0f, 0.0f, -motor_scale); - else - motor_speed = 0.0f; + motor_speed = duty_to_speed(last_enabled_duty, motor_zeropoint, motor_scale); } float MotorState::get_deadzone() const { @@ -154,4 +139,26 @@ namespace motor { 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; + } + }; \ No newline at end of file diff --git a/drivers/motor/motor_state.hpp b/drivers/motor/motor_state.hpp index e182fe77..896c8041 100644 --- a/drivers/motor/motor_state.hpp +++ b/drivers/motor/motor_state.hpp @@ -18,7 +18,7 @@ namespace motor { //-------------------------------------------------- public: static constexpr float DEFAULT_SPEED_SCALE = 1.0f; // The standard motor speed scale - static constexpr float DEFAULT_ZEROPOINT = 0.0f; // The standard motor deadzone + 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 @@ -94,6 +94,9 @@ namespace motor { 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); }; } \ No newline at end of file diff --git a/micropython/modules/motor/motor.c b/micropython/modules/motor/motor.c index d7881712..f3d6ee53 100644 --- a/micropython/modules/motor/motor.c +++ b/micropython/modules/motor/motor.c @@ -50,13 +50,15 @@ MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_to_percent_obj, 3, MotorCluster_to_perce MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_all_to_percent_obj, 2, MotorCluster_all_to_percent); MP_DEFINE_CONST_FUN_OBJ_1(MotorCluster_load_obj, MotorCluster_load); MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_direction_obj, 2, MotorCluster_direction); -MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_all_to_direction_obj, 1, MotorCluster_all_to_direction); +MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_all_directions_obj, 1, MotorCluster_all_directions); MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_speed_scale_obj, 2, MotorCluster_speed_scale); -MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_all_to_speed_scale_obj, 1, MotorCluster_all_to_speed_scale); +MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_all_speed_scales_obj, 1, MotorCluster_all_speed_scales); +MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_zeropoint_obj, 2, MotorCluster_zeropoint); +MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_all_zeropoints_obj, 1, MotorCluster_all_zeropoints); MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_deadzone_obj, 2, MotorCluster_deadzone); -MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_all_to_deadzone_obj, 1, MotorCluster_all_to_deadzone); +MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_all_deadzones_obj, 1, MotorCluster_all_deadzones); MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_decay_mode_obj, 2, MotorCluster_decay_mode); -MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_all_to_decay_mode_obj, 1, MotorCluster_all_to_decay_mode); +MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_all_decay_modes_obj, 1, MotorCluster_all_decay_modes); /***** Binding of Methods *****/ STATIC const mp_rom_map_elem_t Motor_locals_dict_table[] = { @@ -110,13 +112,15 @@ STATIC const mp_rom_map_elem_t MotorCluster_locals_dict_table[] = { { MP_ROM_QSTR(MP_QSTR_all_to_percent), MP_ROM_PTR(&MotorCluster_all_to_percent_obj) }, { MP_ROM_QSTR(MP_QSTR_load), MP_ROM_PTR(&MotorCluster_load_obj) }, { MP_ROM_QSTR(MP_QSTR_direction), MP_ROM_PTR(&MotorCluster_direction_obj) }, - { MP_ROM_QSTR(MP_QSTR_all_to_direction), MP_ROM_PTR(&MotorCluster_direction_obj) }, + { MP_ROM_QSTR(MP_QSTR_all_directions), MP_ROM_PTR(&MotorCluster_all_directions_obj) }, { MP_ROM_QSTR(MP_QSTR_speed_scale), MP_ROM_PTR(&MotorCluster_speed_scale_obj) }, - { MP_ROM_QSTR(MP_QSTR_all_to_speed_scale), MP_ROM_PTR(&MotorCluster_all_to_speed_scale_obj) }, + { MP_ROM_QSTR(MP_QSTR_all_speed_scales), MP_ROM_PTR(&MotorCluster_all_speed_scales_obj) }, + { MP_ROM_QSTR(MP_QSTR_zeropoint), MP_ROM_PTR(&MotorCluster_zeropoint_obj) }, + { MP_ROM_QSTR(MP_QSTR_all_zeropoints), MP_ROM_PTR(&MotorCluster_all_zeropoints_obj) }, { MP_ROM_QSTR(MP_QSTR_deadzone), MP_ROM_PTR(&MotorCluster_deadzone_obj) }, - { MP_ROM_QSTR(MP_QSTR_all_to_deadzone), MP_ROM_PTR(&MotorCluster_all_to_deadzone_obj) }, + { MP_ROM_QSTR(MP_QSTR_all_deadzones), MP_ROM_PTR(&MotorCluster_all_deadzones_obj) }, { MP_ROM_QSTR(MP_QSTR_decay_mode), MP_ROM_PTR(&MotorCluster_decay_mode_obj) }, - { MP_ROM_QSTR(MP_QSTR_all_to_decay_mode), MP_ROM_PTR(&MotorCluster_all_to_decay_mode_obj) }, + { MP_ROM_QSTR(MP_QSTR_all_decay_modes), MP_ROM_PTR(&MotorCluster_all_decay_modes_obj) }, }; STATIC MP_DEFINE_CONST_DICT(Motor_locals_dict, Motor_locals_dict_table); @@ -255,8 +259,6 @@ STATIC const mp_map_elem_t motor_globals_table[] = { { MP_OBJ_NEW_QSTR(MP_QSTR_pico_motor_shim), (mp_obj_t)&pico_motor_shim_user_cmodule }, { MP_OBJ_NEW_QSTR(MP_QSTR_motor2040), (mp_obj_t)&motor2040_user_cmodule }, - { MP_ROM_QSTR(MP_QSTR_NORMAL), MP_ROM_INT(0x00) }, - { MP_ROM_QSTR(MP_QSTR_REVERSED), MP_ROM_INT(0x01) }, { MP_ROM_QSTR(MP_QSTR_FAST_DECAY), MP_ROM_INT(0x00) }, { MP_ROM_QSTR(MP_QSTR_SLOW_DECAY), MP_ROM_INT(0x01) }, }; diff --git a/micropython/modules/motor/motor.cpp b/micropython/modules/motor/motor.cpp index 8f2db642..dad3fbba 100644 --- a/micropython/modules/motor/motor.cpp +++ b/micropython/modules/motor/motor.cpp @@ -2301,7 +2301,7 @@ extern mp_obj_t MotorCluster_direction(size_t n_args, const mp_obj_t *pos_args, return mp_const_none; } -extern mp_obj_t MotorCluster_all_to_direction(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { +extern mp_obj_t MotorCluster_all_directions(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { enum { ARG_self, ARG_direction }; static const mp_arg_t allowed_args[] = { { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, @@ -2322,7 +2322,7 @@ extern mp_obj_t MotorCluster_all_to_direction(size_t n_args, const mp_obj_t *pos if(direction < 0 || direction > 1) { mp_raise_ValueError("direction out of range. Expected NORMAL_DIR (0) or REVERSED_DIR (1)"); } - self->cluster->all_to_direction((Direction)direction); + self->cluster->all_directions((Direction)direction); } return mp_const_none; } @@ -2428,7 +2428,7 @@ extern mp_obj_t MotorCluster_speed_scale(size_t n_args, const mp_obj_t *pos_args return mp_const_none; } -extern mp_obj_t MotorCluster_all_to_speed_scale(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { +extern mp_obj_t MotorCluster_all_speed_scales(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { enum { ARG_self, ARG_speed_scale }; static const mp_arg_t allowed_args[] = { { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, @@ -2449,7 +2449,134 @@ extern mp_obj_t MotorCluster_all_to_speed_scale(size_t n_args, const mp_obj_t *p if(speed_scale < FLT_EPSILON) { mp_raise_ValueError("speed_scale out of range. Expected greater than 0.0"); } - self->cluster->all_to_speed_scale(speed_scale); + self->cluster->all_speed_scales(speed_scale); + } + return mp_const_none; +} + +extern mp_obj_t MotorCluster_zeropoint(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { + if(n_args <= 2) { + enum { ARG_self, ARG_motor }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_motor, MP_ARG_REQUIRED | MP_ARG_INT }, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _MotorCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _MotorCluster_obj_t); + + int motor = args[ARG_motor].u_int; + int motor_count = (int)self->cluster->count(); + if(motor_count == 0) + mp_raise_ValueError("this cluster does not have any motors"); + else if(motor < 0 || motor >= motor_count) + mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("motor out of range. Expected 0 to %d"), motor_count - 1); + else + return mp_obj_new_float(self->cluster->zeropoint((uint)motor)); + } + else { + enum { ARG_self, ARG_motors, ARG_zeropoint }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_motors, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_zeropoint, MP_ARG_REQUIRED | MP_ARG_OBJ }, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _MotorCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _MotorCluster_obj_t); + + int motor_count = (int)self->cluster->count(); + if(motor_count == 0) + mp_raise_ValueError("this cluster does not have any motors"); + else { + // Determine what motor(s) to modify + const mp_obj_t object = args[ARG_motors].u_obj; + if(mp_obj_is_int(object)) { + int motor = mp_obj_get_int(object); + if(motor < 0 || motor >= motor_count) + mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("motor out of range. Expected 0 to %d"), motor_count - 1); + else { + float zeropoint = mp_obj_get_float(args[ARG_zeropoint].u_obj); + if(zeropoint < 0.0f || zeropoint > 1.0f - FLT_EPSILON) { + mp_raise_ValueError("zeropoint out of range. Expected 0.0 to less than 1.0"); + } + self->cluster->zeropoint((uint)motor, zeropoint); + } + } + else { + size_t length = 0; + mp_obj_t *items = nullptr; + if(mp_obj_is_type(object, &mp_type_list)) { + mp_obj_list_t *list = MP_OBJ_TO_PTR2(object, mp_obj_list_t); + length = list->len; + items = list->items; + } + else if(mp_obj_is_type(object, &mp_type_tuple)) { + mp_obj_tuple_t *tuple = MP_OBJ_TO_PTR2(object, mp_obj_tuple_t); + length = tuple->len; + items = tuple->items; + } + + if(items == nullptr) + mp_raise_TypeError("cannot convert object to a list or tuple of integers, or a single integer"); + else if(length == 0) + mp_raise_TypeError("list or tuple must contain at least one integer"); + else { + // Create and populate a local array of motor indices + uint8_t *motors = new uint8_t[length]; + for(size_t i = 0; i < length; i++) { + int motor = mp_obj_get_int(items[i]); + if(motor < 0 || motor >= motor_count) { + delete[] motors; + mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("a motor in the list or tuple is out of range. Expected 0 to %d"), motor_count - 1); + } + else { + motors[i] = (uint8_t)motor; + } + } + float zeropoint = mp_obj_get_float(args[ARG_zeropoint].u_obj); + if(zeropoint < 0.0f || zeropoint > 1.0f - FLT_EPSILON) { + delete[] motors; + mp_raise_ValueError("zeropoint out of range. Expected 0.0 to less than 1.0"); + } + + self->cluster->zeropoint(motors, length, zeropoint); + delete[] motors; + } + } + } + } + return mp_const_none; +} + +extern mp_obj_t MotorCluster_all_zeropoints(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { + enum { ARG_self, ARG_zeropoint }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_zeropoint, MP_ARG_REQUIRED | MP_ARG_OBJ }, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _MotorCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _MotorCluster_obj_t); + + int motor_count = (int)self->cluster->count(); + if(motor_count == 0) + mp_raise_ValueError("this cluster does not have any motors"); + else { + float zeropoint = mp_obj_get_float(args[ARG_zeropoint].u_obj); + if(zeropoint < 0.0f || zeropoint > 1.0f - FLT_EPSILON) { + mp_raise_ValueError("zeropoint out of range. Expected 0.0 to less than 1.0"); + } + self->cluster->all_zeropoints(zeropoint); } return mp_const_none; } @@ -2555,7 +2682,7 @@ extern mp_obj_t MotorCluster_deadzone(size_t n_args, const mp_obj_t *pos_args, m return mp_const_none; } -extern mp_obj_t MotorCluster_all_to_deadzone(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { +extern mp_obj_t MotorCluster_all_deadzones(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { enum { ARG_self, ARG_deadzone, ARG_load }; static const mp_arg_t allowed_args[] = { { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, @@ -2577,7 +2704,7 @@ enum { ARG_self, ARG_deadzone, ARG_load }; if(deadzone < 0.0f || deadzone > 1.0f) { mp_raise_ValueError("deadzone out of range. Expected 0.0 to 1.0"); } - self->cluster->all_to_deadzone(deadzone, args[ARG_load].u_bool); + self->cluster->all_deadzones(deadzone, args[ARG_load].u_bool); } return mp_const_none; } @@ -2683,7 +2810,7 @@ extern mp_obj_t MotorCluster_decay_mode(size_t n_args, const mp_obj_t *pos_args, return mp_const_none; } -extern mp_obj_t MotorCluster_all_to_decay_mode(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { +extern mp_obj_t MotorCluster_all_decay_modes(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { enum { ARG_self, ARG_mode, ARG_load }; static const mp_arg_t allowed_args[] = { { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, @@ -2705,7 +2832,7 @@ extern mp_obj_t MotorCluster_all_to_decay_mode(size_t n_args, const mp_obj_t *po if(mode < 0 || mode > 1) { mp_raise_ValueError("mode out of range. Expected FAST_DECAY (0) or SLOW_DECAY (1)"); } - self->cluster->all_to_decay_mode((DecayMode)mode, args[ARG_load].u_bool); + self->cluster->all_decay_modes((DecayMode)mode, args[ARG_load].u_bool); } return mp_const_none; } diff --git a/micropython/modules/motor/motor.h b/micropython/modules/motor/motor.h index 7ce746a4..d9763f69 100644 --- a/micropython/modules/motor/motor.h +++ b/micropython/modules/motor/motor.h @@ -59,10 +59,12 @@ extern mp_obj_t MotorCluster_to_percent(size_t n_args, const mp_obj_t *pos_args, extern mp_obj_t MotorCluster_all_to_percent(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); extern mp_obj_t MotorCluster_load(mp_obj_t self_in); extern mp_obj_t MotorCluster_direction(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); -extern mp_obj_t MotorCluster_all_to_direction(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); +extern mp_obj_t MotorCluster_all_directions(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); extern mp_obj_t MotorCluster_speed_scale(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); -extern mp_obj_t MotorCluster_all_to_speed_scale(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); +extern mp_obj_t MotorCluster_all_speed_scales(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); +extern mp_obj_t MotorCluster_zeropoint(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); +extern mp_obj_t MotorCluster_all_zeropoints(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); extern mp_obj_t MotorCluster_deadzone(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); -extern mp_obj_t MotorCluster_all_to_deadzone(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); +extern mp_obj_t MotorCluster_all_deadzones(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); extern mp_obj_t MotorCluster_decay_mode(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); -extern mp_obj_t MotorCluster_all_to_decay_mode(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); +extern mp_obj_t MotorCluster_all_decay_modes(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); From 90af916fa18fa2d2a136d4ddb4b7c55fe2f02881 Mon Sep 17 00:00:00 2001 From: ZodiusInfuser Date: Tue, 3 May 2022 18:09:43 +0100 Subject: [PATCH 43/60] Finalised PicoExplorer encoder example --- examples/pico_explorer_encoder/demo.cpp | 145 +++++++++++++++--------- 1 file changed, 92 insertions(+), 53 deletions(-) diff --git a/examples/pico_explorer_encoder/demo.cpp b/examples/pico_explorer_encoder/demo.cpp index 0f4e9ce7..98d4a614 100644 --- a/examples/pico_explorer_encoder/demo.cpp +++ b/examples/pico_explorer_encoder/demo.cpp @@ -5,35 +5,74 @@ #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 //-------------------------------------------------- -static const pin_pair ENCODER_PINS = {1, 0}; -static const uint ENCODER_PIN_C = PIN_UNUSED; -static const uint ENCODER_SWITCH_PIN = 4; -static constexpr float COUNTS_PER_REVOLUTION = 24; // 24 is for rotary encoders. For motor magnetic encoders uses - // 12 times the gear ratio (e.g. 12 * 20 with a 20:1 ratio motor -static const bool COUNT_MICROSTEPS = false; // Set to true for motor magnetic encoders +// 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; -static const uint16_t FREQ_DIVIDER = 1; // Increase this to deal with switch bounce. 250 Gives a 1ms debounce +// The counts per revolution of the encoder's output shaft +static constexpr float COUNTS_PER_REV = encoder::ROTARY_CPR; -static const int32_t TIME_BETWEEN_SAMPLES_US = 100; // Time between each sample, in microseconds -static const int32_t WINDOW_DURATION_US = 1000000; // The full time window that will be stored +// Set to true if using a motor with a magnetic encoder +static const bool COUNT_MICROSTEPS = false; -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 +// Increase this to deal with switch bounce. 250 Gives a 1ms debounce +static const uint16_t FREQ_DIVIDER = 1; -static const bool QUADRATURE_OUT_ENABLED = true; -static constexpr float QUADRATURE_OUT_FREQ = 800; // The frequency the quadrature output will run at (note that counting microsteps will show 4x this value) -static const float QUADRATURE_OUT_1ST_PIN = 6; // Which first pin to output the quadrature signal to (e.g. pins 6 and 7) +// Time between each sample, in microseconds +static const int32_t TIME_BETWEEN_SAMPLES_US = 100; -static const uint64_t MAIN_LOOP_TIME_US = 50000; // How long there should be in microseconds between each screen refresh -static const uint16_t EDGE_ALIGN_ABOVE_ZOOM = 4; // The zoom level beyond which edge alignment will be enabled to ma +// 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; @@ -51,29 +90,28 @@ enum DrawState { //-------------------------------------------------- // Variables //-------------------------------------------------- - uint16_t buffer[PicoExplorer::WIDTH * PicoExplorer::HEIGHT]; PicoExplorer pico_explorer(buffer); -Encoder enc(pio0, 0, ENCODER_PINS, ENCODER_PIN_C, NORMAL_DIR, COUNTS_PER_REVOLUTION, COUNT_MICROSTEPS, FREQ_DIVIDER); +Encoder enc(pio0, 0, ENCODER_PINS, ENCODER_COMMON_PIN, NORMAL_DIR, COUNTS_PER_REV, COUNT_MICROSTEPS, FREQ_DIVIDER); -volatile bool encA_readings[READINGS_SIZE]; -volatile bool encB_readings[READINGS_SIZE]; -volatile bool encA_scratch[SCRATCH_SIZE]; -volatile bool encB_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; +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 readingPos, bool edge_align) { +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 = (readingPos + (READINGS_SIZE - reading_window)); + 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; @@ -82,7 +120,7 @@ uint32_t draw_plot(Point p1, Point p2, volatile bool (&readings)[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 - readingPos); + 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++) { @@ -145,24 +183,26 @@ uint32_t draw_plot(Point p1, Point p2, volatile bool (&readings)[READINGS_SIZE], //////////////////////////////////////////////////////////////////////////////////////////////////// bool repeating_timer_callback(struct repeating_timer *t) { + bool_pair state = enc.state(); if(drawing_to_screen && next_scratch_index < SCRATCH_SIZE) { - encA_scratch[next_scratch_index] = enc.state().a; - encB_scratch[next_scratch_index] = enc.state().b; + enc_a_scratch[next_scratch_index] = state.a; + enc_b_scratch[next_scratch_index] = state.b; next_scratch_index++; } else { - encA_readings[next_reading_index] = enc.state().a; - encB_readings[next_reading_index] = enc.state().b; + 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) + if(next_reading_index >= READINGS_SIZE) { next_reading_index = 0; + } } return true; } - +//////////////////////////////////////////////////////////////////////////////////////////////////// void setup() { stdio_init_all(); @@ -182,11 +222,10 @@ void setup() { enc.init(); - bool encA = enc.state().a; - bool encB = enc.state().b; + bool_pair state = enc.state(); for(uint i = 0; i < READINGS_SIZE; i++) { - encA_readings[i] = encA; - encB_readings[i] = encB; + enc_a_readings[i] = state.a; + enc_b_readings[i] = state.b; } if(QUADRATURE_OUT_ENABLED) { @@ -212,8 +251,8 @@ int main() { struct repeating_timer timer; add_repeating_timer_us(-TIME_BETWEEN_SAMPLES_US, repeating_timer_callback, NULL, &timer); - bool aPressedLatch = false; - bool xPressedLatch = false; + bool button_latch_a = false; + bool button_latch_x = false; uint64_t last_time = time_us_64(); while(true) { @@ -246,24 +285,24 @@ int main() { // If A has been pressed, zoom the view out to a min of x1 if(pico_explorer.is_pressed(PicoExplorer::A)) { - if(!aPressedLatch) { - aPressedLatch = true; + if(!button_latch_a) { + button_latch_a = true; current_zoom_level = std::max(current_zoom_level / 2, 1); } } else { - aPressedLatch = false; + 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(!xPressedLatch) { - xPressedLatch = true; + if(!button_latch_x) { + button_latch_x = true; current_zoom_level = std::min(current_zoom_level * 2, 512); } } else { - xPressedLatch = false; + button_latch_x = false; } //-------------------------------------------------- @@ -275,16 +314,16 @@ int main() { drawing_to_screen = true; pico_explorer.set_pen(255, 255, 0); - uint32_t localPos = next_reading_index; - uint32_t alignment_offset = draw_plot(Point(0, 10), Point(PicoExplorer::WIDTH, 10 + 50), encA_readings, localPos, current_zoom_level > EDGE_ALIGN_ABOVE_ZOOM); + 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), encB_readings, (localPos + (READINGS_SIZE - alignment_offset)) % READINGS_SIZE, false); + 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++) { - encA_readings[next_reading_index] = encA_scratch[i]; - encB_readings[next_reading_index] = encB_scratch[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) From b782287c633ce17b9a6a48438448cb10e1a392fe Mon Sep 17 00:00:00 2001 From: ZodiusInfuser Date: Thu, 5 May 2022 16:54:02 +0100 Subject: [PATCH 44/60] Added readme for encoder --- drivers/encoder/encoder.hpp | 2 +- micropython/modules/encoder/README.md | 154 ++++++++++++++++++++++++++ 2 files changed, 155 insertions(+), 1 deletion(-) create mode 100644 micropython/modules/encoder/README.md diff --git a/drivers/encoder/encoder.hpp b/drivers/encoder/encoder.hpp index e28e73b5..14cbf496 100644 --- a/drivers/encoder/encoder.hpp +++ b/drivers/encoder/encoder.hpp @@ -14,7 +14,7 @@ namespace encoder { // Constants //-------------------------------------------------- public: - static constexpr float DEFAULT_COUNTS_PER_REV = 24; + static constexpr float DEFAULT_COUNTS_PER_REV = ROTARY_CPR; static const bool DEFAULT_COUNT_MICROSTEPS = false; static const uint16_t DEFAULT_FREQ_DIVIDER = 1; diff --git a/micropython/modules/encoder/README.md b/micropython/modules/encoder/README.md new file mode 100644 index 00000000..c1082f4d --- /dev/null +++ b/micropython/modules/encoder/README.md @@ -0,0 +1,154 @@ +# Encoders + +The Encoder library lets you read incremental rotary encoders from a Raspberry Pi Pico or any other RP2040-based board, such as the [Pimoroni Motor 2040](https://pimoroni.com/motor2040). + +This library offers an `Encoder` class that uses Programmable IO (PIO) hardware to read a single encoder, with support for up to 8 encoders. + + +## Table of Content +- [Encoder](#encoder) + - [Getting Started](#getting-started) + - [Count and Angle](#count-and-angle) + - [Count Delta](#count-delta) + - [Step and Turn](#step-and-turn) + - [Changing the Direction](#changing-the-direction) + - [Resetting to Zero](#resetting-to-zero) + - [Capture](#capture) + - [State](#state) + - [Function Reference](#function-reference) + - [Constants Reference](#constants-reference) + - [PIO Limitations](#pio-limitations) + + +## Encoder + +### Getting Started + +To start using encoders, you will need to first import the `Encoder` class. +```python +from encoder import Encoder +``` + +To create your encoder, specify the PIO, PIO state-machine and GPIO pins it will be connected to, and pass them into `Encoder`. The example below shows the typical setup for a rotary encoder that has A, B and C pins. +```python +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) +``` + +If using a board such as Motor 2040 with its motor encoder support, then the encoder's GPIO pins can be assigned using one of the handy constants of `motor2040`. +```python +from encoder import MMME_CPR +from motor import motor2040 +enc = Encoder(0, 0, motor2040.ENCODER_A, counts_per_rev=MMME_CPR, count_microsteps=True) +``` +As the above example shows, motor encoders often have a different number of counts per revolution than a traditional rotary encoder. For our MMME shims the `MMME_CPR` constant is provided. Also, as motor encoders lack the tactile steps of rotary encoders, their counting resolution can be increased by a factor of four by including each microstep. + +Following either of the above setups gives you an `Encoder` class called `enc` that will monitor the state of the physical encoder. + +From this point on the encoder can be read in several ways, depending on the intended application. These are covered in more detail in the following sections. + + +### Count and Angle + +The Encoder class keeps track of an encoder's current count, which gets increased or reduced as the physical encoder is turned. This count is read by calling `.count()`. + +The count can also be read back as either the number of `.revolutions()` of the encoder, or the angle in `.degrees()` or `.radians()`. These use the encoder's counts per revolution, which can either be specified when the object is created, or updated by calling `.counts_per_rev(counts_per_rev)`. + +Be aware that the count is stored as an integer, if it is continually increased or decreased it will eventually wrap at `+2147483647` and `-2147483648`. This will cause a jump in the returned by `.revolutions()`, `degrees()` and `.radians()`, that will need handling by your code. + + +### Count Delta + +Often you are not interested in the exact count that the encoder is at, but rather if the count has changed since the last time you checked. This change can be read by calling `.delta()` at regular intervals. The returned value can then be used with a check in code, for the value being non-zero. + +The delta can also be read back as either the change in revolutions or angle, by calling `.revolutions_delta()`, `.degrees_delta()`, and `.radians_delta()`, respectively. These use the encoder's counts per revolution, which can either be specified when the object is created, or updated by calling `.counts_per_rev(counts_per_rev)`. + + +### Step and Turn + +If using a rotary encoder with a physical direction marker, it can be useful to know its position in the form of which step it is at and how many turns have occurred. The current step can be read by calling `.step()`, which returns a value from zero up to the encoder's `counts_per_rev - 1`. The number of turns can be read by calling `.turn()`. + +These functions differ from reading the `.count()` or `.revolutions()` by using seperate counters, and so avoid the wrapping issue that these functions experience. + + +### Changing the Direction + +The counting direction of an encoder can be changed either by providing `direction=REVERSED_DIR` when creating the `Encoder` object, or by calling `.direction(REVERSED_DIR)` at any time in code. The `REVERSED_DIR` constant comes from the `pimoroni` module. There is also a `NORMAL_DIR` constant, though this is the default. + + +### Resetting to Zero + +There are times where an encoder's count (and related values) need to be reset back to zero. This can be done by calling `.zero()`. + + +### Capture + +There are times with encoders when you want to query multiple pieces of information from a single moment in time. Typically this would be done by reading the values up-front for use later, rather than reading them at the point they are needed in your code. Unfortunately even this offers no guarantee that the values were from a single moment, particularly when dealing with high speed encoders. + +To support this usage the encoder class has a `.capture()` function, which returns a named tuple containing the encoder's: +``` +count +delta +frequency +revolutions +degrees +radians +revolutions_delta +radians_delta +revolutions_per_second +revolutions_per_minute +degrees_per_second +radians_per_second +``` + +Internally `.capture()` does the same up-front reading of values but does so more optimally within the underlying C++ driver. As an added bonus, it calculates encoder speeds too, by using the captured `delta` along with timing information returned by the PIO, more accurately than estimating a speed from the `delta` alone. + + +### State + +The raw state of an encoder's pins can be read using `.state()`, which returns pair of booleans for the A and B pins. This can be useful for visualising the signals of an encoder alongside its other values. + + +### Function Reference + +Here is the complete list of functions available on the `Encoder` class: +```python +Encoder(pio, sm, pins, common_pin=PIN_UNUSED, direction=NORMAL_DIR, counts_per_rev=ROTARY_CPR, count_microsteps=False, freq_divider=1) +pins() +common_pin() +state() +count() +delta() +zero() +step() +turn() +revolutions() +degrees() +radians() +direction() +direction(direction) +counts_per_rev() +counts_per_rev(counts_per_rev) +capture() +``` + +### Constants Reference + +Here is the complete list of constants on the `encoder` module: + +* `ROTARY_CPR` = `24` +* `MMME_CPR` = `12` + +Here are useful constants from the `pimoroni` module: + +* `NORMAL_DIR` = `0` +* `REVERSED_DIR` = `1` + + +### PIO Limitations + +The RP2040 features two PIOs with four state machines each. This places a hard limit on how many Encoders can be created of 8. + +When creating an Encoder, in most cases you'll use `0` for PIO and `0` for PIO state-machine. You should change these though if you plan on running multiple encoders, or using encoders alongside something else that uses PIO, such as our [Plasma library](https://github.com/pimoroni/pimoroni-pico/tree/main/micropython/modules/plasma). From de0e908defb9938bcb5af758acfec8708ac57e77 Mon Sep 17 00:00:00 2001 From: ZodiusInfuser Date: Mon, 9 May 2022 09:58:33 +0100 Subject: [PATCH 45/60] Encoder readme tweaks --- micropython/modules/encoder/README.md | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/micropython/modules/encoder/README.md b/micropython/modules/encoder/README.md index c1082f4d..ceff789d 100644 --- a/micropython/modules/encoder/README.md +++ b/micropython/modules/encoder/README.md @@ -63,8 +63,6 @@ Be aware that the count is stored as an integer, if it is continually increased Often you are not interested in the exact count that the encoder is at, but rather if the count has changed since the last time you checked. This change can be read by calling `.delta()` at regular intervals. The returned value can then be used with a check in code, for the value being non-zero. -The delta can also be read back as either the change in revolutions or angle, by calling `.revolutions_delta()`, `.degrees_delta()`, and `.radians_delta()`, respectively. These use the encoder's counts per revolution, which can either be specified when the object is created, or updated by calling `.counts_per_rev(counts_per_rev)`. - ### Step and Turn @@ -96,6 +94,7 @@ revolutions degrees radians revolutions_delta +degrees_delta radians_delta revolutions_per_second revolutions_per_minute From 7499cc03782bd7a3ee9301e6c92f8d22bde5e870 Mon Sep 17 00:00:00 2001 From: ZodiusInfuser Date: Mon, 9 May 2022 17:07:14 +0100 Subject: [PATCH 46/60] Further Motor documentation --- micropython/modules/motor/README.md | 279 ++++++++++++++++++++++------ micropython/modules/motor/motor.c | 1 + micropython/modules/servo/README.md | 6 +- 3 files changed, 226 insertions(+), 60 deletions(-) diff --git a/micropython/modules/motor/README.md b/micropython/modules/motor/README.md index 752babe1..8028c041 100644 --- a/micropython/modules/motor/README.md +++ b/micropython/modules/motor/README.md @@ -1,38 +1,58 @@ # Motors and Motor 2040 -The Motor library lets you drive DC motors from a Raspberry Pi Pico or any other RP2040-based board, such as the [Pimoroni Motor 2040](https://pimoroni.com/servo2040). +The Motor library lets you drive DC motors from a Raspberry Pi Pico or any other RP2040-based board via connected h-bridge drivers. +An easy way to add an h-bridge driver to the Pico is to attach a [Pimoroni Pico Motor Shim](https://pimoroni.com/picomotorshim). +Alternatively, an RP2040-based board with integrated drivers could be used, such as the [Pimoroni Motor 2040](https://pimoroni.com/motor2040) (coming soon). This library offers two motor implementations: * a `Motor` class that uses hardware PWM to drive a single motor, with support for up to 8 motors. -* a `MotorCluster` class that uses Programmable IO (PIO) hardware to drive up to 15 servos. +* a `MotorCluster` class that uses Programmable IO (PIO) hardware to drive up to 15 motors. ## Table of Content -- [Motor 2040](#motor-2040) +- [Pico Motor Shim](#pico-motor-shim) - [Reading the User Button](#reading-the-user-button) - - [Reading the Sensors](#reading-the-sensors) - - [Fault Sensing and Configuring Pulls](#fault-sensing-and-configuring-pulls) - - [Controlling the LED](#controlling-the-led) - [Pin Constants](#pin-constants) - - [Motor Pin Tuples](#motor-pin-tuples) - - [Encoder Pin Tuples](#encoder-pin-tuples) - - [LED Pin](#led-pin) + - [Motor Pins](#motor-pins) - [I2C Pins](#i2c-pins) - [Button Pin](#button-pin) + - [Other Constants](#other-constants) + +- [Motor 2040](#motor-2040) + - [Reading the User Button](#reading-the-user-button-1) + - [Reading the Sensors](#reading-the-sensors) + - [Configuring Pulls](#configuring-pulls) + - [Fault Sensing](#fault-sensing) + - [Reading the Encoders](#reading-the-encoders) + - [Controlling the LED](#controlling-the-led) + - [Pin Constants](#pin-constants-1) + - [Motor Pins](#motor-pins-1) + - [Encoder Pins](#encoder-pins-1) + - [LED Pin](#led-pin) + - [I2C Pins](#i2c-pins-1) + - [Button Pin](#button-pin-1) - [Address Pins](#address-pins) - [ADC Pins](#adc-pins) - - [Other Constants](#other-constants) + - [Other Constants](#other-constants-1) - [Counts](#counts) - [Addresses](#addresses) - [Sensing](#sensing) - [Motor](#motor) - [Getting Started](#getting-started) - [Control by Speed](#control-by-speed) + - [Full Speed](#full-speed) + - [Stopping](#stopping) + - [Calibration](#calibration) - [Control by Percent](#control-by-percent) - [Control by Duty Cycle](#control-by-duty-cycle) + - [Duty Deadzone](#duty-deadzone) - [Frequency Control](#frequency-control) - [Configuration](#configuration) + - [Direction](#direction) + - [Decay Mode](#decay-mode) + - [Driver Type](#driver-type) - [Function Reference](#function-reference) + - [Constants Reference](#constants-reference) - [PWM Limitations](#pwm-limitations) - [MotorCluster](#motorcluster) - [Getting Started](#getting-started-1) @@ -47,9 +67,76 @@ This library offers two motor implementations: - [PIO Limitations](#pio-limitations) +## Pico Motor Shim + +Pico Motor Shim (or Motor Shim for Pico, if you prefer) is a small board featuring a DRV8833 dual h-bridge motor driver IC, a user button, and a Qw/st connector. It attaches to the underside of a Raspberry Pi Pico towards the USB end, and connects to two motors with Motor Connector Shims (MCS) attached via 2 pin JST-ZH cables. + +For more information on this board and its accessories, check out the store page: [pimoroni.com/picomotorshim](https://pimoroni.com/picomotorshim). + + +### Reading the User Button + +The Pico Motor Shim has a handy button onboard, offering a tactile way to interact with your program. To simplify the use of this and other buttons, the `pimoroni` module contains a `Button` class that handles button debounce and auto-repeat. + +```python +Button(button, invert=True, repeat_time=200, hold_time=1000) +``` + +To set up the user button, first import the `Button` class from the `pimoroni` module and the pin constant for the button from `motor`: +```python +from pimoroni import Button +from motor import picomotorshim +``` + +Then create an instance of `Button` for the user button: +```python +button_a = Button(picomotorshim.BUTTON_A) +``` + +To get the button state, call `.read()`. If the button is held down, then this will return `True` at the interval specified by `repeat_time` until `hold_time` is reached, at which point it will return `True` every `repeat_time / 3` milliseconds. This is useful for rapidly increasing/decreasing values: + +```python +state = button_a.read() +``` + +It is also possible to read the raw button state without the repeat feature, if you prefer: +```python +state = button_a.raw() +``` + + +### Pin Constants + +The `motor` module contains a `picomotorshim` sub module with constants for the motor and button pins. + + +#### Motor Pin Tuples + +* `MOTOR_1` = `(6, 7)` +* `MOTOR_2` = `(27, 26)` + + +#### I2C Pins + +* `SDA` = `4` +* `SCL` = `5` + + +#### Button Pin + +* `BUTTON_A` = `2` + + +### Other Constants + +The `picomotorshim` sub module also contains a constant for the number of motors on Pico Motor Shim: + +* `NUM_MOTORS` = `2` + + ## Motor 2040 -Motor 2040 is a **standalone motor controller** for driving DC motors with encoder feedback. It has JST-SH connectors for plugging in up to 4 motors, with additional bells and whistles like sensor headers, current monitoring, an RGB LED, and a user button that make it ideal for many robotics projects! +Motor 2040 is a **standalone motor controller** for driving DC motors with encoder feedback. It has JST-SH connectors for plugging in up to 4 motors, with additional bells and whistles like sensor headers, current and voltage monitoring, an RGB LED, and a user button that make it ideal for many robotics projects! For more information on this board, check out the store page: [pimoroni.com/motor2040](https://pimoroni.com/motor2040). @@ -97,7 +184,7 @@ These could be used just for monitoring, or as the trigger to turn off motors sa To allow for all of these inputs, Motor 2040 has an onboard analog multiplexer that expands a single analog pin into eight, by use of three digital address pins that select a single input at a time. As such, the setup for these sensors is more involved than it would be to just read eight analog pins directly. -To begin reading these inputs, first import the `Analog` and `AnalogMux` classes from `pimoroni` and the pin, address, and gain constants from `servo`: +To begin reading these inputs, first import the `Analog` and `AnalogMux` classes from `pimoroni` and the pin, address, and gain constants from `motor`: ```python from pimoroni import Analog @@ -107,7 +194,7 @@ from motor import motor2040 Then set up three instances of `Analog` for the sensor and fault, voltage, and current sensing: ```python -sen_fault_adc = Analog(motor2040.SHARED_ADC) +sen_adc = Analog(motor2040.SHARED_ADC) vol_adc = Analog(motor2040.SHARED_ADC, motor2040.VOLTAGE_GAIN) cur_adc = Analog(motor2040.SHARED_ADC, motor2040.CURRENT_GAIN, motor2040.SHUNT_RESISTOR, motor2040.CURRENT_OFFSET) @@ -127,7 +214,7 @@ To read the two sensor headers: ```python for addr in range(motor2040.NUM_SENSORS): mux.select(addr + motor2040.SENSOR_1_ADDR) - print("Sensor", addr + 1, "=", sen_fault_adc.read_voltage()) + print("Sensor", addr + 1, "=", sen_adc.read_voltage()) ``` To read the voltage sense: @@ -146,47 +233,64 @@ for addr in range(motor2040.NUM_MOTORS): #### Configuring Pulls -For some sensors, as well as the internal fault sensor, you may need to have the input be pulled high or low before taking a reading. To support this there is an optional `muxed_pin` parameter that can be passed into the `AnalogMux` when creating it, which gives the multiplexer access to the pin to control the pulls. +For the internal fault sensor, as well as some external sensors, you may need to have the input be pulled high or low before taking a reading. To support this there is an optional `muxed_pin` parameter that can be passed into the `AnalogMux` when creating it, which gives the multiplexer access to the pin to control the pulls. ```python mux = AnalogMux(motor2040.ADC_ADDR_0, motor2040.ADC_ADDR_1, motor2040.ADC_ADDR_2, - muxed_pin=Pin(servo2040.SHARED_ADC)) + muxed_pin=Pin(motor2040.SHARED_ADC)) ``` From there the pull state of each of the multiplexer's addresses can be configured independently by calling `.configure_pull()`, with the address and the pull state (either `Pin.PULL_UP`, `Pin.PULL_DOWN`, or `None`). The below example shows how to set both sensor addresses to have pull-downs: ```python -sensor_addrs = list(range(motor2040.SENSOR_1_ADDR, motor2040.SENSOR_2_ADDR + 1)) -for addr in sensor_addrs: - mux.configure_pull(addr, Pin.PULL_DOWN) +for addr in range(motor2040.NUM_SENSORS): + mux.configure_pull(addr + motor2040.SENSOR_1_ADDR, Pin.PULL_DOWN) ``` #### Fault Sensing -The drivers on Motor 2040 can detect when there is a fault with their connected motors, such as if a short occurs, and shut themselves off for safety. As part of this they also signal that a fault has occurred, which can be read. The way they signal this is by pulling the line to ground. This means that the line needs to be high, and so the 'AnalogMux' needs to be configured accordingly. +The drivers on Motor 2040 can detect when there is a fault with their connected motors and shut themselves off for safety. When this occurs they also signal the fault by pulling a line to ground, but requires that the line be pulled up by default. + +The `AnalogMux` can be set with a pull up on the fault sensor line, by doing the following: ```python -adc_as_io = Pin(motor2040.SHARED_ADC) -mux = AnalogMux(motor2040.ADC_ADDR_0, motor2040.ADC_ADDR_1, motor2040.ADC_ADDR_2, - muxed_pin=adc_as_io) mux.configure_pull(motor2040.FAULT_SENSE_ADDR, Pin.PULL_UP) ``` -Then in your main code: +Then the fault can be read as a digital value calling `.read()` on the `AnalogMux` itself, with the value being inverted with `not` to give `True` for when a fault has occurred: ```python mux.select(motor2040.FAULT_SENSE_ADDR) -if not adc_as_io.value(): - print("Fault!") -else: - print("No Fault") +print("Fault =", not mux.read()) ``` -//TODO make this neater +This internally reads the value of the `Pin` provided to `muxed_pin` during the creation of the `mux` object, so could be used to read any digital sensors attached to the external sensor headers of Motor 2040 too. -#### Controlling the LED +### Reading the Encoders -Between Motor 2040's for motor connectors is a single addressable RGB LEDs. This works using the same chainable 1-wire signalling as WS2812 LED's, commonly known as Neopixels. As such, it can be controlled using the same Plasma Library used by the [Pimoroni Plasma 2040](https://shop.pimoroni.com/products/plasma-2040). +On the top edge of Motor 2040 are four JST-SH 6 pin connectors for N20 style motors with magnetic encoders. With these encoders you can measure the position and speed or your motors, opening up many advanced possibilities! + +To start using encoders, you will need to first import the `Encoder` class. +```python +from encoder import Encoder +``` + +To create your encoder, specify the PIO, PIO state-machine and GPIO pins it will be connected to, and pass them into `Encoder`. For this example we will use one of the handy constants of the `motor2040`. +```python +from encoder import MMME_CPR +from motor import motor2040 +enc = Encoder(0, 0, motor2040.ENCODER_A, counts_per_rev=MMME_CPR, count_microsteps=True) +``` +Motor encoders often have a different number of counts per revolution than a traditional rotary encoder. For our MMME shims the `MMME_CPR` constant is provided. Also, as motor encoders lack the tactile steps of rotary encoders, their counting resolution can be increased by a factor of four by including each microstep. + +With the created encoder class, the current position can be read by calling `.revolutions()`, `.degrees()` or `.radians()`. + +For full details on encoders, including how to read speeds, please refer to the [Encoder Library](https://github.com/pimoroni/pimoroni-pico/tree/main/micropython/modules/encoder). + + +### Controlling the LED + +Between Motor 2040's four motor connectors is a single addressable RGB LEDs. This works using the same chainable 1-wire signalling as WS2812 LED's, commonly known as Neopixels. As such, it can be controlled using the Plasma Library, as used by the [Pimoroni Plasma 2040](https://shop.pimoroni.com/products/plasma-2040). To set up the LED, first import the `WS2812` class from the `plasma` module and the pin constants for the LED from `motor`: ```python @@ -212,7 +316,7 @@ For more information on how to control the LED, please refer to the [Plasma Libr The `motor` module contains a `motor2040` sub module with constants for the motor, encoder, LED, sensor and button pins. -#### Motor Pin Tuples +#### Motor Pins * `MOTOR_A` = `(4, 5)` * `MOTOR_B` = `(6, 7)` @@ -220,7 +324,7 @@ The `motor` module contains a `motor2040` sub module with constants for the moto * `MOTOR_D` = `(10, 11)` -#### Encoder Pin Tuples +#### Encoder Pins * `ENCODER_A` = `(0, 1)` * `ENCODER_B` = `(2, 3)` @@ -295,8 +399,8 @@ The `motor2040` sub module also contains other constants to help with the contro * `VOLTAGE_GAIN` = `0.28058` * `SHUNT_RESISTOR` = `0.47` -* `CURRENT_GAIN` = `5` -* `CURRENT_OFFSET` = `-0.02` +* `CURRENT_GAIN` = `1` +* `CURRENT_OFFSET` = `-0.005` ## Motor @@ -307,7 +411,7 @@ To start using motors with your Motor 2040, you will need to first import the `M ```python from motor import Motor, motor2040 ``` -If you are using another RP2040 based board, then `motor2040` can be omitted from the above line +If you are using another RP2040 based board, then `motor2040` can be omitted from the above line. To create your motor, choose which GPIO pins it will be connected to, and pass that into `Motor`. For this example we will use one of the handy constants of the `motor2040`. ```python @@ -331,70 +435,117 @@ From here the motor can be controlled in several ways. These are covered in more ### Control by Speed -The most intuitive way of controlling a motor is by speed. Speed can be any number that has a real-world meaning for that type of motor, for example revolutions per minute, or the linear or angular speed of the mechanism it is driving. By default the speed is a value ranging from `-1.0` to `1.0` but this can be changed by setting a new `speed_scale`. See [Configuration](#configuration) for more details. +The most intuitive way of controlling a motor is by speed. Speed can be any number that has a real-world meaning for that type of motor, for example revolutions per minute, or the linear or angular speed of the mechanism it is driving. By default the speed is a value ranging from `-1.0` to `1.0` but this can be changed by setting a new `speed_scale`. See [Calibration](#calibration) for more details. The speed of a motor can be set by calling `.speed(speed)`, which takes a float as its `speed` input. If the motor is disabled, this will enable it. The resulting duty cycle will also be stored. To read back the current speed of the motor, call `.speed()` without any input. If the motor is disabled, this will be the last speed that was provided when enabled. -At this time the speed of a motor is the same as `m.duty() * m.speed_scale()`, though there could be the option in the future to add a curve to a motor's speed that make the relationship between speed and duty cycle become non-linear. + +#### Full Speed + +To simplify certain motion patterns, a motor can be commanded to its full negative, and full positive speeds. These are performed by calling `.full_negative()`, and `.full_positive()`, respectively. If the motor is disabled, these will enable it. + +The value of the full negative and full positive speed can be read back using `.speed_scale()`. This can be useful as an input to equations that provide numbers directly to `.speed(speed)`, for example. -#### Common Speeds +#### Stopping -To simplify certain motion patterns, a motor can be commanded to three common speeds. These are, full negative, full positive, and stopped. These are performed by calling `.full_negative()`, `.full_positive()`, and `.stop()`, respectively. If the motor is disabled, these will enable it. +The easiest way to stop a motor is by calling `.stop()`. This is equivalent to calling `.speed(0.0)` and stops the motor using the currently assigned decay mode of the `Motor` object. See [Decay Modes](#decay-modes) for more details. -The full negative and full positive speed can be read back using `.speed_scale()`. This can be useful as an input to equations that provide numbers directly to `.speed(speed)`, for example. +It is also possible to explicitly have the motor coast or brake to a stop by calling `.coast()` or `.brake()`. + +If the motor is disabled, these will enable it. + + +### Calibration + +It is very rare for a motor to perfectly drive at the speed we want them to. As such, the `Motor` class offers two parameters for adjusting how the value provided to `.speed(speed)` is converted to the PWM duty cycle that is actually sent to the motor, a speed scale, and a zeropoint. + +Speed scale, as the name implies, is a value that scalues the duty cycle up or down to better reflect the measured speed of the motor when driving at `.full_negative()` or `.full_positive()`. This can be set by calling `.speed_scale(speed_scale)`, which accepts a value greater than `0.0`. The current speed scale can also be read by calling `.speed_scale()`. + +Zeropoint is a value that sets what duty cycle should count as the zero speed of the motor. By default this is `0.0` and usually it is fine to leave it at that, but there are cases at low speeds where the expected speed does not match the measured speed, which small adjustments to the zeropoint will fix. This can be set by calling `.zeropoint(zeropoint)`, which accepts a value from `0.0` to less than `1.0`. The current zeropoint can also be read by calling `.zeropoint()`. + +Both parameters can also be provided during the creation of a new `Motor` object. ### Control by Percent Sometimes there are projects where a motor needs to drive based on the reading from a sensor or another device, but the numbers given out are not easy to convert to speeds the motor accepts. To overcome this the library lets you drive the motor at a percent between its negative and positive speeds, or two speeds provided, based on that input. -With an input between `-1.0` and `1.0`, a motor can be driven at a percent between its negative and positive speeds using `.at_percent(in)`. +With an input between `-1.0` and `1.0`, a motor can be set to a percent between its negative and positive speeds using `.to_percent(in)`. -With an input between a provided min and max, a motor can be driven at a percent between its negative and positive speeds using `.at_percent(in, in_min, in_max)`. +With an input between a provided min and max, a motor can be set to a percent between its negative and positive speeds using `.to_percent(in, in_min, in_max)`. -With an input between a provided min and max, a motor can be driven at a percent between two provided speeds using `.at_percent(in, in_min, value_min, value_max)`. +With an input between a provided min and max, a motor can be set to a percent between two provided speeds using `.to_percent(in, in_min, value_min, value_max)`. If the motor is disabled, these will enable it. ### Control by Duty Cycle -At a hardware level DC motors operate by receiving a voltage across their two terminals, with positive causing a motion in one direction and negative causing a motion in the other. To avoid needing a negative voltage supply, motor drivers employ a H-Bridge arrangement of transistors or mosfets to flip which side of the motor is connected to ground and which is connected to power. By rapidly turing these transistors or mosfets on and off both the speed and direction of the motor can be varied. The common way this is achieved is by using a pair of pulse width modulated signals, where the duty cycle of the active signal controls the speed, and which signal is active controls the direction. Braking can also be controlled (see //TODO) +Motor drivers accept pulse width modulated (PWM) signals to control the speed and direction of their connected motors. The percentage of time that these signals are active for is know as their duty cycle. This is typically measured as a value between `0.0` and `1.0`, but as motors use two pins for their control signals, here negative values are added to denote the reverse direction. The duty cycle of a motor can be set by calling `.duty(duty)`, which takes a float from `-1.0` to `1.0` as its `duty` input. If the motor is disabled this will enable it. This function will also recalculate the related speed. To read back the current duty cycle of the motor, call `.duty()` without any input. If the motor is disabled, this will be the last duty that was provided when enabled. -At this time the duty cycle of a motor is the same as `m.speed() / m.speed_scale()`, though there could be the option in the future to add a curve to a motor's speed that make the relationship between speed and duty cycle become non-linear. + +#### Duty Deadzone + +Most motors have a duty cycle value below which their is too much friction for them to move. This may not be a concern, except for when running motors at audable frequencies, where the buzzing of the motor trying to move without success can get annoying. + +To overcome this, a duty cycle deadzone can be set on a per motor basis by calling `.deadzone(deadzone)`, which accepts a float from `0.0` to less than `1.0`. Whenever a duty cycle is set, either directly or via speed or percent functions, it will only be output to the motor if it is greater than or equal to the deadzone. If it's below, the motor will be stopped instead. By default the deadzone is `0.05`. + +To read back the current duty deadzone of the motor, call `.deadzone()` without any input. + +Note that the motor keeps track of its duty cycle, so if the deadzone is changed the check will be performed again, and may either start a motor that was previously stopped or visa versa. ### Frequency Control -Motors can be driven at a variety of frequencies, with a common values being above the range of human hearing. As such this library uses 25KHz as its default, but this can easily be changed. +Motors can be driven at a variety of frequencies, with common values being above the range of human hearing. As such this library uses 25KHz as its default, but this can easily be changed. -The frequency (in Hz) of a motor can be set by calling `.frequency(freq)`, which takes a float as its `freq` input. //TODO The supported range of this input is `10` to `350` Hz. +The frequency (in Hz) of a motor can be set by calling `.frequency(freq)`, which takes a float as its `freq` input. The supported range of this input is `10` Hz to `400` KHz, though not all motor drivers can handle the very high frequencies. To read back the current frequency (in Hz) of the motor, call `.frequency()` without any input. -Note that changing the frequency does not change the duty cycle sent to the motors, only how frequently pulses are sent. - +Note that changing the frequency does not change the duty cycle or speed sent to the motors, only how frequently pulses are sent. ### Configuration -There are several options for configuring a motor. As mentioned in earlier sections, the first is to adjust their speed scale. There is also the option to change their direction, change their decay mode, and add in a dead-zone. This is a percentage of the duty cycle below which the motor will be stopped. This is included to avoid annoying situations where the duty cycle is too low for a motor to move, but enough for it to make an audable sound. +### Direction -Decay mode is //TODO `.brake()` `.coast()` +The driving direction of a motor can be changed either by providing `direction=REVERSED_DIR` when creating the `Motor` object, or by calling `.direction(REVERSED_DIR)` at any time in code. The `REVERSED_DIR` constant comes from the `pimoroni` module. There is also a `NORMAL_DIR` constant, though this is the default. + + +### Decay Mode + +If you have ever had a motor directly connected to a power source and turned the power off, or disconnected the wire, you may have noticed that the motor continues to spin for a second or two before it reaches a stop. This is because the magnetic field the power source was generating has decayed away quickly, so the only thing slowing the motor down is friction. This results in the motor coasting to a stop. + +By contrast, if you were to wire your circuit up such that instead of disconnecting the power, the off position joined the two ends of the motor together, it would take longer for the magnetic field to decay away. This has the effect of braking the motor, causing it to stop quicker than with friction alone. + +These examples describe the two decay modes supported by the `Motor` class, `FAST_DECAY`, and `SLOW_DECAY`, respectively. Generally slow decay offers better motor performance, particularly with low speeds, so this is the default when creating a new `Motor`. + +If fast decay is wanted then it can either be changed by providing `mode=FAST_DECAY` during the class creation or by calling `.decay_mode(FAST_DECAY)`. The current decay mode can also be read with `.decay_mode()`. + +For more information about motor decay modes, it's highly recommended that you check out the Adafruit Learn Guide titled [Improve Brushed DC Motor Performance](https://learn.adafruit.com/improve-brushed-dc-motor-performance) + + +### Driver Type + +There are two common types of DC motor drivers, based on the signals they expect for controlling the motor. +* Dual PWMs, where both pins control the speed, direction, and decay mode of the motor. +* Phase/Enable, where a single PWM pin controls speed and a single GPIO pin controls direction. There is no decay mode control. + +By default all `Motor` objects are initialised for dual PWM drivers, but if you are using the other type of driver this can be configured by providing `ph_en_driver=True` during object creation. ### Function Reference Here is the complete list of functions available on the `Motor` class: ```python -//TODO -Motor(pins, calibration=ANGULAR, freq=50) # calibration can either be an integer or a Calibration class +Motor(pins, direction=NORMAL_DIR, speed_scale=1.0, zeropoint=0.0, deadzone=0.05, freq=25000, mode=SLOW_DECAY, ph_en_driver=False) pins() enable() disable() @@ -410,19 +561,33 @@ coast() brake() full_negative() full_positive() -at_percent(in) -at_percent(in, in_min, in_max) -at_percent(in, in_min, in_max, speed_min, speed_max) +to_percent(in) +to_percent(in, in_min, in_max) +to_percent(in, in_min, in_max, speed_min, speed_max) direction() direction(direction) speed_scale() speed_scale(speed_scale) +zeropoint() +zeropoint(zeropoint) deadzone() deadzone(deadzone) decay_mode() decay_mode(mode) ``` +### Constants Reference + +Here is the complete list of constants on the `motor` module: + +* `FAST_DECAY` = `0` +* `SLOW_DECAY` = `1` + +Here are useful constants from the `pimoroni` module: + +* `NORMAL_DIR` = `0` +* `REVERSED_DIR` = `1` + ### PWM Limitations diff --git a/micropython/modules/motor/motor.c b/micropython/modules/motor/motor.c index f3d6ee53..e1d1731d 100644 --- a/micropython/modules/motor/motor.c +++ b/micropython/modules/motor/motor.c @@ -78,6 +78,7 @@ STATIC const mp_rom_map_elem_t Motor_locals_dict_table[] = { { MP_ROM_QSTR(MP_QSTR_to_percent), MP_ROM_PTR(&Motor_to_percent_obj) }, { MP_ROM_QSTR(MP_QSTR_direction), MP_ROM_PTR(&Motor_direction_obj) }, { MP_ROM_QSTR(MP_QSTR_speed_scale), MP_ROM_PTR(&Motor_speed_scale_obj) }, + { MP_ROM_QSTR(MP_QSTR_zeropoint), MP_ROM_PTR(&Motor_zeropoint_obj) }, { MP_ROM_QSTR(MP_QSTR_deadzone), MP_ROM_PTR(&Motor_deadzone_obj) }, { MP_ROM_QSTR(MP_QSTR_decay_mode), MP_ROM_PTR(&Motor_decay_mode_obj) }, }; diff --git a/micropython/modules/servo/README.md b/micropython/modules/servo/README.md index 7fe0890b..4ebaf934 100644 --- a/micropython/modules/servo/README.md +++ b/micropython/modules/servo/README.md @@ -172,9 +172,9 @@ for addr in sensor_addrs: ``` -#### Controlling the LED Bar +### Controlling the LED Bar -Alongside Servo 2040's six sensor headers are six addressable RGB LEDs. These work using the same chainable 1-wire signalling as WS2812 LED's, commonly known as Neopixels. As such, they can be controlled using the same Plasma Library used by the [Pimoroni Plasma 2040](https://shop.pimoroni.com/products/plasma-2040). +Alongside Servo 2040's six sensor headers are six addressable RGB LEDs. These work using the same chainable 1-wire signalling as WS2812 LED's, commonly known as Neopixels. As such, they can be controlled using the Plasma Library, as used by the [Pimoroni Plasma 2040](https://shop.pimoroni.com/products/plasma-2040). To set up the LED bar, first import the `WS2812` class from the `plasma` module and the pin constants for the LEDs from `servo`: ```python @@ -294,7 +294,7 @@ To start using servos with your Servo 2040, you will need to first import the `S ```python from servo import Servo, servo2040 ``` -If you are using another RP2040 based board, then `servo2040` can be omitted from the above line +If you are using another RP2040 based board, then `servo2040` can be omitted from the above line. To create your servo, choose which GPIO pin it will be connected to, and pass that into `Servo`. For this example we will use one of the handy constants of the `servo2040`. ```python From 6c9f4a9235a084ced0e4312190f240e539e7b508 Mon Sep 17 00:00:00 2001 From: ZodiusInfuser Date: Mon, 9 May 2022 17:26:44 +0100 Subject: [PATCH 47/60] Further Motor documentation --- micropython/modules/motor/README.md | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/micropython/modules/motor/README.md b/micropython/modules/motor/README.md index 8028c041..34348ba4 100644 --- a/micropython/modules/motor/README.md +++ b/micropython/modules/motor/README.md @@ -1,6 +1,7 @@ # Motors and Motor 2040 The Motor library lets you drive DC motors from a Raspberry Pi Pico or any other RP2040-based board via connected h-bridge drivers. + An easy way to add an h-bridge driver to the Pico is to attach a [Pimoroni Pico Motor Shim](https://pimoroni.com/picomotorshim). Alternatively, an RP2040-based board with integrated drivers could be used, such as the [Pimoroni Motor 2040](https://pimoroni.com/motor2040) (coming soon). @@ -17,7 +18,6 @@ This library offers two motor implementations: - [I2C Pins](#i2c-pins) - [Button Pin](#button-pin) - [Other Constants](#other-constants) - - [Motor 2040](#motor-2040) - [Reading the User Button](#reading-the-user-button-1) - [Reading the Sensors](#reading-the-sensors) @@ -110,7 +110,7 @@ state = button_a.raw() The `motor` module contains a `picomotorshim` sub module with constants for the motor and button pins. -#### Motor Pin Tuples +#### Motor Pins * `MOTOR_1` = `(6, 7)` * `MOTOR_2` = `(27, 26)` @@ -281,8 +281,6 @@ from encoder import MMME_CPR from motor import motor2040 enc = Encoder(0, 0, motor2040.ENCODER_A, counts_per_rev=MMME_CPR, count_microsteps=True) ``` -Motor encoders often have a different number of counts per revolution than a traditional rotary encoder. For our MMME shims the `MMME_CPR` constant is provided. Also, as motor encoders lack the tactile steps of rotary encoders, their counting resolution can be increased by a factor of four by including each microstep. - With the created encoder class, the current position can be read by calling `.revolutions()`, `.degrees()` or `.radians()`. For full details on encoders, including how to read speeds, please refer to the [Encoder Library](https://github.com/pimoroni/pimoroni-pico/tree/main/micropython/modules/encoder). @@ -451,14 +449,14 @@ The value of the full negative and full positive speed can be read back using `. #### Stopping -The easiest way to stop a motor is by calling `.stop()`. This is equivalent to calling `.speed(0.0)` and stops the motor using the currently assigned decay mode of the `Motor` object. See [Decay Modes](#decay-modes) for more details. +The easiest way to stop a motor is by calling `.stop()`. This is equivalent to calling `.speed(0.0)` and stops the motor using the currently assigned decay mode of the `Motor` object. See [Decay Mode](#decay-mode) for more details. It is also possible to explicitly have the motor coast or brake to a stop by calling `.coast()` or `.brake()`. If the motor is disabled, these will enable it. -### Calibration +#### Calibration It is very rare for a motor to perfectly drive at the speed we want them to. As such, the `Motor` class offers two parameters for adjusting how the value provided to `.speed(speed)` is converted to the PWM duty cycle that is actually sent to the motor, a speed scale, and a zeropoint. From 13ac475b7c48c72275e9956c7d64c99b4ffd6de6 Mon Sep 17 00:00:00 2001 From: ZodiusInfuser Date: Mon, 9 May 2022 18:16:41 +0100 Subject: [PATCH 48/60] Further Motor documentation --- micropython/modules/motor/README.md | 240 +++++++++++----------------- 1 file changed, 89 insertions(+), 151 deletions(-) diff --git a/micropython/modules/motor/README.md b/micropython/modules/motor/README.md index 34348ba4..329d3800 100644 --- a/micropython/modules/motor/README.md +++ b/micropython/modules/motor/README.md @@ -27,7 +27,7 @@ This library offers two motor implementations: - [Controlling the LED](#controlling-the-led) - [Pin Constants](#pin-constants-1) - [Motor Pins](#motor-pins-1) - - [Encoder Pins](#encoder-pins-1) + - [Encoder Pins](#encoder-pins) - [LED Pin](#led-pin) - [I2C Pins](#i2c-pins-1) - [Button Pin](#button-pin-1) @@ -460,7 +460,7 @@ If the motor is disabled, these will enable it. It is very rare for a motor to perfectly drive at the speed we want them to. As such, the `Motor` class offers two parameters for adjusting how the value provided to `.speed(speed)` is converted to the PWM duty cycle that is actually sent to the motor, a speed scale, and a zeropoint. -Speed scale, as the name implies, is a value that scalues the duty cycle up or down to better reflect the measured speed of the motor when driving at `.full_negative()` or `.full_positive()`. This can be set by calling `.speed_scale(speed_scale)`, which accepts a value greater than `0.0`. The current speed scale can also be read by calling `.speed_scale()`. +Speed scale, as the name implies, is a value that scales the duty cycle up or down to better reflect the measured speed of the motor when driving at full speed. This can be set by calling `.speed_scale(speed_scale)`, which accepts a value greater than `0.0`. The current speed scale can also be read by calling `.speed_scale()`. Zeropoint is a value that sets what duty cycle should count as the zero speed of the motor. By default this is `0.0` and usually it is fine to leave it at that, but there are cases at low speeds where the expected speed does not match the measured speed, which small adjustments to the zeropoint will fix. This can be set by calling `.zeropoint(zeropoint)`, which accepts a value from `0.0` to less than `1.0`. The current zeropoint can also be read by calling `.zeropoint()`. @@ -516,6 +516,8 @@ Note that changing the frequency does not change the duty cycle or speed sent to The driving direction of a motor can be changed either by providing `direction=REVERSED_DIR` when creating the `Motor` object, or by calling `.direction(REVERSED_DIR)` at any time in code. The `REVERSED_DIR` constant comes from the `pimoroni` module. There is also a `NORMAL_DIR` constant, though this is the default. +The current direction of a motor can be read back by calling `.direction()`. + ### Decay Mode @@ -613,7 +615,7 @@ An alternative way of controlling motors with your Motor 2040 is to use the `Mot ```python from motor import MotorCluster, motor2040 ``` -(If you are using another RP2040 based board, then `motor2040` can be omitted from the above line) +If you are using another RP2040 based board, then `motor2040` can be omitted from the above line. The next step is to choose which GPIO pins the cluster will be connected to and store them in a `list`. For example, using the handy constants of the `motor2040`, the below line creates the list `[ (4, 5), (6, 7), (8, 9), (10, 11) ]` ```python @@ -621,7 +623,6 @@ pins = [ motor2040.MOTOR_A, motor2040.MOTOR_B, motor2040.MOTOR_C, motor2040.MOTO ``` To create your motor cluster, specify the PIO, PIO state-machine and GPIO pins you chose a moment ago, and pass those into `MotorCluster`. - ```python cluster = MotorCluster(0, 0, pins) ``` @@ -653,69 +654,105 @@ From here the motors can be controlled in several ways. These are covered in mor ### Control by Speed -The most intuitive way of controlling a motor is by speed. Speed can be any number that has a real-world meaning for that type of motor, for example revolutions per minute, or the linear or angular speed of the mechanism it is driving. By default the speed is a value ranging from `-1.0` to `1.0` but this can be changed by setting a new `speed_scale`. See [Configuration](#configuration-1) for more details. +The most intuitive way of controlling a motor is by speed. Speed can be any number that has a real-world meaning for that type of motor, for example revolutions per minute, or the linear or angular speed of the mechanism it is driving. By default the speed is a value ranging from `-1.0` to `1.0` but this can be changed by setting a new `speed_scale`. See [Calibration](#calibration-1) for more details. -The speed of a motor on a cluster can be set calling `.speed(motor, speed)` or `.all_at_speed(speed)`, which take a float as their `speed` input. If a motor is disabled, these will enable it. The resulting duty cycle will also be stored. +The speed of a motor on a cluster can be set calling `.speed(motor, speed)` or `.all_to_speed(speed)`, which take a float as their `speed` input. If a motor on the cluster is disabled, these will enable it. The resulting duty cycle will also be stored. To read back the current speed of a motor on the cluster, call `.speed(motor)`. If the motor is disabled, this will be the last speed that was provided when enabled. -#### Common Speeds +#### Full Speed -To simplify certain motion patterns, motors on a cluster can be commanded to three common speeds. These are, full negative, full positive, and stopped. These are performed by calling `.full_negative(motor)`, `.full_positive(motor)`, and `.stop(servo)`, respectively. If the motor is disabled, these will enable it. There are also `.all_full_negative()`, `.all_full_positive()`, and `.stop_all()` for having all the motors on the cluster drive at once. +To simplify certain motion patterns, motors on a cluster can be commanded to their full negative, and full positive speeds. These are performed by calling `.full_negative(motor)`, and `.full_positive(motor)`, respectively. If the motor is disabled, these will enable it. There are also `.all_full_negative()`, and `.all_full_positive()` for having all the motors on the cluster drive at once. -The full negative and full positive speed of each motor on a cluster can be read back using `.speed_scale(motor)`. This can be useful as an input to equations that provide numbers directly to `.speed(motor, speed)`, for example. +The value of the full negative and full positive speed of each motor on a cluster can be read back using `.speed_scale(motor)`. This can be useful as an input to equations that provide numbers directly to `.speed(motor, speed)`, for example. + + +#### Stopping + +The easiest way to stop a motor on a cluster is by calling `.stop(motor)`. This is equivalent to calling `.speed(motor, 0.0)` and stops the motor using the currently assigned decay mode of the `Motor` object. See [Decay Mode](#decay-mode) for more details. All motors can be stopped at once by calling `.stop_all()`. + +It is also possible to explicitly have the motors on a cluster coast or brake to a stop by calling `.coast(motor)`, `.coast_all()`, `.brake(motor)`, or `.brake_all()`. + +If a motor on the cluster is disabled, these will enable it. + +#### Calibration + +It is very rare for a motor to perfectly drive at the speed we want them to. As such, the each motor on a cluster offers two parameters for adjusting how the value provided to `.speed(speed)` is converted to the PWM duty cycle that is actually sent to each motor, a speed scale, and a zeropoint. + +Speed scale, as the name implies, is a value that scales the duty cycle up or down to better reflect the measured speed of a motor on the cluster when driving at full speed. This can be set for each motor by calling `.speed_scale(motor, speed_scale)` or `.all_speed_scales(speed_scale)`, which both accept a value greater than `0.0`. The current speed scale of a motor can also be read by calling `.speed_scale(motor)`. + +Zeropoint is a value that sets what duty cycle should count as the zero speed of a motor on a cluster. By default this is `0.0` and usually it is fine to leave it at that, but there are cases at low speeds where the expected speed does not match the measured speed, which small adjustments to the zeropoint will fix. This can be set by calling `.zeropoint(motor, zeropoint)` or `.all_zeropoints(zeropoint)`, which both accept a value from `0.0` to less than `1.0`. The current zeropoint of a motor can also be read by calling `.zeropoint(motor)`. + +Both parameters can also be provided during the creation of a new `MotorCluster` object, though this will apply to all motors. ### Control by Percent Sometimes there are projects where motors need to move based on the readings from sensors or another devices, but the numbers given out are not easy to convert to speeds the motors accept. To overcome this the library lets you drive the motors on a cluster at a percent between their negative and positive speeds, or two speeds provided, based on that input. -With an input between `-1.0` and `1.0`, a motor on a cluster can be driven at a percent between its negative and positive speeds using `.at_percent(motor, in)`, or all motors using `.all_at_percent(in)`. +With an input between `-1.0` and `1.0`, a motor on a cluster can be set to a percent between its negative and positive speeds using `.to_percent(motor, in)`, or all motors using `.all_to_percent(in)`. -With an input between a provided min and max, a motor on a cluster can be driven at a percent between its negative and postive speeds using `.at_percent(motor, in, in_min, in_max)`, or all motors using `.all_at_percent(in, in_min, in_max)`. +With an input between a provided min and max, a motor on a cluster can be set to a percent between its negative and postive speeds using `.at_percent(motor, in, in_min, in_max)`, or all motors using `.all_at_percent(in, in_min, in_max)`. -With an input between a provided min and max, a motor on a cluster can be driven at a percent between two provided speeds using `.at_percent(motor, in, in_min, speed_min, speed_max)`, or all motors using `.all_at_percent(in, in_min, speed_min, speed_max)`. +With an input between a provided min and max, a motor on a cluster can be set to a percent between two provided speeds using `.at_percent(motor, in, in_min, speed_min, speed_max)`, or all motors using `.all_at_percent(in, in_min, speed_min, speed_max)`. -If the motor is disabled, these will enable it. +If a motor on the cluster is disabled, these will enable it. ### Control by Duty Cycle -At a hardware level DC motors operate by receiving a voltage across their two terminals, with positive causing a motion in one direction and negative causing a motion in the other. To avoid needing a negative voltage supply, motor drivers employ a H-Bridge arrangement of transistors or mosfets to flip which side of the motor is connected to ground and which is connected to power. By rapidly turing these transistors or mosfets on and off both the speed and direction of the motor can be varied. The common way this is achieved is by using a pair of pulse width modulated signals, where the duty cycle of the active signal controls the speed, and which signal is active controls the direction. Braking can also be controlled (see //TODO) +Motor drivers accept pulse width modulated (PWM) signals to control the speed and direction of their connected motors. The percentage of time that these signals are active for is know as their duty cycle. This is typically measured as a value between `0.0` and `1.0`, but as motors use two pins for their control signals, here negative values are added to denote the reverse direction. -The duty cycle of a motor on a cluster can be set by calling `.duty(motor, duty)` or `.all_at_duty(duty)`, which take a float as their `duty` input. If a motor is disabled, these will enable it. These functions will also recalculate the related speed. +The duty cycle of a motor on the cluster can be set by calling `.duty(motor, duty)` or `.all_to_duty(duty)`, which take a float from `-1.0` to `1.0` as their `duty` input. If a motor on the cluster is disabled these will enable it. These functions will also recalculate the related speed. -To read back the current duty cycle of a motor on a cluster, call `.duty(motor)`. If the motor is disabled, this will be the last duty that was provided when enabled. +To read back the current duty cycle of the motor, call `.duty()` without any input. If the motor is disabled, this will be the last duty that was provided when enabled. ### Frequency Control Motors can be driven at a variety of frequencies, with a common values being above the range of human hearing. As such this library uses 25KHz as its default, but this can easily be changed. -The frequency (in Hz) of all the motors on a cluster can be set by calling `.frequency(freq)`, which takes a float as its `freq` input. //TODO The supported range of this input is `10` to `350` Hz. Due to how `MotorCluster` works, there is no way to set independent frequencies for each motor. +The frequency (in Hz) of all the motors on a cluster can be set by calling `.frequency(freq)`, which takes a float as its `freq` input. The supported range of this input is `10` Hz to `400` KHz, though not all motor drivers can handle the very high frequencies. Due to how `MotorCluster` works, there is no way to set independent frequencies for each motor. To read back the current frequency (in Hz) of all the motors on a cluster, call `.frequency()` without any input. -Note that changing the frequency does not change the duty cycle sent to the motors, only how frequently pulses are sent. +Note that changing the frequency does not change the duty cycle or speed sent to the motors, only how frequently pulses are sent. Also, be aware that currently the frequency changes immediately, even if part-way through outputting a pulse. It is therefore recommended to disable all motors first before changing the frequency. ### Phase Control -When dealing with many servos, there can often be large current draw spikes caused by them all responding to pulses at the same time. To minimise this, the ServoCluster class allows for the start time of each servo's pulses to be delayed by a percentage of the available time period. This is called their phase. +The MotorCluster class allows for the start time of each motor's pulses to be delayed by a percentage of the available time period. This is called their phase. -The phase of a servo on a cluster can be set by calling `.phase(servo, phase)` or `.all_to_phase(phase)`, which take a float between `0.0` and `1.0` as their `phase` input. +The phase of a motor on a cluster can be set by calling `.phase(motor, phase)` or `.all_to_phase(phase)`, which take a float between `0.0` and `1.0` as their `phase` input. -To read back the current phase of a servo on a cluster, call `.phase(servo)`. +To read back the current phase of a motor on a cluster, call `.phase(motor)`. -By default all servos on a cluster will start with different phases unless `auto_phase=False` is provided when creating the `ServoCluster`. +By default all motors on a cluster will start with different phases unless `auto_phase=False` is provided when creating the `MotorCluster`. -### Calibration +### Configuration -There are different types of servos, with `ANGULAR`, `LINEAR`, and `CONTINUOUS` being common. To support these different types, each `ServoCluster` class contains calibration objects for each of its servos that store the specific value to pulse mappings needed for their types. A copy of a servo's calibration on a cluster can be accessed using `.calibration(servo)`. It is also possible to provide a servo on a cluster with a new calibration using `.calibration(servo, calibration)`. +### Direction + +The driving direction of a motor on the cluster can be changed by calling `.direction(motor, REVERSED_DIR)` or `.all_directions(REVERSED_DIR)` at any time in code. The `REVERSED_DIR` constant comes from the `pimoroni` module. There is also a `NORMAL_DIR` constant, though this is the default. + +The current direction of a motor on the cluster can be read back by calling `.direction(motor)`. + + +### Decay Mode + +If you have ever had a motor directly connected to a power source and turned the power off, or disconnected the wire, you may have noticed that the motor continues to spin for a second or two before it reaches a stop. This is because the magnetic field the power source was generating has decayed away quickly, so the only thing slowing the motor down is friction. This results in the motor coasting to a stop. + +By contrast, if you were to wire your circuit up such that instead of disconnecting the power, the off position joined the two ends of the motor together, it would take longer for the magnetic field to decay away. This has the effect of braking the motor, causing it to stop quicker than with friction alone. + +These examples describe the two decay modes supported by the `MotorCluster` class, `FAST_DECAY`, and `SLOW_DECAY`, respectively. Generally slow decay offers better motor performance, particularly with low speeds, so this is the default when creating motors on a new `MotorCluster`. + +If fast decay is wanted then it can be changed by calling `.decay_mode(motor, FAST_DECAY)` or `.all_decay_modes(FAST_DECAY)`. The current decay mode of a motor on the cluster can also be read with `.decay_mode(motor)`. + +For more information about motor decay modes, it's highly recommended that you check out the Adafruit Learn Guide titled [Improve Brushed DC Motor Performance](https://learn.adafruit.com/improve-brushed-dc-motor-performance) ### Delayed Loading @@ -730,8 +767,7 @@ For this purpose, all the functions that modify a motor state on a cluster inclu Here is the complete list of functions available on the `MotorCluster` class: ```python -//TODO -MotorCluster(pio, sm, pins, calibration=ANGULAR, freq=50, auto_phase=True) # calibration can either be an integer or a Calibration class +MotorCluster(pio, sm, pins, direction=NORMAL_DIR, speed_scale=1.0, zeropoint=0.0, deadzone=0.05, freq=25000, mode=SLOW_DECAY, auto_phase=True) count() pins(motor) enable(motor, load=True) @@ -741,32 +777,47 @@ disable_all(load=True) is_enabled(motor) duty(motor) duty(motor, duty, load=True) -all_at_duty(motor, load=True) +all_to_duty(motor, load=True) speed(motor) speed(motor, speed, load=True) -all_at_speed(speed, load=True) +all_to_speed(speed, load=True) phase(motor) phase(motor, phase, load=True) all_to_phase(phase, load=True) frequency() frequency(freq) -full_negative(motor, load=True) -all_full_negative(load=True) -full_positive(motor, load=True) -all_full_positive(load=True) stop(motor, load=True) stop_all(load=True) coast(motor, load=True) coast_all(load=True) brake(motor, load=True) brake_all(load=True) -at_percent(motor, in, load=True) -at_percent(motor, in, in_min, in_max, load=True) -at_percent(motor, in, in_min, in_max, value_min, value_max, load=True) -all_at_percent(in, load=True) -all_at_percent(in, in_min, in_max, load=True) -all_at_percent(in, in_min, in_max, value_min, value_max, load=True) +full_negative(motor, load=True) +all_full_negative(load=True) +full_positive(motor, load=True) +all_full_positive(load=True) +to_percent(motor, in, load=True) +to_percent(motor, in, in_min, in_max, load=True) +to_percent(motor, in, in_min, in_max, speed_min, speed_max, load=True) +all_to_percent(in, load=True) +all_to_percent(in, in_min, in_max, load=True) +all_to_percent(in, in_min, in_max, speed_min, speed_max, load=True) load() +direction(motor) +direction(motor, direction) +all_directions(direction) +speed_scale(motor) +speed_scale(motor, speed_scale) +all_speed_scales(speed_scale) +zeropoint(motor) +zeropoint(motor, zeropoint) +all_zeropoints(zeropoint) +deadzone(motor) +deadzone(motor, deadzone, load=True) +all_deadzones(deadzone, load=True) +decay_mode(motor) +decay_mode(motor, mode, load=True) +all_decay_modes(mode, load=True) ``` @@ -775,116 +826,3 @@ load() The RP2040 features two PIOs with four state machines each. This places a hard limit on how many MotorClusters can be created. As this class is capable of driving all 30 GPIO pins, the only time this limit will be of concern is when motors with different frequencies are wanted, as all the outputs a MotorCluster controls share the same frequency. Relating this to the hardware PWM, think of it as a single PWM slice with up to 30 sub channels, A, B, C, D etc. When creating a MotorCluster, in most cases you'll use `0` for PIO and `0` for PIO state-machine. You should change these though if you plan on running multiple clusters, or using a cluster alongside something else that uses PIO, such as our [Plasma library](https://github.com/pimoroni/pimoroni-pico/tree/main/micropython/modules/plasma). - - -## Configuration - -After using servos for a while, you may notice that some don't quite go to the positions you would expect. Or perhaps you are giving values to a continuous rotation servo in degrees when it would make more sense to use a speed or rpm. To compensate for these cases, each `Servo` class or servo within a `ServoCluster` has an individual `Calibration` class. This class contains a set of pulse-value pairs that are used by the `.value(value)` functions (and those similar) to convert real-world numbers into pulses each servo understand. - -### Common Types - -There are three common `type`s of servo's supported: -* `ANGULAR` = `0` - A servo with a value that ranges from -90 to +90 degrees. -* `LINEAR` = `1` - A servo with a value that ranges from 0 to +1.0. -* `CONTINUOUS` = `2` - A servo with a value that ranges from -1.0 to +1.0. - -By default all `Servo` classes or servo within a `ServoCluster` are `ANGULAR`. This can be changed by providing one of the other types as a parameter during their creation, as shown below: -```python -angular = Servo(servo2040.SERVO_1) # ANGULAR is the default so does not need to be specified here -linear = Servo(servo2040.SERVO_2, LINEAR) -continuous = Servo(servo2040.SERVO_3, CONTINUOUS) -``` - - -### Custom Calibration - -As well as the common types, a custom calibration can also be provided to one or more servos during creation. Below is an example that creates an angular servo that can only travel from -45 degrees to 45 degrees. - -```python -cal = Calibration() -cal.apply_two_pairs(1000, 2000, -45, 45) -s = Servo(servo2040.SERVO_1, cal) -``` - -This could be useful for example if the servo turning beyond those values would cause damage to whatever mechanism it is driving, since it would not be possible to go to angles beyond these unless limits were disabled (see [Limits](#limits)). Also it lets the exact pulse widths matching the angles be set (the `1000` and `2000` in the example). Discovering these values can take some trial and error, and will offen be different for each servo. - - - -# Modifying a Calibration - -It is also possible to access and modify the calibration of a `Servo` or a servo on a `ServoCluster` after their creation. This is done by first getting a copy of the servo's calibration using `.calibration()` or `.calibration(servo)`, modifying its pulses or values, then applying the modified calibration back onto to the servo. - -Below, an angular servo is modified to increase its reported rotation range from 180 degrees to 270 degrees. -```python -wide_angle = Servo(servo2040.SERVO_1) -cal = wide_angle.calibration() -cal.first_value(-135) -cal.last_value(+135) -wide_angle.calibration(cal) -``` - - -### Movement Limits - -As well as providing a mapping between pulses and values, the calibration class also limits a servo from going beyond its minimum and maximum values. In some cases this may not be wanted, so the state of the limits can be modified by calling `.limit_to_calibration(lower, upper)` where `lower` and `upper` are booleans. Additionally, the current state of these limits can be queried by calling `.has_lower_limit()` and `.has_upper_limit()`, respectively. - -A case where you may want to disable limits is if you want a servo to go to a value (e.g. 90 degrees), but are not physically able to get a pulse measurement for that but can do another value instead (e.g. 60 degrees). - -Note, even with limits disables, servos still have hard limits of `400` and `2600` microsecond pulse widths. These are intended to protect servos from receiving pulses that are too far beyond their expected range. These can vary from servo to servo though, with some hitting a physical end-stop before getting to the typical `500` and `2500` associated with -90 and +90 degrees. - - -### Populating a Calibration - -To aid in populating a `Calibration` class, there are five helper functions that fill the class with pulse-value pairs: -* `apply_blank_pairs(size)` - Fills the calibration with the specified number of zero pairs -* `apply_two_pairs(min_pulse, max_pulse, min_value, max_value)` - Fills the calibration with two pairs using the min and max numbers provided -* `apply_three_pairs(min_pulse, mid_pulse, max_pulse, min_value, mid_value, max_value)` - Fills the calibration with three pairs using the min, mid and max numbers provided -* `apply_uniform_pairs(size, min_pulse, max_pulse, min_value, max_value)` - Fills the calibration with the specified number of pairs, interpolated from the min to max numbers provided -* `apply_default_pairs(type)` - Fills the calibration with the pairs of one of the common types - -Once a `Calibration` class contains pairs (as checked `.size() > 0`), these can then be accessed by calling `.pair(index)` and can then be modified by calling `.pair(index, pair)`. The former function returns a list containing the pulse and value of the pair, and the latter accepts a list or tuple containing the pulse and value. For situations when only a single element of each pair is needed, `.pulse(index)` and `.value(index)` will return the current numbers, and `.pulse(index, pulse)` and `.value(index, value)` will override them. - -For further convenience there are functions for accessing and modifying the `.first()` and `.last()` pair/pulse/value of the calibration. - - -### Viewing the Mapping - -To aid in visualising a calibration's pulse-value mapping, the pulse for any given value can be queried by calling `.value_to_pulse(value)`. Similarly, the value for any given pulse can be queried by calling `.pulse_to_value(pulse)`. These are the same functions used by `Servo` and `ServoCluster` when controlling their servos. - - -### Function Reference - -Here is the complete list of functions available on the `Calibration` class: -```python -Calibration() -Calibration(type) -apply_blank_pairs(size) -apply_two_pairs(min_pulse, max_pulse, min_value, max_value) -apply_three_pairs(min_pulse, mid_pulse, max_pulse, min_value, mid_value, max_value) -apply_uniform_pairs(size, min_pulse, max_pulse, min_value, max_value) -apply_default_pairs(type) -size() -pair(index) -pair(index, pair) -pulse(index) -pulse(index, pulse) -value(index) -value(index, value) -first() -first(pair) -first_pulse() -first_pulse(pulse) -first_value() -first_value(value) -last() -last(pair) -last_pulse() -last_pulse(pulse) -last_value() -last_value(value) -has_lower_limit() -has_upper_limit() -limit_to_calibration(lower, upper) -value_to_pulse(value) -pulse_to_value(pulse) -``` From 3bbf733bfb69f4231aed0ae0569e73d546369299 Mon Sep 17 00:00:00 2001 From: ZodiusInfuser Date: Mon, 9 May 2022 18:19:48 +0100 Subject: [PATCH 49/60] Further Motor documentation --- micropython/modules/motor/README.md | 22 +++++++++++++++++++++- 1 file changed, 21 insertions(+), 1 deletion(-) diff --git a/micropython/modules/motor/README.md b/micropython/modules/motor/README.md index 329d3800..f02c1568 100644 --- a/micropython/modules/motor/README.md +++ b/micropython/modules/motor/README.md @@ -57,13 +57,20 @@ This library offers two motor implementations: - [MotorCluster](#motorcluster) - [Getting Started](#getting-started-1) - [Control by Speed](#control-by-speed-1) + - [Full Speed](#full-speed-1) + - [Stopping](#stopping-1) + - [Calibration](#calibration-1) - [Control by Percent](#control-by-percent-1) - [Control by Duty Cycle](#control-by-duty-cycle-1) + - [Duty Deadzone](#duty-deadzone-1) - [Frequency Control](#frequency-control-1) - [Phase Control](#phase-control) - [Configuration](#configuration-1) + - [Direction](#direction-1) + - [Decay Mode](#decay-mode-1) - [Delayed Loading](#delayed-loading) - - [Function Reference](#function-reference-1) + - [Function Reference](#function-reference) + - [Constants Reference](#constants-reference) - [PIO Limitations](#pio-limitations) @@ -821,6 +828,19 @@ all_decay_modes(mode, load=True) ``` +### Constants Reference + +Here is the complete list of constants on the `motor` module: + +* `FAST_DECAY` = `0` +* `SLOW_DECAY` = `1` + +Here are useful constants from the `pimoroni` module: + +* `NORMAL_DIR` = `0` +* `REVERSED_DIR` = `1` + + ### PIO Limitations The RP2040 features two PIOs with four state machines each. This places a hard limit on how many MotorClusters can be created. As this class is capable of driving all 30 GPIO pins, the only time this limit will be of concern is when motors with different frequencies are wanted, as all the outputs a MotorCluster controls share the same frequency. Relating this to the hardware PWM, think of it as a single PWM slice with up to 30 sub channels, A, B, C, D etc. From 71f0ea76ed1cd30e4c05b0e24d63cbb2445c5795 Mon Sep 17 00:00:00 2001 From: ZodiusInfuser Date: Mon, 9 May 2022 18:20:48 +0100 Subject: [PATCH 50/60] Further Motor documentation --- micropython/modules/motor/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/micropython/modules/motor/README.md b/micropython/modules/motor/README.md index f02c1568..f88481ce 100644 --- a/micropython/modules/motor/README.md +++ b/micropython/modules/motor/README.md @@ -677,7 +677,7 @@ The value of the full negative and full positive speed of each motor on a cluste #### Stopping -The easiest way to stop a motor on a cluster is by calling `.stop(motor)`. This is equivalent to calling `.speed(motor, 0.0)` and stops the motor using the currently assigned decay mode of the `Motor` object. See [Decay Mode](#decay-mode) for more details. All motors can be stopped at once by calling `.stop_all()`. +The easiest way to stop a motor on a cluster is by calling `.stop(motor)`. This is equivalent to calling `.speed(motor, 0.0)` and stops the motor using the currently assigned decay mode of the `Motor` object. See [Decay Mode](#decay-mode-1) for more details. All motors can be stopped at once by calling `.stop_all()`. It is also possible to explicitly have the motors on a cluster coast or brake to a stop by calling `.coast(motor)`, `.coast_all()`, `.brake(motor)`, or `.brake_all()`. From b26d13bab17b73e0d65636003ddc79e6b9a0c0ce Mon Sep 17 00:00:00 2001 From: ZodiusInfuser Date: Tue, 10 May 2022 18:34:59 +0100 Subject: [PATCH 51/60] Fix for motor duty cycle not obeying direction --- drivers/motor/motor_state.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/drivers/motor/motor_state.cpp b/drivers/motor/motor_state.cpp index 6d0ad992..84618e97 100644 --- a/drivers/motor/motor_state.cpp +++ b/drivers/motor/motor_state.cpp @@ -31,7 +31,7 @@ namespace motor { } float MotorState::get_duty() const { - return last_enabled_duty; + return (motor_direction == NORMAL_DIR) ? last_enabled_duty : 0.0f - last_enabled_duty; } float MotorState::get_deadzoned_duty() const { @@ -47,6 +47,10 @@ namespace motor { } 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); From c75b35265bb5080e9ce4a14b5e23033bcf2f8683 Mon Sep 17 00:00:00 2001 From: ZodiusInfuser Date: Tue, 10 May 2022 18:35:33 +0100 Subject: [PATCH 52/60] Removed old motor_shim module --- micropython/modules/micropython.cmake | 1 - .../modules/pico_motor_shim/micropython.cmake | 18 --------- .../modules/pico_motor_shim/pico_motor_shim.c | 40 ------------------- .../modules/pico_motor_shim/pico_motor_shim.h | 3 -- 4 files changed, 62 deletions(-) delete mode 100644 micropython/modules/pico_motor_shim/micropython.cmake delete mode 100644 micropython/modules/pico_motor_shim/pico_motor_shim.c delete mode 100644 micropython/modules/pico_motor_shim/pico_motor_shim.h diff --git a/micropython/modules/micropython.cmake b/micropython/modules/micropython.cmake index 0dacef18..7380d885 100644 --- a/micropython/modules/micropython.cmake +++ b/micropython/modules/micropython.cmake @@ -36,7 +36,6 @@ include(pico_unicorn/micropython) include(pico_display/micropython) include(pico_display_2/micropython) include(pico_explorer/micropython) -include(pico_motor_shim/micropython) include(pico_wireless/micropython) include(bitmap_fonts/micropython) diff --git a/micropython/modules/pico_motor_shim/micropython.cmake b/micropython/modules/pico_motor_shim/micropython.cmake deleted file mode 100644 index f444ad49..00000000 --- a/micropython/modules/pico_motor_shim/micropython.cmake +++ /dev/null @@ -1,18 +0,0 @@ -set(MOD_NAME picomotorshim) -string(TOUPPER ${MOD_NAME} MOD_NAME_UPPER) -add_library(usermod_${MOD_NAME} INTERFACE) - -target_sources(usermod_${MOD_NAME} INTERFACE - ${CMAKE_CURRENT_LIST_DIR}/pico_motor_shim.c -) - -target_include_directories(usermod_${MOD_NAME} INTERFACE - ${CMAKE_CURRENT_LIST_DIR} - ${CMAKE_CURRENT_LIST_DIR}/../../../libraries/pico_motor_shim/ -) - -target_compile_definitions(usermod_${MOD_NAME} INTERFACE - -DMODULE_${MOD_NAME_UPPER}_ENABLED=1 -) - -target_link_libraries(usermod INTERFACE usermod_${MOD_NAME}) \ No newline at end of file diff --git a/micropython/modules/pico_motor_shim/pico_motor_shim.c b/micropython/modules/pico_motor_shim/pico_motor_shim.c deleted file mode 100644 index d860bbdd..00000000 --- a/micropython/modules/pico_motor_shim/pico_motor_shim.c +++ /dev/null @@ -1,40 +0,0 @@ -#include "pico_motor_shim.h" - -/***** Constants *****/ -enum pins -{ - BUTTON_A = 2, - - MOTOR_1_POS = 6, - MOTOR_1_NEG = 7, - - MOTOR_2_POS = 27, - MOTOR_2_NEG = 26, -}; - - -//////////////////////////////////////////////////////////////////////////////////////////////////// -// picomotorshim Module -//////////////////////////////////////////////////////////////////////////////////////////////////// - -/***** Globals Table *****/ -STATIC const mp_map_elem_t picomotorshim_globals_table[] = { - { MP_ROM_QSTR(MP_QSTR___name__), MP_ROM_QSTR(MP_QSTR_picomotorshim) }, - { MP_ROM_QSTR(MP_QSTR_PIN_BUTTON_A), MP_ROM_INT(BUTTON_A) }, - { MP_ROM_QSTR(MP_QSTR_PIN_MOTOR_1_POS), MP_ROM_INT(MOTOR_1_POS) }, - { MP_ROM_QSTR(MP_QSTR_PIN_MOTOR_1_NEG), MP_ROM_INT(MOTOR_1_NEG) }, - { MP_ROM_QSTR(MP_QSTR_PIN_MOTOR_2_POS), MP_ROM_INT(MOTOR_2_POS) }, - { MP_ROM_QSTR(MP_QSTR_PIN_MOTOR_2_NEG), MP_ROM_INT(MOTOR_2_NEG) }, -}; -STATIC MP_DEFINE_CONST_DICT(mp_module_picomotorshim_globals, picomotorshim_globals_table); - -/***** Module Definition *****/ -const mp_obj_module_t picomotorshim_user_cmodule = { - .base = { &mp_type_module }, - .globals = (mp_obj_dict_t*)&mp_module_picomotorshim_globals, -}; - -//////////////////////////////////////////////////////////////////////////////////////////////////// -MP_REGISTER_MODULE(MP_QSTR_picomotorshim, picomotorshim_user_cmodule, MODULE_PICOMOTORSHIM_ENABLED); -//////////////////////////////////////////////////////////////////////////////////////////////////// -//////////////////////////////////////////////////////////////////////////////////////////////////// \ No newline at end of file diff --git a/micropython/modules/pico_motor_shim/pico_motor_shim.h b/micropython/modules/pico_motor_shim/pico_motor_shim.h deleted file mode 100644 index 707d6769..00000000 --- a/micropython/modules/pico_motor_shim/pico_motor_shim.h +++ /dev/null @@ -1,3 +0,0 @@ -// Include MicroPython API. -#include "py/runtime.h" -#include "py/objstr.h" \ No newline at end of file From 59019ab850731590591f902f8a7cf14c9bcf2f4e Mon Sep 17 00:00:00 2001 From: ZodiusInfuser Date: Tue, 10 May 2022 18:36:44 +0100 Subject: [PATCH 53/60] Added MP examples for pico motor shim, and tweaked others --- examples/pico_motor_shim/song/demo.cpp | 14 +- .../examples/motor2040/multiple_motors.py | 4 +- .../motor2040/quad_velocity_sequence.py | 2 +- .../examples/pico_motor_shim/dual_motors.py | 71 +++++++ .../examples/pico_motor_shim/motor_song.py | 191 ++++++++++++++++++ .../examples/pico_motor_shim/movements.py | 109 ++++++++++ .../examples/pico_motor_shim/single_motor.py | 54 +++++ .../examples/pico_motor_shim/stop_motors.py | 11 + 8 files changed, 446 insertions(+), 10 deletions(-) create mode 100644 micropython/examples/pico_motor_shim/dual_motors.py create mode 100644 micropython/examples/pico_motor_shim/motor_song.py create mode 100644 micropython/examples/pico_motor_shim/movements.py create mode 100644 micropython/examples/pico_motor_shim/single_motor.py create mode 100644 micropython/examples/pico_motor_shim/stop_motors.py diff --git a/examples/pico_motor_shim/song/demo.cpp b/examples/pico_motor_shim/song/demo.cpp index bd2f2ad5..584dd3a0 100644 --- a/examples/pico_motor_shim/song/demo.cpp +++ b/examples/pico_motor_shim/song/demo.cpp @@ -39,13 +39,8 @@ static const uint STATIONARY_TOGGLE_US = 2000; Button button_a(pico_motor_shim::BUTTON_A, Polarity::ACTIVE_LOW, 0); -#ifdef USE_FAST_DECAY - Motor motor_1(pico_motor_shim::MOTOR_1, NORMAL_DIR, MotorState::DEFAULT_SPEED_SCALE, MotorState::DEFAULT_FREQUENCY, FAST_DECAY); - Motor motor_2(pico_motor_shim::MOTOR_2, NORMAL_DIR, MotorState::DEFAULT_SPEED_SCALE, MotorState::DEFAULT_FREQUENCY, FAST_DECAY); -#else - Motor motor_1(pico_motor_shim::MOTOR_1, NORMAL_DIR, MotorState::DEFAULT_SPEED_SCALE, MotorState::DEFAULT_FREQUENCY, SLOW_DECAY); - Motor motor_2(pico_motor_shim::MOTOR_2, NORMAL_DIR, MotorState::DEFAULT_SPEED_SCALE, MotorState::DEFAULT_FREQUENCY, SLOW_DECAY); -#endif +Motor motor_1(pico_motor_shim::MOTOR_1); +Motor motor_2(pico_motor_shim::MOTOR_2); static bool button_toggle = false; @@ -70,6 +65,11 @@ int main() { gpio_init(PICO_DEFAULT_LED_PIN); gpio_set_dir(PICO_DEFAULT_LED_PIN, GPIO_OUT); + #ifdef USE_FAST_DECAY + motor_1.decay_mode(FAST_DECAY); + motor_2.decay_mode(FAST_DECAY); + #endif + //Initialise the motor if(!motor_1.init() || !motor_2.init()) { printf("Cannot initialise the motors. Check the provided parameters\n"); diff --git a/micropython/examples/motor2040/multiple_motors.py b/micropython/examples/motor2040/multiple_motors.py index 008e963b..22866c60 100644 --- a/micropython/examples/motor2040/multiple_motors.py +++ b/micropython/examples/motor2040/multiple_motors.py @@ -45,8 +45,8 @@ SPEED_EXTENT = 1.0 # How far from zero to drive the motors when sweeping for j in range(SWEEPS): for i in range(360): speed = math.sin(math.radians(i)) * SPEED_EXTENT - for s in motors: - s.speed(speed) + for m in motors: + m.speed(speed) time.sleep(0.02) # Do a stepped speed sweep diff --git a/micropython/examples/motor2040/quad_velocity_sequence.py b/micropython/examples/motor2040/quad_velocity_sequence.py index 65183356..c153ad2d 100644 --- a/micropython/examples/motor2040/quad_velocity_sequence.py +++ b/micropython/examples/motor2040/quad_velocity_sequence.py @@ -28,7 +28,7 @@ TIME_FOR_EACH_MOVE = 2 # The time to travel between each value UPDATES_PER_MOVE = TIME_FOR_EACH_MOVE * UPDATES PRINT_DIVIDER = 4 # How many of the updates should be printed (i.e. 2 would be every other update) -DRIVING_SPEED = 1.0 # The speed to drive the wheels at +DRIVING_SPEED = 1.0 # The speed to drive the wheels at, from 0.0 to SPEED_SCALE # PID values VEL_KP = 30.0 # Velocity proportional (P) gain diff --git a/micropython/examples/pico_motor_shim/dual_motors.py b/micropython/examples/pico_motor_shim/dual_motors.py new file mode 100644 index 00000000..13ee9b39 --- /dev/null +++ b/micropython/examples/pico_motor_shim/dual_motors.py @@ -0,0 +1,71 @@ +import time +import math +from motor import Motor, pico_motor_shim +# from pimoroni import REVERSED_DIR + +""" +Demonstrates how to create two Motor objects and control them together. +""" + +# Create a list of motors +MOTOR_PINS = [pico_motor_shim.MOTOR_1, pico_motor_shim.MOTOR_2] +motors = [Motor(pins) for pins in MOTOR_PINS] + +# Uncomment the below lines (and the top import) to +# reverse the driving direction of a motor +# motors[0].direction(REVERSED_DIR) +# motors[1].direction(REVERSED_DIR) + +# Enable all motors +for m in motors: + m.enable() +time.sleep(2) + +# Drive at full positive +for m in motors: + m.full_positive() +time.sleep(2) + +# Stop moving +for m in motors: + m.stop() +time.sleep(2) + +# Drive at full negative +for m in motors: + m.full_negative() +time.sleep(2) + +# Coast to a gradual stop +for m in motors: + m.coast() +time.sleep(2) + + +SWEEPS = 2 # How many speed sweeps of the motors to perform +STEPS = 10 # The number of discrete sweep steps +STEPS_INTERVAL = 0.5 # The time in seconds between each step of the sequence +SPEED_EXTENT = 1.0 # How far from zero to drive the motors when sweeping + +# Do a sine speed sweep +for j in range(SWEEPS): + for i in range(360): + speed = math.sin(math.radians(i)) * SPEED_EXTENT + for m in motors: + m.speed(speed) + time.sleep(0.02) + +# Do a stepped speed sweep +for j in range(SWEEPS): + for i in range(0, STEPS): + for m in motors: + m.to_percent(i, 0, STEPS, 0.0 - SPEED_EXTENT, SPEED_EXTENT) + time.sleep(STEPS_INTERVAL) + for i in range(0, STEPS): + for m in motors: + m.to_percent(i, STEPS, 0, 0.0 - SPEED_EXTENT, SPEED_EXTENT) + time.sleep(STEPS_INTERVAL) + +# Disable the motors +for m in motors: + m.disable() diff --git a/micropython/examples/pico_motor_shim/motor_song.py b/micropython/examples/pico_motor_shim/motor_song.py new file mode 100644 index 00000000..25285ffb --- /dev/null +++ b/micropython/examples/pico_motor_shim/motor_song.py @@ -0,0 +1,191 @@ +import gc +import time +from machine import Pin +from motor import Motor, pico_motor_shim, SLOW_DECAY # , FAST_DECAY +from pimoroni import Button + +""" +A fun example of how to change a motor's frequency to have it play a song. +""" + +# This handy dictonary converts notes into frequencies +TONES = { + "B0": 31, + "C1": 33, + "CS1": 35, + "D1": 37, + "DS1": 39, + "E1": 41, + "F1": 44, + "FS1": 46, + "G1": 49, + "GS1": 52, + "A1": 55, + "AS1": 58, + "B1": 62, + "C2": 65, + "CS2": 69, + "D2": 73, + "DS2": 78, + "E2": 82, + "F2": 87, + "FS2": 93, + "G2": 98, + "GS2": 104, + "A2": 110, + "AS2": 117, + "B2": 123, + "C3": 131, + "CS3": 139, + "D3": 147, + "DS3": 156, + "E3": 165, + "F3": 175, + "FS3": 185, + "G3": 196, + "GS3": 208, + "A3": 220, + "AS3": 233, + "B3": 247, + "C4": 262, + "CS4": 277, + "D4": 294, + "DS4": 311, + "E4": 330, + "F4": 349, + "FS4": 370, + "G4": 392, + "GS4": 415, + "A4": 440, + "AS4": 466, + "B4": 494, + "C5": 523, + "CS5": 554, + "D5": 587, + "DS5": 622, + "E5": 659, + "F5": 698, + "FS5": 740, + "G5": 784, + "GS5": 831, + "A5": 880, + "AS5": 932, + "B5": 988, + "C6": 1047, + "CS6": 1109, + "D6": 1175, + "DS6": 1245, + "E6": 1319, + "F6": 1397, + "FS6": 1480, + "G6": 1568, + "GS6": 1661, + "A6": 1760, + "AS6": 1865, + "B6": 1976, + "C7": 2093, + "CS7": 2217, + "D7": 2349, + "DS7": 2489, + "E7": 2637, + "F7": 2794, + "FS7": 2960, + "G7": 3136, + "GS7": 3322, + "A7": 3520, + "AS7": 3729, + "B7": 3951, + "C8": 4186, + "CS8": 4435, + "D8": 4699, + "DS8": 4978 +} + +# Put the notes for your song in here! +SONG = ("F6", "F6", "E6", "F6", "F5", "P", "F5", "P", "C6", "AS5", "A5", "C6", "F6", "P", "F6", "P", "G6", "FS6", "G6", "G5", "P", "G5", "P", "G6", "F6", "E6", "D6", "C6", "P", "C6", "P", "D6", "E6", "F6", "E6", "D6", "C6", "D6", "C6", "AS5", "A5", "AS5", "A5", "G5", "F5", "G5", "F5", "E5", "D5", "C5", "D5", "E5", "F5", "G5", "AS5", "A5", "G5", "A5", "F5", "P", "F5") + +NOTE_DURATION = 0.150 # The time (in seconds) to play each note for. Change this to make the song play faster or slower +STATIONARY_TOGGLE_US = 2000 # The time (in microseconds) between each direction switch of the motor when using STATIONARY_PLAYBACK +STATIONARY_PLAYBACK = False # Whether to play the song with or without the motors spinning +DECAY_MODE = SLOW_DECAY # The motor decay mode to use, either FAST_DECAY (0) or SLOW_DECAY (1). Affects the song's quality + + +# Free up hardware resources ahead of creating new Motors +# (avoids occasional issues where the song just stops playing) +gc.collect() + +# Create two motor objects with a given decay mode +motor_1 = Motor(pico_motor_shim.MOTOR_1, mode=DECAY_MODE) +motor_2 = Motor(pico_motor_shim.MOTOR_2, mode=DECAY_MODE) + +# Create a pin for the Pico's onboard LED +led = Pin(25, Pin.OUT) +led.value(False) + +# Create the user button +button_a = Button(pico_motor_shim.BUTTON_A, repeat_time=0) + +# Variable for recording if the button has been toggled +# Starting as True makes the song play automatically +button_toggle = True + + +# Function to check if the button has been toggled +def check_button_toggle(): + global button_toggle + if button_a.read(): + button_toggle = not button_toggle + return button_toggle + + +while True: + # Has the button been toggled? + if check_button_toggle(): + # Turn the Pico's LED on to show that the song has started + led.value(True) + + # Play the song + for i in range(len(SONG)): + if check_button_toggle(): + if SONG[i] == "P": + # This is a "pause" note, so stop the motors + motor_1.stop() + motor_2.stop() + time.sleep(NOTE_DURATION) + else: + # Get the frequency of the note and set the motors to it + freq = TONES[SONG[i]] + 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 + t = 0 + while t < NOTE_DURATION * 1000000: + motor_1.duty(0.5) + motor_2.duty(0.5) + time.sleep_us(STATIONARY_TOGGLE_US) + t += STATIONARY_TOGGLE_US + + motor_1.duty(-0.5) + motor_2.duty(-0.5) + time.sleep_us(STATIONARY_TOGGLE_US) + t += STATIONARY_TOGGLE_US + else: + # Set the motors to 50% duty cycle to play the note whilst spinning + motor_1.duty(0.5) + motor_2.duty(0.5) + time.sleep(NOTE_DURATION) + else: + # The button was toggled again, so stop playing the song + break + + button_toggle = False + + # The song has finished, so turn off the Pico's LED and disable the motors + led.value(False) + motor_1.disable() + motor_2.disable() + + time.sleep(0.01) diff --git a/micropython/examples/pico_motor_shim/movements.py b/micropython/examples/pico_motor_shim/movements.py new file mode 100644 index 00000000..3b716fbe --- /dev/null +++ b/micropython/examples/pico_motor_shim/movements.py @@ -0,0 +1,109 @@ +import time +from motor import Motor, pico_motor_shim +from pimoroni import NORMAL_DIR, REVERSED_DIR + +""" +An example of how to perform simple movements of a 2-wheeled driving robot. +""" + +SPEED_SCALE = 5.4 # The scaling to apply to each motor's speed to match its real-world speed +DRIVING_SPEED = SPEED_SCALE # The speed to drive the wheels at, from 0.0 to 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 +left = Motor(pico_motor_shim.MOTOR_1, direction=NORMAL_DIR, speed_scale=SPEED_SCALE) +right = Motor(pico_motor_shim.MOTOR_2, direction=REVERSED_DIR, speed_scale=SPEED_SCALE) + + +# Helper functions for driving in common directions +def forward(speed=DRIVING_SPEED): + left.speed(speed) + right.speed(speed) + + +def backward(speed=DRIVING_SPEED): + left.speed(-speed) + right.speed(-speed) + + +def turn_left(speed=DRIVING_SPEED): + left.speed(-speed) + right.speed(speed) + + +def turn_right(speed=DRIVING_SPEED): + left.speed(speed) + right.speed(-speed) + + +def curve_forward_left(speed=DRIVING_SPEED): + left.speed(0.0) + right.speed(speed) + + +def curve_forward_right(speed=DRIVING_SPEED): + left.speed(speed) + right.speed(0.0) + + +def curve_backward_left(speed=DRIVING_SPEED): + left.speed(0.0) + right.speed(-speed) + + +def curve_backward_right(speed=DRIVING_SPEED): + left.speed(-speed) + right.speed(0.0) + + +def stop(): + left.stop() + right.stop() + + +def coast(): + left.coast() + right.coast() + + +# Demo each of the move methods +forward() +time.sleep(1) + +backward() +time.sleep(1) + +curve_forward_right() +time.sleep(1) + +curve_forward_left() +time.sleep(1) + +turn_right() +time.sleep(1) + +forward(0.5 * DRIVING_SPEED) # Half speed +time.sleep(1) + +turn_left(0.5 * DRIVING_SPEED) # Half speed +time.sleep(1) + +curve_backward_right(0.75 * DRIVING_SPEED) # Three quarters speed +time.sleep(1) + +forward() # Full speed +time.sleep(0.5) + +coast() # Come to a halt gently +time.sleep(1) + +forward() +time.sleep(0.5) + +stop() # Apply the brakes +time.sleep(1.0) + + +# Disable the motors +left.disable() +right.disable() diff --git a/micropython/examples/pico_motor_shim/single_motor.py b/micropython/examples/pico_motor_shim/single_motor.py new file mode 100644 index 00000000..9fca60e5 --- /dev/null +++ b/micropython/examples/pico_motor_shim/single_motor.py @@ -0,0 +1,54 @@ +import time +import math +from motor import Motor, pico_motor_shim + +""" +Demonstrates how to create a Motor object and control it. +""" + +# Create a motor +m = Motor(pico_motor_shim.MOTOR_1) + +# Enable the motor +m.enable() +time.sleep(2) + +# Drive at full positive +m.full_positive() +time.sleep(2) + +# Stop moving +m.stop() +time.sleep(2) + +# Drive at full negative +m.full_negative() +time.sleep(2) + +# Coast to a gradual stop +m.coast() +time.sleep(2) + + +SWEEPS = 2 # How many speed sweeps of the motor to perform +STEPS = 10 # The number of discrete sweep steps +STEPS_INTERVAL = 0.5 # The time in seconds between each step of the sequence +SPEED_EXTENT = 1.0 # How far from zero to drive the motor when sweeping + +# Do a sine speed sweep +for j in range(SWEEPS): + for i in range(360): + m.speed(math.sin(math.radians(i)) * SPEED_EXTENT) + time.sleep(0.02) + +# Do a stepped speed sweep +for j in range(SWEEPS): + for i in range(0, STEPS): + m.to_percent(i, 0, STEPS, 0.0 - SPEED_EXTENT, SPEED_EXTENT) + time.sleep(STEPS_INTERVAL) + for i in range(0, STEPS): + m.to_percent(i, STEPS, 0, 0.0 - SPEED_EXTENT, SPEED_EXTENT) + time.sleep(STEPS_INTERVAL) + +# Disable the motor +m.disable() diff --git a/micropython/examples/pico_motor_shim/stop_motors.py b/micropython/examples/pico_motor_shim/stop_motors.py new file mode 100644 index 00000000..673c08d8 --- /dev/null +++ b/micropython/examples/pico_motor_shim/stop_motors.py @@ -0,0 +1,11 @@ +from motor import Motor, pico_motor_shim + +""" +A simple program that stops the motors. +""" + +# Create two motor objects. +# This will initialise the pins, stopping any +# previous movement that may be stuck on +m1 = Motor(pico_motor_shim.MOTOR_1) +m2 = Motor(pico_motor_shim.MOTOR_2) From 3c3a8c1752c9b6e499b0cb996a70b467b67631ad Mon Sep 17 00:00:00 2001 From: ZodiusInfuser Date: Thu, 12 May 2022 10:17:24 +0100 Subject: [PATCH 54/60] Typo --- micropython/examples/pico_motor_shim/motor_song.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/micropython/examples/pico_motor_shim/motor_song.py b/micropython/examples/pico_motor_shim/motor_song.py index 25285ffb..299cb979 100644 --- a/micropython/examples/pico_motor_shim/motor_song.py +++ b/micropython/examples/pico_motor_shim/motor_song.py @@ -8,7 +8,7 @@ from pimoroni import Button A fun example of how to change a motor's frequency to have it play a song. """ -# This handy dictonary converts notes into frequencies +# This handy dictionary converts notes into frequencies TONES = { "B0": 31, "C1": 33, From fac3e7365a37ba180e3071877d953554a6206e55 Mon Sep 17 00:00:00 2001 From: ZodiusInfuser Date: Thu, 12 May 2022 13:20:52 +0100 Subject: [PATCH 55/60] Ported shim examples to C++, and tidy up --- examples/motor2040/README.md | 36 ++-- .../motor2040/motor2040_position_control.cpp | 2 +- ...motor2040_position_on_velocity_control.cpp | 2 +- .../motor2040_position_on_velocity_tuning.cpp | 2 +- .../motor2040/motor2040_position_tuning.cpp | 2 +- .../motor2040_quad_position_wave.cpp | 2 +- .../motor2040_quad_velocity_sequence.cpp | 2 +- .../motor2040/motor2040_reactive_encoder.cpp | 2 +- examples/motor2040/motor2040_single_motor.cpp | 2 +- .../motor2040/motor2040_velocity_control.cpp | 2 +- .../motor2040/motor2040_velocity_tuning.cpp | 2 +- examples/pico_motor_shim/CMakeLists.txt | 7 +- examples/pico_motor_shim/README.md | 33 ++++ .../motorshim_dual_motors.cmake | 12 ++ .../pico_motor_shim/motorshim_dual_motors.cpp | 105 +++++++++++ .../motorshim_motor_song.cmake | 13 ++ .../demo.cpp => motorshim_motor_song.cpp} | 85 ++++----- .../pico_motor_shim/motorshim_movements.cmake | 12 ++ .../pico_motor_shim/motorshim_movements.cpp | 126 +++++++++++++ .../motorshim_single_motor.cmake | 12 ++ .../motorshim_single_motor.cpp | 78 ++++++++ .../pico_motor_shim/sequence/CMakeLists.txt | 16 -- examples/pico_motor_shim/sequence/demo.cpp | 170 ------------------ examples/pico_motor_shim/song/CMakeLists.txt | 16 -- .../examples/pico_motor_shim/README.md | 40 +++++ .../examples/pico_motor_shim/motor_song.py | 2 + 26 files changed, 508 insertions(+), 275 deletions(-) create mode 100644 examples/pico_motor_shim/README.md create mode 100644 examples/pico_motor_shim/motorshim_dual_motors.cmake create mode 100644 examples/pico_motor_shim/motorshim_dual_motors.cpp create mode 100644 examples/pico_motor_shim/motorshim_motor_song.cmake rename examples/pico_motor_shim/{song/demo.cpp => motorshim_motor_song.cpp} (53%) create mode 100644 examples/pico_motor_shim/motorshim_movements.cmake create mode 100644 examples/pico_motor_shim/motorshim_movements.cpp create mode 100644 examples/pico_motor_shim/motorshim_single_motor.cmake create mode 100644 examples/pico_motor_shim/motorshim_single_motor.cpp delete mode 100644 examples/pico_motor_shim/sequence/CMakeLists.txt delete mode 100644 examples/pico_motor_shim/sequence/demo.cpp delete mode 100644 examples/pico_motor_shim/song/CMakeLists.txt create mode 100644 micropython/examples/pico_motor_shim/README.md diff --git a/examples/motor2040/README.md b/examples/motor2040/README.md index ad152b7b..17b07b81 100644 --- a/examples/motor2040/README.md +++ b/examples/motor2040/README.md @@ -5,7 +5,6 @@ - [Multiple Motors](#multiple-motors) - [Motor Cluster](#motor-cluster) - [Motor Wave](#motor-wave) - - [Stop Motors](#stop-motors) - [Function Examples](#function-examples) - [Read Sensors](#read-sensors) - [Read Encoders](#read-encoders) @@ -28,25 +27,25 @@ ## Motor Examples ### Single Motor -[motor2040_single_motor.py](motor2040_single_motor.py) +[motor2040_single_motor.cpp](motor2040_single_motor.cpp) Demonstrates how to create a Motor object and control it. ### Multiple Motors -[motor2040_multiple_motors.py](motor2040_multiple_motors.py) +[motor2040_multiple_motors.cpp](motor2040_multiple_motors.cpp) Demonstrates how to create multiple Motor objects and control them together. ### Motor Cluster -[motor2040_motor_cluster.py](motor2040_motor_cluster.py) +[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.py](motor2040_motor_wave.py) +[motor2040_motor_wave.cpp](motor2040_motor_wave.cpp) An example of applying a wave pattern to a group of motors and the LEDs. @@ -54,26 +53,26 @@ An example of applying a wave pattern to a group of motors and the LEDs. ## Function Examples ### Read Sensors -[motor2040_read_sensors.py](motor2040_read_sensors.py) +[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.py](motor2040_read_encoders.py) +[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.py](motor2040_motor_profiler.py) +[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.py](motor2040_led_rainbow.py) +[motor2040_led_rainbow.cpp](motor2040_led_rainbow.cpp) Displays a rotating rainbow pattern on the Motor 2040's onboard LED. @@ -81,36 +80,37 @@ Displays a rotating rainbow pattern on the Motor 2040's onboard LED. ## Control Examples ### Position Control -[motor2040_position_control.py](motor2040_position_control.py) +[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.py](motor2040_velocity_control.py) +[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.py](motor2040_position_on_velocity_control.py) +[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.py](motor2040_reactive_encoder.py) +[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.py](motor2040_quad_position_wave.py) +[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.py](motor2040_quad_velocity_sequence.py) +[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. @@ -118,18 +118,18 @@ A demonstration of driving all four of Motor 2040's motor outputs through a sequ ## Tuning Examples ### Position Tuning -[motor2040_position_tuning.py](motor2040_position_tuning.py) +[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.py](motor2040_velocity_tuning.py) +[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.py](motor2040_position_on_velocity_tuning.py) +[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. diff --git a/examples/motor2040/motor2040_position_control.cpp b/examples/motor2040/motor2040_position_control.cpp index 99e94a0c..925d336a 100644 --- a/examples/motor2040/motor2040_position_control.cpp +++ b/examples/motor2040/motor2040_position_control.cpp @@ -31,7 +31,7 @@ constexpr float COUNTS_PER_REV = MMME_CPR * GEAR_RATIO; const Direction DIRECTION = NORMAL_DIR; // The scaling to apply to the motor's speed to match its real-world speed -float SPEED_SCALE = 5.4f; +constexpr float SPEED_SCALE = 5.4f; // How many times to update the motor per second const uint UPDATES = 100; diff --git a/examples/motor2040/motor2040_position_on_velocity_control.cpp b/examples/motor2040/motor2040_position_on_velocity_control.cpp index 43c76a6b..3bd8c9a8 100644 --- a/examples/motor2040/motor2040_position_on_velocity_control.cpp +++ b/examples/motor2040/motor2040_position_on_velocity_control.cpp @@ -31,7 +31,7 @@ constexpr float COUNTS_PER_REV = MMME_CPR * GEAR_RATIO; const Direction DIRECTION = NORMAL_DIR; // The scaling to apply to the motor's speed to match its real-world speed -float SPEED_SCALE = 5.4f; +constexpr float SPEED_SCALE = 5.4f; // How many times to update the motor per second const uint UPDATES = 100; diff --git a/examples/motor2040/motor2040_position_on_velocity_tuning.cpp b/examples/motor2040/motor2040_position_on_velocity_tuning.cpp index c2db35a4..04b8b1a4 100644 --- a/examples/motor2040/motor2040_position_on_velocity_tuning.cpp +++ b/examples/motor2040/motor2040_position_on_velocity_tuning.cpp @@ -33,7 +33,7 @@ constexpr float COUNTS_PER_REV = MMME_CPR * GEAR_RATIO; const Direction DIRECTION = NORMAL_DIR; // The scaling to apply to the motor's speed to match its real-world speed -float SPEED_SCALE = 5.4f; +constexpr float SPEED_SCALE = 5.4f; // How many times to update the motor per second const uint UPDATES = 100; diff --git a/examples/motor2040/motor2040_position_tuning.cpp b/examples/motor2040/motor2040_position_tuning.cpp index 28358262..f1c52d31 100644 --- a/examples/motor2040/motor2040_position_tuning.cpp +++ b/examples/motor2040/motor2040_position_tuning.cpp @@ -33,7 +33,7 @@ constexpr float COUNTS_PER_REV = MMME_CPR * GEAR_RATIO; const Direction DIRECTION = NORMAL_DIR; // The scaling to apply to the motor's speed to match its real-world speed -float SPEED_SCALE = 5.4f; +constexpr float SPEED_SCALE = 5.4f; // How many times to update the motor per second const uint UPDATES = 100; diff --git a/examples/motor2040/motor2040_quad_position_wave.cpp b/examples/motor2040/motor2040_quad_position_wave.cpp index 8fa2428c..0f9c6d65 100644 --- a/examples/motor2040/motor2040_quad_position_wave.cpp +++ b/examples/motor2040/motor2040_quad_position_wave.cpp @@ -23,7 +23,7 @@ constexpr float GEAR_RATIO = 50.0f; constexpr float COUNTS_PER_REV = MMME_CPR * GEAR_RATIO; // The scaling to apply to the motor's speed to match its real-world speed -float SPEED_SCALE = 5.4f; +constexpr float SPEED_SCALE = 5.4f; // How many times to update the motor per second const uint UPDATES = 100; diff --git a/examples/motor2040/motor2040_quad_velocity_sequence.cpp b/examples/motor2040/motor2040_quad_velocity_sequence.cpp index dd4598c3..6da67661 100644 --- a/examples/motor2040/motor2040_quad_velocity_sequence.cpp +++ b/examples/motor2040/motor2040_quad_velocity_sequence.cpp @@ -29,7 +29,7 @@ constexpr float GEAR_RATIO = 50.0f; constexpr float COUNTS_PER_REV = MMME_CPR * GEAR_RATIO; // The scaling to apply to the motor's speed to match its real-world speed -float SPEED_SCALE = 5.4f; +constexpr float SPEED_SCALE = 5.4f; // How many times to update the motor per second const uint UPDATES = 100; diff --git a/examples/motor2040/motor2040_reactive_encoder.cpp b/examples/motor2040/motor2040_reactive_encoder.cpp index 8a2317f2..2c282bc1 100644 --- a/examples/motor2040/motor2040_reactive_encoder.cpp +++ b/examples/motor2040/motor2040_reactive_encoder.cpp @@ -33,7 +33,7 @@ constexpr float COUNTS_PER_REV = MMME_CPR * GEAR_RATIO; const Direction DIRECTION = NORMAL_DIR; // The scaling to apply to the motor's speed to match its real-world speed -float SPEED_SCALE = 5.4f; +constexpr float SPEED_SCALE = 5.4f; // How many times to update the motor per second const uint UPDATES = 100; diff --git a/examples/motor2040/motor2040_single_motor.cpp b/examples/motor2040/motor2040_single_motor.cpp index bda6ed44..d93c3ed3 100644 --- a/examples/motor2040/motor2040_single_motor.cpp +++ b/examples/motor2040/motor2040_single_motor.cpp @@ -17,7 +17,7 @@ const uint STEPS = 10; // The time in milliseconds between each step of the sequence const uint STEPS_INTERVAL_MS = 500; -// How far from zero to move the motor when sweeping +// How far from zero to drive the motor when sweeping constexpr float SPEED_EXTENT = 1.0f; diff --git a/examples/motor2040/motor2040_velocity_control.cpp b/examples/motor2040/motor2040_velocity_control.cpp index 09e18509..64e17c61 100644 --- a/examples/motor2040/motor2040_velocity_control.cpp +++ b/examples/motor2040/motor2040_velocity_control.cpp @@ -31,7 +31,7 @@ constexpr float COUNTS_PER_REV = MMME_CPR * GEAR_RATIO; const Direction DIRECTION = NORMAL_DIR; // The scaling to apply to the motor's speed to match its real-world speed -float SPEED_SCALE = 5.4f; +constexpr float SPEED_SCALE = 5.4f; // How many times to update the motor per second const uint UPDATES = 100; diff --git a/examples/motor2040/motor2040_velocity_tuning.cpp b/examples/motor2040/motor2040_velocity_tuning.cpp index 1348fe54..ab7ddc76 100644 --- a/examples/motor2040/motor2040_velocity_tuning.cpp +++ b/examples/motor2040/motor2040_velocity_tuning.cpp @@ -33,7 +33,7 @@ constexpr float COUNTS_PER_REV = MMME_CPR * GEAR_RATIO; const Direction DIRECTION = NORMAL_DIR; // The scaling to apply to the motor's speed to match its real-world speed -float SPEED_SCALE = 5.4f; +constexpr float SPEED_SCALE = 5.4f; // How many times to update the motor per second const uint UPDATES = 100; diff --git a/examples/pico_motor_shim/CMakeLists.txt b/examples/pico_motor_shim/CMakeLists.txt index 91e9e29f..04d2d981 100644 --- a/examples/pico_motor_shim/CMakeLists.txt +++ b/examples/pico_motor_shim/CMakeLists.txt @@ -1,3 +1,4 @@ -add_subdirectory(balance) -add_subdirectory(sequence) -add_subdirectory(song) \ No newline at end of file +include(motorshim_dual_motors.cmake) +include(motorshim_movements.cmake) +include(motorshim_motor_song.cmake) +include(motorshim_single_motor.cmake) \ No newline at end of file diff --git a/examples/pico_motor_shim/README.md b/examples/pico_motor_shim/README.md new file mode 100644 index 00000000..5fd51595 --- /dev/null +++ b/examples/pico_motor_shim/README.md @@ -0,0 +1,33 @@ +# Pico Motor Shim C++ Examples + +- [Examples](#examples) + - [Single Motor](#single-motor) + - [Dual Motors](#dual-motors) + - [Movements](#movements) + - [Motor Song](#motor-song) + + +## Motor 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. diff --git a/examples/pico_motor_shim/motorshim_dual_motors.cmake b/examples/pico_motor_shim/motorshim_dual_motors.cmake new file mode 100644 index 00000000..7ef38692 --- /dev/null +++ b/examples/pico_motor_shim/motorshim_dual_motors.cmake @@ -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}) diff --git a/examples/pico_motor_shim/motorshim_dual_motors.cpp b/examples/pico_motor_shim/motorshim_dual_motors.cpp new file mode 100644 index 00000000..4bae2021 --- /dev/null +++ b/examples/pico_motor_shim/motorshim_dual_motors.cpp @@ -0,0 +1,105 @@ +#include +#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(); + } +} diff --git a/examples/pico_motor_shim/motorshim_motor_song.cmake b/examples/pico_motor_shim/motorshim_motor_song.cmake new file mode 100644 index 00000000..3f2518fa --- /dev/null +++ b/examples/pico_motor_shim/motorshim_motor_song.cmake @@ -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}) diff --git a/examples/pico_motor_shim/song/demo.cpp b/examples/pico_motor_shim/motorshim_motor_song.cpp similarity index 53% rename from examples/pico_motor_shim/song/demo.cpp rename to examples/pico_motor_shim/motorshim_motor_song.cpp index 584dd3a0..134295fc 100644 --- a/examples/pico_motor_shim/song/demo.cpp +++ b/examples/pico_motor_shim/motorshim_motor_song.cpp @@ -1,13 +1,13 @@ #include -#include "pico_motor_shim.hpp" +#include "pico/stdlib.h" -#include "common/pimoroni_common.hpp" -#include "motor.hpp" +#include "pico_motor_shim.hpp" #include "button.hpp" /* -Play a song using a motor! Works by setting the PWM duty cycle to 50% and changing the frequency on the fly. -Plug a motor into connector 1, and press "A" to start the song playing (does not loop). Press the button again will stop the song early. +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; @@ -21,43 +21,41 @@ constexpr float SONG[] = {1397, 1397, 1319, 1397, 698, 0, 698, 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 = sizeof(SONG) / sizeof(float); +constexpr uint SONG_LENGTH = count_of(SONG); -//The time (in milliseconds) to play each note for. Change this to make the song play faster or slower -static const uint NOTE_DURATION_MS = 150; +// 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; -//Uncomment this lineto have the song be played without the motor turning -//Note, this will affect the audio quality of the sound produced -//#define STATIONARY_PLAYBACK +// The time (in microseconds) between each direction switch of the motor when using STATIONARY_PLAYBACK +const uint STATIONARY_TOGGLE_US = 2000; -//The time (in microseconds) between each direction switch of the motor when using STATIONARY_PLAYBACK -static const uint STATIONARY_TOGGLE_US = 2000; +// Whether to play the song with or without the motors spinning +const bool STATIONARY_PLAYBACK = false; -//Uncomment this line to use the fast decay (coasting) motor mode. -//This seems to produce a louder sound with STATIONARY_PLAYBACK enabled, but will make movement poorer when STATIONARY_PLAYBACK is disabled -//#define USE_FAST_DECAY +// 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; -Button button_a(pico_motor_shim::BUTTON_A, Polarity::ACTIVE_LOW, 0); +// 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); -static bool button_toggle = false; +// Create the user button +Button button_a(pico_motor_shim::BUTTON_A, Polarity::ACTIVE_LOW, 0); -/** - * Checks if the button has been pressed, toggling a value that is also returned. - */ +// 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() { - bool button_pressed = button_a.read(); - if(button_pressed) { + if(button_a.read()) { button_toggle = !button_toggle; } return button_toggle; } -/** - * The entry point of the program. - */ + int main() { stdio_init_all(); @@ -65,28 +63,30 @@ int main() { gpio_init(PICO_DEFAULT_LED_PIN); gpio_set_dir(PICO_DEFAULT_LED_PIN, GPIO_OUT); - #ifdef USE_FAST_DECAY - motor_1.decay_mode(FAST_DECAY); - motor_2.decay_mode(FAST_DECAY); - #endif + // Se the two motor's decay modes + motor_1.decay_mode(DECAY_MODE); + motor_2.decay_mode(DECAY_MODE); - //Initialise the motor + // 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 + // Turn the Pico's LED on to show that the song has started gpio_put(PICO_DEFAULT_LED_PIN, true); - //Play the song + // Play the song for(uint i = 0; i < SONG_LENGTH && check_button_toggle(); i++) { - if(motor_1.frequency(SONG[i]) && motor_2.frequency(SONG[i])) { - #ifdef 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 + // 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); @@ -99,15 +99,16 @@ int main() { sleep_us(STATIONARY_TOGGLE_US); t += STATIONARY_TOGGLE_US; } - #else - //Set the motors to 50% duty cycle to play the note + } + 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); - #endif + } } else { - //The frequency was invalid, so we are treating that to mean this is a pause note + // 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); @@ -115,7 +116,7 @@ int main() { } button_toggle = false; - //The song has finished, so turn off the Pico's LED and disable the motors + // 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(); diff --git a/examples/pico_motor_shim/motorshim_movements.cmake b/examples/pico_motor_shim/motorshim_movements.cmake new file mode 100644 index 00000000..e606e2da --- /dev/null +++ b/examples/pico_motor_shim/motorshim_movements.cmake @@ -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}) diff --git a/examples/pico_motor_shim/motorshim_movements.cpp b/examples/pico_motor_shim/motorshim_movements.cpp new file mode 100644 index 00000000..aea27567 --- /dev/null +++ b/examples/pico_motor_shim/motorshim_movements.cpp @@ -0,0 +1,126 @@ +#include +#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(); +} diff --git a/examples/pico_motor_shim/motorshim_single_motor.cmake b/examples/pico_motor_shim/motorshim_single_motor.cmake new file mode 100644 index 00000000..18562424 --- /dev/null +++ b/examples/pico_motor_shim/motorshim_single_motor.cmake @@ -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}) diff --git a/examples/pico_motor_shim/motorshim_single_motor.cpp b/examples/pico_motor_shim/motorshim_single_motor.cpp new file mode 100644 index 00000000..9df3a840 --- /dev/null +++ b/examples/pico_motor_shim/motorshim_single_motor.cpp @@ -0,0 +1,78 @@ +#include +#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(); +} diff --git a/examples/pico_motor_shim/sequence/CMakeLists.txt b/examples/pico_motor_shim/sequence/CMakeLists.txt deleted file mode 100644 index eafeebe8..00000000 --- a/examples/pico_motor_shim/sequence/CMakeLists.txt +++ /dev/null @@ -1,16 +0,0 @@ -set(OUTPUT_NAME motor_shim_sequence) - -add_executable( - ${OUTPUT_NAME} - demo.cpp -) - -# enable usb output, disable uart output -pico_enable_stdio_usb(${OUTPUT_NAME} 1) -pico_enable_stdio_uart(${OUTPUT_NAME} 1) - -# Pull in pico libraries that we need -target_link_libraries(${OUTPUT_NAME} pico_stdlib pico_motor_shim motor button) - -# create map/bin/hex file etc. -pico_add_extra_outputs(${OUTPUT_NAME}) diff --git a/examples/pico_motor_shim/sequence/demo.cpp b/examples/pico_motor_shim/sequence/demo.cpp deleted file mode 100644 index 61ffda82..00000000 --- a/examples/pico_motor_shim/sequence/demo.cpp +++ /dev/null @@ -1,170 +0,0 @@ -#include -#include "pico_motor_shim.hpp" - -#include "common/pimoroni_common.hpp" -#include "motor.hpp" -#include "button.hpp" - -/* -Program showing how the two motors of the Pico Motor Shim can be perform a sequence of movements. -Press "A" to start and stop the movement sequence -*/ - -using namespace pimoroni; -using namespace motor; - -static constexpr float TOP_SPEED = 1.0f; //A value between 0 and 1 -static const uint32_t ACCELERATE_TIME_MS = 2000; -static const uint32_t WAIT_TIME_MS = 1000; -static const uint32_t STOP_TIME_MS = 1000; - - -Button button_a(pico_motor_shim::BUTTON_A, Polarity::ACTIVE_LOW, 0); - -Motor motor_1(pico_motor_shim::MOTOR_1);//, Motor::DEFAULT_PWM_FREQUENCY, Motor::DEFAULT_DECAY_MODE); -Motor motor_2(pico_motor_shim::MOTOR_2);//, Motor::DEFAULT_PWM_FREQUENCY, Motor::DEFAULT_DECAY_MODE); - -static bool button_toggle = false; - -/** - * Checks if the button has been pressed, toggling a value that is also returned. - */ -bool check_button_toggle() { - bool button_pressed = button_a.read(); - if(button_pressed) { - button_toggle = !button_toggle; - } - return button_toggle; -} - -/** - * Waits for a given amount of time (in milliseconds). - * Exits early if the user presses the button to stop the sequence, returning false. - */ -bool wait_for(uint32_t duration_ms) { - uint32_t start_time = millis(); - uint32_t ellapsed = 0; - - //Loops until the duration has elapsed, checking the button state every millisecond - while(ellapsed < duration_ms) { - if(!check_button_toggle()) - return false; - - sleep_ms(1); - ellapsed = millis() - start_time; - } - return true; -} - -/** - * Accelerate/Decelerate the motors from their current speed to the target speed over the given amount of time (in milliseconds). - * Exits early if the user presses the button to stop the sequence, returning false. - */ -bool accelerate_over(float left_speed, float right_speed, uint32_t duration_ms) { - uint32_t start_time = millis(); - uint32_t ellapsed = 0; - - //Get the current motor speeds - float last_left = motor_1.speed(); - float last_right = motor_2.speed(); - - //Loops until the duration has elapsed, checking the button state every millisecond, and updating motor speeds - while(ellapsed <= duration_ms) { - if(!check_button_toggle()) - return false; - - //Calculate and set the new motor speeds - float percentage = (float)ellapsed / (float)duration_ms; - motor_1.speed(((left_speed - last_left) * percentage) + last_left); - motor_2.speed(((right_speed - last_right) * percentage) + last_right); - - sleep_ms(1); - ellapsed = millis() - start_time; - } - - //Set the final motor speeds as loop may not reach 100% - motor_1.speed(left_speed); - motor_2.speed(right_speed); - - return true; -} - -/** - * The function that performs the driving sequence. - * Exits early if the user presses the button to stop the sequence, returning false. - */ -bool sequence() { - printf("accelerate forward\n"); - if(!accelerate_over(-TOP_SPEED, TOP_SPEED, ACCELERATE_TIME_MS)) - return false; //Early exit if the button was toggled - - printf("driving forward\n"); - if(!wait_for(WAIT_TIME_MS)) - return false; //Early exit if the button was toggled - - printf("deccelerate forward\n"); - if(!accelerate_over(0.0f, 0.0f, ACCELERATE_TIME_MS)) - return false; //Early exit if the button was toggled - - printf("stop\n"); - motor_1.stop(); - motor_2.stop(); - if(!wait_for(STOP_TIME_MS)) - return false; //Early exit if the button was toggled - - printf("accelerate turn left\n"); - if(!accelerate_over(TOP_SPEED * 0.5f, TOP_SPEED * 0.5f, ACCELERATE_TIME_MS * 0.5f)) - return false; //Early exit if the button was toggled - - printf("turning left\n"); - if(!wait_for(WAIT_TIME_MS)) - return false; //Early exit if the button was toggled - - printf("deccelerate turn left\n"); - if(!accelerate_over(0.0f, 0.0f, ACCELERATE_TIME_MS * 0.5f)) - return false; //Early exit if the button was toggled - - printf("stop\n"); - motor_1.stop(); - motor_2.stop(); - if(!wait_for(STOP_TIME_MS)) - return false; //Early exit if the button was toggled - - //Signal that the sequence completed successfully - return true; -} - -/** - * The entry point of the program. - */ -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); - - //Initialise the two motors - if(!motor_1.init() || !motor_2.init()) { - printf("Cannot initialise motors. Check the provided parameters\n"); - return 0; - } - - while(true) { - //Has the user has pressed the button to start the sequence - if(check_button_toggle()) { - - //Turn the Pico's LED on to show that the sequence has started - gpio_put(PICO_DEFAULT_LED_PIN, true); - - //Run the sequence in a perpetual loop, exiting early if the button is pressed again - while(sequence()); - } - - //The sequence loop has ended, so turn off the Pico's LED and disable the motors - gpio_put(PICO_DEFAULT_LED_PIN, false); - motor_1.disable(); - motor_2.disable(); - } - return 0; -} diff --git a/examples/pico_motor_shim/song/CMakeLists.txt b/examples/pico_motor_shim/song/CMakeLists.txt deleted file mode 100644 index b5adb316..00000000 --- a/examples/pico_motor_shim/song/CMakeLists.txt +++ /dev/null @@ -1,16 +0,0 @@ -set(OUTPUT_NAME motor_shim_song) - -add_executable( - ${OUTPUT_NAME} - demo.cpp -) - -# enable usb output, disable uart output -pico_enable_stdio_usb(${OUTPUT_NAME} 1) -pico_enable_stdio_uart(${OUTPUT_NAME} 1) - -# Pull in pico libraries that we need -target_link_libraries(${OUTPUT_NAME} pico_stdlib pico_motor_shim motor button breakout_msa301) - -# create map/bin/hex file etc. -pico_add_extra_outputs(${OUTPUT_NAME}) diff --git a/micropython/examples/pico_motor_shim/README.md b/micropython/examples/pico_motor_shim/README.md new file mode 100644 index 00000000..9bc04211 --- /dev/null +++ b/micropython/examples/pico_motor_shim/README.md @@ -0,0 +1,40 @@ +# Pico Motor Shim C++ Examples + +- [Examples](#examples) + - [Single Motor](#single-motor) + - [Dual Motors](#dual-motors) + - [Movements](#movements) + - [Motor Song](#motor-song) + - [Stop Motors](#stop-motors) + + +## Motor Examples + +### Single Motor +[single_motor.py](single_motor.py) + +Demonstrates how to create a Motor object and control it. + + +### Dual Motors +[dual_motors.py](dual_motors.py) + +Demonstrates how to create two Motor objects and control them together. + + +### Movements +[movements.py](movements.py) + +An example of how to perform simple movements of a 2-wheeled driving robot. + + +### Motor Song +[motor_song.py](motor_song.py) + +A fun example of how to change a motor's frequency to have it play a song. + + +### Stop Motors +[stop_motors.py](motorshim_motor_song.py) + +A simple program that stops the motors. diff --git a/micropython/examples/pico_motor_shim/motor_song.py b/micropython/examples/pico_motor_shim/motor_song.py index 299cb979..0bfc5b3a 100644 --- a/micropython/examples/pico_motor_shim/motor_song.py +++ b/micropython/examples/pico_motor_shim/motor_song.py @@ -6,6 +6,8 @@ from pimoroni import Button """ 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. """ # This handy dictionary converts notes into frequencies From 0c88f7892ba80d59bfd4103f5028117e4c532d26 Mon Sep 17 00:00:00 2001 From: ZodiusInfuser Date: Thu, 12 May 2022 14:45:20 +0100 Subject: [PATCH 56/60] Added MP encoder examples --- examples/pico_motor_shim/README.md | 2 +- micropython/examples/encoder/README.md | 40 +++++++++++++++++++ micropython/examples/encoder/item_selector.py | 39 ++++++++++++++++++ ...rotary_encoder_delta.py => read_change.py} | 5 ++- ...tary_encoder_simple.py => read_encoder.py} | 3 +- ...otary_encoder_capture.py => read_speed.py} | 16 ++++---- micropython/examples/encoder/value_dial.py | 40 +++++++++++++++++++ .../examples/pico_motor_shim/README.md | 6 +-- 8 files changed, 136 insertions(+), 15 deletions(-) create mode 100644 micropython/examples/encoder/README.md create mode 100644 micropython/examples/encoder/item_selector.py rename micropython/examples/encoder/{rotary_encoder_delta.py => read_change.py} (93%) rename micropython/examples/encoder/{rotary_encoder_simple.py => read_encoder.py} (99%) rename micropython/examples/encoder/{rotary_encoder_capture.py => read_speed.py} (62%) create mode 100644 micropython/examples/encoder/value_dial.py diff --git a/examples/pico_motor_shim/README.md b/examples/pico_motor_shim/README.md index 5fd51595..0556d1fb 100644 --- a/examples/pico_motor_shim/README.md +++ b/examples/pico_motor_shim/README.md @@ -7,7 +7,7 @@ - [Motor Song](#motor-song) -## Motor Examples +## Examples ### Single Motor [motorshim_single_motor.cpp](motorshim_single_motor.cpp) diff --git a/micropython/examples/encoder/README.md b/micropython/examples/encoder/README.md new file mode 100644 index 00000000..92c6ea76 --- /dev/null +++ b/micropython/examples/encoder/README.md @@ -0,0 +1,40 @@ +# Encoder MicroPython Examples + +- [Examples](#examples) + - [Read Encoder](#read-encoder) + - [Read Change](#read-change) + - [Read Speed](#read-speed) + - [Value Dial](#value-dial) + - [Item Selector](#item-selector) + + +## Examples + +### Read Encoder +[read_encoder.py](read_encoder.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. diff --git a/micropython/examples/encoder/item_selector.py b/micropython/examples/encoder/item_selector.py new file mode 100644 index 00000000..78dc97e7 --- /dev/null +++ b/micropython/examples/encoder/item_selector.py @@ -0,0 +1,39 @@ +import gc +from encoder import Encoder, ROTARY_CPR +# from encoder import REVERSED + +""" +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 (e.g. upwards) at the start of every program run. +""" + +# 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, counts_per_rev=ROTARY_CPR) + +# Uncomment the below line (and the top import) to reverse the counting direction +# enc.direction(REVERSED) + + +# 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, ": ", ITEMS[step], sep="") + else: + print(step, ": ", "Undefined", sep="") + last_step = step diff --git a/micropython/examples/encoder/rotary_encoder_delta.py b/micropython/examples/encoder/read_change.py similarity index 93% rename from micropython/examples/encoder/rotary_encoder_delta.py rename to micropython/examples/encoder/read_change.py index 6e7430a5..1adda86c 100644 --- a/micropython/examples/encoder/rotary_encoder_delta.py +++ b/micropython/examples/encoder/read_change.py @@ -3,7 +3,7 @@ from encoder import Encoder # from encoder import REVERSED """ -An example of how to read a mechanical rotary encoder, only when it has turned +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 @@ -18,6 +18,7 @@ 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) + # Print out the initial count, step, and turn (they should all be zero) print("Count =", enc.count(), end=", ") print("Step =", enc.step(), end=", ") @@ -29,4 +30,4 @@ while True: # Print out the new count, step, and turn print("Count =", enc.count(), end=", ") print("Step =", enc.step(), end=", ") - print("Turn =", enc.turn()) \ No newline at end of file + print("Turn =", enc.turn()) diff --git a/micropython/examples/encoder/rotary_encoder_simple.py b/micropython/examples/encoder/read_encoder.py similarity index 99% rename from micropython/examples/encoder/rotary_encoder_simple.py rename to micropython/examples/encoder/read_encoder.py index 60056bb7..3f8e7a23 100644 --- a/micropython/examples/encoder/rotary_encoder_simple.py +++ b/micropython/examples/encoder/read_encoder.py @@ -19,9 +19,10 @@ 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) + # Loop forever while True: - + # Print out the count, delta, step, and turn print("Count =", enc.count(), end=", ") print("Delta =", enc.delta(), end=", ") diff --git a/micropython/examples/encoder/rotary_encoder_capture.py b/micropython/examples/encoder/read_speed.py similarity index 62% rename from micropython/examples/encoder/rotary_encoder_capture.py rename to micropython/examples/encoder/read_speed.py index afbb3fc1..78ecbbcf 100644 --- a/micropython/examples/encoder/rotary_encoder_capture.py +++ b/micropython/examples/encoder/read_speed.py @@ -4,7 +4,7 @@ from encoder import Encoder # from encoder import REVERSED """ -An example of how to read a mechanical rotary encoder, only when it has turned +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 @@ -19,14 +19,14 @@ 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) + # Loop forever while True: - capture = enc.take_snapshot() + capture = enc.capture() + + print("Count =", capture.count, end=", ") + print("Angle =", capture.degrees, end=", ") + print("Freq =", capture.frequency, end=", ") + print("Speed =", capture.degrees_per_second) - 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) - diff --git a/micropython/examples/encoder/value_dial.py b/micropython/examples/encoder/value_dial.py new file mode 100644 index 00000000..1e8c773b --- /dev/null +++ b/micropython/examples/encoder/value_dial.py @@ -0,0 +1,40 @@ +import gc +from encoder import Encoder +# from encoder import REVERSED + +""" +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) + +# The min and max value +MIN_VALUE = 0 +MAX_VALUE = 11 + +value = 1 + +# 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) diff --git a/micropython/examples/pico_motor_shim/README.md b/micropython/examples/pico_motor_shim/README.md index 9bc04211..13b8fdc2 100644 --- a/micropython/examples/pico_motor_shim/README.md +++ b/micropython/examples/pico_motor_shim/README.md @@ -1,4 +1,4 @@ -# Pico Motor Shim C++ Examples +# Pico Motor Shim MicroPython Examples - [Examples](#examples) - [Single Motor](#single-motor) @@ -8,7 +8,7 @@ - [Stop Motors](#stop-motors) -## Motor Examples +## Examples ### Single Motor [single_motor.py](single_motor.py) @@ -35,6 +35,6 @@ A fun example of how to change a motor's frequency to have it play a song. ### Stop Motors -[stop_motors.py](motorshim_motor_song.py) +[stop_motors.py](stop_motors.py) A simple program that stops the motors. From 19ec07e8c68074e045de01a82e249ff82800d9ad Mon Sep 17 00:00:00 2001 From: ZodiusInfuser Date: Thu, 12 May 2022 16:15:40 +0100 Subject: [PATCH 57/60] Added encoder C++ examples --- examples/CMakeLists.txt | 1 + examples/encoder/CMakeLists.txt | 5 ++ examples/encoder/README.md | 40 ++++++++++++++ examples/encoder/encoder_item_selector.cmake | 12 +++++ examples/encoder/encoder_item_selector.cpp | 54 +++++++++++++++++++ examples/encoder/encoder_read_change.cmake | 12 +++++ examples/encoder/encoder_read_change.cpp | 45 ++++++++++++++++ examples/encoder/encoder_read_counts.cmake | 12 +++++ examples/encoder/encoder_read_counts.cpp | 38 +++++++++++++ examples/encoder/encoder_read_speed.cmake | 12 +++++ examples/encoder/encoder_read_speed.cpp | 39 ++++++++++++++ examples/encoder/encoder_value_dial.cmake | 12 +++++ examples/encoder/encoder_value_dial.cpp | 54 +++++++++++++++++++ micropython/examples/encoder/README.md | 6 +-- micropython/examples/encoder/item_selector.py | 14 ++--- micropython/examples/encoder/read_change.py | 4 +- .../{read_encoder.py => read_counts.py} | 4 +- micropython/examples/encoder/read_speed.py | 4 +- micropython/examples/encoder/value_dial.py | 8 +-- 19 files changed, 356 insertions(+), 20 deletions(-) create mode 100644 examples/encoder/CMakeLists.txt create mode 100644 examples/encoder/README.md create mode 100644 examples/encoder/encoder_item_selector.cmake create mode 100644 examples/encoder/encoder_item_selector.cpp create mode 100644 examples/encoder/encoder_read_change.cmake create mode 100644 examples/encoder/encoder_read_change.cpp create mode 100644 examples/encoder/encoder_read_counts.cmake create mode 100644 examples/encoder/encoder_read_counts.cpp create mode 100644 examples/encoder/encoder_read_speed.cmake create mode 100644 examples/encoder/encoder_read_speed.cpp create mode 100644 examples/encoder/encoder_value_dial.cmake create mode 100644 examples/encoder/encoder_value_dial.cpp rename micropython/examples/encoder/{read_encoder.py => read_counts.py} (92%) diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 94868c4f..24de6b59 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -44,3 +44,4 @@ add_subdirectory(badger2040) add_subdirectory(interstate75) add_subdirectory(servo2040) add_subdirectory(motor2040) +add_subdirectory(encoder) diff --git a/examples/encoder/CMakeLists.txt b/examples/encoder/CMakeLists.txt new file mode 100644 index 00000000..4311badb --- /dev/null +++ b/examples/encoder/CMakeLists.txt @@ -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) \ No newline at end of file diff --git a/examples/encoder/README.md b/examples/encoder/README.md new file mode 100644 index 00000000..2fb5d7e9 --- /dev/null +++ b/examples/encoder/README.md @@ -0,0 +1,40 @@ +# Encoder C++ Examples + +- [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. diff --git a/examples/encoder/encoder_item_selector.cmake b/examples/encoder/encoder_item_selector.cmake new file mode 100644 index 00000000..b0ad3744 --- /dev/null +++ b/examples/encoder/encoder_item_selector.cmake @@ -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}) diff --git a/examples/encoder/encoder_item_selector.cpp b/examples/encoder/encoder_item_selector.cpp new file mode 100644 index 00000000..f5829e66 --- /dev/null +++ b/examples/encoder/encoder_item_selector.cpp @@ -0,0 +1,54 @@ +#include +#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; + } + } +} diff --git a/examples/encoder/encoder_read_change.cmake b/examples/encoder/encoder_read_change.cmake new file mode 100644 index 00000000..b9d6df96 --- /dev/null +++ b/examples/encoder/encoder_read_change.cmake @@ -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}) diff --git a/examples/encoder/encoder_read_change.cpp b/examples/encoder/encoder_read_change.cpp new file mode 100644 index 00000000..660032e8 --- /dev/null +++ b/examples/encoder/encoder_read_change.cpp @@ -0,0 +1,45 @@ +#include +#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()); + } + } +} diff --git a/examples/encoder/encoder_read_counts.cmake b/examples/encoder/encoder_read_counts.cmake new file mode 100644 index 00000000..843ac3d2 --- /dev/null +++ b/examples/encoder/encoder_read_counts.cmake @@ -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}) diff --git a/examples/encoder/encoder_read_counts.cpp b/examples/encoder/encoder_read_counts.cpp new file mode 100644 index 00000000..85831089 --- /dev/null +++ b/examples/encoder/encoder_read_counts.cpp @@ -0,0 +1,38 @@ +#include +#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); + } +} diff --git a/examples/encoder/encoder_read_speed.cmake b/examples/encoder/encoder_read_speed.cmake new file mode 100644 index 00000000..5914bf41 --- /dev/null +++ b/examples/encoder/encoder_read_speed.cmake @@ -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}) diff --git a/examples/encoder/encoder_read_speed.cpp b/examples/encoder/encoder_read_speed.cpp new file mode 100644 index 00000000..e50e59f6 --- /dev/null +++ b/examples/encoder/encoder_read_speed.cpp @@ -0,0 +1,39 @@ +#include +#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); + } +} diff --git a/examples/encoder/encoder_value_dial.cmake b/examples/encoder/encoder_value_dial.cmake new file mode 100644 index 00000000..fa9fdf83 --- /dev/null +++ b/examples/encoder/encoder_value_dial.cmake @@ -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}) diff --git a/examples/encoder/encoder_value_dial.cpp b/examples/encoder/encoder_value_dial.cpp new file mode 100644 index 00000000..e2cf4ef8 --- /dev/null +++ b/examples/encoder/encoder_value_dial.cpp @@ -0,0 +1,54 @@ +#include +#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); + } + } +} diff --git a/micropython/examples/encoder/README.md b/micropython/examples/encoder/README.md index 92c6ea76..6e89a78e 100644 --- a/micropython/examples/encoder/README.md +++ b/micropython/examples/encoder/README.md @@ -1,7 +1,7 @@ # Encoder MicroPython Examples - [Examples](#examples) - - [Read Encoder](#read-encoder) + - [Read Counts](#read-counts) - [Read Change](#read-change) - [Read Speed](#read-speed) - [Value Dial](#value-dial) @@ -10,8 +10,8 @@ ## Examples -### Read Encoder -[read_encoder.py](read_encoder.py) +### Read Counts +[read_counts.py](read_counts.py) An example of how to read a mechanical rotary encoder. diff --git a/micropython/examples/encoder/item_selector.py b/micropython/examples/encoder/item_selector.py index 78dc97e7..58ff8f6f 100644 --- a/micropython/examples/encoder/item_selector.py +++ b/micropython/examples/encoder/item_selector.py @@ -1,13 +1,13 @@ import gc -from encoder import Encoder, ROTARY_CPR -# from encoder import REVERSED +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 (e.g. upwards) at the start of every program run. +direction at the start of every program run (e.g. upwards). """ # Free up hardware resources ahead of creating a new Encoder @@ -17,10 +17,10 @@ gc.collect() 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, counts_per_rev=ROTARY_CPR) +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) +# enc.direction(REVERSED_DIR) # A list of items, up to the encoder's counts_per_rev @@ -33,7 +33,7 @@ while True: step = enc.step() if step != last_step: if step < len(ITEMS): - print(step, ": ", ITEMS[step], sep="") + print(step, "/", int(enc.counts_per_rev()) - 1, ": ", ITEMS[step], sep="") else: - print(step, ": ", "Undefined", sep="") + print(step, "/", int(enc.counts_per_rev()) - 1, ": ", "Undefined", sep="") last_step = step diff --git a/micropython/examples/encoder/read_change.py b/micropython/examples/encoder/read_change.py index 1adda86c..17748d90 100644 --- a/micropython/examples/encoder/read_change.py +++ b/micropython/examples/encoder/read_change.py @@ -1,6 +1,6 @@ import gc from encoder import Encoder -# from encoder import REVERSED +# from encoder import REVERSED_DIR """ An example of how to read a mechanical rotary encoder, only when a change has occurred. @@ -16,7 +16,7 @@ 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) +# enc.direction(REVERSED_DIR) # Print out the initial count, step, and turn (they should all be zero) diff --git a/micropython/examples/encoder/read_encoder.py b/micropython/examples/encoder/read_counts.py similarity index 92% rename from micropython/examples/encoder/read_encoder.py rename to micropython/examples/encoder/read_counts.py index 3f8e7a23..46d71add 100644 --- a/micropython/examples/encoder/read_encoder.py +++ b/micropython/examples/encoder/read_counts.py @@ -1,7 +1,7 @@ import gc import time from encoder import Encoder -# from encoder import REVERSED +# from encoder import REVERSED_DIR """ An example of how to read a mechanical rotary encoder. @@ -17,7 +17,7 @@ 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) +# enc.direction(REVERSED_DIR) # Loop forever diff --git a/micropython/examples/encoder/read_speed.py b/micropython/examples/encoder/read_speed.py index 78ecbbcf..7ad81619 100644 --- a/micropython/examples/encoder/read_speed.py +++ b/micropython/examples/encoder/read_speed.py @@ -1,7 +1,7 @@ import gc import time from encoder import Encoder -# from encoder import REVERSED +# from encoder import REVERSED_DIR """ An example of how to read the speed a mechanical rotary encoder is being turned at. @@ -17,7 +17,7 @@ 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) +# enc.direction(REVERSED_DIR) # Loop forever diff --git a/micropython/examples/encoder/value_dial.py b/micropython/examples/encoder/value_dial.py index 1e8c773b..c498792c 100644 --- a/micropython/examples/encoder/value_dial.py +++ b/micropython/examples/encoder/value_dial.py @@ -1,6 +1,6 @@ import gc from encoder import Encoder -# from encoder import REVERSED +# from encoder import REVERSED_DIR """ A demonstration of a rotary encoder being used to control a value. @@ -16,13 +16,13 @@ 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) +# enc.direction(REVERSED_DIR) # The min and max value MIN_VALUE = 0 -MAX_VALUE = 11 +MAX_VALUE = 100 -value = 1 +value = 50 # Print out the initial value print("Value =", value) From d5468b96162c644c7ef35c07f08e15d76d1bca87 Mon Sep 17 00:00:00 2001 From: ZodiusInfuser Date: Thu, 12 May 2022 16:54:27 +0100 Subject: [PATCH 58/60] Removed old balance demo --- .../pico_motor_shim/balance/CMakeLists.txt | 16 --- examples/pico_motor_shim/balance/demo.cpp | 119 ------------------ 2 files changed, 135 deletions(-) delete mode 100644 examples/pico_motor_shim/balance/CMakeLists.txt delete mode 100644 examples/pico_motor_shim/balance/demo.cpp diff --git a/examples/pico_motor_shim/balance/CMakeLists.txt b/examples/pico_motor_shim/balance/CMakeLists.txt deleted file mode 100644 index ee58efe4..00000000 --- a/examples/pico_motor_shim/balance/CMakeLists.txt +++ /dev/null @@ -1,16 +0,0 @@ -set(OUTPUT_NAME motor_shim_balance) - -add_executable( - ${OUTPUT_NAME} - demo.cpp -) - -# enable usb output, disable uart output -pico_enable_stdio_usb(${OUTPUT_NAME} 1) -pico_enable_stdio_uart(${OUTPUT_NAME} 1) - -# Pull in pico libraries that we need -target_link_libraries(${OUTPUT_NAME} pico_stdlib pico_motor_shim motor button breakout_msa301) - -# create map/bin/hex file etc. -pico_add_extra_outputs(${OUTPUT_NAME}) diff --git a/examples/pico_motor_shim/balance/demo.cpp b/examples/pico_motor_shim/balance/demo.cpp deleted file mode 100644 index 39ae4000..00000000 --- a/examples/pico_motor_shim/balance/demo.cpp +++ /dev/null @@ -1,119 +0,0 @@ -#include -#include "pico_motor_shim.hpp" - -#include "common/pimoroni_common.hpp" -#include "motor.hpp" -#include "button.hpp" -#include "breakout_msa301.hpp" -#include - -/* -A very basic balancing robot implementation, using an MSA301 to give accelerating values that are passed to the motors using proportional control. -Press "A" to start and stop the balancer -*/ - -using namespace pimoroni; -using namespace motor; - -static constexpr float TOP_SPEED = 1.0f; //A value between 0 and 1 -static constexpr float Z_BIAS_CORRECTION = 0.5f; //A magic number that seems to correct the MSA301's Z bias -static constexpr float PROPORTIONAL = 0.03f; - - - -Button button_a(pico_motor_shim::BUTTON_A, Polarity::ACTIVE_LOW, 0); - -Motor motor_1(pico_motor_shim::MOTOR_1);//, Motor::DEFAULT_PWM_FREQUENCY, Motor::DEFAULT_DECAY_MODE); -Motor motor_2(pico_motor_shim::MOTOR_2);//, Motor::DEFAULT_PWM_FREQUENCY, Motor::DEFAULT_DECAY_MODE); - -I2C i2c(BOARD::BREAKOUT_GARDEN); -BreakoutMSA301 msa301(&i2c); - -static bool button_toggle = false; -static float target_angle = 0.0f; - -/** - * Checks if the button has been pressed, toggling a value that is also returned. - */ -bool check_button_toggle() { - bool button_pressed = button_a.read(); - if(button_pressed) { - button_toggle = !button_toggle; - } - return button_toggle; -} - -/** - * Takes and angle and wraps it around so that it stays within a -180 to +180 degree range. - * - * Note, it will only work for values between -540 and +540 degrees. - * This can be resolved by changing the 'if's into 'while's, but for most uses it is unnecessary - */ -float wrap_angle(float angle) { - if(angle <= -180.0f) - angle += 360.0f; - - if(angle > 180.0f) - angle -= 360.0f; - - return angle; -} - -/** - * The entry point of the program. - */ -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); - for(int i = 0; i < 20; i++) { - gpio_put(PICO_DEFAULT_LED_PIN, true); - sleep_ms(250); - gpio_put(PICO_DEFAULT_LED_PIN, false); - sleep_ms(250); - } - - //Initialise the two motors - if(!motor_1.init() || !motor_2.init()) { - printf("Cannot initialise motors. Check the provided parameters\n"); - return 0; - } - - if(!msa301.init()) { - printf("Cannot initialise msa301. Check that it is connected\n"); - return 0; - } - - printf("Ready\n"); - - while(true) { - //Turn the Pico's LED on to show that the sequence has started - gpio_put(PICO_DEFAULT_LED_PIN, true); - sleep_ms(50); - - //Has the user has pressed the button to start the sequence - while(check_button_toggle()) { - float y = msa301.get_y_axis(); - float z = msa301.get_z_axis() + Z_BIAS_CORRECTION; - - float current_angle = (atan2(z, -y) * 180.0f) / M_PI; - float angle_error = wrap_angle(target_angle - current_angle); - printf("Y: %f, Z: %f, AngErr: %f\n", y, z, angle_error); - - float output = angle_error * PROPORTIONAL; //No need to clamp this value as set_speed does this internally - motor_1.speed(output); - motor_2.speed(-output); - - sleep_ms(1); - } - - //The sequence loop has ended, 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(50); - } - return 0; -} From 51d08bebd919c7a29272ccf71933125ef332ed47 Mon Sep 17 00:00:00 2001 From: ZodiusInfuser Date: Thu, 12 May 2022 23:49:31 +0100 Subject: [PATCH 59/60] Optimized most motor functions --- micropython/modules/micropython.cmake | 2 +- micropython/modules/motor/motor.cpp | 1532 ++++++++----------------- 2 files changed, 456 insertions(+), 1078 deletions(-) diff --git a/micropython/modules/micropython.cmake b/micropython/modules/micropython.cmake index 49089b9e..dc6461e8 100644 --- a/micropython/modules/micropython.cmake +++ b/micropython/modules/micropython.cmake @@ -29,7 +29,7 @@ include(breakout_bme280/micropython) include(breakout_bmp280/micropython) include(breakout_icp10125/micropython) include(breakout_scd41/micropython) -include(breakout_vl53l5cx/micropython) +# include(breakout_vl53l5cx/micropython) include(pico_scroll/micropython) include(pico_rgb_keypad/micropython) diff --git a/micropython/modules/motor/motor.cpp b/micropython/modules/motor/motor.cpp index dad3fbba..490f334d 100644 --- a/micropython/modules/motor/motor.cpp +++ b/micropython/modules/motor/motor.cpp @@ -13,6 +13,39 @@ extern "C" { #include "py/builtin.h" #include "float.h" +void pimoroni_tuple_or_list(const mp_obj_t &object, mp_obj_t **items, size_t *length) { + if(mp_obj_is_type(object, &mp_type_list)) { + mp_obj_list_t *list = MP_OBJ_TO_PTR2(object, mp_obj_list_t); + *length = list->len; + *items = list->items; + } + else if(mp_obj_is_type(object, &mp_type_tuple)) { + mp_obj_tuple_t *tuple = MP_OBJ_TO_PTR2(object, mp_obj_tuple_t); + *length = tuple->len; + *items = tuple->items; + } + if(*items == nullptr) { + mp_raise_TypeError("cannot convert object to a list or tuple of integers, or a single integer"); + } else if(*length == 0) { + mp_raise_TypeError("list or tuple must contain at least one integer"); + } +} + +uint8_t* pimoroni_motors_from_items(mp_obj_t *items, size_t length, int motor_count) { + uint8_t *motors = new uint8_t[length]; + for(size_t i = 0; i < length; i++) { + int motor = mp_obj_get_int(items[i]); + if(motor < 0 || motor >= motor_count) { + delete[] motors; + mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("a motor in the list or tuple is out of range. Expected 0 to %d"), motor_count - 1); + } + else { + motors[i] = (uint8_t)motor; + } + } + return motors; +} + /********** Motor **********/ @@ -203,106 +236,67 @@ extern mp_obj_t Motor_is_enabled(mp_obj_t self_in) { } extern mp_obj_t Motor_duty(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { + enum { ARG_self, ARG_duty }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_duty, MP_ARG_OBJ, { .u_obj = mp_const_none }}, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _Motor_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Motor_obj_t); + if(n_args <= 1) { - enum { ARG_self }; - static const mp_arg_t allowed_args[] = { - { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, - }; - - // Parse args. - mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; - mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); - - _Motor_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Motor_obj_t); - return mp_obj_new_float(self->motor->duty()); } else { - enum { ARG_self, ARG_duty }; - static const mp_arg_t allowed_args[] = { - { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, - { MP_QSTR_duty, MP_ARG_REQUIRED | MP_ARG_OBJ }, - }; - - // Parse args. - mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; - mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); - - _Motor_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Motor_obj_t); - - float duty = mp_obj_get_float(args[ARG_duty].u_obj); - - self->motor->duty(duty); + self->motor->duty(mp_obj_get_float(args[ARG_duty].u_obj)); return mp_const_none; } } extern mp_obj_t Motor_speed(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { + enum { ARG_self, ARG_speed }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_speed, MP_ARG_OBJ, { .u_obj = mp_const_none }}, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _Motor_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Motor_obj_t); + if(n_args <= 1) { - enum { ARG_self }; - static const mp_arg_t allowed_args[] = { - { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, - }; - - // Parse args. - mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; - mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); - - _Motor_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Motor_obj_t); - return mp_obj_new_float(self->motor->speed()); } else { - enum { ARG_self, ARG_speed }; - static const mp_arg_t allowed_args[] = { - { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, - { MP_QSTR_speed, MP_ARG_REQUIRED | MP_ARG_OBJ }, - }; - - // Parse args. - mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; - mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); - - _Motor_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Motor_obj_t); - - float speed = mp_obj_get_float(args[ARG_speed].u_obj); - - self->motor->speed(speed); + self->motor->speed(mp_obj_get_float(args[ARG_speed].u_obj)); return mp_const_none; } } extern mp_obj_t Motor_frequency(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { + enum { ARG_self, ARG_freq }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_freq, MP_ARG_OBJ, { .u_obj = mp_const_none }}, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _Motor_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Motor_obj_t); + if(n_args <= 1) { - enum { ARG_self }; - static const mp_arg_t allowed_args[] = { - { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, - }; - - // Parse args. - mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; - mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); - - _Motor_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Motor_obj_t); - return mp_obj_new_float(self->motor->frequency()); } else { - enum { ARG_self, ARG_freq }; - static const mp_arg_t allowed_args[] = { - { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, - { MP_QSTR_freq, MP_ARG_REQUIRED | MP_ARG_OBJ }, - }; - - // Parse args. - mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; - mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); - - _Motor_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Motor_obj_t); - - float freq = mp_obj_get_float(args[ARG_freq].u_obj); - - if(!self->motor->frequency(freq)) { + if(!self->motor->frequency(mp_obj_get_float(args[ARG_freq].u_obj))) { mp_raise_ValueError("freq out of range. Expected 10Hz to 400KHz"); } return mp_const_none; @@ -408,34 +402,24 @@ extern mp_obj_t Motor_to_percent(size_t n_args, const mp_obj_t *pos_args, mp_map } extern mp_obj_t Motor_direction(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { - if(n_args <= 1) { - enum { ARG_self }; - static const mp_arg_t allowed_args[] = { - { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, - }; - - // Parse args. - mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; - mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); - - _Motor_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Motor_obj_t); - - return mp_obj_new_int(self->motor->direction()); - } - else { enum { ARG_self, ARG_direction }; static const mp_arg_t allowed_args[] = { { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, - { MP_QSTR_direction, MP_ARG_REQUIRED | MP_ARG_INT }, + { MP_QSTR_direction, MP_ARG_OBJ, { .u_obj = mp_const_none }}, }; - // Parse args. - mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; - mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); - _Motor_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Motor_obj_t); + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); - int direction = args[ARG_direction].u_int; + _Motor_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Motor_obj_t); + + if(n_args <= 1) { + return mp_obj_new_int(self->motor->direction()); + } + else { + int direction = mp_obj_get_int(args[ARG_direction].u_obj); if(direction < 0 || direction > 1) { mp_raise_ValueError("direction out of range. Expected NORMAL_DIR (0) or REVERSED_DIR (1)"); } @@ -445,33 +429,22 @@ extern mp_obj_t Motor_direction(size_t n_args, const mp_obj_t *pos_args, mp_map_ } extern mp_obj_t Motor_speed_scale(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { + enum { ARG_self, ARG_speed_scale }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_speed_scale, MP_ARG_OBJ, { .u_obj = mp_const_none }}, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _Motor_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Motor_obj_t); + if(n_args <= 1) { - enum { ARG_self }; - static const mp_arg_t allowed_args[] = { - { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, - }; - - // Parse args. - mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; - mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); - - _Motor_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Motor_obj_t); - return mp_obj_new_float(self->motor->speed_scale()); } else { - enum { ARG_self, ARG_speed_scale }; - static const mp_arg_t allowed_args[] = { - { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, - { MP_QSTR_speed_scale, MP_ARG_REQUIRED | MP_ARG_OBJ }, - }; - - // Parse args. - mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; - mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); - - _Motor_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Motor_obj_t); - float speed_scale = mp_obj_get_float(args[ARG_speed_scale].u_obj); if(speed_scale < FLT_EPSILON) { mp_raise_ValueError("speed_scale out of range. Expected greater than 0.0"); @@ -482,33 +455,22 @@ extern mp_obj_t Motor_speed_scale(size_t n_args, const mp_obj_t *pos_args, mp_ma } extern mp_obj_t Motor_zeropoint(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { + enum { ARG_self, ARG_zeropoint }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_zeropoint, MP_ARG_OBJ, { .u_obj = mp_const_none }}, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _Motor_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Motor_obj_t); + if(n_args <= 1) { - enum { ARG_self }; - static const mp_arg_t allowed_args[] = { - { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, - }; - - // Parse args. - mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; - mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); - - _Motor_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Motor_obj_t); - return mp_obj_new_float(self->motor->zeropoint()); } else { - enum { ARG_self, ARG_zeropoint }; - static const mp_arg_t allowed_args[] = { - { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, - { MP_QSTR_zeropoint, MP_ARG_REQUIRED | MP_ARG_OBJ }, - }; - - // Parse args. - mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; - mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); - - _Motor_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Motor_obj_t); - float zeropoint = mp_obj_get_float(args[ARG_zeropoint].u_obj); if(zeropoint < 0.0f || zeropoint > 1.0f - FLT_EPSILON) { mp_raise_ValueError("zeropoint out of range. Expected 0.0 to less than 1.0"); @@ -519,33 +481,22 @@ extern mp_obj_t Motor_zeropoint(size_t n_args, const mp_obj_t *pos_args, mp_map_ } extern mp_obj_t Motor_deadzone(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { + enum { ARG_self, ARG_deadzone }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_deadzone, MP_ARG_OBJ, { .u_obj = mp_const_none }}, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _Motor_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Motor_obj_t); + if(n_args <= 1) { - enum { ARG_self }; - static const mp_arg_t allowed_args[] = { - { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, - }; - - // Parse args. - mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; - mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); - - _Motor_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Motor_obj_t); - return mp_obj_new_float(self->motor->deadzone()); } else { - enum { ARG_self, ARG_deadzone }; - static const mp_arg_t allowed_args[] = { - { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, - { MP_QSTR_deadzone, MP_ARG_REQUIRED | MP_ARG_OBJ }, - }; - - // Parse args. - mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; - mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); - - _Motor_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Motor_obj_t); - float deadzone = mp_obj_get_float(args[ARG_deadzone].u_obj); if(deadzone < 0.0f || deadzone > 1.0f) { mp_raise_ValueError("deadzone out of range. Expected 0.0 to 1.0"); @@ -556,34 +507,23 @@ extern mp_obj_t Motor_deadzone(size_t n_args, const mp_obj_t *pos_args, mp_map_t } extern mp_obj_t Motor_decay_mode(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { + enum { ARG_self, ARG_mode }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_mode, MP_ARG_OBJ, { .u_obj = mp_const_none }}, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _Motor_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Motor_obj_t); + if(n_args <= 1) { - enum { ARG_self }; - static const mp_arg_t allowed_args[] = { - { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, - }; - - // Parse args. - mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; - mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); - - _Motor_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Motor_obj_t); - return mp_obj_new_int(self->motor->decay_mode()); } else { - enum { ARG_self, ARG_mode }; - static const mp_arg_t allowed_args[] = { - { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, - { MP_QSTR_mode, MP_ARG_REQUIRED | MP_ARG_INT }, - }; - - // Parse args. - mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; - mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); - - _Motor_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Motor_obj_t); - - int mode = args[ARG_mode].u_int; + int mode = mp_obj_get_int(args[ARG_mode].u_obj); if(mode < 0 || mode > 1) { mp_raise_ValueError("mode out of range. Expected FAST_DECAY (0) or SLOW_DECAY (1)"); } @@ -892,38 +832,14 @@ extern mp_obj_t MotorCluster_enable(size_t n_args, const mp_obj_t *pos_args, mp_ } else { size_t length = 0; - mp_obj_t *items = nullptr; - if(mp_obj_is_type(object, &mp_type_list)) { - mp_obj_list_t *list = MP_OBJ_TO_PTR2(object, mp_obj_list_t); - length = list->len; - items = list->items; - } - else if(mp_obj_is_type(object, &mp_type_tuple)) { - mp_obj_tuple_t *tuple = MP_OBJ_TO_PTR2(object, mp_obj_tuple_t); - length = tuple->len; - items = tuple->items; - } + mp_obj_t *items = nullptr; + pimoroni_tuple_or_list(object, &items, &length); - if(items == nullptr) - mp_raise_TypeError("cannot convert object to a list or tuple of integers, or a single integer"); - else if(length == 0) - mp_raise_TypeError("list or tuple must contain at least one integer"); - else { - // Create and populate a local array of motor indices - uint8_t *motors = new uint8_t[length]; - for(size_t i = 0; i < length; i++) { - int motor = mp_obj_get_int(items[i]); - if(motor < 0 || motor >= motor_count) { - delete[] motors; - mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("a motor in the list or tuple is out of range. Expected 0 to %d"), motor_count - 1); - } - else { - motors[i] = (uint8_t)motor; - } - } - self->cluster->enable(motors, length, args[ARG_load].u_bool); - delete[] motors; - } + // Create and populate a local array of motor indices + uint8_t *motors = pimoroni_motors_from_items(items, length, motor_count); + + self->cluster->enable(motors, length, args[ARG_load].u_bool); + delete[] motors; } } @@ -977,37 +893,13 @@ extern mp_obj_t MotorCluster_disable(size_t n_args, const mp_obj_t *pos_args, mp else { size_t length = 0; mp_obj_t *items = nullptr; - if(mp_obj_is_type(object, &mp_type_list)) { - mp_obj_list_t *list = MP_OBJ_TO_PTR2(object, mp_obj_list_t); - length = list->len; - items = list->items; - } - else if(mp_obj_is_type(object, &mp_type_tuple)) { - mp_obj_tuple_t *tuple = MP_OBJ_TO_PTR2(object, mp_obj_tuple_t); - length = tuple->len; - items = tuple->items; - } + pimoroni_tuple_or_list(object, &items, &length); - if(items == nullptr) - mp_raise_TypeError("cannot convert object to a list or tuple of integers, or a single integer"); - else if(length == 0) - mp_raise_TypeError("list or tuple must contain at least one integer"); - else { - // Create and populate a local array of motor indices - uint8_t *motors = new uint8_t[length]; - for(size_t i = 0; i < length; i++) { - int motor = mp_obj_get_int(items[i]); - if(motor < 0 || motor >= motor_count) { - delete[] motors; - mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("a motor in the list or tuple is out of range. Expected 0 to %d"), motor_count - 1); - } - else { - motors[i] = (uint8_t)motor; - } - } - self->cluster->disable(motors, length, args[ARG_load].u_bool); - delete[] motors; - } + // Create and populate a local array of motor indices + uint8_t *motors = pimoroni_motors_from_items(items, length, motor_count); + + self->cluster->disable(motors, length, args[ARG_load].u_bool); + delete[] motors; } } @@ -1057,49 +949,34 @@ extern mp_obj_t MotorCluster_is_enabled(size_t n_args, const mp_obj_t *pos_args, } extern mp_obj_t MotorCluster_duty(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { - if(n_args <= 2) { - enum { ARG_self, ARG_motor }; - static const mp_arg_t allowed_args[] = { - { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, - { MP_QSTR_motor, MP_ARG_REQUIRED | MP_ARG_INT }, - }; + enum { ARG_self, ARG_motor, ARG_duty, ARG_load }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_motor, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_duty, MP_ARG_OBJ, { .u_obj = mp_const_none }}, + { MP_QSTR_load, MP_ARG_BOOL, { .u_bool = true }}, + }; - // Parse args. - mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; - mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); - _MotorCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _MotorCluster_obj_t); + _MotorCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _MotorCluster_obj_t); - int motor = args[ARG_motor].u_int; - int motor_count = (int)self->cluster->count(); - if(motor_count == 0) - mp_raise_ValueError("this cluster does not have any motors"); - else if(motor < 0 || motor >= motor_count) - mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("motor out of range. Expected 0 to %d"), motor_count - 1); - else - return mp_obj_new_float(self->cluster->duty((uint)motor)); - } + int motor_count = (int)self->cluster->count(); + if(motor_count == 0) + mp_raise_ValueError("this cluster does not have any motors"); else { - enum { ARG_self, ARG_motors, ARG_duty, ARG_load }; - static const mp_arg_t allowed_args[] = { - { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, - { MP_QSTR_motors, MP_ARG_REQUIRED | MP_ARG_OBJ }, - { MP_QSTR_duty, MP_ARG_REQUIRED | MP_ARG_OBJ }, - { MP_QSTR_load, MP_ARG_BOOL, { .u_bool = true }}, - }; - - // Parse args. - mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; - mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); - - _MotorCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _MotorCluster_obj_t); - - int motor_count = (int)self->cluster->count(); - if(motor_count == 0) - mp_raise_ValueError("this cluster does not have any motors"); + if(n_args <= 2) { + int motor = mp_obj_get_int(args[ARG_motor].u_obj); + if(motor < 0 || motor >= motor_count) + mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("motor out of range. Expected 0 to %d"), motor_count - 1); + else + return mp_obj_new_float(self->cluster->duty((uint)motor)); + } else { // Determine what motor(s) to modify - const mp_obj_t object = args[ARG_motors].u_obj; + const mp_obj_t object = args[ARG_motor].u_obj; if(mp_obj_is_int(object)) { int motor = mp_obj_get_int(object); if(motor < 0 || motor >= motor_count) @@ -1112,38 +989,14 @@ extern mp_obj_t MotorCluster_duty(size_t n_args, const mp_obj_t *pos_args, mp_ma else { size_t length = 0; mp_obj_t *items = nullptr; - if(mp_obj_is_type(object, &mp_type_list)) { - mp_obj_list_t *list = MP_OBJ_TO_PTR2(object, mp_obj_list_t); - length = list->len; - items = list->items; - } - else if(mp_obj_is_type(object, &mp_type_tuple)) { - mp_obj_tuple_t *tuple = MP_OBJ_TO_PTR2(object, mp_obj_tuple_t); - length = tuple->len; - items = tuple->items; - } + pimoroni_tuple_or_list(object, &items, &length); - if(items == nullptr) - mp_raise_TypeError("cannot convert object to a list or tuple of integers, or a single integer"); - else if(length == 0) - mp_raise_TypeError("list or tuple must contain at least one integer"); - else { - // Create and populate a local array of motor indices - uint8_t *motors = new uint8_t[length]; - for(size_t i = 0; i < length; i++) { - int motor = mp_obj_get_int(items[i]); - if(motor < 0 || motor >= motor_count) { - delete[] motors; - mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("a motor in the list or tuple is out of range. Expected 0 to %d"), motor_count - 1); - } - else { - motors[i] = (uint8_t)motor; - } - } - float duty = mp_obj_get_float(args[ARG_duty].u_obj); - self->cluster->duty(motors, length, duty, args[ARG_load].u_bool); - delete[] motors; - } + // Create and populate a local array of motor indices + uint8_t *motors = pimoroni_motors_from_items(items, length, motor_count); + + float duty = mp_obj_get_float(args[ARG_duty].u_obj); + self->cluster->duty(motors, length, duty, args[ARG_load].u_bool); + delete[] motors; } } } @@ -1175,49 +1028,34 @@ extern mp_obj_t MotorCluster_all_to_duty(size_t n_args, const mp_obj_t *pos_args } extern mp_obj_t MotorCluster_speed(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { - if(n_args <= 2) { - enum { ARG_self, ARG_motor }; - static const mp_arg_t allowed_args[] = { - { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, - { MP_QSTR_motor, MP_ARG_REQUIRED | MP_ARG_INT }, - }; + enum { ARG_self, ARG_motor, ARG_speed, ARG_load }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_motor, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_speed, MP_ARG_OBJ, { .u_obj = mp_const_none }}, + { MP_QSTR_load, MP_ARG_BOOL, { .u_bool = true }}, + }; - // Parse args. - mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; - mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); - _MotorCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _MotorCluster_obj_t); + _MotorCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _MotorCluster_obj_t); - int motor = args[ARG_motor].u_int; - int motor_count = (int)self->cluster->count(); - if(motor_count == 0) - mp_raise_ValueError("this cluster does not have any motors"); - else if(motor < 0 || motor >= motor_count) - mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("motor out of range. Expected 0 to %d"), motor_count - 1); - else - return mp_obj_new_float(self->cluster->speed((uint)motor)); - } + int motor_count = (int)self->cluster->count(); + if(motor_count == 0) + mp_raise_ValueError("this cluster does not have any motors"); else { - enum { ARG_self, ARG_motors, ARG_speed, ARG_load }; - static const mp_arg_t allowed_args[] = { - { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, - { MP_QSTR_motors, MP_ARG_REQUIRED | MP_ARG_OBJ }, - { MP_QSTR_speed, MP_ARG_REQUIRED | MP_ARG_OBJ }, - { MP_QSTR_load, MP_ARG_BOOL, { .u_bool = true }}, - }; - - // Parse args. - mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; - mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); - - _MotorCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _MotorCluster_obj_t); - - int motor_count = (int)self->cluster->count(); - if(motor_count == 0) - mp_raise_ValueError("this cluster does not have any motors"); + if(n_args <= 2) { + int motor = mp_obj_get_int(args[ARG_motor].u_obj); + if(motor < 0 || motor >= motor_count) + mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("motor out of range. Expected 0 to %d"), motor_count - 1); + else + return mp_obj_new_float(self->cluster->speed((uint)motor)); + } else { // Determine what motor(s) to modify - const mp_obj_t object = args[ARG_motors].u_obj; + const mp_obj_t object = args[ARG_motor].u_obj; if(mp_obj_is_int(object)) { int motor = mp_obj_get_int(object); if(motor < 0 || motor >= motor_count) @@ -1230,38 +1068,14 @@ extern mp_obj_t MotorCluster_speed(size_t n_args, const mp_obj_t *pos_args, mp_m else { size_t length = 0; mp_obj_t *items = nullptr; - if(mp_obj_is_type(object, &mp_type_list)) { - mp_obj_list_t *list = MP_OBJ_TO_PTR2(object, mp_obj_list_t); - length = list->len; - items = list->items; - } - else if(mp_obj_is_type(object, &mp_type_tuple)) { - mp_obj_tuple_t *tuple = MP_OBJ_TO_PTR2(object, mp_obj_tuple_t); - length = tuple->len; - items = tuple->items; - } + pimoroni_tuple_or_list(object, &items, &length); - if(items == nullptr) - mp_raise_TypeError("cannot convert object to a list or tuple of integers, or a single integer"); - else if(length == 0) - mp_raise_TypeError("list or tuple must contain at least one integer"); - else { - // Create and populate a local array of motor indices - uint8_t *motors = new uint8_t[length]; - for(size_t i = 0; i < length; i++) { - int motor = mp_obj_get_int(items[i]); - if(motor < 0 || motor >= motor_count) { - delete[] motors; - mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("a motor in the list or tuple is out of range. Expected 0 to %d"), motor_count - 1); - } - else { - motors[i] = (uint8_t)motor; - } - } - float speed = mp_obj_get_float(args[ARG_speed].u_obj); - self->cluster->speed(motors, length, speed, args[ARG_load].u_bool); - delete[] motors; - } + // Create and populate a local array of motor indices + uint8_t *motors = pimoroni_motors_from_items(items, length, motor_count); + + float speed = mp_obj_get_float(args[ARG_speed].u_obj); + self->cluster->speed(motors, length, speed, args[ARG_load].u_bool); + delete[] motors; } } } @@ -1293,49 +1107,35 @@ extern mp_obj_t MotorCluster_all_to_speed(size_t n_args, const mp_obj_t *pos_arg } extern mp_obj_t MotorCluster_phase(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { - if(n_args <= 2) { - enum { ARG_self, ARG_motor }; - static const mp_arg_t allowed_args[] = { - { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, - { MP_QSTR_motor, MP_ARG_REQUIRED | MP_ARG_INT }, - }; + + enum { ARG_self, ARG_motor, ARG_phase, ARG_load }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_motor, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_phase, MP_ARG_OBJ, { .u_obj = mp_const_none }}, + { MP_QSTR_load, MP_ARG_BOOL, { .u_bool = true }}, + }; - // Parse args. - mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; - mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); - _MotorCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _MotorCluster_obj_t); + _MotorCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _MotorCluster_obj_t); - int motor = args[ARG_motor].u_int; - int motor_count = (int)self->cluster->count(); - if(motor_count == 0) - mp_raise_ValueError("this cluster does not have any motors"); - else if(motor < 0 || motor >= motor_count) - mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("motor out of range. Expected 0 to %d"), motor_count - 1); - else - return mp_obj_new_float(self->cluster->phase((uint)motor)); - } + int motor_count = (int)self->cluster->count(); + if(motor_count == 0) + mp_raise_ValueError("this cluster does not have any motors"); else { - enum { ARG_self, ARG_motors, ARG_phase, ARG_load }; - static const mp_arg_t allowed_args[] = { - { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, - { MP_QSTR_motors, MP_ARG_REQUIRED | MP_ARG_OBJ }, - { MP_QSTR_phase, MP_ARG_REQUIRED | MP_ARG_OBJ }, - { MP_QSTR_load, MP_ARG_BOOL, { .u_bool = true }}, - }; - - // Parse args. - mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; - mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); - - _MotorCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _MotorCluster_obj_t); - - int motor_count = (int)self->cluster->count(); - if(motor_count == 0) - mp_raise_ValueError("this cluster does not have any motors"); + if(n_args <= 2) { + int motor = mp_obj_get_int(args[ARG_motor].u_obj); + if(motor < 0 || motor >= motor_count) + mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("motor out of range. Expected 0 to %d"), motor_count - 1); + else + return mp_obj_new_float(self->cluster->phase((uint)motor)); + } else { // Determine what motor(s) to modify - const mp_obj_t object = args[ARG_motors].u_obj; + const mp_obj_t object = args[ARG_motor].u_obj; if(mp_obj_is_int(object)) { int motor = mp_obj_get_int(object); if(motor < 0 || motor >= motor_count) @@ -1348,38 +1148,14 @@ extern mp_obj_t MotorCluster_phase(size_t n_args, const mp_obj_t *pos_args, mp_m else { size_t length = 0; mp_obj_t *items = nullptr; - if(mp_obj_is_type(object, &mp_type_list)) { - mp_obj_list_t *list = MP_OBJ_TO_PTR2(object, mp_obj_list_t); - length = list->len; - items = list->items; - } - else if(mp_obj_is_type(object, &mp_type_tuple)) { - mp_obj_tuple_t *tuple = MP_OBJ_TO_PTR2(object, mp_obj_tuple_t); - length = tuple->len; - items = tuple->items; - } + pimoroni_tuple_or_list(object, &items, &length); - if(items == nullptr) - mp_raise_TypeError("cannot convert object to a list or tuple of integers, or a single integer"); - else if(length == 0) - mp_raise_TypeError("list or tuple must contain at least one integer"); - else { - // Create and populate a local array of motor indices - uint8_t *motors = new uint8_t[length]; - for(size_t i = 0; i < length; i++) { - int motor = mp_obj_get_int(items[i]); - if(motor < 0 || motor >= motor_count) { - delete[] motors; - mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("a motor in the list or tuple is out of range. Expected 0 to %d"), motor_count - 1); - } - else { - motors[i] = (uint8_t)motor; - } - } - float phase = mp_obj_get_float(args[ARG_phase].u_obj); - self->cluster->phase(motors, length, phase, args[ARG_load].u_bool); - delete[] motors; - } + // Create and populate a local array of motor indices + uint8_t *motors = pimoroni_motors_from_items(items, length, motor_count); + + float phase = mp_obj_get_float(args[ARG_phase].u_obj); + self->cluster->phase(motors, length, phase, args[ARG_load].u_bool); + delete[] motors; } } } @@ -1411,33 +1187,22 @@ extern mp_obj_t MotorCluster_all_to_phase(size_t n_args, const mp_obj_t *pos_arg } extern mp_obj_t MotorCluster_frequency(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { + enum { ARG_self, ARG_freq }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_freq, MP_ARG_OBJ, { .u_obj = mp_const_none }}, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _MotorCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _MotorCluster_obj_t); + if(n_args <= 1) { - enum { ARG_self }; - static const mp_arg_t allowed_args[] = { - { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, - }; - - // Parse args. - mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; - mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); - - _MotorCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _MotorCluster_obj_t); - return mp_obj_new_float(self->cluster->frequency()); } else { - enum { ARG_self, ARG_freq }; - static const mp_arg_t allowed_args[] = { - { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, - { MP_QSTR_freq, MP_ARG_REQUIRED | MP_ARG_OBJ }, - }; - - // Parse args. - mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; - mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); - - _MotorCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _MotorCluster_obj_t); - float freq = mp_obj_get_float(args[ARG_freq].u_obj); if(!self->cluster->frequency(freq)) @@ -1477,37 +1242,13 @@ extern mp_obj_t MotorCluster_stop(size_t n_args, const mp_obj_t *pos_args, mp_ma else { size_t length = 0; mp_obj_t *items = nullptr; - if(mp_obj_is_type(object, &mp_type_list)) { - mp_obj_list_t *list = MP_OBJ_TO_PTR2(object, mp_obj_list_t); - length = list->len; - items = list->items; - } - else if(mp_obj_is_type(object, &mp_type_tuple)) { - mp_obj_tuple_t *tuple = MP_OBJ_TO_PTR2(object, mp_obj_tuple_t); - length = tuple->len; - items = tuple->items; - } + pimoroni_tuple_or_list(object, &items, &length); - if(items == nullptr) - mp_raise_TypeError("cannot convert object to a list or tuple of integers, or a single integer"); - else if(length == 0) - mp_raise_TypeError("list or tuple must contain at least one integer"); - else { - // Create and populate a local array of motor indices - uint8_t *motors = new uint8_t[length]; - for(size_t i = 0; i < length; i++) { - int motor = mp_obj_get_int(items[i]); - if(motor < 0 || motor >= motor_count) { - delete[] motors; - mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("a motor in the list or tuple is out of range. Expected 0 to %d"), motor_count - 1); - } - else { - motors[i] = (uint8_t)motor; - } - } - self->cluster->stop(motors, length, args[ARG_load].u_bool); - delete[] motors; - } + // Create and populate a local array of motor indices + uint8_t *motors = pimoroni_motors_from_items(items, length, motor_count); + + self->cluster->stop(motors, length, args[ARG_load].u_bool); + delete[] motors; } } return mp_const_none; @@ -1565,37 +1306,13 @@ extern mp_obj_t MotorCluster_coast(size_t n_args, const mp_obj_t *pos_args, mp_m else { size_t length = 0; mp_obj_t *items = nullptr; - if(mp_obj_is_type(object, &mp_type_list)) { - mp_obj_list_t *list = MP_OBJ_TO_PTR2(object, mp_obj_list_t); - length = list->len; - items = list->items; - } - else if(mp_obj_is_type(object, &mp_type_tuple)) { - mp_obj_tuple_t *tuple = MP_OBJ_TO_PTR2(object, mp_obj_tuple_t); - length = tuple->len; - items = tuple->items; - } + pimoroni_tuple_or_list(object, &items, &length); - if(items == nullptr) - mp_raise_TypeError("cannot convert object to a list or tuple of integers, or a single integer"); - else if(length == 0) - mp_raise_TypeError("list or tuple must contain at least one integer"); - else { - // Create and populate a local array of motor indices - uint8_t *motors = new uint8_t[length]; - for(size_t i = 0; i < length; i++) { - int motor = mp_obj_get_int(items[i]); - if(motor < 0 || motor >= motor_count) { - delete[] motors; - mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("a motor in the list or tuple is out of range. Expected 0 to %d"), motor_count - 1); - } - else { - motors[i] = (uint8_t)motor; - } - } - self->cluster->coast(motors, length, args[ARG_load].u_bool); - delete[] motors; - } + // Create and populate a local array of motor indices + uint8_t *motors = pimoroni_motors_from_items(items, length, motor_count); + + self->cluster->coast(motors, length, args[ARG_load].u_bool); + delete[] motors; } } return mp_const_none; @@ -1653,37 +1370,13 @@ extern mp_obj_t MotorCluster_brake(size_t n_args, const mp_obj_t *pos_args, mp_m else { size_t length = 0; mp_obj_t *items = nullptr; - if(mp_obj_is_type(object, &mp_type_list)) { - mp_obj_list_t *list = MP_OBJ_TO_PTR2(object, mp_obj_list_t); - length = list->len; - items = list->items; - } - else if(mp_obj_is_type(object, &mp_type_tuple)) { - mp_obj_tuple_t *tuple = MP_OBJ_TO_PTR2(object, mp_obj_tuple_t); - length = tuple->len; - items = tuple->items; - } + pimoroni_tuple_or_list(object, &items, &length); - if(items == nullptr) - mp_raise_TypeError("cannot convert object to a list or tuple of integers, or a single integer"); - else if(length == 0) - mp_raise_TypeError("list or tuple must contain at least one integer"); - else { - // Create and populate a local array of motor indices - uint8_t *motors = new uint8_t[length]; - for(size_t i = 0; i < length; i++) { - int motor = mp_obj_get_int(items[i]); - if(motor < 0 || motor >= motor_count) { - delete[] motors; - mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("a motor in the list or tuple is out of range. Expected 0 to %d"), motor_count - 1); - } - else { - motors[i] = (uint8_t)motor; - } - } - self->cluster->brake(motors, length, args[ARG_load].u_bool); - delete[] motors; - } + // Create and populate a local array of motor indices + uint8_t *motors = pimoroni_motors_from_items(items, length, motor_count); + + self->cluster->brake(motors, length, args[ARG_load].u_bool); + delete[] motors; } } return mp_const_none; @@ -1741,37 +1434,13 @@ extern mp_obj_t MotorCluster_full_negative(size_t n_args, const mp_obj_t *pos_ar else { size_t length = 0; mp_obj_t *items = nullptr; - if(mp_obj_is_type(object, &mp_type_list)) { - mp_obj_list_t *list = MP_OBJ_TO_PTR2(object, mp_obj_list_t); - length = list->len; - items = list->items; - } - else if(mp_obj_is_type(object, &mp_type_tuple)) { - mp_obj_tuple_t *tuple = MP_OBJ_TO_PTR2(object, mp_obj_tuple_t); - length = tuple->len; - items = tuple->items; - } + pimoroni_tuple_or_list(object, &items, &length); - if(items == nullptr) - mp_raise_TypeError("cannot convert object to a list or tuple of integers, or a single integer"); - else if(length == 0) - mp_raise_TypeError("list or tuple must contain at least one integer"); - else { - // Create and populate a local array of motor indices - uint8_t *motors = new uint8_t[length]; - for(size_t i = 0; i < length; i++) { - int motor = mp_obj_get_int(items[i]); - if(motor < 0 || motor >= motor_count) { - delete[] motors; - mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("a motor in the list or tuple is out of range. Expected 0 to %d"), motor_count - 1); - } - else { - motors[i] = (uint8_t)motor; - } - } - self->cluster->full_negative(motors, length, args[ARG_load].u_bool); - delete[] motors; - } + // Create and populate a local array of motor indices + uint8_t *motors = pimoroni_motors_from_items(items, length, motor_count); + + self->cluster->full_negative(motors, length, args[ARG_load].u_bool); + delete[] motors; } } return mp_const_none; @@ -1829,37 +1498,13 @@ extern mp_obj_t MotorCluster_full_positive(size_t n_args, const mp_obj_t *pos_ar else { size_t length = 0; mp_obj_t *items = nullptr; - if(mp_obj_is_type(object, &mp_type_list)) { - mp_obj_list_t *list = MP_OBJ_TO_PTR2(object, mp_obj_list_t); - length = list->len; - items = list->items; - } - else if(mp_obj_is_type(object, &mp_type_tuple)) { - mp_obj_tuple_t *tuple = MP_OBJ_TO_PTR2(object, mp_obj_tuple_t); - length = tuple->len; - items = tuple->items; - } + pimoroni_tuple_or_list(object, &items, &length); - if(items == nullptr) - mp_raise_TypeError("cannot convert object to a list or tuple of integers, or a single integer"); - else if(length == 0) - mp_raise_TypeError("list or tuple must contain at least one integer"); - else { - // Create and populate a local array of motor indices - uint8_t *motors = new uint8_t[length]; - for(size_t i = 0; i < length; i++) { - int motor = mp_obj_get_int(items[i]); - if(motor < 0 || motor >= motor_count) { - delete[] motors; - mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("a motor in the list or tuple is out of range. Expected 0 to %d"), motor_count - 1); - } - else { - motors[i] = (uint8_t)motor; - } - } - self->cluster->full_positive(motors, length, args[ARG_load].u_bool); - delete[] motors; - } + // Create and populate a local array of motor indices + uint8_t *motors = pimoroni_motors_from_items(items, length, motor_count); + + self->cluster->full_positive(motors, length, args[ARG_load].u_bool); + delete[] motors; } } return mp_const_none; @@ -1921,38 +1566,14 @@ extern mp_obj_t MotorCluster_to_percent(size_t n_args, const mp_obj_t *pos_args, else { size_t length = 0; mp_obj_t *items = nullptr; - if(mp_obj_is_type(object, &mp_type_list)) { - mp_obj_list_t *list = MP_OBJ_TO_PTR2(object, mp_obj_list_t); - length = list->len; - items = list->items; - } - else if(mp_obj_is_type(object, &mp_type_tuple)) { - mp_obj_tuple_t *tuple = MP_OBJ_TO_PTR2(object, mp_obj_tuple_t); - length = tuple->len; - items = tuple->items; - } + pimoroni_tuple_or_list(object, &items, &length); - if(items == nullptr) - mp_raise_TypeError("cannot convert object to a list or tuple of integers, or a single integer"); - else if(length == 0) - mp_raise_TypeError("list or tuple must contain at least one integer"); - else { - // Create and populate a local array of motor indices - uint8_t *motors = new uint8_t[length]; - for(size_t i = 0; i < length; i++) { - int motor = mp_obj_get_int(items[i]); - if(motor < 0 || motor >= motor_count) { - delete[] motors; - mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("a motor in the list or tuple is out of range. Expected 0 to %d"), motor_count - 1); - } - else { - motors[i] = (uint8_t)motor; - } - } - float in = mp_obj_get_float(args[ARG_in].u_obj); - self->cluster->to_percent(motors, length, in, args[ARG_load].u_bool); - delete[] motors; - } + // Create and populate a local array of motor indices + uint8_t *motors = pimoroni_motors_from_items(items, length, motor_count); + + float in = mp_obj_get_float(args[ARG_in].u_obj); + self->cluster->to_percent(motors, length, in, args[ARG_load].u_bool); + delete[] motors; } } } @@ -1993,40 +1614,16 @@ extern mp_obj_t MotorCluster_to_percent(size_t n_args, const mp_obj_t *pos_args, else { size_t length = 0; mp_obj_t *items = nullptr; - if(mp_obj_is_type(object, &mp_type_list)) { - mp_obj_list_t *list = MP_OBJ_TO_PTR2(object, mp_obj_list_t); - length = list->len; - items = list->items; - } - else if(mp_obj_is_type(object, &mp_type_tuple)) { - mp_obj_tuple_t *tuple = MP_OBJ_TO_PTR2(object, mp_obj_tuple_t); - length = tuple->len; - items = tuple->items; - } + pimoroni_tuple_or_list(object, &items, &length); - if(items == nullptr) - mp_raise_TypeError("cannot convert object to a list or tuple of integers, or a single integer"); - else if(length == 0) - mp_raise_TypeError("list or tuple must contain at least one integer"); - else { - // Create and populate a local array of motor indices - uint8_t *motors = new uint8_t[length]; - for(size_t i = 0; i < length; i++) { - int motor = mp_obj_get_int(items[i]); - if(motor < 0 || motor >= motor_count) { - delete[] motors; - mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("a motor in the list or tuple is out of range. Expected 0 to %d"), motor_count - 1); - } - else { - motors[i] = (uint8_t)motor; - } - } - float in = mp_obj_get_float(args[ARG_in].u_obj); - float in_min = mp_obj_get_float(args[ARG_in_min].u_obj); - float in_max = mp_obj_get_float(args[ARG_in_max].u_obj); - self->cluster->to_percent(motors, length, in, in_min, in_max, args[ARG_load].u_bool); - delete[] motors; - } + // Create and populate a local array of motor indices + uint8_t *motors = pimoroni_motors_from_items(items, length, motor_count); + + float in = mp_obj_get_float(args[ARG_in].u_obj); + float in_min = mp_obj_get_float(args[ARG_in_min].u_obj); + float in_max = mp_obj_get_float(args[ARG_in_max].u_obj); + self->cluster->to_percent(motors, length, in, in_min, in_max, args[ARG_load].u_bool); + delete[] motors; } } } @@ -2071,42 +1668,18 @@ extern mp_obj_t MotorCluster_to_percent(size_t n_args, const mp_obj_t *pos_args, else { size_t length = 0; mp_obj_t *items = nullptr; - if(mp_obj_is_type(object, &mp_type_list)) { - mp_obj_list_t *list = MP_OBJ_TO_PTR2(object, mp_obj_list_t); - length = list->len; - items = list->items; - } - else if(mp_obj_is_type(object, &mp_type_tuple)) { - mp_obj_tuple_t *tuple = MP_OBJ_TO_PTR2(object, mp_obj_tuple_t); - length = tuple->len; - items = tuple->items; - } + pimoroni_tuple_or_list(object, &items, &length); - if(items == nullptr) - mp_raise_TypeError("cannot convert object to a list or tuple of integers, or a single integer"); - else if(length == 0) - mp_raise_TypeError("list or tuple must contain at least one integer"); - else { - // Create and populate a local array of motor indices - uint8_t *motors = new uint8_t[length]; - for(size_t i = 0; i < length; i++) { - int motor = mp_obj_get_int(items[i]); - if(motor < 0 || motor >= motor_count) { - delete[] motors; - mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("a motor in the list or tuple is out of range. Expected 0 to %d"), motor_count - 1); - } - else { - motors[i] = (uint8_t)motor; - } - } - float in = mp_obj_get_float(args[ARG_in].u_obj); - float in_min = mp_obj_get_float(args[ARG_in_min].u_obj); - float in_max = mp_obj_get_float(args[ARG_in_max].u_obj); - float speed_min = mp_obj_get_float(args[ARG_speed_min].u_obj); - float speed_max = mp_obj_get_float(args[ARG_speed_max].u_obj); - self->cluster->to_percent(motors, length, in, in_min, in_max, speed_min, speed_max, args[ARG_load].u_bool); - delete[] motors; - } + // Create and populate a local array of motor indices + uint8_t *motors = pimoroni_motors_from_items(items, length, motor_count); + + float in = mp_obj_get_float(args[ARG_in].u_obj); + float in_min = mp_obj_get_float(args[ARG_in_min].u_obj); + float in_max = mp_obj_get_float(args[ARG_in_max].u_obj); + float speed_min = mp_obj_get_float(args[ARG_speed_min].u_obj); + float speed_max = mp_obj_get_float(args[ARG_speed_max].u_obj); + self->cluster->to_percent(motors, length, in, in_min, in_max, speed_min, speed_max, args[ARG_load].u_bool); + delete[] motors; } } } @@ -2202,54 +1775,39 @@ extern mp_obj_t MotorCluster_load(mp_obj_t self_in) { } extern mp_obj_t MotorCluster_direction(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { - if(n_args <= 2) { - enum { ARG_self, ARG_motor }; - static const mp_arg_t allowed_args[] = { - { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, - { MP_QSTR_motor, MP_ARG_REQUIRED | MP_ARG_INT }, - }; + enum { ARG_self, ARG_motor, ARG_direction }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_motor, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_direction, MP_ARG_OBJ, { .u_obj = mp_const_none }}, + }; - // Parse args. - mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; - mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); - _MotorCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _MotorCluster_obj_t); + _MotorCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _MotorCluster_obj_t); - int motor = args[ARG_motor].u_int; - int motor_count = (int)self->cluster->count(); - if(motor_count == 0) - mp_raise_ValueError("this cluster does not have any motors"); - else if(motor < 0 || motor >= motor_count) - mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("motor out of range. Expected 0 to %d"), motor_count - 1); - else - return mp_obj_new_int((int)self->cluster->direction((uint)motor)); - } + int motor_count = (int)self->cluster->count(); + if(motor_count == 0) + mp_raise_ValueError("this cluster does not have any motors"); else { - enum { ARG_self, ARG_motors, ARG_direction }; - static const mp_arg_t allowed_args[] = { - { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, - { MP_QSTR_motors, MP_ARG_REQUIRED | MP_ARG_OBJ }, - { MP_QSTR_direction, MP_ARG_REQUIRED | MP_ARG_INT }, - }; - - // Parse args. - mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; - mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); - - _MotorCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _MotorCluster_obj_t); - - int motor_count = (int)self->cluster->count(); - if(motor_count == 0) - mp_raise_ValueError("this cluster does not have any motors"); + if(n_args <= 2) { + int motor = mp_obj_get_int(args[ARG_motor].u_obj); + if(motor < 0 || motor >= motor_count) + mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("motor out of range. Expected 0 to %d"), motor_count - 1); + else + return mp_obj_new_int((int)self->cluster->direction((uint)motor)); + } else { // Determine what motor(s) to modify - const mp_obj_t object = args[ARG_motors].u_obj; + const mp_obj_t object = args[ARG_motor].u_obj; if(mp_obj_is_int(object)) { int motor = mp_obj_get_int(object); if(motor < 0 || motor >= motor_count) mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("motor out of range. Expected 0 to %d"), motor_count - 1); else { - int direction = args[ARG_direction].u_int; + int direction = mp_obj_get_int(args[ARG_direction].u_obj); if(direction < 0 || direction > 1) { mp_raise_ValueError("direction out of range. Expected NORMAL_DIR (0) or REVERSED_DIR (1)"); } @@ -2259,42 +1817,18 @@ extern mp_obj_t MotorCluster_direction(size_t n_args, const mp_obj_t *pos_args, else { size_t length = 0; mp_obj_t *items = nullptr; - if(mp_obj_is_type(object, &mp_type_list)) { - mp_obj_list_t *list = MP_OBJ_TO_PTR2(object, mp_obj_list_t); - length = list->len; - items = list->items; - } - else if(mp_obj_is_type(object, &mp_type_tuple)) { - mp_obj_tuple_t *tuple = MP_OBJ_TO_PTR2(object, mp_obj_tuple_t); - length = tuple->len; - items = tuple->items; - } + pimoroni_tuple_or_list(object, &items, &length); - if(items == nullptr) - mp_raise_TypeError("cannot convert object to a list or tuple of integers, or a single integer"); - else if(length == 0) - mp_raise_TypeError("list or tuple must contain at least one integer"); - else { - // Create and populate a local array of motor indices - uint8_t *motors = new uint8_t[length]; - for(size_t i = 0; i < length; i++) { - int motor = mp_obj_get_int(items[i]); - if(motor < 0 || motor >= motor_count) { - delete[] motors; - mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("a motor in the list or tuple is out of range. Expected 0 to %d"), motor_count - 1); - } - else { - motors[i] = (uint8_t)motor; - } - } - int direction = args[ARG_direction].u_int; - if(direction < 0 || direction > 1) { - delete[] motors; - mp_raise_ValueError("direction out of range. Expected NORMAL_DIR (0) or REVERSED_DIR (1)"); - } - self->cluster->direction(motors, length, (Direction)direction); + // Create and populate a local array of motor indices + uint8_t *motors = pimoroni_motors_from_items(items, length, motor_count); + + int direction = mp_obj_get_int(args[ARG_direction].u_obj); + if(direction < 0 || direction > 1) { delete[] motors; + mp_raise_ValueError("direction out of range. Expected NORMAL_DIR (0) or REVERSED_DIR (1)"); } + self->cluster->direction(motors, length, (Direction)direction); + delete[] motors; } } } @@ -2328,48 +1862,33 @@ extern mp_obj_t MotorCluster_all_directions(size_t n_args, const mp_obj_t *pos_a } extern mp_obj_t MotorCluster_speed_scale(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { - if(n_args <= 2) { - enum { ARG_self, ARG_motor }; - static const mp_arg_t allowed_args[] = { - { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, - { MP_QSTR_motor, MP_ARG_REQUIRED | MP_ARG_INT }, - }; + enum { ARG_self, ARG_motor, ARG_speed_scale }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_motor, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_speed_scale, MP_ARG_OBJ, { .u_obj = mp_const_none }}, + }; - // Parse args. - mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; - mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); - _MotorCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _MotorCluster_obj_t); + _MotorCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _MotorCluster_obj_t); - int motor = args[ARG_motor].u_int; - int motor_count = (int)self->cluster->count(); - if(motor_count == 0) - mp_raise_ValueError("this cluster does not have any motors"); - else if(motor < 0 || motor >= motor_count) - mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("motor out of range. Expected 0 to %d"), motor_count - 1); - else - return mp_obj_new_float(self->cluster->speed_scale((uint)motor)); - } + int motor_count = (int)self->cluster->count(); + if(motor_count == 0) + mp_raise_ValueError("this cluster does not have any motors"); else { - enum { ARG_self, ARG_motors, ARG_speed_scale }; - static const mp_arg_t allowed_args[] = { - { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, - { MP_QSTR_motors, MP_ARG_REQUIRED | MP_ARG_OBJ }, - { MP_QSTR_speed_scale, MP_ARG_REQUIRED | MP_ARG_OBJ }, - }; - - // Parse args. - mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; - mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); - - _MotorCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _MotorCluster_obj_t); - - int motor_count = (int)self->cluster->count(); - if(motor_count == 0) - mp_raise_ValueError("this cluster does not have any motors"); + if(n_args <= 2) { + int motor = mp_obj_get_int(args[ARG_motor].u_obj); + if(motor < 0 || motor >= motor_count) + mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("motor out of range. Expected 0 to %d"), motor_count - 1); + else + return mp_obj_new_float(self->cluster->speed_scale((uint)motor)); + } else { // Determine what motor(s) to modify - const mp_obj_t object = args[ARG_motors].u_obj; + const mp_obj_t object = args[ARG_motor].u_obj; if(mp_obj_is_int(object)) { int motor = mp_obj_get_int(object); if(motor < 0 || motor >= motor_count) @@ -2385,43 +1904,19 @@ extern mp_obj_t MotorCluster_speed_scale(size_t n_args, const mp_obj_t *pos_args else { size_t length = 0; mp_obj_t *items = nullptr; - if(mp_obj_is_type(object, &mp_type_list)) { - mp_obj_list_t *list = MP_OBJ_TO_PTR2(object, mp_obj_list_t); - length = list->len; - items = list->items; - } - else if(mp_obj_is_type(object, &mp_type_tuple)) { - mp_obj_tuple_t *tuple = MP_OBJ_TO_PTR2(object, mp_obj_tuple_t); - length = tuple->len; - items = tuple->items; - } + pimoroni_tuple_or_list(object, &items, &length); - if(items == nullptr) - mp_raise_TypeError("cannot convert object to a list or tuple of integers, or a single integer"); - else if(length == 0) - mp_raise_TypeError("list or tuple must contain at least one integer"); - else { - // Create and populate a local array of motor indices - uint8_t *motors = new uint8_t[length]; - for(size_t i = 0; i < length; i++) { - int motor = mp_obj_get_int(items[i]); - if(motor < 0 || motor >= motor_count) { - delete[] motors; - mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("a motor in the list or tuple is out of range. Expected 0 to %d"), motor_count - 1); - } - else { - motors[i] = (uint8_t)motor; - } - } - float speed_scale = mp_obj_get_float(args[ARG_speed_scale].u_obj); - if(speed_scale < FLT_EPSILON) { - delete[] motors; - mp_raise_ValueError("speed_scale out of range. Expected greater than 0.0"); - } + // Create and populate a local array of motor indices + uint8_t *motors = pimoroni_motors_from_items(items, length, motor_count); - self->cluster->speed_scale(motors, length, speed_scale); + float speed_scale = mp_obj_get_float(args[ARG_speed_scale].u_obj); + if(speed_scale < FLT_EPSILON) { delete[] motors; + mp_raise_ValueError("speed_scale out of range. Expected greater than 0.0"); } + + self->cluster->speed_scale(motors, length, speed_scale); + delete[] motors; } } } @@ -2455,48 +1950,33 @@ extern mp_obj_t MotorCluster_all_speed_scales(size_t n_args, const mp_obj_t *pos } extern mp_obj_t MotorCluster_zeropoint(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { - if(n_args <= 2) { - enum { ARG_self, ARG_motor }; - static const mp_arg_t allowed_args[] = { - { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, - { MP_QSTR_motor, MP_ARG_REQUIRED | MP_ARG_INT }, - }; + enum { ARG_self, ARG_motor, ARG_zeropoint }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_motor, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_zeropoint, MP_ARG_OBJ, { .u_obj = mp_const_none }}, + }; - // Parse args. - mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; - mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); - _MotorCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _MotorCluster_obj_t); + _MotorCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _MotorCluster_obj_t); - int motor = args[ARG_motor].u_int; - int motor_count = (int)self->cluster->count(); - if(motor_count == 0) - mp_raise_ValueError("this cluster does not have any motors"); - else if(motor < 0 || motor >= motor_count) - mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("motor out of range. Expected 0 to %d"), motor_count - 1); - else - return mp_obj_new_float(self->cluster->zeropoint((uint)motor)); - } + int motor_count = (int)self->cluster->count(); + if(motor_count == 0) + mp_raise_ValueError("this cluster does not have any motors"); else { - enum { ARG_self, ARG_motors, ARG_zeropoint }; - static const mp_arg_t allowed_args[] = { - { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, - { MP_QSTR_motors, MP_ARG_REQUIRED | MP_ARG_OBJ }, - { MP_QSTR_zeropoint, MP_ARG_REQUIRED | MP_ARG_OBJ }, - }; - - // Parse args. - mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; - mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); - - _MotorCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _MotorCluster_obj_t); - - int motor_count = (int)self->cluster->count(); - if(motor_count == 0) - mp_raise_ValueError("this cluster does not have any motors"); + if(n_args <= 2) { + int motor = mp_obj_get_int(args[ARG_motor].u_obj); + if(motor < 0 || motor >= motor_count) + mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("motor out of range. Expected 0 to %d"), motor_count - 1); + else + return mp_obj_new_float(self->cluster->zeropoint((uint)motor)); + } else { // Determine what motor(s) to modify - const mp_obj_t object = args[ARG_motors].u_obj; + const mp_obj_t object = args[ARG_motor].u_obj; if(mp_obj_is_int(object)) { int motor = mp_obj_get_int(object); if(motor < 0 || motor >= motor_count) @@ -2512,43 +1992,19 @@ extern mp_obj_t MotorCluster_zeropoint(size_t n_args, const mp_obj_t *pos_args, else { size_t length = 0; mp_obj_t *items = nullptr; - if(mp_obj_is_type(object, &mp_type_list)) { - mp_obj_list_t *list = MP_OBJ_TO_PTR2(object, mp_obj_list_t); - length = list->len; - items = list->items; - } - else if(mp_obj_is_type(object, &mp_type_tuple)) { - mp_obj_tuple_t *tuple = MP_OBJ_TO_PTR2(object, mp_obj_tuple_t); - length = tuple->len; - items = tuple->items; - } + pimoroni_tuple_or_list(object, &items, &length); - if(items == nullptr) - mp_raise_TypeError("cannot convert object to a list or tuple of integers, or a single integer"); - else if(length == 0) - mp_raise_TypeError("list or tuple must contain at least one integer"); - else { - // Create and populate a local array of motor indices - uint8_t *motors = new uint8_t[length]; - for(size_t i = 0; i < length; i++) { - int motor = mp_obj_get_int(items[i]); - if(motor < 0 || motor >= motor_count) { - delete[] motors; - mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("a motor in the list or tuple is out of range. Expected 0 to %d"), motor_count - 1); - } - else { - motors[i] = (uint8_t)motor; - } - } - float zeropoint = mp_obj_get_float(args[ARG_zeropoint].u_obj); - if(zeropoint < 0.0f || zeropoint > 1.0f - FLT_EPSILON) { - delete[] motors; - mp_raise_ValueError("zeropoint out of range. Expected 0.0 to less than 1.0"); - } + // Create and populate a local array of motor indices + uint8_t *motors = pimoroni_motors_from_items(items, length, motor_count); - self->cluster->zeropoint(motors, length, zeropoint); + float zeropoint = mp_obj_get_float(args[ARG_zeropoint].u_obj); + if(zeropoint < 0.0f || zeropoint > 1.0f - FLT_EPSILON) { delete[] motors; + mp_raise_ValueError("zeropoint out of range. Expected 0.0 to less than 1.0"); } + + self->cluster->zeropoint(motors, length, zeropoint); + delete[] motors; } } } @@ -2582,49 +2038,34 @@ extern mp_obj_t MotorCluster_all_zeropoints(size_t n_args, const mp_obj_t *pos_a } extern mp_obj_t MotorCluster_deadzone(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { - if(n_args <= 2) { - enum { ARG_self, ARG_motor }; - static const mp_arg_t allowed_args[] = { - { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, - { MP_QSTR_motor, MP_ARG_REQUIRED | MP_ARG_INT }, - }; + enum { ARG_self, ARG_motor, ARG_deadzone, ARG_load }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_motor, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_deadzone, MP_ARG_OBJ, { .u_obj = mp_const_none }}, + { MP_QSTR_load, MP_ARG_BOOL, { .u_bool = true }}, + }; - // Parse args. - mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; - mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); - _MotorCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _MotorCluster_obj_t); + _MotorCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _MotorCluster_obj_t); - int motor = args[ARG_motor].u_int; - int motor_count = (int)self->cluster->count(); - if(motor_count == 0) - mp_raise_ValueError("this cluster does not have any motors"); - else if(motor < 0 || motor >= motor_count) - mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("motor out of range. Expected 0 to %d"), motor_count - 1); - else - return mp_obj_new_float(self->cluster->deadzone((uint)motor)); - } + int motor_count = (int)self->cluster->count(); + if(motor_count == 0) + mp_raise_ValueError("this cluster does not have any motors"); else { - enum { ARG_self, ARG_motors, ARG_deadzone, ARG_load }; - static const mp_arg_t allowed_args[] = { - { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, - { MP_QSTR_motors, MP_ARG_REQUIRED | MP_ARG_OBJ }, - { MP_QSTR_deadzone, MP_ARG_REQUIRED | MP_ARG_OBJ }, - { MP_QSTR_load, MP_ARG_BOOL, { .u_bool = true }}, - }; - - // Parse args. - mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; - mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); - - _MotorCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _MotorCluster_obj_t); - - int motor_count = (int)self->cluster->count(); - if(motor_count == 0) - mp_raise_ValueError("this cluster does not have any motors"); + if(n_args <= 2) { + int motor = mp_obj_get_int(args[ARG_motor].u_obj); + if(motor < 0 || motor >= motor_count) + mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("motor out of range. Expected 0 to %d"), motor_count - 1); + else + return mp_obj_new_float(self->cluster->deadzone((uint)motor)); + } else { // Determine what motor(s) to modify - const mp_obj_t object = args[ARG_motors].u_obj; + const mp_obj_t object = args[ARG_motor].u_obj; if(mp_obj_is_int(object)) { int motor = mp_obj_get_int(object); if(motor < 0 || motor >= motor_count) @@ -2640,42 +2081,18 @@ extern mp_obj_t MotorCluster_deadzone(size_t n_args, const mp_obj_t *pos_args, m else { size_t length = 0; mp_obj_t *items = nullptr; - if(mp_obj_is_type(object, &mp_type_list)) { - mp_obj_list_t *list = MP_OBJ_TO_PTR2(object, mp_obj_list_t); - length = list->len; - items = list->items; - } - else if(mp_obj_is_type(object, &mp_type_tuple)) { - mp_obj_tuple_t *tuple = MP_OBJ_TO_PTR2(object, mp_obj_tuple_t); - length = tuple->len; - items = tuple->items; - } + pimoroni_tuple_or_list(object, &items, &length); - if(items == nullptr) - mp_raise_TypeError("cannot convert object to a list or tuple of integers, or a single integer"); - else if(length == 0) - mp_raise_TypeError("list or tuple must contain at least one integer"); - else { - // Create and populate a local array of motor indices - uint8_t *motors = new uint8_t[length]; - for(size_t i = 0; i < length; i++) { - int motor = mp_obj_get_int(items[i]); - if(motor < 0 || motor >= motor_count) { - delete[] motors; - mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("a motor in the list or tuple is out of range. Expected 0 to %d"), motor_count - 1); - } - else { - motors[i] = (uint8_t)motor; - } - } - float deadzone = mp_obj_get_float(args[ARG_deadzone].u_obj); - if(deadzone < 0.0f || deadzone > 1.0f) { - delete[] motors; - mp_raise_ValueError("deadzone out of range. Expected 0.0 to 1.0"); - } - self->cluster->deadzone(motors, length, deadzone, args[ARG_load].u_bool); + // Create and populate a local array of motor indices + uint8_t *motors = pimoroni_motors_from_items(items, length, motor_count); + + float deadzone = mp_obj_get_float(args[ARG_deadzone].u_obj); + if(deadzone < 0.0f || deadzone > 1.0f) { delete[] motors; + mp_raise_ValueError("deadzone out of range. Expected 0.0 to 1.0"); } + self->cluster->deadzone(motors, length, deadzone, args[ARG_load].u_bool); + delete[] motors; } } } @@ -2710,49 +2127,34 @@ enum { ARG_self, ARG_deadzone, ARG_load }; } extern mp_obj_t MotorCluster_decay_mode(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { - if(n_args <= 2) { - enum { ARG_self, ARG_motor }; - static const mp_arg_t allowed_args[] = { - { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, - { MP_QSTR_motor, MP_ARG_REQUIRED | MP_ARG_INT }, - }; + enum { ARG_self, ARG_motor, ARG_mode, ARG_load }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_motor, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_mode, MP_ARG_OBJ, { .u_obj = mp_const_none }}, + { MP_QSTR_load, MP_ARG_BOOL, { .u_bool = true }}, + }; - // Parse args. - mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; - mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); - _MotorCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _MotorCluster_obj_t); + _MotorCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _MotorCluster_obj_t); - int motor = args[ARG_motor].u_int; - int motor_count = (int)self->cluster->count(); - if(motor_count == 0) - mp_raise_ValueError("this cluster does not have any motors"); - else if(motor < 0 || motor >= motor_count) - mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("motor out of range. Expected 0 to %d"), motor_count - 1); - else - return mp_obj_new_int((int)self->cluster->decay_mode((uint)motor)); - } + int motor_count = (int)self->cluster->count(); + if(motor_count == 0) + mp_raise_ValueError("this cluster does not have any motors"); else { - enum { ARG_self, ARG_motors, ARG_mode, ARG_load }; - static const mp_arg_t allowed_args[] = { - { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, - { MP_QSTR_motors, MP_ARG_REQUIRED | MP_ARG_OBJ }, - { MP_QSTR_mode, MP_ARG_REQUIRED | MP_ARG_INT }, - { MP_QSTR_load, MP_ARG_BOOL, { .u_bool = true }}, - }; - - // Parse args. - mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; - mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); - - _MotorCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _MotorCluster_obj_t); - - int motor_count = (int)self->cluster->count(); - if(motor_count == 0) - mp_raise_ValueError("this cluster does not have any motors"); + if(n_args <= 2) { + int motor = mp_obj_get_int(args[ARG_motor].u_obj); + if(motor < 0 || motor >= motor_count) + mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("motor out of range. Expected 0 to %d"), motor_count - 1); + else + return mp_obj_new_int((int)self->cluster->decay_mode((uint)motor)); + } else { // Determine what motor(s) to modify - const mp_obj_t object = args[ARG_motors].u_obj; + const mp_obj_t object = args[ARG_motor].u_obj; if(mp_obj_is_int(object)) { int motor = mp_obj_get_int(object); if(motor < 0 || motor >= motor_count) @@ -2768,42 +2170,18 @@ extern mp_obj_t MotorCluster_decay_mode(size_t n_args, const mp_obj_t *pos_args, else { size_t length = 0; mp_obj_t *items = nullptr; - if(mp_obj_is_type(object, &mp_type_list)) { - mp_obj_list_t *list = MP_OBJ_TO_PTR2(object, mp_obj_list_t); - length = list->len; - items = list->items; - } - else if(mp_obj_is_type(object, &mp_type_tuple)) { - mp_obj_tuple_t *tuple = MP_OBJ_TO_PTR2(object, mp_obj_tuple_t); - length = tuple->len; - items = tuple->items; - } + pimoroni_tuple_or_list(object, &items, &length); - if(items == nullptr) - mp_raise_TypeError("cannot convert object to a list or tuple of integers, or a single integer"); - else if(length == 0) - mp_raise_TypeError("list or tuple must contain at least one integer"); - else { - // Create and populate a local array of motor indices - uint8_t *motors = new uint8_t[length]; - for(size_t i = 0; i < length; i++) { - int motor = mp_obj_get_int(items[i]); - if(motor < 0 || motor >= motor_count) { - delete[] motors; - mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("a motor in the list or tuple is out of range. Expected 0 to %d"), motor_count - 1); - } - else { - motors[i] = (uint8_t)motor; - } - } - int mode = args[ARG_mode].u_int; - if(mode < 0 || mode > 1) { - delete[] motors; - mp_raise_ValueError("mode out of range. Expected FAST_DECAY (0) or SLOW_DECAY (1)"); - } - self->cluster->decay_mode(motors, length, (DecayMode)mode, args[ARG_load].u_bool); + // Create and populate a local array of motor indices + uint8_t *motors = pimoroni_motors_from_items(items, length, motor_count); + + int mode = args[ARG_mode].u_int; + if(mode < 0 || mode > 1) { delete[] motors; + mp_raise_ValueError("mode out of range. Expected FAST_DECAY (0) or SLOW_DECAY (1)"); } + self->cluster->decay_mode(motors, length, (DecayMode)mode, args[ARG_load].u_bool); + delete[] motors; } } } From 717ad908cb6c377f4693a1891353d4e0740e072c Mon Sep 17 00:00:00 2001 From: ZodiusInfuser Date: Thu, 12 May 2022 23:57:22 +0100 Subject: [PATCH 60/60] Optimized some encoder functions --- micropython/modules/encoder/encoder.cpp | 72 +++++++++---------------- 1 file changed, 25 insertions(+), 47 deletions(-) diff --git a/micropython/modules/encoder/encoder.cpp b/micropython/modules/encoder/encoder.cpp index 4768f2c3..e6f289e2 100644 --- a/micropython/modules/encoder/encoder.cpp +++ b/micropython/modules/encoder/encoder.cpp @@ -228,34 +228,23 @@ extern mp_obj_t Encoder_radians(mp_obj_t self_in) { } extern mp_obj_t Encoder_direction(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { + enum { ARG_self, ARG_direction }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_direction, MP_ARG_OBJ, { .u_obj = mp_const_none }}, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _Encoder_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Encoder_obj_t); + if(n_args <= 1) { - enum { ARG_self }; - static const mp_arg_t allowed_args[] = { - { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, - }; - - // Parse args. - mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; - mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); - - _Encoder_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Encoder_obj_t); - return mp_obj_new_int(self->encoder->direction()); } else { - enum { ARG_self, ARG_direction }; - static const mp_arg_t allowed_args[] = { - { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, - { MP_QSTR_direction, MP_ARG_REQUIRED | MP_ARG_INT }, - }; - - // Parse args. - mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; - mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); - - _Encoder_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Encoder_obj_t); - - int direction = args[ARG_direction].u_int; + int direction = mp_obj_get_int(args[ARG_direction].u_obj); if(direction < 0 || direction > 1) { mp_raise_ValueError("direction out of range. Expected NORMAL (0) or REVERSED_DIR (1)"); } @@ -265,33 +254,22 @@ extern mp_obj_t Encoder_direction(size_t n_args, const mp_obj_t *pos_args, mp_ma } extern mp_obj_t Encoder_counts_per_rev(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { + enum { ARG_self, ARG_counts_per_rev }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_counts_per_rev, MP_ARG_OBJ, { .u_obj = mp_const_none }}, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _Encoder_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Encoder_obj_t); + if(n_args <= 1) { - enum { ARG_self }; - static const mp_arg_t allowed_args[] = { - { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, - }; - - // Parse args. - mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; - mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); - - _Encoder_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Encoder_obj_t); - return mp_obj_new_float(self->encoder->counts_per_rev()); } else { - enum { ARG_self, ARG_counts_per_rev }; - static const mp_arg_t allowed_args[] = { - { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, - { MP_QSTR_counts_per_rev, MP_ARG_REQUIRED | MP_ARG_OBJ }, - }; - - // Parse args. - mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; - mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); - - _Encoder_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Encoder_obj_t); - float counts_per_rev = mp_obj_get_float(args[ARG_counts_per_rev].u_obj); if(counts_per_rev < FLT_EPSILON) { mp_raise_ValueError("counts_per_rev out of range. Expected greater than 0.0");