testing
Dependencies: mbed tinyshell X_NUCLEO_IHM02A1
main.cpp
- Committer:
- tom_astranis
- Date:
- 2021-04-02
- Revision:
- 31:ebd464b0d0d8
- Parent:
- 30:a23204ee2be2
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 }