From 9640a5b747da80be6f4fbfa144c8d2d9e95b30a4 Mon Sep 17 00:00:00 2001
From: AlexanderHD27 <alexander@hal>
Date: Wed, 25 Sep 2024 16:39:05 +0200
Subject: [PATCH] Implement Controlled Pulses via PIO

---
 motor-control/firmware/CMakeLists.txt         |  8 ++--
 motor-control/firmware/src/main.cpp           | 45 +++++++++++++++++--
 motor-control/firmware/src/tmc2209/pulser.pio | 30 +++++++++++++
 3 files changed, 77 insertions(+), 6 deletions(-)
 create mode 100644 motor-control/firmware/src/tmc2209/pulser.pio

diff --git a/motor-control/firmware/CMakeLists.txt b/motor-control/firmware/CMakeLists.txt
index b76dbc6..1249716 100755
--- a/motor-control/firmware/CMakeLists.txt
+++ b/motor-control/firmware/CMakeLists.txt
@@ -45,18 +45,20 @@ add_executable(main
 
 target_include_directories(main PUBLIC 
     include/
+    build/
 )
 
 include(${PROJECT_SOURCE_DIR}/cmake/tmc2209.cmake)
 
 # Adding PIO
-#pico_generate_pio_header(main ${CMAKE_CURRENT_LIST_DIR}/src/dac/dac.pio)
+pico_generate_pio_header(main ${CMAKE_CURRENT_LIST_DIR}/src/tmc2209/pulser.pio)
 
 # pull in common dependencies
 target_link_libraries(main 
-    pico_stdlib 
+    pico_stdlib
     pico_multicore
-    
+    hardware_pio
+
     tmc2209_driver
 )
 
diff --git a/motor-control/firmware/src/main.cpp b/motor-control/firmware/src/main.cpp
index 100ece5..476e9ff 100755
--- a/motor-control/firmware/src/main.cpp
+++ b/motor-control/firmware/src/main.cpp
@@ -13,6 +13,8 @@
 #include "pico/stdio.h"
 #include "hardware/uart.h"
 
+#include "pulser.pio.h"
+
 #define LED_PIN 25
 
 #define ENABLE0_PIN 2
@@ -32,9 +34,9 @@ int main() {
     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(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);
@@ -48,6 +50,43 @@ 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;
+
+    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_UART uart_driver0 = TMC2209_UART(uart0, 0, 19200, 0, 1);
     TMC2209_UART uart_driver1 = TMC2209_UART(uart1, 0, 19200, 4, 5);
     uart_driver0.init();
diff --git a/motor-control/firmware/src/tmc2209/pulser.pio b/motor-control/firmware/src/tmc2209/pulser.pio
new file mode 100644
index 0000000..a659633
--- /dev/null
+++ b/motor-control/firmware/src/tmc2209/pulser.pio
@@ -0,0 +1,30 @@
+
+.program pulser
+clock_div:
+    PULL NOBLOCK
+    MOV X, OSR
+    MOV Y, X
+l0:
+    JMP Y-- l0
+
+    IRQ CLEAR 0 REL
+    ; SM 0 + 0 -> IRQ 0 
+    ; SM 1 + 0 -> IRQ 1
+
+    JMP clock_div
+
+counter:
+    ; SM 2 + 2 -> IRQ 0
+    ; SM 3 + 2 -> IRQ 1
+    PULL
+    MOV X, OSR
+    IRQ WAIT 2 REL
+l1:
+    IRQ WAIT 2 REL  [1]
+    SET PINS, 1
+    IRQ WAIT 2 REL
+    SET PINS, 0
+    JMP X-- l1
+
+    JMP counter
+