main.cpp

00001 #include "mbed.h"
00002 
00003 /* Helper header files. */
00004 #include "DevSPI.h"
00005 
00006 /* Expansion Board specific header files. */
00007 #include "XNucleoIHM02A1.h"
00008 
00009 /* Tinyshell: https://os.mbed.com/users/murilopontes/code/tinyshell/ */
00010 #include "tinysh.h"
00011 
00012 #include <cstdlib>
00013 #include <string>
00014 
00015 /* Definitions ---------------------------------------------------------------*/
00016 
00017 /* Motor specs */
00018 #define MOTOR_SUPPLY_VOLTAGE_V      12.0
00019 #define STEPS_PER_REV               200.0
00020 #define MAX_PHASE_CURRENT_A         0.67
00021 #define PHASE_RES_OHMS              12.0
00022 #define MOTOR_INITIAL_SPEED_SPS     1000.0
00023 #define MOTOR_ACCEL_SPS2            1000.0
00024 #define MOTOR_MAX_SPEED_SPS         1000.0
00025 #define FULL_STEP_TH_SPS            602.7
00026 #define DUMMY_KVAL_V                3.06
00027 #define BEMF_ICPT_SPS               61.52
00028 #define START_SLOPE                 392.1569e-6
00029 #define FINAL_SLOPE                 643.1372e-6
00030 #define OCD_TH_MA                   600.0
00031 #define STALL_TH_MA                 1000.0
00032 
00033 /* Behavioral stuff */
00034 #define STATUS_LOG_RATE_HZ 1
00035 #define MIN_LOOP_TIME_MS 1
00036 #define HARD_STOP_WAIT_MS 100
00037 
00038 /* Variables -----------------------------------------------------------------*/
00039 
00040 /* Motor Control Expansion Board. */
00041 XNucleoIHM02A1 *x_nucleo_ihm02a1;
00042 
00043 /* Initialization parameters of the motors connected to the expansion board. */
00044 L6470_init_t init[L6470DAISYCHAINSIZE] = {
00045     /* First Motor. */
00046     {
00047         MOTOR_SUPPLY_VOLTAGE_V,                  /* Motor supply voltage in V. */
00048         STEPS_PER_REV,                           /* Min number of steps per revolution for the motor. */
00049         MAX_PHASE_CURRENT_A,                     /* Max motor phase voltage in A. */
00050         MAX_PHASE_CURRENT_A * PHASE_RES_OHMS,    /* Max motor phase voltage in V. (12 ohms per phase) */
00051         MOTOR_INITIAL_SPEED_SPS,                 /* Motor initial speed [step/s]. */
00052         MOTOR_ACCEL_SPS2,                        /* Motor acceleration [step/s^2] (comment for infinite acceleration mode). */
00053         MOTOR_ACCEL_SPS2,                        /* Motor deceleration [step/s^2] (comment for infinite deceleration mode). */
00054         MOTOR_MAX_SPEED_SPS,                     /* Motor maximum speed [step/s]. */
00055         0.0,                                     /* Motor minimum speed [step/s]. */
00056         FULL_STEP_TH_SPS,                        /* Motor full-step speed threshold [step/s]. */
00057         DUMMY_KVAL_V,                            /* Holding kval [V]. */
00058         DUMMY_KVAL_V,                            /* Constant speed kval [V]. */
00059         DUMMY_KVAL_V,                            /* Acceleration starting kval [V]. */
00060         DUMMY_KVAL_V,                            /* Deceleration starting kval [V]. */
00061         BEMF_ICPT_SPS,                           /* Intersect speed for bemf compensation curve slope changing [step/s]. */
00062         START_SLOPE,                             /* Start slope [s/step]. */
00063         FINAL_SLOPE,                             /* Acceleration final slope [s/step]. */
00064         FINAL_SLOPE,                             /* Deceleration final slope [s/step]. */
00065         0,                                       /* Thermal compensation factor (range [0, 15]). */
00066         OCD_TH_MA,                               /* Ocd threshold [ma] (range [375 ma, 6000 ma]). */
00067         STALL_TH_MA,                             /* Stall threshold [ma] (range [31.25 ma, 4000 ma]). */
00068         StepperMotor::STEP_MODE_FULL,            /* Step mode selection. */
00069         0xFF,                                    /* Alarm conditions enable. */
00070         0x2E88                                   /* Ic configuration. */
00071     },
00072 
00073     /* Second Motor. */
00074     {
00075         MOTOR_SUPPLY_VOLTAGE_V,                  /* Motor supply voltage in V. */
00076         STEPS_PER_REV,                           /* Min number of steps per revolution for the motor. */
00077         MAX_PHASE_CURRENT_A,                     /* Max motor phase voltage in A. */
00078         MAX_PHASE_CURRENT_A * PHASE_RES_OHMS,    /* Max motor phase voltage in V. (12 ohms per phase) */
00079         MOTOR_INITIAL_SPEED_SPS,                 /* Motor initial speed [step/s]. */
00080         MOTOR_ACCEL_SPS2,                        /* Motor acceleration [step/s^2] (comment for infinite acceleration mode). */
00081         MOTOR_ACCEL_SPS2,                        /* Motor deceleration [step/s^2] (comment for infinite deceleration mode). */
00082         MOTOR_MAX_SPEED_SPS,                     /* Motor maximum speed [step/s]. */
00083         0.0,                                     /* Motor minimum speed [step/s]. */
00084         FULL_STEP_TH_SPS,                        /* Motor full-step speed threshold [step/s]. */
00085         DUMMY_KVAL_V,                            /* Holding kval [V]. */
00086         DUMMY_KVAL_V,                            /* Constant speed kval [V]. */
00087         DUMMY_KVAL_V,                            /* Acceleration starting kval [V]. */
00088         DUMMY_KVAL_V,                            /* Deceleration starting kval [V]. */
00089         BEMF_ICPT_SPS,                           /* Intersect speed for bemf compensation curve slope changing [step/s]. */
00090         START_SLOPE,                             /* Start slope [s/step]. */
00091         FINAL_SLOPE,                             /* Acceleration final slope [s/step]. */
00092         FINAL_SLOPE,                             /* Deceleration final slope [s/step]. */
00093         0,                                       /* Thermal compensation factor (range [0, 15]). */
00094         OCD_TH_MA,                               /* Ocd threshold [ma] (range [375 ma, 6000 ma]). */
00095         STALL_TH_MA,                             /* Stall threshold [ma] (range [31.25 ma, 4000 ma]). */
00096         StepperMotor::STEP_MODE_FULL,            /* Step mode selection. */
00097         0xFF,                                    /* Alarm conditions enable. */
00098         0x2E88                                   /* Ic configuration. */
00099     }
00100 };
00101 
00102 /* Serial Port for console */
00103 Serial pc(USBTX, USBRX);
00104 
00105 /* Nasty globals haha */
00106 bool limit_flag = false;
00107 bool status_flag = false;
00108 bool run_flag = false;
00109 bool stop_flag = false;
00110 bool cmd_both = false;
00111 int  which_axis = 0;
00112 int  speed_sps = 0;
00113 int  go_steps = 0;
00114 int  dir_fwd = true;
00115 
00116 /* Tinyshell command handler functions */
00117 void set_status_flag(int argc, char **argv)
00118 {
00119     status_flag = true;
00120 }
00121 
00122 // gosteps dir steps (axis)
00123 void set_gosteps_flag(int argc, char **argv)
00124 {    
00125     if (argc < 3 || argc > 4) {
00126         printf("\r\nIncorrect number of arguments for: gosteps dir steps (axis)\r\n");
00127         return;
00128     }
00129     
00130     dir_fwd = true;
00131     if (argv[1] == std::string("bwd")) dir_fwd = false;
00132     go_steps = atoi(argv[2]);
00133     
00134     if (argc == 3) {
00135         // both axes
00136         cmd_both = true;
00137     } else {
00138         cmd_both = false;
00139         which_axis = 0;
00140         if (argv[3] == std::string("y")) which_axis = 1;
00141     }
00142 }
00143 
00144 // run dir speed (axis)
00145 void set_run_flag(int argc, char **argv)
00146 {
00147     if (argc < 3 || argc > 4) {
00148         printf("\r\nIncorrect number of arguments for: run dir steps (axis)\r\n");
00149         return;
00150     }
00151 
00152     run_flag = true;
00153     dir_fwd = true;
00154     if (argv[1] == std::string("bwd")) dir_fwd = false;
00155     speed_sps = atoi(argv[2]);
00156 
00157     if (argc == 3) {
00158         cmd_both = true;
00159     } else {
00160         cmd_both = false;
00161         which_axis = 0;
00162         if (argv[3] == std::string("y")) which_axis = 1;
00163     }
00164 }
00165 
00166 // stop (axis)
00167 void set_stop_flag(int argc, char **argv)
00168 {
00169     if (argc > 2) {
00170         printf("\r\nIncorrect number of arguments for: stop (axis)\r\n");
00171         return;
00172     }
00173     
00174     stop_flag = true;
00175     
00176     if (argc == 1) {
00177         // both axes
00178         cmd_both = true;
00179     } else {
00180         cmd_both = false;
00181         which_axis = 0;
00182         if (argv[1] == std::string("y")) which_axis = 1;
00183     }    
00184 }
00185 
00186 // parent cmd (0 for top) cmd input name, help string, usage string, 
00187 //   function to launch, arg when called, next (0 at init), child (0 at init)
00188 tinysh_cmd_t print_status_cmd = {0, "status", 
00189     "\r\nPrint content of status registers\r\n", 
00190     "[no args]", set_status_flag,0,0,0};
00191 tinysh_cmd_t go_steps_cmd = {0, "gosteps", 
00192     "\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", 
00193     "[dir, steps, (axis)]", set_gosteps_flag,0,0,0};
00194 tinysh_cmd_t run_cmd = {0, "run", 
00195     "\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", 
00196     "[dir, speed, (axis)]", set_run_flag,0,0,0};
00197 tinysh_cmd_t stop_cmd = {0, "stop", 
00198     "\r\nStop motor, optional axis = 'x' or 'y'; both stopped if omitted\r\n", 
00199     "[(axis)]", set_stop_flag,0,0,0};
00200 
00201 /* mandatory tiny shell output function */
00202 void tinysh_char_out(unsigned char c)
00203 {
00204     pc.putc(c);
00205 }
00206 
00207 /* Main ----------------------------------------------------------------------*/
00208 
00209 int main()
00210 {
00211     /*----- Initialization. -----*/
00212     unsigned int status_bytes[L6470DAISYCHAINSIZE];
00213     
00214     /* Set up tinyshell */
00215     pc.baud(115200);
00216     pc.printf("\r\nMotor controller ready\r\n");
00217     tinysh_set_prompt("> ");
00218     
00219     // Add all tinyshell commands here
00220     tinysh_add_command(&print_status_cmd);
00221     tinysh_add_command(&run_cmd);
00222     tinysh_add_command(&go_steps_cmd);
00223     tinysh_add_command(&stop_cmd);
00224 
00225     /* Initializing SPI bus. */
00226 #ifdef TARGET_STM32F429
00227     DevSPI dev_spi(D11, D12, D13);
00228 #else
00229     DevSPI dev_spi(D11, D12, D3);
00230 #endif
00231 
00232     /* Initializing Motor Control Expansion Board. */
00233     x_nucleo_ihm02a1 = new XNucleoIHM02A1(&init[0], &init[1], A4, A5, D4, A2, &dev_spi);
00234 
00235     /* Building a list of motor control components. */
00236     L6470 **motors = x_nucleo_ihm02a1->get_components();
00237    
00238     // Main loop
00239     while(1) {
00240         /* 1: Check for hardware flags ---------------------------------------*/
00241         if (limit_flag) {
00242             // clear flag
00243             limit_flag = false;
00244 
00245             // Hard stop 
00246             for (int m = 0; m < L6470DAISYCHAINSIZE; m++) {
00247                 motors[m]->prepare_hard_stop();
00248             }
00249             x_nucleo_ihm02a1->perform_prepared_actions();
00250             
00251             wait_ms(HARD_STOP_WAIT_MS);
00252             
00253             // High Z
00254             for (int m = 0; m < L6470DAISYCHAINSIZE; m++) {
00255                 motors[m]->prepare_hard_hiz();
00256             }
00257             x_nucleo_ihm02a1->perform_prepared_actions();
00258         
00259             printf("\r\nReached limit\r\n");
00260             /* force prompt display by generating empty command */
00261             tinysh_char_in('\r');
00262         }
00263                 
00264         /* 2: Handle Commands ------------------------------------------------*/
00265         if (go_steps) {
00266             // Stop first
00267             if (cmd_both) {
00268                 for (int m = 0; m < L6470DAISYCHAINSIZE; m++) {
00269                     motors[m]->prepare_hard_stop();
00270                 }
00271                 x_nucleo_ihm02a1->perform_prepared_actions();
00272             } else {
00273                 motors[which_axis]->hard_stop();
00274             }
00275             wait_ms(HARD_STOP_WAIT_MS);
00276             
00277             if (cmd_both) {
00278                 for (int m = 0; m < L6470DAISYCHAINSIZE; m++) {
00279                     if (dir_fwd) motors[m]->prepare_move(StepperMotor::FWD, go_steps);
00280                     else motors[m]->prepare_move(StepperMotor::BWD, go_steps);
00281                 }
00282                 x_nucleo_ihm02a1->perform_prepared_actions();
00283             } else {
00284                 if (dir_fwd) motors[which_axis]->move(StepperMotor::FWD, go_steps);
00285                 else motors[which_axis]->move(StepperMotor::BWD, go_steps);
00286             }
00287  
00288             printf("\r\nGoing %s %d steps\r\n", dir_fwd ? "Forward" : "Backward", go_steps);
00289             /* force prompt display by generating empty command */
00290             tinysh_char_in('\r');
00291             
00292             // Clear flags
00293             go_steps = 0;
00294             cmd_both = false;
00295         }
00296             
00297         if (run_flag) {
00298             // Stop first
00299             if (cmd_both) {
00300                 // stop both at same time
00301                 for (int m = 0; m < L6470DAISYCHAINSIZE; m++) {
00302                     motors[m]->prepare_hard_stop();
00303                 }
00304                 x_nucleo_ihm02a1->perform_prepared_actions();
00305             } else {
00306                 motors[which_axis]->hard_stop();
00307             }
00308             wait_ms(HARD_STOP_WAIT_MS);
00309             
00310             if (cmd_both) {
00311                 for (int m = 0; m < L6470DAISYCHAINSIZE; m++) {
00312                     if (dir_fwd) motors[m]->prepare_run(StepperMotor::FWD, speed_sps);
00313                     else motors[m]->prepare_run(StepperMotor::BWD, speed_sps);
00314                 }
00315                 x_nucleo_ihm02a1->perform_prepared_actions();
00316             } else {
00317                 if (dir_fwd) motors[which_axis]->run(StepperMotor::FWD, speed_sps);
00318                 else motors[which_axis]->run(StepperMotor::BWD, speed_sps);
00319             }
00320 
00321             printf("\r\nRunning\r\n");
00322             /* force prompt display by generating empty command */
00323             tinysh_char_in('\r');
00324 
00325             // Clear flags
00326             run_flag = false;
00327             cmd_both = false;
00328         }
00329 
00330         if (stop_flag) {
00331 
00332             printf("\r\nStopping\r\n");
00333             /* force prompt display by generating empty command */
00334             tinysh_char_in('\r');
00335 
00336             // Hard stop 
00337             if (cmd_both) {
00338                 // stop both at same time
00339                 for (int m = 0; m < L6470DAISYCHAINSIZE; m++) {
00340                     motors[m]->prepare_hard_stop();
00341                 }
00342                 x_nucleo_ihm02a1->perform_prepared_actions();
00343             } else {
00344                 motors[which_axis]->hard_stop();
00345             }
00346             wait_ms(HARD_STOP_WAIT_MS);
00347             
00348             // High Z
00349             if (cmd_both) {
00350                 for (int m = 0; m < L6470DAISYCHAINSIZE; m++) {
00351                     motors[m]->prepare_hard_hiz();
00352                 }
00353                 x_nucleo_ihm02a1->perform_prepared_actions();
00354             } else {
00355                 motors[which_axis]->hard_hiz();
00356             }
00357             
00358             // clear flags
00359             stop_flag = false;
00360             cmd_both = false;
00361         }
00362         
00363     
00364         if (status_flag) {
00365             // Fetch and parse the status register for each motor
00366             for (int m = 0; m < L6470DAISYCHAINSIZE; m++) {
00367                 status_bytes[m] = motors[m]->get_status();
00368                 printf("\r\nStatus reg %d: 0x%02X\r\n", m, status_bytes[m]);
00369                 printf("    STEP-CLOCK MODE:       ");
00370                 printf(status_bytes[m] & 0x8000 ? "SET\r\n" : "NOT SET\r\n");
00371                 printf("    STEP_LOSS_B:           ");
00372                 printf(status_bytes[m] & 0x4000 ? "SET\r\n" : "NOT SET\r\n");
00373                 printf("    STEP_LOSS_A:           ");
00374                 printf(status_bytes[m] & 0x2000 ? "SET\r\n" : "NOT SET\r\n");
00375                 printf("    OVERCURRENT DETECT:    ");
00376                 printf(status_bytes[m] & 0x1000 ? "SET\r\n" : "NOT SET\r\n");
00377                 printf("    THERMAL SHUTDOWN:      ");
00378                 printf(status_bytes[m] & 0x0800 ? "SET\r\n" : "NOT SET\r\n");
00379                 printf("    THERMAL WARN:          ");
00380                 printf(status_bytes[m] & 0x0400 ? "SET\r\n" : "NOT SET\r\n");
00381                 printf("    UNDERVOLTAGE LOCKOUT:  ");
00382                 printf(status_bytes[m] & 0x0200 ? "SET\r\n" : "NOT SET\r\n");
00383                 printf("    WRONG_CMD:             ");
00384                 printf(status_bytes[m] & 0x0100 ? "SET\r\n" : "NOT SET\r\n");
00385                 printf("    NOTPERF_CMD:           ");
00386                 printf(status_bytes[m] & 0x0080 ? "SET\r\n" : "NOT SET\r\n");
00387                 printf("    MOTOR_STATUS:          ");
00388                 if ((status_bytes[m] && 0x0060) >> 5 == 0x00) printf("STOPPED\r\n");
00389                 if ((status_bytes[m] && 0x0060) >> 5 == 0x01) printf("ACCELERATING\r\n");
00390                 if ((status_bytes[m] && 0x0060) >> 5 == 0x10) printf("DECELERATING\r\n");
00391                 if ((status_bytes[m] && 0x0060) >> 5 == 0x11) printf("CONSTANT SPEED\r\n");
00392                 printf("    DIRECTION:             ");
00393                 printf(status_bytes[m] & 0x0010 ? "FWD\r\n" : "REV\r\n");
00394                 printf("    SWITCH TURN-ON EVENT:  ");
00395                 printf(status_bytes[m] & 0x0008 ? "SET\r\n" : "NOT SET\r\n");
00396                 printf("    SWITCH STATUS:         ");
00397                 printf(status_bytes[m] & 0x0004 ? "CLOSED\r\n" : "OPEN\r\n");
00398                 printf("    BUSY:                  ");
00399                 printf(status_bytes[m] & 0x0002 ? "SET\r\n" : "NOT SET\r\n");
00400                 printf("    HI_Z:                  ");
00401                 printf(status_bytes[m] & 0x0001 ? "SET\r\n" : "NOT SET\r\n");
00402             }
00403             // clear flag
00404             status_flag = false;
00405             /* force prompt display by generating empty command */
00406             tinysh_char_in('\r');
00407         }
00408         
00409         /* 3: Fetch new chars for Tinyshell ----------------------------------*/
00410         tinysh_char_in(pc.getc());
00411         
00412     } // end main loop
00413 }