stepper_example.cpp

/**
 * Stepper Motor Control Example using PIO
 * 
 * This example demonstrates how to use the PIO state machine to control
 * a stepper motor with adjustable frequency and step counting.
 */

#include "pico/stdlib.h"
#include "hardware/pio.h"
#include "hardware/clocks.h"
#include "pico/multicore.h"
#include "stepgen.h"
//#include "stepper.pio.h"
#include <stdio.h>


// these are apparently C headers
extern "C"  {
#include "port_common.h"
#include "wizchip_conf.h"
#include "wizchip_spi.h"
#include "socket.h"
#include "wizchip_conf.h"
}

#include <cmath>

//#include "loopback.h"


// Pin definitions
// #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 ESTOP_PIN 4       // GPIO pin for hardware estop input (active-low)

// PIO instance
 
/* Socket & PORT */
#define SOCKET_LOOPBACK 0
#define PORT_LOOPBACK 5000
#define ETHERNET_BUF_MAX_SIZE (1024 * 2)
#define DATA_BUF_SIZE			2048

static uint8_t g_ethernet_buf[ETHERNET_BUF_MAX_SIZE] = {
    0,
};

static wiz_NetInfo g_net_info = {
    .mac = {0x00, 0x08, 0xDC, 0x12, 0x34, 0x56}, // MAC address
    .ip = {10, 10, 10, 11},                     // IP address
    .sn = {255, 255, 255, 0},                    // Subnet Mask
    .gw = {10, 10, 10, 1},                     // Gateway
    .dns = {8, 8, 8, 8},                         // DNS server
};

#define MAX_STEPGEN (4)

typedef enum {
    CMD_REQUEST_SEND=0xfa0001fa,
    CMD_REQUEST_RECV=0xfa0002fa,
} PacketCmd;

typedef enum {
    RESPONSE_DATA=0xaf0001af,
    RESPONSE_ACK=0xaf0002af,
    RESPONSE_ESTOP=0xaf0003af,
} ResponseCode;

typedef struct  {
    int32_t cmd; // PacketCmd
    uint32_t seq;
    int32_t frequency[MAX_STEPGEN];
    uint32_t outputs;
} SendPacketRequest;

typedef struct {
    int32_t response; // ResponseCode
} SendPacketResponse;

typedef struct {
    int32_t cmd; // PacketCmd
    uint32_t seq;
} RecvPacketRequest;

typedef struct {
    int32_t response; // ResponseCode
    int32_t position_feedback[MAX_STEPGEN];
    uint32_t inputs;
} RecvPacketResponse;

static uint8_t txbuf[1024];
static int sends = 0;
static int last_sends = 0;
static int errors = 0;
static uint32_t last_seq  =0;

static int32_t min_freq = 1<<30;
static int32_t max_freq = 0;

// Watchdog of last us
uint64_t last_recv_us = time_us_64();  // Watchdog: last packet receive time

// Estop state
static bool in_estop = false;
static uint64_t estop_entered_us = 0;
#define ESTOP_COOLDOWN_MS 100  // Min time before auto-recovery


