stepper.pio

;
; PIO State Machine for Stepper Motor Control
; Features:
; - Steps motor at configurable frequency
; - Maintains step count accessible via Y register
; - Frequency and direction controlled via OSR
; - Generates step pulse on GPIO
; - Direction control via side-set pin (from bit 0 of OSR)
; - Uses two separate loops to maintain direction state via PC
;
; OSR format:
; - Bits 0-30: Delay cycles for low pulse
; - Bit 31: Direction (0 = reverse, 1 = forward)
;

.program stepper
.side_set 1 opt            ; Side-set for step pin
.wrap_target

; Forward direction loop (DIR = 1)  
forward_start:  
    set x, 0   side 0 ; store 0 so initial state is stop
    mov osr, x  side 0 ; store 0 so initial state is stop
;forward_d elay_loop:
;    jmp x--, forward_delay_loop side 1
;    mov x, osr side 1 ; remember direction is 1

forward_loop:
    mov x, osr    side 0
    pull noblock  side 0      ; Check for new delay+direction value (non-blocking)
    mov x, osr   side 0
    jmp !x stop  side 0        ; all zero control word, so go to stop statte

    in x, 32       side 0      ; Shift everything into isr
    in null, 1   side 1        ; discard direction bit
    mov x, isr   side 1        ; reload delay

    mov isr, y    side 1       ; Copy step count to ISR
    push noblock  side 1       ; Push step count to FIFO
    jmp PIN inc side 1
    jmp y-- postdec_fwd side 1 
postdec_fwd:
    nop side 1
    jmp fwd_low_delay side 1
inc:
    mov y, ~y     side 1           ; Increment Y register (step counter)
    jmp y-- postinc_fwd     side 1           ; by doing double inversion
postinc_fwd:
    mov y, ~y     side 1
fwd_low_delay:
    jmp x-- fwd_low_delay side 0   ; Delay loop for low pulse
    jmp forward_loop side 0                    ; Loop back to check for new config
stop:
    mov isr, y                ; output count constantly otherwise deadlock on read
    push noblock 
    mov x, osr                ; save osr just in case
    pull noblock              
    mov x, osr               
    jmp !x, stop              ; still 0 word, so keep in stop state
    in x,1                    ; pull direction bit out
    in null, 31               ; pad so the direction is in lsb
    mov pins, isr  [7]       ; set direciton pin delay 13 cycles because of direction change
    nop  [6]       ; set direciton pin delay 13 cycles because of direction change
    jmp forward_loop 


.wrap


% c-sdk {
#include <stdio.h>
#include <math.h>
#include "pico/stdlib.h"
#include "hardware/pio.h"
#include "hardware/clocks.h"

static int directions[4] = {0,0,0,0};

static inline void stepper_program_init(PIO pio, uint sm, uint offset, uint step_pin, uint dir_pin, float freq_hz) {
    // Get default config
    pio_sm_config c = stepper_program_get_default_config(offset);
    
    // Configure STEP pin as output
    pio_gpio_init(pio, step_pin);
    pio_sm_set_consecutive_pindirs(pio, sm, step_pin, 1, true);
    
    // Configure DIR pin as output (side-set)
    pio_gpio_init(pio, dir_pin);
    pio_sm_set_consecutive_pindirs(pio, sm, dir_pin, 1, true);
    
    // Map SET pins to step_pin
    sm_config_set_set_pins(&c, dir_pin, 1);
    sm_config_set_out_pins(&c, dir_pin, 1);
    
    // Map side-set pins to dir_pin
    sm_config_set_sideset_pins(&c, step_pin);
    
    // 
    sm_config_set_jmp_pin(&c, dir_pin);


    // Set clock divider for base frequency
    // The actual step frequency will be: (system_clock / clkdiv) / (2 * (delay_value + 1))
    float clkdiv = clock_get_hz(clk_sys) / (freq_hz * 100.0f); // Base divider
    sm_config_set_clkdiv(&c, clkdiv);
    
    // Initialize state machine
    pio_sm_init(pio, sm, offset, &c);
    
    // Clear Y register (step counter)
    pio_sm_exec(pio, sm, pio_encode_set(pio_y, 0));
    
    // Set initial delay value with direction
    //pio_sm_put_blocking(pio, sm, 1);
    
    // Enable state machine
    pio_sm_set_enabled(pio, sm, true);
}


// TODO: switch this to using integer math versions of set divider
static inline void stepper_set_params(PIO pio, uint sm, uint32_t freq_hz, int direction) {
    if(freq_hz == 0) {
        // Special case: stop motor
        pio_sm_put_blocking(pio, sm, 0);
        pio_sm_set_clkdiv(pio, sm, 128);
        //printf("STOPPING!");
        return;
    }

    // Compute frequencies
    float clkdiv = 0.;
    if(freq_hz < 62000) {
        // 125 khz
        clkdiv = 128;
    } else {
        clkdiv = clock_get_hz(clk_sys) / (freq_hz * 16.0f); // Base divider
    }
    float delay = clock_get_hz(clk_sys) /  (clkdiv * freq_hz ) - 15.f;
    int i_delay = (int)round(delay);
    if(i_delay < 1) delay = 1;
    //printf("  Setting frequency to %f with divider %f and delay %f", freq_hz, clkdiv, delay);

    if(direction != directions[sm]) {
        directions[sm] = direction;
        // Must stop motor for direction to happen!
        pio_sm_put(pio, sm, 0);
    }
    // Now set clk divider and restart motor.
    pio_sm_set_clkdiv(pio, sm, clkdiv);
    pio_sm_put(pio, sm, (i_delay << 1)  | direction);
}



// Function to read step count (blocking version)
static inline int32_t stepper_get_step_count_blocking(PIO pio, uint sm) {
    // Wait for step count from RX FIFO
    // return pio_sm_get_blocking(pio, sm);

    uint ret;
    int n;

    // if the FIFO has N entries, we fetch them to drain the FIFO,
    // plus one entry which will be guaranteed to not be stale
    n = pio_sm_get_rx_fifo_level(pio, sm) + 1;
    while (n > 0) {
        ret = pio_sm_get_blocking(pio, sm);
        n--;
    }
    static_assert(sizeof(ret) == sizeof(int32_t), "size mismatch");
    return *(int32_t*)(&ret);

}

// Function to reset step count
static inline void stepper_reset_count(PIO pio, uint sm) {
    pio_sm_exec(pio, sm, pio_encode_set(pio_y, 0));
}
%}