Merge pull request #31 from eamars/minor_motor_driver_fix

Revert PIO stepper implementation

.vscode/settings.json

@@ -26,6 +26,8 @@
     "files.associations": {
         "neopixel_led.h": "c",
         "app.h": "c",
-        "queue.h": "c"
+        "queue.h": "c",
+        "limits": "c",
+        "*.tcc": "c"
     }
 }

src/motors.c

@@ -18,6 +18,9 @@
 #include "eeprom.h"
 #include "common.h"
 
+#define STEPPER_LOW_CYCLE_COUNT 13  // Defined as the implementation of stepper.pio
+#define MAX_RESPONSE_TIME   0.01f   // Maximum response time for PIO stepper
+
 
 // Internal data structure for speed control between tasks
 typedef struct {
@@ -154,33 +157,27 @@ TMC_uart_write_datagram_t *tmc_uart_read (trinamic_motor_t driver, TMC_uart_read
     return &wdgr;
 }
 
-
 uint32_t speed_to_period(float speed, uint32_t pio_clock_speed, uint32_t full_rotation_steps) {
-    // uint32_t pio_clock_speed = clock_get_hz(clk_sys);
-
-    // Step speed (step/s) is calculated by full rotation steps x rotations per second
-    float delay_period_f;
-    if (speed < 1e-3) {
-        delay_period_f = 0.0;
-    }
-    else {
-        float steps_speed = full_rotation_steps * speed;
-        delay_period_f = pio_clock_speed / steps_speed;
-    }
+    // speed: rev/s
+    float step_speed = full_rotation_steps * speed;    // in steps/s
 
-    delay_period_f /= 2;
+    uint32_t full_cycle_count = lroundf(pio_clock_speed / step_speed);
 
-    // Discount the period when holds low
-    if (delay_period_f >= 4) {
-        delay_period_f -= 4;
+    // Limit by maximum response time
+    uint32_t max_response_steps = pio_clock_speed * MAX_RESPONSE_TIME;
+    if (full_cycle_count > max_response_steps){ 
+        full_cycle_count = 0;
     }
-    else {
-        delay_period_f = 0;
+
+    // Avoid wrap around
+    if (full_cycle_count < STEPPER_LOW_CYCLE_COUNT) {
+        full_cycle_count = STEPPER_LOW_CYCLE_COUNT;
     }
 
-    uint32_t delay_period = lroundf(delay_period_f);
+    // High cycle should be calculated as full_cycle - low cycle.
+    uint32_t high_cycle_steps = full_cycle_count - STEPPER_LOW_CYCLE_COUNT;
 
-    return delay_period;
+    return high_cycle_steps;
 }
 
 
@@ -369,13 +366,13 @@ void speed_ramp(motor_config_t * motor_config, float prev_speed, float new_speed
 
     // Calculate termination condition
     uint32_t ramp_time_us = (uint32_t) (fabs(ramp_time_s) * 1e6);
-    uint64_t start_time = time_us_64();
-    uint64_t stop_time = start_time + ramp_time_us;
+    uint32_t start_time = time_us_32();
+    uint32_t stop_time = start_time + ramp_time_us;
 
     float current_speed;
     uint32_t current_period;
     while (true) {
-        uint64_t current_time = time_us_64();
+        uint32_t current_time = time_us_32();
         if (current_time > stop_time) {
             break;
         }
@@ -384,10 +381,12 @@ void speed_ramp(motor_config_t * motor_config, float prev_speed, float new_speed
 
         current_speed = prev_speed + dv * percentage;
         current_period = speed_to_period(current_speed, pio_speed, full_rotation_steps);
+        pio_sm_clear_fifos(MOTOR_STEPPER_PIO, motor_config->pio_sm);
         pio_sm_put(MOTOR_STEPPER_PIO, motor_config->pio_sm, current_period);
     }
 
     current_period = speed_to_period(new_speed, pio_speed, full_rotation_steps);
+    pio_sm_clear_fifos(MOTOR_STEPPER_PIO, motor_config->pio_sm);
     pio_sm_put_blocking(MOTOR_STEPPER_PIO, motor_config->pio_sm, current_period);
 }
 

src/stepper.pio

@@ -1,5 +1,4 @@
-; 4 additional instructions per cycle.
-; actual_period = (target_period - 4) / 2
+; 13 cycles at low
 .program stepper
 .side_set 1 opt
 init:
@@ -8,15 +7,21 @@ init:
     mov x, osr            ; Copy the value back from the OSR to X
     jmp !x init           ; If no X is provided then wrap back to the beginning
 
-    mov y, x              ; Hold at low for half of the period
-hold_low:
-    jmp y-- hold_low
-
-    mov y, x       side 1 ; Copy the period to the counter (y)
-                          ; Also set the SETP pin to high
-    nop                   ; 3x NOP to match the time that holds low.
+    ; Delay 10 cycles (10x8ns=80ns)
+    nop
+    nop
+    nop
+    nop
+    nop
     nop
     nop
+    nop
+    nop
+    nop
+    ; At this point we have dwelled 13 cycles (13*8=104 ns), fufilled typ dwell time at low
+
+    mov y, x       side 1 ; 
+
 hold_high:                ; Wait for the counter
     jmp y-- hold_high     ; Hold high while looping
 

targets/raspberrypi_pico_w_config.h

@@ -41,7 +41,7 @@
 #define SCALE_UART_TX 0
 #define SCALE_UART_RX 1
 
-#define MOTOR_STEPPER_PIO pio0
+#define MOTOR_STEPPER_PIO pio1
 
 #define EEPROM_I2C i2c1
 #define EEPROM_SDA_PIN 10