int32_t loopback_udps(uint8_t socket_number, uint8_t* rxbuf, uint16_t port, uint64_t timestamp) {
    int32_t  ret;
    uint16_t size, sentsize;
    uint8_t  destip[4];
    uint16_t destport;

    // Check estop conditions before processing
    static uint32_t errors_at_last_check = 0;
    static uint64_t last_error_check_us = 0;

    // Check error rate every 100ms
    if(timestamp - last_error_check_us > 100000) {
        float error_rate = (float)(errors - errors_at_last_check) / (float)(sends - last_sends);
        errors_at_last_check = errors;
        last_error_check_us = timestamp;

        // Estop if error rate exceeds 10%
        if(error_rate > 0.1 && (sends - last_sends) > 10) {
            in_estop = true;
            estop_entered_us = timestamp;
        }
    }
    // Check hardware estop (active-low)
    if(gpio_get(ESTOP_PIN) == 0) {
        in_estop = true;
        estop_entered_us = timestamp;
    }

    switch (getSn_SR(socket_number)) {
    case SOCK_UDP :
        if ((size = getSn_RX_RSR(socket_number)) > 0) {
            //printf("TRRYING TO read! size is \n", size);
            if (size > DATA_BUF_SIZE) {
                size = DATA_BUF_SIZE;
            }
            ret = recvfrom(socket_number, rxbuf, size, destip, (uint16_t*)&destport);
            if (ret <= 0) {
                return ret;
            }
            // printf("recvfrom %d\n", ret);
            uint32_t cmd = *((uint32_t*)rxbuf);
            uint16_t size = 0;
            switch(cmd) {
                case CMD_REQUEST_SEND: {
                    //printf("send\n");
                    SendPacketRequest* req = (SendPacketRequest*)rxbuf;
                    sends++;
                    if(ret != sizeof(SendPacketRequest)) {
                        errors++;
                        return 1;
                    }
                    last_seq = req->seq;
                    last_recv_us = timestamp;  // Feed watchdog on packet receive

                    // Check high bit of outputs for reset signal
                    stepgen_reset((req->outputs & 0x80000000)  != 0);

                    // Clear estop on explicit reset from driver
                    if(req->outputs & 0x80000000) {
                        in_estop = false;
                    }

                    SendPacketResponse* resp = (SendPacketResponse*)txbuf;
                    for(int k = 0; k < MAX_STEPGEN; k++) {
                        int32_t req_freq = std::abs(req->frequency[k]);
                        min_freq = std::min(min_freq, req_freq);
                        max_freq = std::max(max_freq, req_freq);
                        stepgen_set_frequency(k, req_freq, req->frequency[k] < 0 ? 0 : 1);
                    };
                    if(in_estop) {
                        resp->response = RESPONSE_ESTOP;
                    } else {
                        resp->response = RESPONSE_ACK;
                    }
                    size = sizeof(SendPacketResponse);
                }
                break;
                case CMD_REQUEST_RECV: {
                    //printf("recv\n");
                    if(ret != sizeof(RecvPacketRequest)) {
                        errors++;
                        return 1;
                    }
                    RecvPacketResponse* resp = (RecvPacketResponse*)txbuf;
                    if(in_estop) {
                        resp->response = RESPONSE_ESTOP;
                    } else {
                        resp->response = RESPONSE_DATA;
                    }
                    for(int k = 0; k < MAX_STEPGEN;k ++)
                        resp->position_feedback[k] = stepgen_get_step_count(k);

                    // Position-based inputs
                    resp->inputs = 0;
                    int32_t pos0 = resp->position_feedback[0];
                    int32_t pos1 = resp->position_feedback[1];

                    // Input 0: stepper 0 position > threshold (7.9 * 10204.08 ≈ 80612 steps)
                    if(pos0 > (int32_t)(7.0 * 10204.08)) {
                        resp->inputs |= (1 << 0);
                    }

                    // Input 1: stepper 1 position > threshold (2.875 * 10204.08 ≈ 29336 steps)
                    if(pos1 > (int32_t)(2.5 * 10204.08)) {
                        resp->inputs |= (1 << 1);
                    }
                    size = sizeof(RecvPacketResponse);
                }
                break;
                default:
                errors++;
                // Still need to send something, but this shouldn't happen
                SendPacketResponse* resp = (SendPacketResponse*)txbuf;
                resp->response = in_estop ? RESPONSE_ESTOP : RESPONSE_ACK;
                size = sizeof(SendPacketResponse);
                break;
            }
            // printf("sending response %d\n", size);
            //size = (uint16_t) ret;
            sentsize = 0;
            while (sentsize != size) {
                ret = sendto(socket_number, txbuf + sentsize, size - sentsize, destip, destport);
                if (ret < 0) {
                    return ret;
                }
                sentsize += ret; // Don't care SOCKERR_BUSY, because it is zero.
            }
        }
        break;
    case SOCK_CLOSED:
        printf("Opening UDP socket #%d\r\n",socket_number);
        if ((ret = socket(socket_number, Sn_MR_UDP, port, 0x00)) != socket_number) {
            return ret;
        }
        break;
    default :
        break;
    }
    return 1;
}

