diff --git a/motor-control/firmware/cmake/tmc2209.cmake b/motor-control/firmware/cmake/tmc2209.cmake index f15d80e..68e0b76 100644 --- a/motor-control/firmware/cmake/tmc2209.cmake +++ b/motor-control/firmware/cmake/tmc2209.cmake @@ -1,14 +1,16 @@ add_library(tmc2209_driver STATIC src/tmc2209/uart_interface.cpp src/tmc2209/uart_registers.cpp + src/tmc2209/step.cpp ) target_include_directories(tmc2209_driver PRIVATE include/tmc2209/ + build/ ) target_link_libraries(tmc2209_driver pico_stdlib hardware_uart + hardware_pio ) - diff --git a/motor-control/firmware/include/tmc2209/step.hpp b/motor-control/firmware/include/tmc2209/step.hpp new file mode 100644 index 0000000..1dcb089 --- /dev/null +++ b/motor-control/firmware/include/tmc2209/step.hpp @@ -0,0 +1,62 @@ +#pragma once +#include "hardware/pio.h" +#include "pulser.pio.h" + +class TMC2209_step_dual { +private: + uint step0_pin; + uint step1_pin; + uint dir0_pin; + uint dir1_pin; + uint enable0_pin; + uint enable1_pin; + + const int sm_num_divider0 = 0; + const int sm_num_divider1 = 1; + const int sm_num_counter0 = 2; + const int sm_num_counter1 = 3; + + PIO pio_core; + + pio_sm_config sm_config_divider0; + pio_sm_config sm_config_counter0; + pio_sm_config sm_config_divider1; + pio_sm_config sm_config_counter1; + + uint pulser_program_offset; + const uint subprogram_offset_divider = 0; + const uint subprogram_offset_counter = 6; + + const float base_clock_freq_khz = 500.0; + + const uint32_t sleep_time_us = 1000; + bool done_flag0; + bool done_flag1; + +protected: + void init_pio(); + void init_gpio(); + + void irq_handler0(); + void irq_handler1(); + +public: + TMC2209_step_dual( + uint step0_pin, + uint step1_pin, + uint dir0_pin, + uint dir1_pin, + uint enable0_pin, + uint enable1_pin, + PIO pio_core + ); + + void set_conf0(uint frequency, bool dir); + void set_conf1(uint frequency, bool dir); + + void pulse0(uint n); + void pulse1(uint n); + + bool wait0(int timeout_ms); + bool wait1(int timeout_ms); +}; diff --git a/motor-control/firmware/src/main.cpp b/motor-control/firmware/src/main.cpp index 476e9ff..60c2dfa 100755 --- a/motor-control/firmware/src/main.cpp +++ b/motor-control/firmware/src/main.cpp @@ -14,6 +14,7 @@ #include "hardware/uart.h" #include "pulser.pio.h" +#include "tmc2209/step.hpp" #define LED_PIN 25 @@ -26,6 +27,7 @@ #define DIR1_PIN 9 int main() { +/* gpio_init(ENABLE0_PIN); gpio_set_dir(ENABLE0_PIN, GPIO_OUT); gpio_put(ENABLE0_PIN, 0); @@ -50,7 +52,7 @@ int main() { gpio_set_dir(DIR1_PIN, GPIO_OUT); gpio_put(DIR1_PIN, 0); - + PIO pio_core = pio0; int SM_DIV0 = 0; int SM_COUNT0 = 2; @@ -86,6 +88,19 @@ int main() { const double double_div_freq_khz = 0.5; const uint32_t cycles = (target_freq_khz/double_div_freq_khz) - 6; pio_sm_put_blocking(pio_core, SM_DIV0, cycles); +*/ + + TMC2209_step_dual step_driver = TMC2209_step_dual(STEP0_PIN, STEP1_PIN, DIR0_PIN, DIR1_PIN, ENABLE0_PIN, ENABLE1_PIN, pio0); + step_driver.set_conf0(5000, 0); + step_driver.set_conf1(10000, 0); + + step_driver.pulse0(UINT32_MAX - 10); + sleep_ms(5); + step_driver.pulse1(UINT32_MAX - 10); + step_driver.pulse0(5); + sleep_ms(3); + //step_driver.pulse0(5); + step_driver.pulse1(5); TMC2209_UART uart_driver0 = TMC2209_UART(uart0, 0, 19200, 0, 1); TMC2209_UART uart_driver1 = TMC2209_UART(uart1, 0, 19200, 4, 5); diff --git a/motor-control/firmware/src/tmc2209/pulser.pio b/motor-control/firmware/src/tmc2209/pulser.pio index a659633..dbf41c2 100644 --- a/motor-control/firmware/src/tmc2209/pulser.pio +++ b/motor-control/firmware/src/tmc2209/pulser.pio @@ -20,11 +20,17 @@ counter: MOV X, OSR IRQ WAIT 2 REL l1: - IRQ WAIT 2 REL [1] + PULL NOBLOCK + IRQ WAIT 2 REL SET PINS, 1 + IRQ WAIT 2 REL SET PINS, 0 + MOV X, OSR JMP X-- l1 + IRQ SET 0 REL + ; SM 2 + 0 -> IRQ 2 + ; SM 3 + 0 -> IRQ 3 JMP counter diff --git a/motor-control/firmware/src/tmc2209/step.cpp b/motor-control/firmware/src/tmc2209/step.cpp new file mode 100644 index 0000000..8a2cc6d --- /dev/null +++ b/motor-control/firmware/src/tmc2209/step.cpp @@ -0,0 +1,181 @@ +#include "step.hpp" +#include "pico/stdio.h" +#include "hardware/pio.h" +#include + +TMC2209_step_dual::TMC2209_step_dual( + uint step0_pin, + uint step1_pin, + uint dir0_pin, + uint dir1_pin, + uint enable0_pin, + uint enable1_pin, + PIO pio_core +) { + this->step0_pin = step0_pin; + this->step1_pin = step1_pin; + this->dir0_pin = dir0_pin; + this->dir1_pin = dir1_pin; + this->enable0_pin = enable0_pin; + this->enable1_pin = enable1_pin; + this->pio_core = pio_core; + + this->init_gpio(); + init_pio(); +}; + +void TMC2209_step_dual::init_gpio() { + gpio_init(this->enable0_pin); + gpio_init(this->enable1_pin); + gpio_init(this->dir0_pin); + gpio_init(this->dir1_pin); + + gpio_set_dir(this->enable0_pin, GPIO_OUT); + gpio_set_dir(this->enable1_pin, GPIO_OUT); + gpio_set_dir(this->dir0_pin, GPIO_OUT); + gpio_set_dir(this->dir1_pin, GPIO_OUT); + + gpio_put(this->enable0_pin, 0); + gpio_put(this->enable1_pin, 0); + gpio_put(this->dir0_pin, 0); + gpio_put(this->dir1_pin, 0); +}; + +void TMC2209_step_dual::init_pio() { + pio_sm_set_enabled(this->pio_core, this->sm_num_divider0, false); + pio_sm_set_enabled(this->pio_core, this->sm_num_divider1, false); + pio_sm_set_enabled(this->pio_core, this->sm_num_counter0, false); + pio_sm_set_enabled(this->pio_core, this->sm_num_counter1, false); + + this->pulser_program_offset = pio_add_program(this->pio_core, &pulser_program); + + this->sm_config_divider0 = pulser_program_get_default_config(this->pulser_program_offset); + this->sm_config_divider1 = pulser_program_get_default_config(this->pulser_program_offset); + this->sm_config_counter0 = pulser_program_get_default_config(this->pulser_program_offset); + this->sm_config_counter1 = pulser_program_get_default_config(this->pulser_program_offset); + + pio_gpio_init(this->pio_core, this->step0_pin); + pio_gpio_init(this->pio_core, this->step1_pin); + pio_gpio_init(this->pio_core, this->dir0_pin); + pio_gpio_init(this->pio_core, this->dir1_pin); + + pio_sm_set_consecutive_pindirs(this->pio_core, this->sm_num_counter0, this->step0_pin, 1, true); + pio_sm_set_consecutive_pindirs(this->pio_core, this->sm_num_counter1, this->step1_pin, 1, true); + + sm_config_set_set_pins(&this->sm_config_counter0, this->step0_pin, 1); + sm_config_set_set_pins(&this->sm_config_counter1, this->step1_pin, 1); + + pio_sm_init( + this->pio_core, + this->sm_num_divider0, + this->pulser_program_offset + this->subprogram_offset_divider, + &this->sm_config_divider0 + ); + pio_sm_init( + this->pio_core, + this->sm_num_divider1, + this->pulser_program_offset + this->subprogram_offset_divider, + &this->sm_config_divider1 + ); + pio_sm_init( + this->pio_core, + this->sm_num_counter0, + this->pulser_program_offset + this->subprogram_offset_counter, + &this->sm_config_counter0 + ); + pio_sm_init( + this->pio_core, + this->sm_num_counter1, + this->pulser_program_offset + this->subprogram_offset_counter, + &this->sm_config_counter1 + ); + + float clock_div = ((float)(SYS_CLK_KHZ))/(base_clock_freq_khz); + + pio_sm_set_clkdiv(this->pio_core, this->sm_num_divider0, clock_div); + pio_sm_set_clkdiv(this->pio_core, this->sm_num_divider1, clock_div); + pio_sm_set_clkdiv(this->pio_core, this->sm_num_counter0, clock_div*0.5); + pio_sm_set_clkdiv(this->pio_core, this->sm_num_counter1, clock_div*0.5); + + pio_sm_set_enabled(this->pio_core, this->sm_num_divider0, true); + pio_sm_set_enabled(this->pio_core, this->sm_num_divider1, true); + pio_sm_set_enabled(this->pio_core, this->sm_num_counter0, true); + pio_sm_set_enabled(this->pio_core, this->sm_num_counter1, true); + + set_conf0(1000, 0); + set_conf1(1000, 0); +}; + +void TMC2209_step_dual::set_conf0(uint frequency, bool dir) { + uint32_t cycles = (uint32_t)(base_clock_freq_khz / (((float)(frequency))/1000.0)) - 6; + + gpio_put(this->dir0_pin, dir); + pio_sm_put_blocking( + this->pio_core, + this->sm_num_divider0, + cycles + ); +}; + +void TMC2209_step_dual::set_conf1(uint frequency, bool dir) { + uint32_t cycles = (uint32_t)(base_clock_freq_khz / (((float)(frequency))/1000.0)) - 6; + + gpio_put(this->dir1_pin, dir); + pio_sm_put_blocking( + this->pio_core, + this->sm_num_divider1, + cycles + ); +}; + +void TMC2209_step_dual::pulse0(uint n) { + pio_sm_put_blocking( + this->pio_core, this->sm_num_counter0, n-1 + ); +}; + +void TMC2209_step_dual::pulse1(uint n) { + pio_sm_put_blocking( + this->pio_core, this->sm_num_counter1, n-1 + ); +}; + +bool TMC2209_step_dual::wait0(int timeout_ms) { + int t = timeout_ms*1000; + bool continues = timeout_ms <= 0; + + while(t > 0 || continues) { + + if(done_flag0) { + done_flag0 = false; + return true; + } + + if(!continues) + t -= sleep_time_us; + + sleep_us(sleep_time_us); + } + + return false; +}; + +bool TMC2209_step_dual::wait1(int timeout_ms) { + int t = timeout_ms*1000; + bool continues = timeout_ms <= 0; + + while(t > 0 || continues) { + + if(done_flag0) { + done_flag0 = false; + return true; + } + + if(!continues) + t -= sleep_time_us; + + sleep_us(sleep_time_us); + } + + return false; +}; \ No newline at end of file