From b2ffcb5d7278e68fc40912967cfb600bf514887c Mon Sep 17 00:00:00 2001 From: AlexanderHD27 Date: Thu, 26 Sep 2024 18:46:44 +0200 Subject: [PATCH] Added: Getting steps remaining on stop --- .../firmware/include/tmc2209/step.hpp | 6 + motor-control/firmware/src/main.cpp | 111 ++++++------------ motor-control/firmware/src/tmc2209/pulser.pio | 7 +- motor-control/firmware/src/tmc2209/step.cpp | 37 ++++-- 4 files changed, 76 insertions(+), 85 deletions(-) diff --git a/motor-control/firmware/include/tmc2209/step.hpp b/motor-control/firmware/include/tmc2209/step.hpp index 90fd277..debfeca 100644 --- a/motor-control/firmware/include/tmc2209/step.hpp +++ b/motor-control/firmware/include/tmc2209/step.hpp @@ -35,6 +35,9 @@ private: bool done_flag0_arm; bool done_flag1_arm; + uint remaining_step_count0; + uint remaining_step_count1; + protected: void init_pio(); void init_gpio(); @@ -65,6 +68,9 @@ public: bool wait0(int timeout_ms); bool wait1(int timeout_ms); + + uint get_remaining_steps0(); + uint get_remaining_steps1(); }; extern TMC2209_step_dual * g_step_driver_instance; diff --git a/motor-control/firmware/src/main.cpp b/motor-control/firmware/src/main.cpp index 65ffc44..7c715e7 100755 --- a/motor-control/firmware/src/main.cpp +++ b/motor-control/firmware/src/main.cpp @@ -27,23 +27,6 @@ #define DIR1_PIN 9 int main() { -/* - gpio_init(ENABLE0_PIN); - gpio_set_dir(ENABLE0_PIN, GPIO_OUT); - gpio_put(ENABLE0_PIN, 0); - - gpio_init(ENABLE1_PIN); - gpio_set_dir(ENABLE1_PIN, GPIO_OUT); - gpio_put(ENABLE1_PIN, 0); - - //gpio_init(STEP0_PIN); - //gpio_set_dir(STEP0_PIN, GPIO_OUT); - //gpio_put(STEP0_PIN, 0); - - gpio_init(STEP1_PIN); - gpio_set_dir(STEP1_PIN, GPIO_OUT); - gpio_put(STEP1_PIN, 0); - gpio_init(DIR0_PIN); gpio_set_dir(DIR0_PIN, GPIO_OUT); gpio_put(DIR0_PIN, 0); @@ -52,70 +35,54 @@ int main() { gpio_set_dir(DIR1_PIN, GPIO_OUT); gpio_put(DIR1_PIN, 0); + for(int i=0; i<100; i++) { + gpio_put(DIR0_PIN, 1); + gpio_put(DIR1_PIN, 1); + sleep_ms(2); + gpio_put(DIR0_PIN, 0); + gpio_put(DIR1_PIN, 0); + sleep_ms(2); + } - PIO pio_core = pio0; - int SM_DIV0 = 0; - int SM_COUNT0 = 2; - - uint pulser_offset = pio_add_program(pio_core, &pulser_program); - - pio_sm_config c_div0 = pulser_program_get_default_config(pulser_offset); - pio_sm_config c_count0 = pulser_program_get_default_config(pulser_offset); - - pio_gpio_init(pio_core, STEP0_PIN); - pio_gpio_init(pio_core, STEP1_PIN); - - pio_sm_set_consecutive_pindirs(pio_core, SM_COUNT0, STEP0_PIN, 1, true); - sm_config_set_set_pins(&c_count0, STEP0_PIN, 1); - - pio_sm_init(pio_core, SM_DIV0, pulser_offset, &c_div0); - pio_sm_init(pio_core, SM_COUNT0, pulser_offset + 6, &c_count0); - - const double target_freq_khz = 500; - const double clock_div = ((double)(SYS_CLK_KHZ))/((double)(target_freq_khz)); - pio_sm_set_clkdiv(pio_core, SM_DIV0, clock_div); - pio_sm_set_clkdiv(pio_core, SM_COUNT0, 1.0); - - pio_sm_set_enabled(pio_core, SM_DIV0, true); - pio_sm_set_enabled(pio_core, SM_COUNT0, true); - pio_sm_put_blocking(pio_core, SM_DIV0, 0); - - pio_sm_put_blocking(pio_core, SM_COUNT0, 0); - - // freq = base / 6 + n cyles - // n = (base/freq) - 6 - - 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(10000, 1); - step_driver.set_conf1(10000, 0); - - step_driver.pulse0(500); - step_driver.pulse1(50); - step_driver.wait1(0); - step_driver.pulse0(0); - step_driver.wait0(0); - - step_driver.set_conf0(1000, 1); - step_driver.set_conf1(1000, 1); + TMC2209_step_dual step_driver = TMC2209_step_dual( + STEP0_PIN, STEP1_PIN, + DIR0_PIN, DIR1_PIN, + ENABLE0_PIN, ENABLE1_PIN, + pio0 + ); TMC2209_UART uart_driver0 = TMC2209_UART(uart0, 0, 19200, 0, 1); TMC2209_UART uart_driver1 = TMC2209_UART(uart1, 0, 19200, 4, 5); uart_driver0.init(); uart_driver1.init(); + sleep_ms(5); + + step_driver.set_conf0(10000, 0); + step_driver.set_conf1(10000, 0); + step_driver.pulse0(1000); + step_driver.pulse1(1000); + step_driver.wait0(0); + step_driver.wait1(0); + + sleep_ms(200); + + step_driver.set_conf0(10000, 1); + step_driver.set_conf1(10000, 1); + step_driver.pulse0(1000); + step_driver.pulse1(1000); + step_driver.wait0(0); + step_driver.wait1(0); + + volatile uint t = 500 - step_driver.get_remaining_steps0(); + volatile uint s = step_driver.get_remaining_steps1(); + + step_driver.set_conf0(1000, 1); + step_driver.set_conf1(1000, 1); + + sleep_ms(100); - volatile uint32_t res1 = uart_driver1.read(0x00); - volatile uint32_t res0 = uart_driver0.read(0x00); - - //TMC2209_Registers registers(uart_driver); - //registers.read_all_state(); - gpio_init(LED_PIN); gpio_set_dir(LED_PIN, GPIO_OUT); diff --git a/motor-control/firmware/src/tmc2209/pulser.pio b/motor-control/firmware/src/tmc2209/pulser.pio index 45376eb..747ecab 100644 --- a/motor-control/firmware/src/tmc2209/pulser.pio +++ b/motor-control/firmware/src/tmc2209/pulser.pio @@ -22,15 +22,20 @@ counter: JMP X-- l1 l1: PULL NOBLOCK + MOV Y, OSR + JMP !Y l1_end IRQ WAIT 2 REL - SET PINS, 1 + SET PINS, 1 IRQ WAIT 2 REL + SET PINS, 0 MOV X, OSR JMP X-- l1 l1_end: + MOV ISR, X + PUSH NOBLOCK IRQ SET 0 REL ; SM 2 + 0 -> IRQ 2 ; SM 3 + 0 -> IRQ 3 diff --git a/motor-control/firmware/src/tmc2209/step.cpp b/motor-control/firmware/src/tmc2209/step.cpp index b7d5f18..82bd785 100644 --- a/motor-control/firmware/src/tmc2209/step.cpp +++ b/motor-control/firmware/src/tmc2209/step.cpp @@ -47,18 +47,19 @@ TMC2209_step_dual::TMC2209_step_dual( 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); + sleep_ms(1); + + gpio_init(this->dir0_pin); + gpio_init(this->dir1_pin); + gpio_set_dir(this->dir0_pin, GPIO_OUT); + gpio_set_dir(this->dir1_pin, GPIO_OUT); + gpio_put(this->dir0_pin, 1); + gpio_put(this->dir1_pin, 1); + sleep_ms(1); }; void TMC2209_step_dual::init_pio() { @@ -76,8 +77,6 @@ void TMC2209_step_dual::init_pio() { 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); @@ -114,8 +113,8 @@ void TMC2209_step_dual::init_pio() { 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_clkdiv(this->pio_core, this->sm_num_counter0, 1.0); + pio_sm_set_clkdiv(this->pio_core, this->sm_num_counter1, 1.0); pio_sm_set_enabled(this->pio_core, this->sm_num_divider0, true); pio_sm_set_enabled(this->pio_core, this->sm_num_divider1, true); @@ -166,6 +165,7 @@ void TMC2209_step_dual::set_conf1(uint frequency, bool dir) { }; void TMC2209_step_dual::pulse0(uint n) { + this->remaining_step_count0 = n; this->done_flag0_arm = true; pio_sm_put_blocking( this->pio_core, this->sm_num_counter0, n @@ -173,6 +173,7 @@ void TMC2209_step_dual::pulse0(uint n) { }; void TMC2209_step_dual::pulse1(uint n) { + this->remaining_step_count1 = n; this->done_flag1_arm = true; pio_sm_put_blocking( this->pio_core, this->sm_num_counter1, n @@ -204,6 +205,8 @@ void TMC2209_step_dual::pulse1_int(int n) { }; void TMC2209_step_dual::irq_handler0() { + this->remaining_step_count0 = pio_sm_get(this->pio_core, this->sm_num_counter0); + if(this->done_flag0_arm) { done_flag0 = true; } else { @@ -213,6 +216,8 @@ void TMC2209_step_dual::irq_handler0() { } void TMC2209_step_dual::irq_handler1() { + this->remaining_step_count1 = pio_sm_get(this->pio_core, this->sm_num_counter1); + if(this->done_flag1_arm) { done_flag1 = true; } else { @@ -267,4 +272,12 @@ bool TMC2209_step_dual::wait1(int timeout_ms) { } return false; +}; + +uint TMC2209_step_dual::get_remaining_steps0() { + return this->remaining_step_count0; +}; + +uint TMC2209_step_dual::get_remaining_steps1() { + return this->remaining_step_count1; }; \ No newline at end of file