From e97472c2a33b4cb86542de327b1d3825c2a76a74 Mon Sep 17 00:00:00 2001
From: AlexanderHD27 <alexander@hal>
Date: Wed, 25 Sep 2024 19:23:24 +0200
Subject: [PATCH] Added Wait for compleation funcionality

---
 .../firmware/include/tmc2209/step.hpp         | 11 +++-
 motor-control/firmware/src/main.cpp           | 12 ++--
 motor-control/firmware/src/tmc2209/step.cpp   | 59 ++++++++++++++++++-
 3 files changed, 74 insertions(+), 8 deletions(-)

diff --git a/motor-control/firmware/include/tmc2209/step.hpp b/motor-control/firmware/include/tmc2209/step.hpp
index 1dcb089..df5678d 100644
--- a/motor-control/firmware/include/tmc2209/step.hpp
+++ b/motor-control/firmware/include/tmc2209/step.hpp
@@ -29,18 +29,21 @@ private:
 
     const float base_clock_freq_khz = 500.0;
 
-    const uint32_t sleep_time_us = 1000;
+    const uint32_t sleep_time_us = 500;
     bool done_flag0;
     bool done_flag1;
+    bool done_flag0_arm;
+    bool done_flag1_arm;
 
 protected:
     void init_pio();
     void init_gpio();
 
+
+public:
     void irq_handler0();
     void irq_handler1();
 
-public:
     TMC2209_step_dual(
         uint step0_pin, 
         uint step1_pin, 
@@ -60,3 +63,7 @@ public:
     bool wait0(int timeout_ms);
     bool wait1(int timeout_ms);
 };
+
+extern TMC2209_step_dual * g_step_driver_instance;
+void step_irq0_handler();
+void step_irq1_handler();
diff --git a/motor-control/firmware/src/main.cpp b/motor-control/firmware/src/main.cpp
index 60c2dfa..5f6cf6b 100755
--- a/motor-control/firmware/src/main.cpp
+++ b/motor-control/firmware/src/main.cpp
@@ -94,13 +94,15 @@ int main() {
     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);
+    sleep_ms(5);
+    step_driver.pulse0(UINT32_MAX - 10);
+    step_driver.pulse1(5000);
+    step_driver.pulse0(1);
+    step_driver.wait1(3);
+    step_driver.pulse0(10);
+    step_driver.pulse1(1);
     //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/step.cpp b/motor-control/firmware/src/tmc2209/step.cpp
index 8a2cc6d..73d9d61 100644
--- a/motor-control/firmware/src/tmc2209/step.cpp
+++ b/motor-control/firmware/src/tmc2209/step.cpp
@@ -1,8 +1,19 @@
 #include "step.hpp"
 #include "pico/stdio.h"
 #include "hardware/pio.h"
+#include "hardware/irq.h"
 #include <pico/time.h>
 
+TMC2209_step_dual * g_step_driver_instance;
+
+void step_irq0_handler() {
+    g_step_driver_instance->irq_handler0();
+};
+
+void step_irq1_handler() {
+    g_step_driver_instance->irq_handler1();
+};
+
 TMC2209_step_dual::TMC2209_step_dual(
     uint step0_pin, 
     uint step1_pin, 
@@ -20,7 +31,14 @@ TMC2209_step_dual::TMC2209_step_dual(
     this->enable1_pin = enable1_pin;
     this->pio_core = pio_core;
 
-    this->init_gpio();
+    g_step_driver_instance = this;
+
+    this->done_flag0 = false;
+    this->done_flag1 = false;
+    this->done_flag0_arm = false;
+    this->done_flag1_arm = false;
+
+    init_gpio();
     init_pio();
 };
 
@@ -102,6 +120,23 @@ void TMC2209_step_dual::init_pio() {
     pio_sm_set_enabled(this->pio_core, this->sm_num_counter0, true);
     pio_sm_set_enabled(this->pio_core, this->sm_num_counter1, true);
 
+    pio_set_irq0_source_enabled(this->pio_core, pis_interrupt2, true);
+    pio_set_irq1_source_enabled(this->pio_core, pis_interrupt3, true);
+
+    if(this->pio_core == pio0) {
+        irq_set_exclusive_handler(PIO0_IRQ_0, step_irq0_handler);
+        irq_set_exclusive_handler(PIO0_IRQ_1, step_irq1_handler);
+
+        irq_set_enabled(PIO0_IRQ_0, true);
+        irq_set_enabled(PIO0_IRQ_1, true);
+    } else if(this->pio_core == pio1) {
+        irq_set_exclusive_handler(PIO1_IRQ_0, step_irq0_handler);
+        irq_set_exclusive_handler(PIO1_IRQ_1, step_irq1_handler);
+
+        irq_set_enabled(PIO1_IRQ_0, true);
+        irq_set_enabled(PIO1_IRQ_1, true);
+    }
+
     set_conf0(1000, 0);
     set_conf1(1000, 0);
 };
@@ -140,13 +175,33 @@ void TMC2209_step_dual::pulse1(uint n) {
     );
 };
 
+void TMC2209_step_dual::irq_handler0() {
+    if(this->done_flag0_arm) {
+        done_flag0 = true;
+    } else {
+        done_flag0 = false;
+    }
+    pio_interrupt_clear(this->pio_core, 2);
+}
+
+void TMC2209_step_dual::irq_handler1() {
+    if(this->done_flag1_arm) {
+        done_flag1 = true;
+    } else {
+        done_flag1 = false;
+    }
+    pio_interrupt_clear(this->pio_core, 3);
+}
+
 bool TMC2209_step_dual::wait0(int timeout_ms) {
     int t = timeout_ms*1000;
     bool continues = timeout_ms <= 0;
+    this->done_flag0_arm = true;
 
     while(t > 0 || continues) {
 
         if(done_flag0) {
+            this->done_flag0_arm = false;
             done_flag0 = false;
             return true;
         }
@@ -163,10 +218,12 @@ bool TMC2209_step_dual::wait0(int timeout_ms) {
 bool TMC2209_step_dual::wait1(int timeout_ms) {
     int t = timeout_ms*1000;
     bool continues = timeout_ms <= 0;
+    this->done_flag1_arm = true;
 
     while(t > 0 || continues) {
 
         if(done_flag0) {
+            this->done_flag1_arm = false;
             done_flag0 = false;
             return true;
         }