diff --git a/motor-control/firmware/include/tmc2209/step.hpp b/motor-control/firmware/include/tmc2209/step.hpp
index df5678d..90fd277 100644
--- a/motor-control/firmware/include/tmc2209/step.hpp
+++ b/motor-control/firmware/include/tmc2209/step.hpp
@@ -60,6 +60,9 @@ public:
     void pulse0(uint n);
     void pulse1(uint n);
 
+    void pulse0_int(int n);
+    void pulse1_int(int 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 5f6cf6b..65ffc44 100755
--- a/motor-control/firmware/src/main.cpp
+++ b/motor-control/firmware/src/main.cpp
@@ -91,18 +91,17 @@ int main() {
 */
 
     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_conf0(10000, 1);
     step_driver.set_conf1(10000, 0);
 
-    step_driver.pulse1(UINT32_MAX - 10);
-    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.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_UART uart_driver0 = TMC2209_UART(uart0, 0, 19200, 0, 1);
     TMC2209_UART uart_driver1 = TMC2209_UART(uart1, 0, 19200, 4, 5);
@@ -123,6 +122,7 @@ int main() {
     while (true) {
         sleep_ms(50);
         gpio_put(LED_PIN, 1);
+
         sleep_ms(50);
         gpio_put(LED_PIN, 0);
     }
diff --git a/motor-control/firmware/src/tmc2209/pulser.pio b/motor-control/firmware/src/tmc2209/pulser.pio
index dbf41c2..45376eb 100644
--- a/motor-control/firmware/src/tmc2209/pulser.pio
+++ b/motor-control/firmware/src/tmc2209/pulser.pio
@@ -18,7 +18,8 @@ counter:
     ; SM 3 + 2 -> IRQ 1
     PULL
     MOV X, OSR
-    IRQ WAIT 2 REL
+    JMP !X l1_end
+    JMP X-- l1
 l1:
     PULL NOBLOCK
     IRQ WAIT 2 REL
@@ -29,6 +30,7 @@ l1:
     MOV X, OSR
     JMP X-- l1
 
+l1_end:
     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 73d9d61..b7d5f18 100644
--- a/motor-control/firmware/src/tmc2209/step.cpp
+++ b/motor-control/firmware/src/tmc2209/step.cpp
@@ -4,6 +4,8 @@
 #include "hardware/irq.h"
 #include <pico/time.h>
 
+#include <math.h>
+
 TMC2209_step_dual * g_step_driver_instance;
 
 void step_irq0_handler() {
@@ -164,14 +166,40 @@ void TMC2209_step_dual::set_conf1(uint frequency, bool dir) {
 };
 
 void TMC2209_step_dual::pulse0(uint n) {
+    this->done_flag0_arm = true;
     pio_sm_put_blocking(
-        this->pio_core, this->sm_num_counter0, n-1
+        this->pio_core, this->sm_num_counter0, n
     );
 };
 
 void TMC2209_step_dual::pulse1(uint n) {
+    this->done_flag1_arm = true;
     pio_sm_put_blocking(
-        this->pio_core, this->sm_num_counter1, n-1
+        this->pio_core, this->sm_num_counter1, n
+    );
+};
+
+void TMC2209_step_dual::pulse0_int(int n) {
+    this->done_flag0_arm = true;
+    if(n < 0) {
+        n = abs(n);
+        gpio_put(this->dir1_pin, gpio_get(this->dir1_pin) ^ 1);
+    }
+
+    pio_sm_put_blocking(
+        this->pio_core, this->sm_num_counter0, n
+    );
+};
+
+void TMC2209_step_dual::pulse1_int(int n) {
+    this->done_flag1_arm = true;
+    if(n < 0) {
+        n = abs(n);
+        gpio_put(this->dir1_pin, gpio_get(this->dir1_pin) ^ 1);
+    }
+
+    pio_sm_put_blocking(
+        this->pio_core, this->sm_num_counter1, n
     );
 };
 
@@ -196,7 +224,9 @@ void TMC2209_step_dual::irq_handler1() {
 bool TMC2209_step_dual::wait0(int timeout_ms) {
     int t = timeout_ms*1000;
     bool continues = timeout_ms <= 0;
-    this->done_flag0_arm = true;
+
+    if(!this->done_flag0_arm)
+        return true;
 
     while(t > 0 || continues) {
 
@@ -218,13 +248,15 @@ 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;
+
+    if(!this->done_flag1_arm)
+        return true;
 
     while(t > 0 || continues) {
 
-        if(done_flag0) {
+        if(done_flag1) {
             this->done_flag1_arm = false;
-            done_flag0 = false;
+            done_flag1 = false;
             return true;
         }