File content as of revision 31:ebd464b0d0d8:

#include "mbed.h"

/* Helper header files. */
#include "DevSPI.h"

/* Expansion Board specific header files. */
#include "XNucleoIHM02A1.h"

/* Tinyshell: https://os.mbed.com/users/murilopontes/code/tinyshell/ */
#include "tinysh.h"

#include <cstdlib>
#include <string>

/* Definitions ---------------------------------------------------------------*/

/* Motor specs */
#define MOTOR_SUPPLY_VOLTAGE_V      12.0
#define STEPS_PER_REV               200.0
#define MAX_PHASE_CURRENT_A         0.67
#define PHASE_RES_OHMS              12.0
#define MOTOR_INITIAL_SPEED_SPS     1000.0
#define MOTOR_ACCEL_SPS2            1000.0
#define MOTOR_MAX_SPEED_SPS         1000.0
#define FULL_STEP_TH_SPS            602.7
#define DUMMY_KVAL_V                3.06
#define BEMF_ICPT_SPS               61.52
#define START_SLOPE                 392.1569e-6
#define FINAL_SLOPE                 643.1372e-6
#define OCD_TH_MA                   600.0
#define STALL_TH_MA                 1000.0

/* Behavioral stuff */
#define STATUS_LOG_RATE_HZ 1
#define MIN_LOOP_TIME_MS 1
#define HARD_STOP_WAIT_MS 100

/* Variables -----------------------------------------------------------------*/

/* Motor Control Expansion Board. */
XNucleoIHM02A1 *x_nucleo_ihm02a1;

/* Initialization parameters of the motors connected to the expansion board. */
L6470_init_t init[L6470DAISYCHAINSIZE] = {
    /* First Motor. */
    {
        MOTOR_SUPPLY_VOLTAGE_V,                  /* Motor supply voltage in V. */
        STEPS_PER_REV,                           /* Min number of steps per revolution for the motor. */
        MAX_PHASE_CURRENT_A,                     /* Max motor phase voltage in A. */
        MAX_PHASE_CURRENT_A * PHASE_RES_OHMS,    /* Max motor phase voltage in V. (12 ohms per phase) */
        MOTOR_INITIAL_SPEED_SPS,                 /* Motor initial speed [step/s]. */
        MOTOR_ACCEL_SPS2,                        /* Motor acceleration [step/s^2] (comment for infinite acceleration mode). */
        MOTOR_ACCEL_SPS2,                        /* Motor deceleration [step/s^2] (comment for infinite deceleration mode). */
        MOTOR_MAX_SPEED_SPS,                     /* Motor maximum speed [step/s]. */
        0.0,                                     /* Motor minimum speed [step/s]. */
        FULL_STEP_TH_SPS,                        /* Motor full-step speed threshold [step/s]. */
        DUMMY_KVAL_V,                            /* Holding kval [V]. */
        DUMMY_KVAL_V,                            /* Constant speed kval [V]. */
        DUMMY_KVAL_V,                            /* Acceleration starting kval [V]. */
        DUMMY_KVAL_V,                            /* Deceleration starting kval [V]. */
        BEMF_ICPT_SPS,                           /* Intersect speed for bemf compensation curve slope changing [step/s]. */
        START_SLOPE,                             /* Start slope [s/step]. */
        FINAL_SLOPE,                             /* Acceleration final slope [s/step]. */
        FINAL_SLOPE,                             /* Deceleration final slope [s/step]. */
        0,                                       /* Thermal compensation factor (range [0, 15]). */
        OCD_TH_MA,                               /* Ocd threshold [ma] (range [375 ma, 6000 ma]). */
        STALL_TH_MA,                             /* Stall threshold [ma] (range [31.25 ma, 4000 ma]). */
        StepperMotor::STEP_MODE_FULL,            /* Step mode selection. */
        0xFF,                                    /* Alarm conditions enable. */
        0x2E88                                   /* Ic configuration. */
    },

    /* Second Motor. */
    {
        MOTOR_SUPPLY_VOLTAGE_V,                  /* Motor supply voltage in V. */
        STEPS_PER_REV,                           /* Min number of steps per revolution for the motor. */
        MAX_PHASE_CURRENT_A,                     /* Max motor phase voltage in A. */
        MAX_PHASE_CURRENT_A * PHASE_RES_OHMS,    /* Max motor phase voltage in V. (12 ohms per phase) */
        MOTOR_INITIAL_SPEED_SPS,                 /* Motor initial speed [step/s]. */
        MOTOR_ACCEL_SPS2,                        /* Motor acceleration [step/s^2] (comment for infinite acceleration mode). */
        MOTOR_ACCEL_SPS2,                        /* Motor deceleration [step/s^2] (comment for infinite deceleration mode). */
        MOTOR_MAX_SPEED_SPS,                     /* Motor maximum speed [step/s]. */
        0.0,                                     /* Motor minimum speed [step/s]. */
        FULL_STEP_TH_SPS,                        /* Motor full-step speed threshold [step/s]. */
        DUMMY_KVAL_V,                            /* Holding kval [V]. */
        DUMMY_KVAL_V,                            /* Constant speed kval [V]. */
        DUMMY_KVAL_V,                            /* Acceleration starting kval [V]. */
        DUMMY_KVAL_V,                            /* Deceleration starting kval [V]. */
        BEMF_ICPT_SPS,                           /* Intersect speed for bemf compensation curve slope changing [step/s]. */
        START_SLOPE,                             /* Start slope [s/step]. */
        FINAL_SLOPE,                             /* Acceleration final slope [s/step]. */
        FINAL_SLOPE,                             /* Deceleration final slope [s/step]. */
        0,                                       /* Thermal compensation factor (range [0, 15]). */
        OCD_TH_MA,                               /* Ocd threshold [ma] (range [375 ma, 6000 ma]). */
        STALL_TH_MA,                             /* Stall threshold [ma] (range [31.25 ma, 4000 ma]). */
        StepperMotor::STEP_MODE_FULL,            /* Step mode selection. */
        0xFF,                                    /* Alarm conditions enable. */
        0x2E88                                   /* Ic configuration. */
    }
};

