stepgen.cpp

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

#include <memory>
#include <atomic>

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

constexpr int max_steppers = 4;
constexpr int cpu_threshold_low = 10000; // Once we go below this we switch to software
constexpr int cpu_threshold_high = 20000;  // Once we go above this we switch to hardware.
static_assert(cpu_threshold_low < cpu_threshold_high);

#define STEP_PIN 17       // GPIO pin for STEP signal
#define DIR_PIN 9        // GPIO pin for DIR signal (controlled by PIO side-set)
#define STEP2_PIN 18       // GPIO pin for STEP signal
#define DIR2_PIN 10        // GPIO pin for DIR signal (controlled by PIO side-set)
#define STEP3_PIN 19       // GPIO pin for STEP signal
#define DIR3_PIN 11       // GPIO pin for DIR signal (controlled by PIO side-set)
#define STEP4_PIN 27       // GPIO pin for STEP signal
#define DIR4_PIN 12        // GPIO pin for DIR signal (controlled by PIO side-set)
#define DEBUG_PIN 6      // GPIO pin for debugging timer update


// PINS!

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";
}

static_assert(sizeof(int) == sizeof(int32_t));
// This is very carefully designed. I think.
// Essentially, desired is what is written by the other processor, it is never
// written by the processor handled steps, only read.  This prevents read/write races.



// Reset vectoring
std::atomic<int32_t> desired_in_reset; 
int32_t in_reset;

// Current state of steppering
State state[max_steppers];
int sm[max_steppers];
// Desired frequencies and directions commanded from other processor
std::atomic<int32_t> desired_frequencies[max_steppers];
int desired_directions[max_steppers];
// Currently running frequencies and errors
int current_frequencies[max_steppers];
int error_accumulators[max_steppers];
// Stepper counts from software stepping (PIO stepping happens elsewhere)
std::atomic<int32_t> 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");
    in_reset = 0;
    desired_in_reset = 0;
    
    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");
    
    */

    
    uint64_t last_timestamp_us = time_us_64();

    while(1) {
        uint64_t t = time_us_64();
        if(t - last_timestamp_us > 20) {
            last_timestamp_us = t;
            stepgen_update();
        }
        
    }

}


void stepgen_set_frequency(int stepper_index,  int frequency, int direction) {
    // Program changes in frequency unless we are in reset.
    if(in_reset == 0 && desired_in_reset == 0) {
        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]);
}

void stepgen_reset(int32_t newval) {
    if(desired_in_reset == 0 && newval == 1) {
        printf("Entering reset...");
    }
    desired_in_reset = newval;
}

} // 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");

    // If we are flagged to reset, we go into reset, but if not, then we still may
    // be handling the last reset, so we don't overwrite in_reset
    if(desired_in_reset == 1) {
        in_reset = 1;
    }

    for(int k=0; k<max_steppers; k++) {
        int32_t desired_direction = in_reset == 1 ? (0) : desired_directions[k];
        int32_t desired_frequency = in_reset == 1 ? (0) : desired_frequencies[k].load();

        switch(state[k]) {
            case State::DIRECTION_CHANGE: {
                // TODO: make sure direction change happens cleanly
                current_directions[k] = desired_direction;
                pio_sm_exec(pio0, sm[k], pio_encode_set(pio_pins, current_directions[k]));
                if(desired_frequency >= cpu_threshold_high) {
                    // Switch to PIO
                    state[k] = State::PIO;
                } else {
                    state[k] = State::SOFTWARE;
                }
            }
            break;
            case State::SOFTWARE: {
                if(current_directions[k] != desired_direction) {
                    // Change direction
                    state[k] = State::DIRECTION_CHANGE; 
                    
                } else if(desired_frequency >= 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].fetch_add(1, std::memory_order_relaxed); 
                            //stepper_counters[k]++;
                        } 
                        error_accumulators[k] += 2*desired_frequency;
                    } 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].fetch_add(-1, std::memory_order_relaxed); 
                            //stepper_counters[k]--;
                        } 
                        error_accumulators[k] -= 2*desired_frequency;
                    }
                }
            }
            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_direction != 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_frequency) {
                        stepper_set_params(pio0, sm[k], desired_frequency, desired_direction);
                        current_frequencies[k] = desired_frequency;
                    }
                }
            }
            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


    // Handle the lowering the pulse of software stepping
    busy_wait_us(5); // This is the pulse width when in software 
    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));
        }
    }

    if(in_reset == 1) {
        printf("in reset\n");
        // In reset, we wait for all steppers to be stopped.
        // Then we will wait an addition amount of time, and then reset all counts.

        
        bool all_stopped = true;
        for(int stepper=0;stepper<max_steppers;stepper++) {
            if(current_frequencies[stepper] != 0) all_stopped = false;
        }
        printf("al stopped %d -- %d %d %d %d \n", all_stopped ? 1 : 0, current_frequencies[0], current_directions[1], current_directions[2], current_directions[3]);
        if(all_stopped) {
            busy_wait_us(10); //

            // Reset the software counts
            for(int stepper=0;stepper<max_steppers;stepper++) {
                current_frequencies[stepper] = 0;
            }
            // Reset the pio counts
            for(int stepper=0;stepper<max_steppers;stepper++) {
                //pio_sm_exec(pio0, sm[stepper], )
                stepper_counters[stepper] = 0;
                pio_sm_exec(pio0, sm[stepper], pio_encode_set(pio_y, 0) | pio_encode_sideset_opt(1,0));
            }

            in_reset = 0;
            printf("exiting reset\n");
        }

    } 


    gpio_put(DEBUG_PIN, 0);  // Set debug pin low when update is done
}


} // namespace