void wizserver() {

    int retval = 0;




    //stdio_init_all();
    printf("waiting for network to settle.\n");
    sleep_ms(1000);
    printf("initializing network...\n");
    wizchip_spi_initialize();
    wizchip_cris_initialize();

    wizchip_reset();
    wizchip_initialize();
    wizchip_check();
    network_initialize(g_net_info);
    printf("  done.\n");

    print_network_information(g_net_info);

    uint64_t last_timestamp_us = 0;

    while (1) {
        uint64_t current_timestamp_us = time_us_64();
        /* TCP server loopback test */
        if ((retval = loopback_udps(SOCKET_LOOPBACK, g_ethernet_buf, PORT_LOOPBACK, current_timestamp_us)) < 0) {
            printf(" Loopback error : %d\n", retval);

            while (1)
                ;
        }
        // Watchdog check: if no packets for 10ms, zero all frequencies
        if(current_timestamp_us - last_recv_us > 10000) {
            for(int k = 0; k < MAX_STEPGEN; k++) {
                stepgen_set_frequency(k, 0, 1);
            }
        }

        // Estop auto-recovery: clear if conditions are good and cooldown elapsed
        if(in_estop) {
            bool hardware_ok = gpio_get(ESTOP_PIN) != 0;  // Pin high = OK
            bool error_rate_ok = true;
            // Check error rate again
            if(sends - last_sends > 10) {
                float error_rate = (float)(errors - errors_at_last_check) / (float)(sends - last_sends);
                error_rate_ok = error_rate <= 0.1;
            }
            bool cooldown_ok = (current_timestamp_us - estop_entered_us) > (ESTOP_COOLDOWN_MS * 1000);

            if(hardware_ok && error_rate_ok && cooldown_ok) {
                in_estop = false;  // Auto-recover
            }
        }

        if(current_timestamp_us  - last_timestamp_us > 1000000) {
            int steps[MAX_STEPGEN];
            printf("estop=%d errors %d sends %d inc_sends %d last_seq %d minmaxfreq %d %d -- ", in_estop ? 1 : 0, errors, sends, last_sends-sends,  last_seq, min_freq, max_freq);
            for(int k=0;k<MAX_STEPGEN;k++) {
                steps[k] = stepgen_get_step_count(k);
                printf(" %d", steps[k]);
            }
            printf("\n");
            last_timestamp_us = current_timestamp_us;
            last_sends = sends;
        }

    }

    while(1) {
        printf("Tick\n");
        sleep_ms(1000);
    }
}

int main() {
    timer_hw->dbgpause = 0;


    // Setup serial port
    //setup_default_uart(); // uart0, 9600);
    stdio_uart_init_full(uart1, 115200, 20, 21);//   uart_inst_t *uart, uint baud_rate, int tx_pin, int rx_pin);

    // Initialize hardware estop input
    gpio_init(ESTOP_PIN);
    gpio_set_dir(ESTOP_PIN, GPIO_IN);
    gpio_pull_up(ESTOP_PIN);  // Active-low with pullup

    //stepgen_init();
    multicore_launch_core1 (stepgen_init);

    wizserver();

    
    /*
    printf("Stepper motor controller started!\n");
    
    uint32_t loop_count = 0;
    
    //stepper_set_params(pio0, sm2, 100, 1);
    int freqs[] = {0,0,0,0};
    int directions[] = {1,1,1,1};
    int delay = 0;
    stepgen_set_frequency(0, freqs[0], directions[1]);


    // stepper_set_params(pio0, sm, freqs[0], directions[0]);
    // stepper_set_params(pio0, sm2, freqs[1], directions[1]);
    // stepper_set_params(pio0, sm3, freqs[2], directions[2]);
    // stepper_set_params(pio0, sm4, freqs[3], directions[3]);

    int32_t last_counts[4] = {0};
    uint64_t last_timestamp_us = 0;
    
    while (true) {
        int c = getchar_timeout_us(100);
        //int c = PICO_ERROR_TIMEOUT;
        if(c != PICO_ERROR_TIMEOUT) {
            if(c=='0') {
                freqs[0] = 0;
                printf("Stopping stepping.\n");
            }
            else if(c=='+') freqs[0] += 100;
            else if (c=='-') freqs[0] -= 100;
            else if (c=='*') freqs[0] *= 2;
            else if (c=='/') freqs[0] /= 2;
            else if (c=='d') delay += 1;
            else if (c=='D') delay -= 1;
            else if (c=='f') {
                directions[0] = 1;
            }
            else if (c=='r') {
                directions[0] = 0;
            }
            //if(freq < 1) freq = 1;
            stepgen_set_frequency(0, freqs[0], directions[0]);
        }

        if(loop_count % 10 == 0) {
            printf("Trying to read...\n");
            uint64_t current_timestamp_us = time_us_64();
            for(int k=0;k<4;k++) {
                int32_t current_count = stepgen_get_step_count(k);
                int32_t delta = (current_count - last_counts[k]);

                float actual_frequency = 0.0f;
                if (last_timestamp_us != 0 && current_timestamp_us > last_timestamp_us) {
                    uint64_t time_delta_us = current_timestamp_us - last_timestamp_us;
                    if (time_delta_us > 0) {
                        actual_frequency = (float)delta * 1000000.0f / (float)time_delta_us;
                    }
                }

                last_counts[k]  = current_count;

                printf("count: %10d delta: %10d set_freq: %d Hz actual_freq: %.2f Hz state: %s\n",
                    current_count, delta, freqs[k], actual_frequency, stepgen_get_state(k));
            }
            last_timestamp_us = current_timestamp_us;
            printf("Done to read...%d\n", loop_count);

        }
        sleep_ms(100);
        loop_count++;
    }
        */
    
    return 0;
}