/* Serial Port for console */
Serial pc(USBTX, USBRX);

/* Nasty globals haha */
bool limit_flag = false;
bool status_flag = false;
bool run_flag = false;
bool stop_flag = false;
bool cmd_both = false;
int  which_axis = 0;
int  speed_sps = 0;
int  go_steps = 0;
int  dir_fwd = true;

/* Tinyshell command handler functions */
void set_status_flag(int argc, char **argv)
{
    status_flag = true;
}

// gosteps dir steps (axis)
void set_gosteps_flag(int argc, char **argv)
{    
    if (argc < 3 || argc > 4) {
        printf("\r\nIncorrect number of arguments for: gosteps dir steps (axis)\r\n");
        return;
    }
    
    dir_fwd = true;
    if (argv[1] == std::string("bwd")) dir_fwd = false;
    go_steps = atoi(argv[2]);
    
    if (argc == 3) {
        // both axes
        cmd_both = true;
    } else {
        cmd_both = false;
        which_axis = 0;
        if (argv[3] == std::string("y")) which_axis = 1;
    }
}

// run dir speed (axis)
void set_run_flag(int argc, char **argv)
{
    if (argc < 3 || argc > 4) {
        printf("\r\nIncorrect number of arguments for: run dir steps (axis)\r\n");
        return;
    }

    run_flag = true;
    dir_fwd = true;
    if (argv[1] == std::string("bwd")) dir_fwd = false;
    speed_sps = atoi(argv[2]);

    if (argc == 3) {
        cmd_both = true;
    } else {
        cmd_both = false;
        which_axis = 0;
        if (argv[3] == std::string("y")) which_axis = 1;
    }
}

// stop (axis)
void set_stop_flag(int argc, char **argv)
{
    if (argc > 2) {
        printf("\r\nIncorrect number of arguments for: stop (axis)\r\n");
        return;
    }
    
    stop_flag = true;
    
    if (argc == 1) {
        // both axes
        cmd_both = true;
    } else {
        cmd_both = false;
        which_axis = 0;
        if (argv[1] == std::string("y")) which_axis = 1;
    }    
}

// parent cmd (0 for top) cmd input name, help string, usage string, 
//   function to launch, arg when called, next (0 at init), child (0 at init)
tinysh_cmd_t print_status_cmd = {0, "status", 
    "\r\nPrint content of status registers\r\n", 
    "[no args]", set_status_flag,0,0,0};
tinysh_cmd_t go_steps_cmd = {0, "gosteps", 
    "\r\n'Go n steps', dir = 'fwd' for CW or 'bwd' for CCW, steps = n steps, axis = 'x' or 'y' (omit for both)\r\n", 
    "[dir, steps, (axis)]", set_gosteps_flag,0,0,0};
tinysh_cmd_t run_cmd = {0, "run", 
    "\r\nRun motor, dir = 'fwd' for CW or 'bwd' for CCW, speed in steps per second, axis = 'x' or 'y' (omit for both)\r\n", 
    "[dir, speed, (axis)]", set_run_flag,0,0,0};
