stepgen.cpp

#include "stepper.pio.h"
#include "pico/time.h"
#include "stepgen.h"


enum class State {
    DIRECTION_CHANGE,
    SOFTWARE,
    PIO_STARTING,
    PIO_STOPPING,
    PIO
};

constexpr int max_steppers = 4;
constexpr int cpu_threshold_low = 1000;
constexpr int cpu_threshold_high = 2000;

#define STEP_PIN 2       // GPIO pin for STEP signal
#define DIR_PIN 3        // GPIO pin for DIR signal (controlled by PIO side-set)
#define STEP2_PIN 4       // GPIO pin for STEP signal
#define DIR2_PIN 5        // GPIO pin for DIR signal (controlled by PIO side-set)
#define STEP3_PIN 6       // GPIO pin for STEP signal
#define DIR3_PIN 7        // GPIO pin for DIR signal (controlled by PIO side-set)
#define STEP4_PIN 8       // GPIO pin for STEP signal
#define DIR4_PIN 9        // GPIO pin for DIR signal (controlled by PIO side-set)
#define DEBUG_PIN 10      // GPIO pin for debugging timer update


namespace {
void stepgen_update();

const char* state_to_name(State state) {
    switch(state) {
        case State::DIRECTION_CHANGE: return "DIRECTION_CHANGE";
        case State::SOFTWARE: return "SOFTWARE";
        case State::PIO: return "PIO";
    }
    return "UNKNOWN";
}

State state[max_steppers];
int sm[max_steppers];
int desired_frequencies[max_steppers];
int current_frequencies[max_steppers];
int desired_directions[max_steppers];
int error_accumulators[max_steppers];
int stepper_counters[max_steppers];
int step_pins[max_steppers];
int dir_pins[max_steppers];
int current_directions[max_steppers];
struct repeating_timer update_timer;
}

extern "C" {

void stepgen_init() {
    printf("initializing steppers!\n");
    step_pins[0] = STEP_PIN;
    step_pins[1] = STEP2_PIN;
    step_pins[2] = STEP3_PIN;
    step_pins[3] = STEP4_PIN;
    dir_pins[0] = DIR_PIN;
    dir_pins[1] = DIR2_PIN;
    dir_pins[2] = DIR3_PIN;
    dir_pins[3] = DIR4_PIN;

    // Initialize debug pin
    gpio_init(DEBUG_PIN);
    //gpio_init(STEP_PIN);
    gpio_set_dir(DEBUG_PIN, GPIO_OUT);
    gpio_put(DEBUG_PIN, 0);

    // Load PIO program
    uint offset = pio_add_program(pio0, &stepper_program);
    
    // Get state machine
    for(int k= 0; k<max_steppers; k++) {
        sm[k] = pio_claim_unused_sm(pio0, true);
        stepper_counters[k] = 0;
        current_directions[k] = 0;
        current_frequencies[k] = 0;
        desired_frequencies[k] = 0;
        desired_directions[k] = 0;
        error_accumulators[k] = 0;
        state[k] = State::SOFTWARE;
        
        //gpio_set_dir(step_pins[k], GPIO_OUT);
        stepper_program_init(pio0, sm[k], offset, step_pins[k], dir_pins[k], 1000.0f);
    }    

    // Setup a timer interrupt that calls update() at cpu_update_rate
    // Using hardware alarm for periodic interrupt
    /**/
    add_repeating_timer_us(-75, [](struct repeating_timer *t) -> bool {
        stepgen_update();
        return true; // Keep repeating
    }, NULL, &update_timer);
    printf("  done steppers!\n");
    
}



void stepgen_set_frequency(int stepper_index,  int frequency, int direction) {
    desired_frequencies[stepper_index] = frequency;
    desired_directions[stepper_index] = direction;
}

int32_t stepgen_get_step_count(int stepper_index) {
    return stepper_get_step_count_blocking(pio0, sm[stepper_index]) + stepper_counters[stepper_index];
}

const char* stepgen_get_state(int stepper_index) {
    return state_to_name(state[stepper_index]);
}

} // extern "C"


