diff --git a/drivers/CMakeLists.txt b/drivers/CMakeLists.txt index bd2d3af0..8a9ffe27 100644 --- a/drivers/CMakeLists.txt +++ b/drivers/CMakeLists.txt @@ -26,3 +26,4 @@ add_subdirectory(icp10125) add_subdirectory(scd4x) add_subdirectory(hub75) add_subdirectory(uc8151) +add_subdirectory(servo) diff --git a/drivers/servo/CMakeLists.txt b/drivers/servo/CMakeLists.txt new file mode 100644 index 00000000..69c3df37 --- /dev/null +++ b/drivers/servo/CMakeLists.txt @@ -0,0 +1 @@ +include(servo.cmake) \ No newline at end of file diff --git a/drivers/servo/multi_pwm.cpp b/drivers/servo/multi_pwm.cpp new file mode 100644 index 00000000..ff084e52 --- /dev/null +++ b/drivers/servo/multi_pwm.cpp @@ -0,0 +1,274 @@ +#include "multi_pwm.hpp" +#include "common/pimoroni_common.hpp" +#include +#include "hardware/gpio.h" + +namespace servo { + + +int data_dma_channel; +int ctrl_dma_channel; + +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 +struct alignas(8) Transition { + uint32_t mask; + uint32_t delay; + Transition() : mask(0), delay(0) {}; +}; +static const uint NUM_BUFFERS = 3; +struct Sequence { + uint32_t size; + Transition data[BUFFER_SIZE]; +}; + +Sequence sequences[NUM_BUFFERS]; +uint sequence_index = 0; + +//Sequence loading_zone = {3, {Transition(), Transition(), Transition()}}; //Need 6 words to create loading zone behaviour with normal FIFO +Sequence loading_zone = {5, {Transition(), Transition(), Transition(), Transition(), Transition()}}; //Need 6 words to create loading zone behaviour with normal FIFO +bool enter_loading_zone = false; +const bool use_loading_zone = true; +uint loading_zone_size = 3; + +uint gpio = 15; + +void __isr pwm_dma_handler() { + // Clear the interrupt request. + dma_hw->ints0 = 1u << data_dma_channel; + + // if(enter_loading_zone) { + // gpio_put(gpio+1, 1); + // uint32_t transitions = loading_zone.size * 2; + // uint32_t* buffer = (uint32_t *)loading_zone.data; + // dma_channel_set_trans_count(data_dma_channel, transitions, false); + // dma_channel_set_read_addr(data_dma_channel, buffer, true); + + // enter_loading_zone = false; + // gpio_put(gpio+1, 0); + // } + // else { + gpio_put(gpio, 1); + uint32_t transitions = sequences[sequence_index].size * 2; + uint32_t* buffer = (uint32_t *)sequences[sequence_index].data; + + dma_channel_set_trans_count(data_dma_channel, transitions, false); + dma_channel_set_read_addr(data_dma_channel, buffer, true); + + // For some reason sequence 0 is output to the PIO twice, rather than once, despite the below line shifting the index along... + // ^ Seemed related to filling an 8 long fifo buffer. Reducing to 4 removed it. + sequence_index = (sequence_index + 1) % NUM_BUFFERS; + + gpio_put(gpio, 0); + //} +} + + /*** + * From RP2040 datasheet + * * + * One disadvantage of this technique is that we don’t start to reconfigure the channel until some time after the channel +makes its last transfer. If there is heavy interrupt activity on the processor, this may be quite a long time, and therefore +quite a large gap in transfers, which is problematic if we need to sustain a high data throughput. +This is solved by using two channels, with their CHAIN_TO fields crossed over, so that channel A triggers channel B when it +completes, and vice versa. At any point in time, one of the channels is transferring data, and the other is either already +configured to start the next transfer immediately when the current one finishes, or it is in the process of being +reconfigured. When channel A completes, it immediately starts the cued-up transfer on channel B. At the same time, the +interrupt is fired, and the handler reconfigures channel A so that it is ready for when channel B completes. + * */ + + +MultiPWM::MultiPWM(PIO pio, uint sm, uint pin) : pio(pio), sm(sm) { + pio_program_offset = pio_add_program(pio, &multi_pwm_program); + + gpio_init(gpio); + gpio_set_dir(gpio, GPIO_OUT); + gpio_init(gpio+1); + gpio_set_dir(gpio+1, GPIO_OUT); + + for(uint i = pin; i < pin + 15; i++) + pio_gpio_init(pio, i); + + pio_gpio_init(pio, 17); + pio_sm_set_consecutive_pindirs(pio, sm, pin, 17, true); + pio_sm_set_consecutive_pindirs(pio, sm, 17, 1, true); + + pio_sm_config c = multi_pwm_program_get_default_config(pio_program_offset); + sm_config_set_out_pins(&c, pin, 17); + sm_config_set_sideset_pins(&c, 17); + sm_config_set_out_shift(&c, false, true, 32); + //sm_config_set_fifo_join(&c, PIO_FIFO_JOIN_TX); // Joining the FIFOs makes the DMA interrupts occur earlier than we would like + sm_config_set_fifo_join(&c, PIO_FIFO_JOIN_NONE); + + float div = clock_get_hz(clk_sys) / 5000000; + sm_config_set_clkdiv(&c, div); + + pio_sm_init(pio, sm, pio_program_offset, &c); + pio_sm_set_enabled(pio, sm, true); + + data_dma_channel = dma_claim_unused_channel(true); + /*ctrl_dma_channel = dma_claim_unused_channel(true); + + dma_channel_config ctrl_config = dma_channel_get_default_config(ctrl_dma_channel); + channel_config_set_transfer_data_size(&ctrl_config, DMA_SIZE_32); + //channel_config_set_read_increment(&ctrl_config, false); + //channel_config_set_write_increment(&ctrl_config, false); + channel_config_set_read_increment(&ctrl_config, true); + channel_config_set_write_increment(&ctrl_config, true); + channel_config_set_ring(&ctrl_config, true, 3); // 1 << 3 byte boundary on write ptr + channel_config_set_ring(&ctrl_config, false, 3); // 1 << 3 byte boundary on read ptr + + dma_channel_configure( + ctrl_dma_channel, + &ctrl_config, + //The below two work + //&dma_hw->ch[data_dma_channel].al1_transfer_count_trig, + //&transfer_count, + //1, + //These two do not + //&dma_hw->ch[data_dma_channel].al3_read_addr_trig, + //&((uint32_t *)buffer), + &dma_hw->ch[data_dma_channel].al3_transfer_count, // Initial write address + &control_blocks[0], + 2, + false + );*/ + + dma_channel_config data_config = dma_channel_get_default_config(data_dma_channel); + channel_config_set_bswap(&data_config, false); + channel_config_set_dreq(&data_config, pio_get_dreq(pio, sm, true)); + channel_config_set_transfer_data_size(&data_config, DMA_SIZE_32); + channel_config_set_read_increment(&data_config, true); + //channel_config_set_chain_to(&data_config, ctrl_dma_channel); + //channel_config_set_ring(&data_config, false, 7); + + dma_channel_configure( + data_dma_channel, + &data_config, + &pio->txf[sm], + NULL, + 0, + false); + + dma_channel_set_irq0_enabled(data_dma_channel, true); + + // Configure the processor to run dma_handler() when DMA IRQ 0 is asserted + irq_set_exclusive_handler(DMA_IRQ_0, pwm_dma_handler); + irq_set_enabled(DMA_IRQ_0, true); + + { + Sequence& seq = sequences[0]; + Transition* trans = seq.data; + trans[0].mask = (1u << 0); + trans[0].delay = 1000 - 1; + + trans[1].mask = (1u << 1); + trans[1].delay = 1000 - 1; + + trans[2].mask = (1u << 1); + trans[2].delay = 1000 - 1; + + trans[3].mask = 0; + trans[3].delay = (20000 - 3000) - 1; + + //if(use_loading_zone) + //trans[4].delay -= loading_zone.size; + + seq.size = 4; + + if(use_loading_zone){ + trans[seq.size - 1].delay -= loading_zone_size; + for(uint i = 0; i < loading_zone_size; i++) { + trans[seq.size].mask = 0; + trans[seq.size].delay = 0; + seq.size += 1; + } + } + } + + { + Sequence& seq = sequences[1]; + Transition* trans = seq.data; + trans[0].mask = (1u << 5); + trans[0].delay = 10000 - 1; + + trans[1].mask = 0; + trans[1].delay = (20000 - 10000) - 1; + + //if(use_loading_zone) + //trans[1].delay -= loading_zone.size; + + seq.size = 2; + + if(use_loading_zone){ + trans[seq.size - 1].delay -= loading_zone_size; + for(uint i = 0; i < loading_zone_size; i++) { + trans[seq.size].mask = 0; + trans[seq.size].delay = 0; + seq.size += 1; + } + } + } + + { + Sequence& seq = sequences[2]; + Transition* trans = seq.data; + + uint count = 0; + uint last = 14; + for(uint i = 0; i < last; i++) { + trans[i].mask = (1u << i); + trans[i].delay = 1000 - 1; + count += 1000; + } + + trans[last].mask = 0; + trans[last].delay = (20000 - count) - 1; + + //if(use_loading_zone) + // trans[last].delay -= loading_zone.size; + + seq.size = last + 1; + + if(use_loading_zone){ + trans[seq.size - 1].delay -= loading_zone_size; + for(uint i = 0; i < loading_zone_size; i++) { + trans[seq.size].mask = 0; + trans[seq.size].delay = 0; + seq.size += 1; + } + } + } + + // Manually call the handler once, to trigger the first transfer + pwm_dma_handler(); + + //dma_start_channel_mask(1u << ctrl_dma_channel); +} + +MultiPWM::~MultiPWM() { + stop(); + clear(); + dma_channel_unclaim(data_dma_channel); + dma_channel_unclaim(ctrl_dma_channel); + pio_sm_set_enabled(pio, sm, false); + pio_remove_program(pio, &multi_pwm_program, pio_program_offset); +#ifndef MICROPY_BUILD_TYPE + // pio_sm_unclaim seems to hardfault in MicroPython + pio_sm_unclaim(pio, sm); +#endif +} + +bool MultiPWM::start(uint fps) { + //add_repeating_timer_ms(-(1000 / fps), dma_timer_callback, (void*)this, &timer); + return true; +} + +bool MultiPWM::stop() { + return true;//cancel_repeating_timer(&timer); +} + +void MultiPWM::clear() { + //for (auto i = 0u; i < num_leds; ++i) { + // set_rgb(i, 0, 0, 0); + //} +} +} \ No newline at end of file diff --git a/drivers/servo/multi_pwm.hpp b/drivers/servo/multi_pwm.hpp new file mode 100644 index 00000000..7650e05a --- /dev/null +++ b/drivers/servo/multi_pwm.hpp @@ -0,0 +1,40 @@ +/** + * Copyright (c) 2020 Raspberry Pi (Trading) Ltd. + * + * SPDX-License-Identifier: BSD-3-Clause + */ + +/* +This code is significantly modified from the PIO apa102 example +found here: https://github.com/raspberrypi/pico-examples/tree/master/pio/ws2812 +*/ + +#pragma once + +#include +#include + +#include "multi_pwm.pio.h" + +#include "pico/stdlib.h" +#include "hardware/pio.h" +#include "hardware/dma.h" +#include "hardware/irq.h" +#include "hardware/clocks.h" +#include "hardware/timer.h" + +namespace servo { + + class MultiPWM { + public: + MultiPWM(PIO pio, uint sm, uint pin); + ~MultiPWM(); + bool start(uint fps=60); + bool stop(); + void clear(); + private: + PIO pio; + uint sm; + uint pio_program_offset; + }; +} \ No newline at end of file diff --git a/drivers/servo/multi_pwm.pio b/drivers/servo/multi_pwm.pio new file mode 100644 index 00000000..304f1bd3 --- /dev/null +++ b/drivers/servo/multi_pwm.pio @@ -0,0 +1,24 @@ +; +; Copyright (c) 2020 Raspberry Pi (Trading) Ltd. +; +; SPDX-License-Identifier: BSD-3-Clause +; + +.program multi_pwm +.side_set 1 + +.wrap_target + ;pull side 0 ; Pull in the next DWord containing the pin states, and time counter + ;out pins, 16 side 1 ; Immediately set the pins to their new state + ;out y, 16 [1] side 0 ; Set the counter + + pull side 0 ; Pull in the next DWord containing the pin states, and time counter + out pins, 32 side 1 ; Immediately set the pins to their new state + pull side 0 + out y, 32 side 0 ; Set the counter +count_check: + jmp y-- delay side 0 ; Check if the counter is 0, and if so wrap around. If not decrement the counter and jump to the delay +.wrap + +delay: + jmp count_check [3] side 1 ; Wait a few cycles then jump back to the loop diff --git a/drivers/servo/servo.cmake b/drivers/servo/servo.cmake new file mode 100644 index 00000000..3acc59ad --- /dev/null +++ b/drivers/servo/servo.cmake @@ -0,0 +1,16 @@ +set(DRIVER_NAME servo) +add_library(${DRIVER_NAME} INTERFACE) + +target_sources(${DRIVER_NAME} INTERFACE + ${CMAKE_CURRENT_LIST_DIR}/multi_pwm.cpp +) + +target_include_directories(${DRIVER_NAME} INTERFACE ${CMAKE_CURRENT_LIST_DIR}) + +target_link_libraries(${DRIVER_NAME} INTERFACE + pico_stdlib + hardware_pio + hardware_dma + ) + +pico_generate_pio_header(${DRIVER_NAME} ${CMAKE_CURRENT_LIST_DIR}/multi_pwm.pio) \ No newline at end of file diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index e0cd0ad8..e3293cb9 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -40,3 +40,4 @@ add_subdirectory(pico_wireless) add_subdirectory(plasma2040) add_subdirectory(badger2040) add_subdirectory(interstate75) +add_subdirectory(servo2040) diff --git a/examples/servo2040/CMakeLists.txt b/examples/servo2040/CMakeLists.txt new file mode 100644 index 00000000..79d553dd --- /dev/null +++ b/examples/servo2040/CMakeLists.txt @@ -0,0 +1,2 @@ +include(servo2040_functions.cmake) +include(servo2040_rainbow.cmake) \ No newline at end of file diff --git a/examples/servo2040/servo2040_functions.cmake b/examples/servo2040/servo2040_functions.cmake new file mode 100644 index 00000000..a563b769 --- /dev/null +++ b/examples/servo2040/servo2040_functions.cmake @@ -0,0 +1,13 @@ +set(OUTPUT_NAME servo2040_functions) +add_executable(${OUTPUT_NAME} servo2040_functions.cpp) + +target_link_libraries(${OUTPUT_NAME} + pico_stdlib + servo2040 + button + ) + +# enable usb output +pico_enable_stdio_usb(${OUTPUT_NAME} 1) + +pico_add_extra_outputs(${OUTPUT_NAME}) diff --git a/examples/servo2040/servo2040_functions.cpp b/examples/servo2040/servo2040_functions.cpp new file mode 100644 index 00000000..6a331be3 --- /dev/null +++ b/examples/servo2040/servo2040_functions.cpp @@ -0,0 +1,76 @@ +#include +#include +#include + +#include "pico/stdlib.h" + +#include "servo2040.hpp" + +#include "common/pimoroni_common.hpp" +#include "button.hpp" + +/* +A simple balancing game, where you use the MSA301 accelerometer to line up a band with a goal on the strip. +This can either be done using: +- Angle mode: Where position on the strip directly matches the accelerometer's angle +- Velocity mode: Where tilting the accelerometer changes the speed the band moves at +When the goal position is reached, a new position is randomly selected + +Press "A" to change the game mode. +Press "B" to start or stop the game mode. +Press "Boot" to invert the direction of the accelerometer tilt +*/ + +using namespace pimoroni; +using namespace plasma; +using namespace servo; + +// Set how many LEDs you have +const uint N_LEDS = 6; + +// The speed that the LEDs will start cycling at +const uint DEFAULT_SPEED = 10; + +// How many times the LEDs will be updated per second +const uint UPDATES = 60; + +// WS28X-style LEDs with a single signal line. AKA NeoPixel +WS2812 led_bar(N_LEDS, pio0, 0, servo2040::LED_DAT); + + +Button user_sw(servo2040::USER_SW, Polarity::ACTIVE_LOW, 0); + +int main() { + stdio_init_all(); + + led_bar.start(UPDATES); + + sleep_ms(5000); + + MultiPWM pwms(pio1, 0, 0); + + int speed = DEFAULT_SPEED; + float offset = 0.0f; + + while(true) { + bool sw_pressed = user_sw.read(); + + if(sw_pressed) { + speed = DEFAULT_SPEED; + } + speed = std::min((int)255, std::max((int)1, speed)); + + offset += float(speed) / 2000.0f; + + for(auto i = 0u; i < led_bar.num_leds; ++i) { + float hue = float(i) / led_bar.num_leds; + led_bar.set_hsv(i, hue + offset, 1.0f, 0.5f); + } + + //pwms.update(true); + + // Sleep time controls the rate at which the LED buffer is updated + // but *not* the actual framerate at which the buffer is sent to the LEDs + sleep_ms(1000 / UPDATES); + } +} diff --git a/examples/servo2040/servo2040_rainbow.cmake b/examples/servo2040/servo2040_rainbow.cmake new file mode 100644 index 00000000..873b5ce0 --- /dev/null +++ b/examples/servo2040/servo2040_rainbow.cmake @@ -0,0 +1,13 @@ +set(OUTPUT_NAME servo2040_rainbow) +add_executable(${OUTPUT_NAME} servo2040_rainbow.cpp) + +target_link_libraries(${OUTPUT_NAME} + pico_stdlib + servo2040 + button + ) + +# enable usb output +pico_enable_stdio_usb(${OUTPUT_NAME} 1) + +pico_add_extra_outputs(${OUTPUT_NAME}) diff --git a/examples/servo2040/servo2040_rainbow.cpp b/examples/servo2040/servo2040_rainbow.cpp new file mode 100644 index 00000000..f88108a0 --- /dev/null +++ b/examples/servo2040/servo2040_rainbow.cpp @@ -0,0 +1,69 @@ +#include +#include +#include + +#include "pico/stdlib.h" + +#include "servo2040.hpp" + +#include "common/pimoroni_common.hpp" +#include "button.hpp" + +/* +A simple balancing game, where you use the MSA301 accelerometer to line up a band with a goal on the strip. +This can either be done using: +- Angle mode: Where position on the strip directly matches the accelerometer's angle +- Velocity mode: Where tilting the accelerometer changes the speed the band moves at +When the goal position is reached, a new position is randomly selected + +Press "A" to change the game mode. +Press "B" to start or stop the game mode. +Press "Boot" to invert the direction of the accelerometer tilt +*/ + +using namespace pimoroni; +using namespace plasma; + +// Set how many LEDs you have +const uint N_LEDS = 6; + +// The speed that the LEDs will start cycling at +const uint DEFAULT_SPEED = 10; + +// How many times the LEDs will be updated per second +const uint UPDATES = 60; + +// WS28X-style LEDs with a single signal line. AKA NeoPixel +WS2812 led_bar(N_LEDS, pio0, 0, servo2040::LED_DAT); + + +Button user_sw(servo2040::USER_SW, Polarity::ACTIVE_LOW, 0); + +int main() { + stdio_init_all(); + + led_bar.start(UPDATES); + + int speed = DEFAULT_SPEED; + float offset = 0.0f; + + while(true) { + bool sw_pressed = user_sw.read(); + + if(sw_pressed) { + speed = DEFAULT_SPEED; + } + speed = std::min((int)255, std::max((int)1, speed)); + + offset += float(speed) / 2000.0f; + + for(auto i = 0u; i < led_bar.num_leds; ++i) { + float hue = float(i) / led_bar.num_leds; + led_bar.set_hsv(i, hue + offset, 1.0f, 0.5f); + } + + // Sleep time controls the rate at which the LED buffer is updated + // but *not* the actual framerate at which the buffer is sent to the LEDs + sleep_ms(1000 / UPDATES); + } +} diff --git a/libraries/CMakeLists.txt b/libraries/CMakeLists.txt index 50fc59f1..ca810b56 100644 --- a/libraries/CMakeLists.txt +++ b/libraries/CMakeLists.txt @@ -27,3 +27,4 @@ add_subdirectory(pico_rgb_keypad) add_subdirectory(pico_wireless) add_subdirectory(plasma2040) add_subdirectory(badger2040) +add_subdirectory(servo2040) diff --git a/libraries/servo2040/CMakeLists.txt b/libraries/servo2040/CMakeLists.txt new file mode 100644 index 00000000..119b0d49 --- /dev/null +++ b/libraries/servo2040/CMakeLists.txt @@ -0,0 +1 @@ +include(servo2040.cmake) \ No newline at end of file diff --git a/libraries/servo2040/servo2040.cmake b/libraries/servo2040/servo2040.cmake new file mode 100644 index 00000000..95d884b8 --- /dev/null +++ b/libraries/servo2040/servo2040.cmake @@ -0,0 +1,6 @@ +add_library(servo2040 INTERFACE) + +target_include_directories(servo2040 INTERFACE ${CMAKE_CURRENT_LIST_DIR}) + +# Pull in pico libraries that we need +target_link_libraries(servo2040 INTERFACE pico_stdlib plasma servo) \ No newline at end of file diff --git a/libraries/servo2040/servo2040.hpp b/libraries/servo2040/servo2040.hpp new file mode 100644 index 00000000..f83fad67 --- /dev/null +++ b/libraries/servo2040/servo2040.hpp @@ -0,0 +1,40 @@ +#pragma once +#include "pico/stdlib.h" + +#include "ws2812.hpp" +#include "multi_pwm.hpp" + +namespace servo2040 { + 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 LED_DAT = 18; + + const uint ADC_ADDR_0 = 22; + const uint ADC_ADDR_1 = 24; + const uint ADC_ADDR_2 = 25; + + const uint USER_SW = 23; + + const uint SENSE = 29; // The pin used for the board's sensing features + + constexpr float ADC_GAIN = 69; + constexpr float ADC_OFFSET = 0.0145f; + constexpr float SHUNT_RESISTOR = 0.003f; +} \ No newline at end of file