tinysh_cmd_t stop_cmd = {0, "stop", 
    "\r\nStop motor, optional axis = 'x' or 'y'; both stopped if omitted\r\n", 
    "[(axis)]", set_stop_flag,0,0,0};

/* mandatory tiny shell output function */
void tinysh_char_out(unsigned char c)
{
    pc.putc(c);
}

/* Main ----------------------------------------------------------------------*/

int main()
{
    /*----- Initialization. -----*/
    unsigned int status_bytes[L6470DAISYCHAINSIZE];
    
    /* Set up tinyshell */
    pc.baud(115200);
    pc.printf("\r\nMotor controller ready\r\n");
    tinysh_set_prompt("> ");
    
    // Add all tinyshell commands here
    tinysh_add_command(&print_status_cmd);
    tinysh_add_command(&run_cmd);
    tinysh_add_command(&go_steps_cmd);
    tinysh_add_command(&stop_cmd);

    /* Initializing SPI bus. */
#ifdef TARGET_STM32F429
    DevSPI dev_spi(D11, D12, D13);
#else
    DevSPI dev_spi(D11, D12, D3);
#endif

    /* Initializing Motor Control Expansion Board. */
    x_nucleo_ihm02a1 = new XNucleoIHM02A1(&init[0], &init[1], A4, A5, D4, A2, &dev_spi);

    /* Building a list of motor control components. */
    L6470 **motors = x_nucleo_ihm02a1->get_components();
   
    // Main loop
    while(1) {
        /* 1: Check for hardware flags ---------------------------------------*/
        if (limit_flag) {
            // clear flag
            limit_flag = false;

            // Hard stop 
            for (int m = 0; m < L6470DAISYCHAINSIZE; m++) {
                motors[m]->prepare_hard_stop();
            }
            x_nucleo_ihm02a1->perform_prepared_actions();
            
            wait_ms(HARD_STOP_WAIT_MS);
            
            // High Z
            for (int m = 0; m < L6470DAISYCHAINSIZE; m++) {
                motors[m]->prepare_hard_hiz();
            }
            x_nucleo_ihm02a1->perform_prepared_actions();
        
            printf("\r\nReached limit\r\n");
            /* force prompt display by generating empty command */
            tinysh_char_in('\r');
        }
                
        /* 2: Handle Commands ------------------------------------------------*/
        if (go_steps) {
            // Stop first
            if (cmd_both) {
                for (int m = 0; m < L6470DAISYCHAINSIZE; m++) {
                    motors[m]->prepare_hard_stop();
                }
                x_nucleo_ihm02a1->perform_prepared_actions();
            } else {
                motors[which_axis]->hard_stop();
            }
            wait_ms(HARD_STOP_WAIT_MS);
            
            if (cmd_both) {
                for (int m = 0; m < L6470DAISYCHAINSIZE; m++) {
                    if (dir_fwd) motors[m]->prepare_move(StepperMotor::FWD, go_steps);
                    else motors[m]->prepare_move(StepperMotor::BWD, go_steps);
                }
                x_nucleo_ihm02a1->perform_prepared_actions();
            } else {
                if (dir_fwd) motors[which_axis]->move(StepperMotor::FWD, go_steps);
                else motors[which_axis]->move(StepperMotor::BWD, go_steps);
            }
 
            printf("\r\nGoing %s %d steps\r\n", dir_fwd ? "Forward" : "Backward", go_steps);
            /* force prompt display by generating empty command */
            tinysh_char_in('\r');
            
            // Clear flags
            go_steps = 0;
            cmd_both = false;
        }
            
        if (run_flag) {
            // Stop first
            if (cmd_both) {
                // stop both at same time
                for (int m = 0; m < L6470DAISYCHAINSIZE; m++) {
                    motors[m]->prepare_hard_stop();
                }
                x_nucleo_ihm02a1->perform_prepared_actions();
            } else {
                motors[which_axis]->hard_stop();
            }
            wait_ms(HARD_STOP_WAIT_MS);
            
            if (cmd_both) {
                for (int m = 0; m < L6470DAISYCHAINSIZE; m++) {
                    if (dir_fwd) motors[m]->prepare_run(StepperMotor::FWD, speed_sps);
                    else motors[m]->prepare_run(StepperMotor::BWD, speed_sps);
                }
                x_nucleo_ihm02a1->perform_prepared_actions();
            } else {
                if (dir_fwd) motors[which_axis]->run(StepperMotor::FWD, speed_sps);
                else motors[which_axis]->run(StepperMotor::BWD, speed_sps);
            }

            printf("\r\nRunning\r\n");
            /* force prompt display by generating empty command */
            tinysh_char_in('\r');

            // Clear flags
            run_flag = false;
            cmd_both = false;
        }

        if (stop_flag) {

            printf("\r\nStopping\r\n");
            /* force prompt display by generating empty command */
            tinysh_char_in('\r');

            // Hard stop 
            if (cmd_both) {
                // stop both at same time
                for (int m = 0; m < L6470DAISYCHAINSIZE; m++) {
                    motors[m]->prepare_hard_stop();
                }
                x_nucleo_ihm02a1->perform_prepared_actions();
            } else {
                motors[which_axis]->hard_stop();
            }
            wait_ms(HARD_STOP_WAIT_MS);
            
            // High Z
            if (cmd_both) {
                for (int m = 0; m < L6470DAISYCHAINSIZE; m++) {
                    motors[m]->prepare_hard_hiz();
                }
                x_nucleo_ihm02a1->perform_prepared_actions();
            } else {
                motors[which_axis]->hard_hiz();
            }
            
            // clear flags
            stop_flag = false;
            cmd_both = false;
        }
        
    
        if (status_flag) {
            // Fetch and parse the status register for each motor
            for (int m = 0; m < L6470DAISYCHAINSIZE; m++) {
                status_bytes[m] = motors[m]->get_status();
                printf("\r\nStatus reg %d: 0x%02X\r\n", m, status_bytes[m]);
                printf("    STEP-CLOCK MODE:       ");
                printf(status_bytes[m] & 0x8000 ? "SET\r\n" : "NOT SET\r\n");
                printf("    STEP_LOSS_B:           ");
                printf(status_bytes[m] & 0x4000 ? "SET\r\n" : "NOT SET\r\n");
                printf("    STEP_LOSS_A:           ");
                printf(status_bytes[m] & 0x2000 ? "SET\r\n" : "NOT SET\r\n");
                printf("    OVERCURRENT DETECT:    ");
                printf(status_bytes[m] & 0x1000 ? "SET\r\n" : "NOT SET\r\n");
                printf("    THERMAL SHUTDOWN:      ");
                printf(status_bytes[m] & 0x0800 ? "SET\r\n" : "NOT SET\r\n");
                printf("    THERMAL WARN:          ");
                printf(status_bytes[m] & 0x0400 ? "SET\r\n" : "NOT SET\r\n");
                printf("    UNDERVOLTAGE LOCKOUT:  ");
                printf(status_bytes[m] & 0x0200 ? "SET\r\n" : "NOT SET\r\n");
                printf("    WRONG_CMD:             ");
                printf(status_bytes[m] & 0x0100 ? "SET\r\n" : "NOT SET\r\n");
                printf("    NOTPERF_CMD:           ");
                printf(status_bytes[m] & 0x0080 ? "SET\r\n" : "NOT SET\r\n");
                printf("    MOTOR_STATUS:          ");
                if ((status_bytes[m] && 0x0060) >> 5 == 0x00) printf("STOPPED\r\n");
                if ((status_bytes[m] && 0x0060) >> 5 == 0x01) printf("ACCELERATING\r\n");
                if ((status_bytes[m] && 0x0060) >> 5 == 0x10) printf("DECELERATING\r\n");
                if ((status_bytes[m] && 0x0060) >> 5 == 0x11) printf("CONSTANT SPEED\r\n");
                printf("    DIRECTION:             ");
                printf(status_bytes[m] & 0x0010 ? "FWD\r\n" : "REV\r\n");
                printf("    SWITCH TURN-ON EVENT:  ");
                printf(status_bytes[m] & 0x0008 ? "SET\r\n" : "NOT SET\r\n");
                printf("    SWITCH STATUS:         ");
                printf(status_bytes[m] & 0x0004 ? "CLOSED\r\n" : "OPEN\r\n");
                printf("    BUSY:                  ");
                printf(status_bytes[m] & 0x0002 ? "SET\r\n" : "NOT SET\r\n");
                printf("    HI_Z:                  ");
                printf(status_bytes[m] & 0x0001 ? "SET\r\n" : "NOT SET\r\n");
            }
            // clear flag
            status_flag = false;
            /* force prompt display by generating empty command */
            tinysh_char_in('\r');
        }
        
        /* 3: Fetch new chars for Tinyshell ----------------------------------*/
        tinysh_char_in(pc.getc());
        
    } // end main loop
}