static uint64_t last_timestamp_us = 45328;
namespace {
void stepgen_update() {
    uint64_t current_timestamp_us = time_us_64();
    int64_t duration =  current_timestamp_us - last_timestamp_us;
    int64_t actual_update_rate = 1000000 / duration;
    /*
    if(actual_update_rate > cpu_update_rate * 2) {
        printf("Warning: stepgen_update took too long: %dus\n", duration);
    }
        */
    //printf("act last %llu now %llu update rate %llu Hz duration %lld\n", last_timestamp_us, current_timestamp_us, actual_update_rate, duration);
    // printf("act last %llu now %llu \n", last_timestamp_us, current_timestamp_us);
    last_timestamp_us = current_timestamp_us;

    gpio_put(DEBUG_PIN, 1);  // Set debug pin high when update starts
    //printf("Stepgen update called\n");
    for(int k=0; k<max_steppers; k++) {
        switch(state[k]) {
            case State::DIRECTION_CHANGE: {
                // TODO: make sure direction change happens cleanly
                current_directions[k] = desired_directions[k];
                pio_sm_exec(pio0, sm[k], pio_encode_set(pio_pins, current_directions[k]));
                if(desired_frequencies[k] >= cpu_threshold_high) {
                    // Switch to PIO
                    state[k] = State::PIO;
                } else {
                    state[k] = State::SOFTWARE;
                }
            }
            break;
            case State::SOFTWARE: {
                if(current_directions[k] != desired_directions[k]) {
                    // Change direction
                    state[k] = State::DIRECTION_CHANGE; 
                    
                } else if(desired_frequencies[k] >= cpu_threshold_high) {
                    // Switch to PIO
                    // stepper_set_params(pio0, sm[k], desired_frequencies[k], desired_directions[k]);
                    state[k] = State::PIO;
                } else {
                    // Bresenham-style step generation
                    // Accumulate error based on desired frequency
                    // When accumulated error exceeds threshold, generate a step
                    if(current_directions[k] == 1) { // Forward
                        if(error_accumulators[k] > 0) {
                            error_accumulators[k] -= 2* actual_update_rate;
                            pio_sm_exec(pio0, sm[k], pio_encode_nop() |  pio_encode_sideset_opt (1,1));
                            stepper_counters[k]++;
                        } 
                        error_accumulators[k] += 2*desired_frequencies[k];
                    } else {
                        if(error_accumulators[k] < 0) { // Reverse
                            error_accumulators[k] += 2* actual_update_rate;
                            pio_sm_exec(pio0, sm[k], pio_encode_nop() |  pio_encode_sideset_opt (1,1));
                            stepper_counters[k]--;
                        } 
                        error_accumulators[k] -= 2*desired_frequencies[k];
                    }
                }
            }
            break;
            case State::PIO_STOPPING: {
                if(pio_sm_get_tx_fifo_level (pio0, sm[k]) == 0) {
                    // Fifo is clear, which means the stop has been processed.
                    state[k] = desired_directions[k] != current_directions[k] ? State::DIRECTION_CHANGE : State::SOFTWARE;
                    current_frequencies[k] = 0;
                } else {
                    state[k] = State::PIO_STOPPING;
                }
            }
            break;
            case State::PIO: {
                if(desired_frequencies[k] < cpu_threshold_low || desired_directions[k] != current_directions[k]) {
                    //stepper_set_params(pio0, sm[k], 0, 0);
                    stepper_set_params(pio0, sm[k], 0, 0);
                    //stepper_set_params(pio0, sm[k], 0, 0);
                    state[k] = State::PIO_STOPPING;
                } else {
                    // Update frequency/direction in PIO
                    if(current_frequencies[k] != desired_frequencies[k]) {
                        stepper_set_params(pio0, sm[k], desired_frequencies[k], desired_directions[k]);
                        current_frequencies[k] = desired_frequencies[k];
                    }
                }
            }
            break;


        }
        //printf("Stepgen update loop %d state %s freq %d dir %d\n", k, state_to_name(state[k]), desired_frequencies[k], desired_directions[k]);
    } // for loop
    busy_wait_us(5);
    for(int k=0;k<max_steppers;k++) {
        if(state[k] == State::SOFTWARE) {
            pio_sm_exec(pio0, sm[k], pio_encode_nop() |  pio_encode_sideset_opt (1,0));
        }
    }
    gpio_put(DEBUG_PIN, 0);  // Set debug pin low when update is done
}


} // namespace