Thomas Byrne / Mbed 2 deprecated HelloWorld_IHM02A1

testing

Dependencies:   mbed tinyshell X_NUCLEO_IHM02A1

main.cpp@31:ebd464b0d0d8, 2021-04-02 (annotated)

Committer:
tom_astranis
Date:
Fri Apr 02 16:01:18 2021 +0000
Revision:
31:ebd464b0d0d8
Parent:
30:a23204ee2be2 
catching up

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Davidroid 0:5148e9486cf2 1 #include "mbed.h"
Davidroid 0:5148e9486cf2 2
Davidroid 0:5148e9486cf2 3 /* Helper header files. */
Davidroid 0:5148e9486cf2 4 #include "DevSPI.h"
Davidroid 0:5148e9486cf2 5
Davidroid 0:5148e9486cf2 6 /* Expansion Board specific header files. */
Davidroid 26:caec5f51abe8 7 #include "XNucleoIHM02A1.h"
Davidroid 0:5148e9486cf2 8
tom_astranis 28:19b25daa7777 9 /* Tinyshell: https://os.mbed.com/users/murilopontes/code/tinyshell/ */
tom_astranis 28:19b25daa7777 10 #include "tinysh.h"
Davidroid 0:5148e9486cf2 11
tom_astranis 30:a23204ee2be2 12 #include <cstdlib>
tom_astranis 30:a23204ee2be2 13 #include <string>
tom_astranis 30:a23204ee2be2 14
Davidroid 0:5148e9486cf2 15 /* Definitions ---------------------------------------------------------------*/
Davidroid 0:5148e9486cf2 16
tom_astranis 27:2abcf13d90a3 17 /* Motor specs */
tom_astranis 27:2abcf13d90a3 18 #define MOTOR_SUPPLY_VOLTAGE_V 12.0
tom_astranis 27:2abcf13d90a3 19 #define STEPS_PER_REV 200.0
tom_astranis 27:2abcf13d90a3 20 #define MAX_PHASE_CURRENT_A 0.67
tom_astranis 27:2abcf13d90a3 21 #define PHASE_RES_OHMS 12.0
tom_astranis 27:2abcf13d90a3 22 #define MOTOR_INITIAL_SPEED_SPS 1000.0
tom_astranis 30:a23204ee2be2 23 #define MOTOR_ACCEL_SPS2 1000.0
tom_astranis 31:ebd464b0d0d8 24 #define MOTOR_MAX_SPEED_SPS 1000.0
tom_astranis 27:2abcf13d90a3 25 #define FULL_STEP_TH_SPS 602.7
tom_astranis 27:2abcf13d90a3 26 #define DUMMY_KVAL_V 3.06
tom_astranis 27:2abcf13d90a3 27 #define BEMF_ICPT_SPS 61.52
tom_astranis 27:2abcf13d90a3 28 #define START_SLOPE 392.1569e-6
tom_astranis 27:2abcf13d90a3 29 #define FINAL_SLOPE 643.1372e-6
tom_astranis 27:2abcf13d90a3 30 #define OCD_TH_MA 600.0
tom_astranis 27:2abcf13d90a3 31 #define STALL_TH_MA 1000.0
tom_astranis 27:2abcf13d90a3 32
tom_astranis 28:19b25daa7777 33 /* Behavioral stuff */
tom_astranis 28:19b25daa7777 34 #define STATUS_LOG_RATE_HZ 1
tom_astranis 28:19b25daa7777 35 #define MIN_LOOP_TIME_MS 1
tom_astranis 28:19b25daa7777 36 #define HARD_STOP_WAIT_MS 100
tom_astranis 28:19b25daa7777 37
Davidroid 0:5148e9486cf2 38 /* Variables -----------------------------------------------------------------*/
Davidroid 0:5148e9486cf2 39
Davidroid 0:5148e9486cf2 40 /* Motor Control Expansion Board. */
Davidroid 26:caec5f51abe8 41 XNucleoIHM02A1 *x_nucleo_ihm02a1;
Davidroid 0:5148e9486cf2 42
Davidroid 9:f35fbeedb8f4 43 /* Initialization parameters of the motors connected to the expansion board. */
davide.aliprandi@st.com 24:d1f487cb02ba 44 L6470_init_t init[L6470DAISYCHAINSIZE] = {
Davidroid 9:f35fbeedb8f4 45 /* First Motor. */
Davidroid 9:f35fbeedb8f4 46 {
tom_astranis 27:2abcf13d90a3 47 MOTOR_SUPPLY_VOLTAGE_V, /* Motor supply voltage in V. */
tom_astranis 27:2abcf13d90a3 48 STEPS_PER_REV, /* Min number of steps per revolution for the motor. */
tom_astranis 27:2abcf13d90a3 49 MAX_PHASE_CURRENT_A, /* Max motor phase voltage in A. */
tom_astranis 27:2abcf13d90a3 50 MAX_PHASE_CURRENT_A * PHASE_RES_OHMS, /* Max motor phase voltage in V. (12 ohms per phase) */
tom_astranis 27:2abcf13d90a3 51 MOTOR_INITIAL_SPEED_SPS, /* Motor initial speed [step/s]. */
tom_astranis 27:2abcf13d90a3 52 MOTOR_ACCEL_SPS2, /* Motor acceleration [step/s^2] (comment for infinite acceleration mode). */
tom_astranis 27:2abcf13d90a3 53 MOTOR_ACCEL_SPS2, /* Motor deceleration [step/s^2] (comment for infinite deceleration mode). */
tom_astranis 27:2abcf13d90a3 54 MOTOR_MAX_SPEED_SPS, /* Motor maximum speed [step/s]. */
tom_astranis 27:2abcf13d90a3 55 0.0, /* Motor minimum speed [step/s]. */
tom_astranis 27:2abcf13d90a3 56 FULL_STEP_TH_SPS, /* Motor full-step speed threshold [step/s]. */
tom_astranis 27:2abcf13d90a3 57 DUMMY_KVAL_V, /* Holding kval [V]. */
tom_astranis 27:2abcf13d90a3 58 DUMMY_KVAL_V, /* Constant speed kval [V]. */
tom_astranis 27:2abcf13d90a3 59 DUMMY_KVAL_V, /* Acceleration starting kval [V]. */
tom_astranis 27:2abcf13d90a3 60 DUMMY_KVAL_V, /* Deceleration starting kval [V]. */
tom_astranis 27:2abcf13d90a3 61 BEMF_ICPT_SPS, /* Intersect speed for bemf compensation curve slope changing [step/s]. */
tom_astranis 27:2abcf13d90a3 62 START_SLOPE, /* Start slope [s/step]. */
tom_astranis 27:2abcf13d90a3 63 FINAL_SLOPE, /* Acceleration final slope [s/step]. */
tom_astranis 27:2abcf13d90a3 64 FINAL_SLOPE, /* Deceleration final slope [s/step]. */
tom_astranis 27:2abcf13d90a3 65 0, /* Thermal compensation factor (range [0, 15]). */
tom_astranis 27:2abcf13d90a3 66 OCD_TH_MA, /* Ocd threshold [ma] (range [375 ma, 6000 ma]). */
tom_astranis 27:2abcf13d90a3 67 STALL_TH_MA, /* Stall threshold [ma] (range [31.25 ma, 4000 ma]). */
tom_astranis 27:2abcf13d90a3 68 StepperMotor::STEP_MODE_FULL, /* Step mode selection. */
tom_astranis 27:2abcf13d90a3 69 0xFF, /* Alarm conditions enable. */
tom_astranis 27:2abcf13d90a3 70 0x2E88 /* Ic configuration. */
Davidroid 9:f35fbeedb8f4 71 },
Davidroid 9:f35fbeedb8f4 72
Davidroid 9:f35fbeedb8f4 73 /* Second Motor. */
Davidroid 9:f35fbeedb8f4 74 {
tom_astranis 27:2abcf13d90a3 75 MOTOR_SUPPLY_VOLTAGE_V, /* Motor supply voltage in V. */
tom_astranis 27:2abcf13d90a3 76 STEPS_PER_REV, /* Min number of steps per revolution for the motor. */
tom_astranis 27:2abcf13d90a3 77 MAX_PHASE_CURRENT_A, /* Max motor phase voltage in A. */
tom_astranis 27:2abcf13d90a3 78 MAX_PHASE_CURRENT_A * PHASE_RES_OHMS, /* Max motor phase voltage in V. (12 ohms per phase) */
tom_astranis 27:2abcf13d90a3 79 MOTOR_INITIAL_SPEED_SPS, /* Motor initial speed [step/s]. */
tom_astranis 27:2abcf13d90a3 80 MOTOR_ACCEL_SPS2, /* Motor acceleration [step/s^2] (comment for infinite acceleration mode). */
tom_astranis 27:2abcf13d90a3 81 MOTOR_ACCEL_SPS2, /* Motor deceleration [step/s^2] (comment for infinite deceleration mode). */
tom_astranis 27:2abcf13d90a3 82 MOTOR_MAX_SPEED_SPS, /* Motor maximum speed [step/s]. */
tom_astranis 27:2abcf13d90a3 83 0.0, /* Motor minimum speed [step/s]. */
tom_astranis 27:2abcf13d90a3 84 FULL_STEP_TH_SPS, /* Motor full-step speed threshold [step/s]. */
tom_astranis 27:2abcf13d90a3 85 DUMMY_KVAL_V, /* Holding kval [V]. */
tom_astranis 27:2abcf13d90a3 86 DUMMY_KVAL_V, /* Constant speed kval [V]. */
tom_astranis 27:2abcf13d90a3 87 DUMMY_KVAL_V, /* Acceleration starting kval [V]. */
tom_astranis 27:2abcf13d90a3 88 DUMMY_KVAL_V, /* Deceleration starting kval [V]. */
tom_astranis 27:2abcf13d90a3 89 BEMF_ICPT_SPS, /* Intersect speed for bemf compensation curve slope changing [step/s]. */
tom_astranis 27:2abcf13d90a3 90 START_SLOPE, /* Start slope [s/step]. */
tom_astranis 27:2abcf13d90a3 91 FINAL_SLOPE, /* Acceleration final slope [s/step]. */
tom_astranis 27:2abcf13d90a3 92 FINAL_SLOPE, /* Deceleration final slope [s/step]. */
tom_astranis 27:2abcf13d90a3 93 0, /* Thermal compensation factor (range [0, 15]). */
tom_astranis 27:2abcf13d90a3 94 OCD_TH_MA, /* Ocd threshold [ma] (range [375 ma, 6000 ma]). */
tom_astranis 27:2abcf13d90a3 95 STALL_TH_MA, /* Stall threshold [ma] (range [31.25 ma, 4000 ma]). */
tom_astranis 27:2abcf13d90a3 96 StepperMotor::STEP_MODE_FULL, /* Step mode selection. */
tom_astranis 27:2abcf13d90a3 97 0xFF, /* Alarm conditions enable. */
tom_astranis 27:2abcf13d90a3 98 0x2E88 /* Ic configuration. */
Davidroid 9:f35fbeedb8f4 99 }
Davidroid 9:f35fbeedb8f4 100 };
Davidroid 9:f35fbeedb8f4 101
tom_astranis 28:19b25daa7777 102 /* Serial Port for console */
tom_astranis 28:19b25daa7777 103 Serial pc(USBTX, USBRX);
tom_astranis 28:19b25daa7777 104
tom_astranis 28:19b25daa7777 105 /* Nasty globals haha */
tom_astranis 28:19b25daa7777 106 bool limit_flag = false;
tom_astranis 28:19b25daa7777 107 bool status_flag = false;
tom_astranis 29:a510e875936d 108 bool run_flag = false;
tom_astranis 29:a510e875936d 109 bool stop_flag = false;
tom_astranis 30:a23204ee2be2 110 bool cmd_both = false;
tom_astranis 30:a23204ee2be2 111 int which_axis = 0;
tom_astranis 30:a23204ee2be2 112 int speed_sps = 0;
tom_astranis 30:a23204ee2be2 113 int go_steps = 0;
tom_astranis 30:a23204ee2be2 114 int dir_fwd = true;
tom_astranis 28:19b25daa7777 115
tom_astranis 28:19b25daa7777 116 /* Tinyshell command handler functions */
tom_astranis 29:a510e875936d 117 void set_status_flag(int argc, char **argv)
tom_astranis 28:19b25daa7777 118 {
tom_astranis 28:19b25daa7777 119 status_flag = true;
tom_astranis 28:19b25daa7777 120 }
tom_astranis 28:19b25daa7777 121
tom_astranis 30:a23204ee2be2 122 // gosteps dir steps (axis)
tom_astranis 30:a23204ee2be2 123 void set_gosteps_flag(int argc, char **argv)
tom_astranis 30:a23204ee2be2 124 {
tom_astranis 30:a23204ee2be2 125 if (argc < 3 || argc > 4) {
tom_astranis 31:ebd464b0d0d8 126 printf("\r\nIncorrect number of arguments for: gosteps dir steps (axis)\r\n");
tom_astranis 30:a23204ee2be2 127 return;
tom_astranis 30:a23204ee2be2 128 }
tom_astranis 30:a23204ee2be2 129
tom_astranis 30:a23204ee2be2 130 dir_fwd = true;
tom_astranis 30:a23204ee2be2 131 if (argv[1] == std::string("bwd")) dir_fwd = false;
tom_astranis 30:a23204ee2be2 132 go_steps = atoi(argv[2]);
tom_astranis 30:a23204ee2be2 133
tom_astranis 30:a23204ee2be2 134 if (argc == 3) {
tom_astranis 30:a23204ee2be2 135 // both axes
tom_astranis 30:a23204ee2be2 136 cmd_both = true;
tom_astranis 30:a23204ee2be2 137 } else {
tom_astranis 30:a23204ee2be2 138 cmd_both = false;
tom_astranis 30:a23204ee2be2 139 which_axis = 0;
tom_astranis 30:a23204ee2be2 140 if (argv[3] == std::string("y")) which_axis = 1;
tom_astranis 30:a23204ee2be2 141 }
tom_astranis 30:a23204ee2be2 142 }
tom_astranis 30:a23204ee2be2 143
tom_astranis 30:a23204ee2be2 144 // run dir speed (axis)
tom_astranis 29:a510e875936d 145 void set_run_flag(int argc, char **argv)
tom_astranis 29:a510e875936d 146 {
tom_astranis 30:a23204ee2be2 147 if (argc < 3 || argc > 4) {
tom_astranis 31:ebd464b0d0d8 148 printf("\r\nIncorrect number of arguments for: run dir steps (axis)\r\n");
tom_astranis 30:a23204ee2be2 149 return;
tom_astranis 30:a23204ee2be2 150 }
tom_astranis 30:a23204ee2be2 151
tom_astranis 29:a510e875936d 152 run_flag = true;
tom_astranis 30:a23204ee2be2 153 dir_fwd = true;
tom_astranis 30:a23204ee2be2 154 if (argv[1] == std::string("bwd")) dir_fwd = false;
tom_astranis 30:a23204ee2be2 155 speed_sps = atoi(argv[2]);
tom_astranis 30:a23204ee2be2 156
tom_astranis 30:a23204ee2be2 157 if (argc == 3) {
tom_astranis 30:a23204ee2be2 158 cmd_both = true;
tom_astranis 30:a23204ee2be2 159 } else {
tom_astranis 30:a23204ee2be2 160 cmd_both = false;
tom_astranis 30:a23204ee2be2 161 which_axis = 0;
tom_astranis 30:a23204ee2be2 162 if (argv[3] == std::string("y")) which_axis = 1;
tom_astranis 30:a23204ee2be2 163 }
tom_astranis 29:a510e875936d 164 }
tom_astranis 29:a510e875936d 165
tom_astranis 30:a23204ee2be2 166 // stop (axis)
tom_astranis 29:a510e875936d 167 void set_stop_flag(int argc, char **argv)
tom_astranis 29:a510e875936d 168 {
tom_astranis 30:a23204ee2be2 169 if (argc > 2) {
tom_astranis 31:ebd464b0d0d8 170 printf("\r\nIncorrect number of arguments for: stop (axis)\r\n");
tom_astranis 30:a23204ee2be2 171 return;
tom_astranis 30:a23204ee2be2 172 }
tom_astranis 30:a23204ee2be2 173
tom_astranis 29:a510e875936d 174 stop_flag = true;
tom_astranis 30:a23204ee2be2 175
tom_astranis 30:a23204ee2be2 176 if (argc == 1) {
tom_astranis 30:a23204ee2be2 177 // both axes
tom_astranis 30:a23204ee2be2 178 cmd_both = true;
tom_astranis 30:a23204ee2be2 179 } else {
tom_astranis 30:a23204ee2be2 180 cmd_both = false;
tom_astranis 30:a23204ee2be2 181 which_axis = 0;
tom_astranis 30:a23204ee2be2 182 if (argv[1] == std::string("y")) which_axis = 1;
tom_astranis 30:a23204ee2be2 183 }
tom_astranis 29:a510e875936d 184 }
tom_astranis 29:a510e875936d 185
tom_astranis 28:19b25daa7777 186 // parent cmd (0 for top) cmd input name, help string, usage string,
tom_astranis 28:19b25daa7777 187 // function to launch, arg when called, next (0 at init), child (0 at init)
tom_astranis 30:a23204ee2be2 188 tinysh_cmd_t print_status_cmd = {0, "status",
tom_astranis 31:ebd464b0d0d8 189 "\r\nPrint content of status registers\r\n",
tom_astranis 30:a23204ee2be2 190 "[no args]", set_status_flag,0,0,0};
tom_astranis 30:a23204ee2be2 191 tinysh_cmd_t go_steps_cmd = {0, "gosteps",
tom_astranis 31:ebd464b0d0d8 192 "\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",
tom_astranis 30:a23204ee2be2 193 "[dir, steps, (axis)]", set_gosteps_flag,0,0,0};
tom_astranis 30:a23204ee2be2 194 tinysh_cmd_t run_cmd = {0, "run",
tom_astranis 31:ebd464b0d0d8 195 "\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",
tom_astranis 30:a23204ee2be2 196 "[dir, speed, (axis)]", set_run_flag,0,0,0};
tom_astranis 30:a23204ee2be2 197 tinysh_cmd_t stop_cmd = {0, "stop",
tom_astranis 31:ebd464b0d0d8 198 "\r\nStop motor, optional axis = 'x' or 'y'; both stopped if omitted\r\n",
tom_astranis 30:a23204ee2be2 199 "[(axis)]", set_stop_flag,0,0,0};
tom_astranis 28:19b25daa7777 200
tom_astranis 28:19b25daa7777 201 /* mandatory tiny shell output function */
tom_astranis 28:19b25daa7777 202 void tinysh_char_out(unsigned char c)
tom_astranis 28:19b25daa7777 203 {
tom_astranis 28:19b25daa7777 204 pc.putc(c);
tom_astranis 28:19b25daa7777 205 }
Davidroid 0:5148e9486cf2 206
Davidroid 0:5148e9486cf2 207 /* Main ----------------------------------------------------------------------*/
Davidroid 0:5148e9486cf2 208
Davidroid 0:5148e9486cf2 209 int main()
Davidroid 0:5148e9486cf2 210 {
Davidroid 1:9f1974b0960d 211 /*----- Initialization. -----*/
tom_astranis 28:19b25daa7777 212 unsigned int status_bytes[L6470DAISYCHAINSIZE];
tom_astranis 28:19b25daa7777 213
tom_astranis 28:19b25daa7777 214 /* Set up tinyshell */
tom_astranis 28:19b25daa7777 215 pc.baud(115200);
tom_astranis 30:a23204ee2be2 216 pc.printf("\r\nMotor controller ready\r\n");
tom_astranis 30:a23204ee2be2 217 tinysh_set_prompt("> ");
tom_astranis 28:19b25daa7777 218
tom_astranis 28:19b25daa7777 219 // Add all tinyshell commands here
tom_astranis 28:19b25daa7777 220 tinysh_add_command(&print_status_cmd);
tom_astranis 29:a510e875936d 221 tinysh_add_command(&run_cmd);
tom_astranis 30:a23204ee2be2 222 tinysh_add_command(&go_steps_cmd);
tom_astranis 29:a510e875936d 223 tinysh_add_command(&stop_cmd);
Davidroid 1:9f1974b0960d 224
Davidroid 2:41eeee48951b 225 /* Initializing SPI bus. */
Davidroid 23:073b26366d03 226 #ifdef TARGET_STM32F429
Davidroid 23:073b26366d03 227 DevSPI dev_spi(D11, D12, D13);
Davidroid 23:073b26366d03 228 #else
Davidroid 3:fd280b953f77 229 DevSPI dev_spi(D11, D12, D3);
Davidroid 23:073b26366d03 230 #endif
Davidroid 0:5148e9486cf2 231
Davidroid 5:3b8e19bbf386 232 /* Initializing Motor Control Expansion Board. */
Davidroid 26:caec5f51abe8 233 x_nucleo_ihm02a1 = new XNucleoIHM02A1(&init[0], &init[1], A4, A5, D4, A2, &dev_spi);
Davidroid 0:5148e9486cf2 234
Davidroid 1:9f1974b0960d 235 /* Building a list of motor control components. */
davide.aliprandi@st.com 24:d1f487cb02ba 236 L6470 **motors = x_nucleo_ihm02a1->get_components();
tom_astranis 29:a510e875936d 237
tom_astranis 28:19b25daa7777 238 // Main loop
tom_astranis 28:19b25daa7777 239 while(1) {
tom_astranis 30:a23204ee2be2 240 /* 1: Check for hardware flags ---------------------------------------*/
tom_astranis 28:19b25daa7777 241 if (limit_flag) {
tom_astranis 29:a510e875936d 242 // clear flag
tom_astranis 29:a510e875936d 243 limit_flag = false;
tom_astranis 29:a510e875936d 244
tom_astranis 28:19b25daa7777 245 // Hard stop
tom_astranis 28:19b25daa7777 246 for (int m = 0; m < L6470DAISYCHAINSIZE; m++) {
tom_astranis 28:19b25daa7777 247 motors[m]->prepare_hard_stop();
tom_astranis 28:19b25daa7777 248 }
tom_astranis 28:19b25daa7777 249 x_nucleo_ihm02a1->perform_prepared_actions();
tom_astranis 28:19b25daa7777 250
tom_astranis 28:19b25daa7777 251 wait_ms(HARD_STOP_WAIT_MS);
tom_astranis 28:19b25daa7777 252
tom_astranis 28:19b25daa7777 253 // High Z
tom_astranis 28:19b25daa7777 254 for (int m = 0; m < L6470DAISYCHAINSIZE; m++) {
tom_astranis 28:19b25daa7777 255 motors[m]->prepare_hard_hiz();
tom_astranis 28:19b25daa7777 256 }
tom_astranis 28:19b25daa7777 257 x_nucleo_ihm02a1->perform_prepared_actions();
tom_astranis 28:19b25daa7777 258
tom_astranis 31:ebd464b0d0d8 259 printf("\r\nReached limit\r\n");
tom_astranis 31:ebd464b0d0d8 260 /* force prompt display by generating empty command */
tom_astranis 31:ebd464b0d0d8 261 tinysh_char_in('\r');
tom_astranis 28:19b25daa7777 262 }
tom_astranis 31:ebd464b0d0d8 263
tom_astranis 31:ebd464b0d0d8 264 /* 2: Handle Commands ------------------------------------------------*/
tom_astranis 30:a23204ee2be2 265 if (go_steps) {
tom_astranis 30:a23204ee2be2 266 // Stop first
tom_astranis 30:a23204ee2be2 267 if (cmd_both) {
tom_astranis 30:a23204ee2be2 268 for (int m = 0; m < L6470DAISYCHAINSIZE; m++) {
tom_astranis 30:a23204ee2be2 269 motors[m]->prepare_hard_stop();
tom_astranis 30:a23204ee2be2 270 }
tom_astranis 30:a23204ee2be2 271 x_nucleo_ihm02a1->perform_prepared_actions();
tom_astranis 30:a23204ee2be2 272 } else {
tom_astranis 30:a23204ee2be2 273 motors[which_axis]->hard_stop();
tom_astranis 30:a23204ee2be2 274 }
tom_astranis 30:a23204ee2be2 275 wait_ms(HARD_STOP_WAIT_MS);
tom_astranis 30:a23204ee2be2 276
tom_astranis 30:a23204ee2be2 277 if (cmd_both) {
tom_astranis 30:a23204ee2be2 278 for (int m = 0; m < L6470DAISYCHAINSIZE; m++) {
tom_astranis 30:a23204ee2be2 279 if (dir_fwd) motors[m]->prepare_move(StepperMotor::FWD, go_steps);
tom_astranis 30:a23204ee2be2 280 else motors[m]->prepare_move(StepperMotor::BWD, go_steps);
tom_astranis 30:a23204ee2be2 281 }
tom_astranis 30:a23204ee2be2 282 x_nucleo_ihm02a1->perform_prepared_actions();
tom_astranis 30:a23204ee2be2 283 } else {
tom_astranis 30:a23204ee2be2 284 if (dir_fwd) motors[which_axis]->move(StepperMotor::FWD, go_steps);
tom_astranis 30:a23204ee2be2 285 else motors[which_axis]->move(StepperMotor::BWD, go_steps);
tom_astranis 30:a23204ee2be2 286 }
tom_astranis 30:a23204ee2be2 287
tom_astranis 31:ebd464b0d0d8 288 printf("\r\nGoing %s %d steps\r\n", dir_fwd ? "Forward" : "Backward", go_steps);
tom_astranis 31:ebd464b0d0d8 289 /* force prompt display by generating empty command */
tom_astranis 31:ebd464b0d0d8 290 tinysh_char_in('\r');
tom_astranis 30:a23204ee2be2 291
tom_astranis 30:a23204ee2be2 292 // Clear flags
tom_astranis 30:a23204ee2be2 293 go_steps = 0;
tom_astranis 30:a23204ee2be2 294 cmd_both = false;
tom_astranis 30:a23204ee2be2 295 }
tom_astranis 30:a23204ee2be2 296
tom_astranis 29:a510e875936d 297 if (run_flag) {
tom_astranis 30:a23204ee2be2 298 // Stop first
tom_astranis 30:a23204ee2be2 299 if (cmd_both) {
tom_astranis 30:a23204ee2be2 300 // stop both at same time
tom_astranis 30:a23204ee2be2 301 for (int m = 0; m < L6470DAISYCHAINSIZE; m++) {
tom_astranis 30:a23204ee2be2 302 motors[m]->prepare_hard_stop();
tom_astranis 30:a23204ee2be2 303 }
tom_astranis 30:a23204ee2be2 304 x_nucleo_ihm02a1->perform_prepared_actions();
tom_astranis 30:a23204ee2be2 305 } else {
tom_astranis 30:a23204ee2be2 306 motors[which_axis]->hard_stop();
tom_astranis 30:a23204ee2be2 307 }
tom_astranis 30:a23204ee2be2 308 wait_ms(HARD_STOP_WAIT_MS);
tom_astranis 30:a23204ee2be2 309
tom_astranis 30:a23204ee2be2 310 if (cmd_both) {
tom_astranis 30:a23204ee2be2 311 for (int m = 0; m < L6470DAISYCHAINSIZE; m++) {
tom_astranis 30:a23204ee2be2 312 if (dir_fwd) motors[m]->prepare_run(StepperMotor::FWD, speed_sps);
tom_astranis 30:a23204ee2be2 313 else motors[m]->prepare_run(StepperMotor::BWD, speed_sps);
tom_astranis 30:a23204ee2be2 314 }
tom_astranis 30:a23204ee2be2 315 x_nucleo_ihm02a1->perform_prepared_actions();
tom_astranis 30:a23204ee2be2 316 } else {
tom_astranis 30:a23204ee2be2 317 if (dir_fwd) motors[which_axis]->run(StepperMotor::FWD, speed_sps);
tom_astranis 30:a23204ee2be2 318 else motors[which_axis]->run(StepperMotor::BWD, speed_sps);
tom_astranis 30:a23204ee2be2 319 }
tom_astranis 30:a23204ee2be2 320
tom_astranis 31:ebd464b0d0d8 321 printf("\r\nRunning\r\n");
tom_astranis 31:ebd464b0d0d8 322 /* force prompt display by generating empty command */
tom_astranis 31:ebd464b0d0d8 323 tinysh_char_in('\r');
tom_astranis 30:a23204ee2be2 324
tom_astranis 30:a23204ee2be2 325 // Clear flags
tom_astranis 29:a510e875936d 326 run_flag = false;
tom_astranis 30:a23204ee2be2 327 cmd_both = false;
tom_astranis 29:a510e875936d 328 }
tom_astranis 29:a510e875936d 329
tom_astranis 29:a510e875936d 330 if (stop_flag) {
tom_astranis 30:a23204ee2be2 331
tom_astranis 31:ebd464b0d0d8 332 printf("\r\nStopping\r\n");
tom_astranis 31:ebd464b0d0d8 333 /* force prompt display by generating empty command */
tom_astranis 31:ebd464b0d0d8 334 tinysh_char_in('\r');
tom_astranis 30:a23204ee2be2 335
tom_astranis 29:a510e875936d 336 // Hard stop
tom_astranis 30:a23204ee2be2 337 if (cmd_both) {
tom_astranis 30:a23204ee2be2 338 // stop both at same time
tom_astranis 30:a23204ee2be2 339 for (int m = 0; m < L6470DAISYCHAINSIZE; m++) {
tom_astranis 30:a23204ee2be2 340 motors[m]->prepare_hard_stop();
tom_astranis 30:a23204ee2be2 341 }
tom_astranis 30:a23204ee2be2 342 x_nucleo_ihm02a1->perform_prepared_actions();
tom_astranis 30:a23204ee2be2 343 } else {
tom_astranis 30:a23204ee2be2 344 motors[which_axis]->hard_stop();
tom_astranis 29:a510e875936d 345 }
tom_astranis 29:a510e875936d 346 wait_ms(HARD_STOP_WAIT_MS);
tom_astranis 29:a510e875936d 347
tom_astranis 29:a510e875936d 348 // High Z
tom_astranis 30:a23204ee2be2 349 if (cmd_both) {
tom_astranis 30:a23204ee2be2 350 for (int m = 0; m < L6470DAISYCHAINSIZE; m++) {
tom_astranis 30:a23204ee2be2 351 motors[m]->prepare_hard_hiz();
tom_astranis 30:a23204ee2be2 352 }
tom_astranis 30:a23204ee2be2 353 x_nucleo_ihm02a1->perform_prepared_actions();
tom_astranis 30:a23204ee2be2 354 } else {
tom_astranis 30:a23204ee2be2 355 motors[which_axis]->hard_hiz();
tom_astranis 29:a510e875936d 356 }
tom_astranis 30:a23204ee2be2 357
tom_astranis 30:a23204ee2be2 358 // clear flags
tom_astranis 30:a23204ee2be2 359 stop_flag = false;
tom_astranis 30:a23204ee2be2 360 cmd_both = false;
tom_astranis 29:a510e875936d 361 }
tom_astranis 30:a23204ee2be2 362
tom_astranis 29:a510e875936d 363
tom_astranis 28:19b25daa7777 364 if (status_flag) {
tom_astranis 28:19b25daa7777 365 // Fetch and parse the status register for each motor
tom_astranis 28:19b25daa7777 366 for (int m = 0; m < L6470DAISYCHAINSIZE; m++) {
tom_astranis 28:19b25daa7777 367 status_bytes[m] = motors[m]->get_status();
tom_astranis 30:a23204ee2be2 368 printf("\r\nStatus reg %d: 0x%02X\r\n", m, status_bytes[m]);
tom_astranis 28:19b25daa7777 369 printf(" STEP-CLOCK MODE: ");
tom_astranis 28:19b25daa7777 370 printf(status_bytes[m] & 0x8000 ? "SET\r\n" : "NOT SET\r\n");
tom_astranis 28:19b25daa7777 371 printf(" STEP_LOSS_B: ");
tom_astranis 28:19b25daa7777 372 printf(status_bytes[m] & 0x4000 ? "SET\r\n" : "NOT SET\r\n");
tom_astranis 28:19b25daa7777 373 printf(" STEP_LOSS_A: ");
tom_astranis 28:19b25daa7777 374 printf(status_bytes[m] & 0x2000 ? "SET\r\n" : "NOT SET\r\n");
tom_astranis 28:19b25daa7777 375 printf(" OVERCURRENT DETECT: ");
tom_astranis 28:19b25daa7777 376 printf(status_bytes[m] & 0x1000 ? "SET\r\n" : "NOT SET\r\n");
tom_astranis 28:19b25daa7777 377 printf(" THERMAL SHUTDOWN: ");
tom_astranis 28:19b25daa7777 378 printf(status_bytes[m] & 0x0800 ? "SET\r\n" : "NOT SET\r\n");
tom_astranis 28:19b25daa7777 379 printf(" THERMAL WARN: ");
tom_astranis 28:19b25daa7777 380 printf(status_bytes[m] & 0x0400 ? "SET\r\n" : "NOT SET\r\n");
tom_astranis 28:19b25daa7777 381 printf(" UNDERVOLTAGE LOCKOUT: ");
tom_astranis 28:19b25daa7777 382 printf(status_bytes[m] & 0x0200 ? "SET\r\n" : "NOT SET\r\n");
tom_astranis 28:19b25daa7777 383 printf(" WRONG_CMD: ");
tom_astranis 28:19b25daa7777 384 printf(status_bytes[m] & 0x0100 ? "SET\r\n" : "NOT SET\r\n");
tom_astranis 28:19b25daa7777 385 printf(" NOTPERF_CMD: ");
tom_astranis 28:19b25daa7777 386 printf(status_bytes[m] & 0x0080 ? "SET\r\n" : "NOT SET\r\n");
tom_astranis 28:19b25daa7777 387 printf(" MOTOR_STATUS: ");
tom_astranis 28:19b25daa7777 388 if ((status_bytes[m] && 0x0060) >> 5 == 0x00) printf("STOPPED\r\n");
tom_astranis 28:19b25daa7777 389 if ((status_bytes[m] && 0x0060) >> 5 == 0x01) printf("ACCELERATING\r\n");
tom_astranis 28:19b25daa7777 390 if ((status_bytes[m] && 0x0060) >> 5 == 0x10) printf("DECELERATING\r\n");
tom_astranis 28:19b25daa7777 391 if ((status_bytes[m] && 0x0060) >> 5 == 0x11) printf("CONSTANT SPEED\r\n");
tom_astranis 28:19b25daa7777 392 printf(" DIRECTION: ");
tom_astranis 28:19b25daa7777 393 printf(status_bytes[m] & 0x0010 ? "FWD\r\n" : "REV\r\n");
tom_astranis 28:19b25daa7777 394 printf(" SWITCH TURN-ON EVENT: ");
tom_astranis 28:19b25daa7777 395 printf(status_bytes[m] & 0x0008 ? "SET\r\n" : "NOT SET\r\n");
tom_astranis 28:19b25daa7777 396 printf(" SWITCH STATUS: ");
tom_astranis 28:19b25daa7777 397 printf(status_bytes[m] & 0x0004 ? "CLOSED\r\n" : "OPEN\r\n");
tom_astranis 28:19b25daa7777 398 printf(" BUSY: ");
tom_astranis 28:19b25daa7777 399 printf(status_bytes[m] & 0x0002 ? "SET\r\n" : "NOT SET\r\n");
tom_astranis 28:19b25daa7777 400 printf(" HI_Z: ");
tom_astranis 28:19b25daa7777 401 printf(status_bytes[m] & 0x0001 ? "SET\r\n" : "NOT SET\r\n");
tom_astranis 28:19b25daa7777 402 }
tom_astranis 30:a23204ee2be2 403 // clear flag
tom_astranis 30:a23204ee2be2 404 status_flag = false;
tom_astranis 31:ebd464b0d0d8 405 /* force prompt display by generating empty command */
tom_astranis 31:ebd464b0d0d8 406 tinysh_char_in('\r');
tom_astranis 28:19b25daa7777 407 }
tom_astranis 28:19b25daa7777 408
tom_astranis 31:ebd464b0d0d8 409 /* 3: Fetch new chars for Tinyshell ----------------------------------*/
tom_astranis 31:ebd464b0d0d8 410 tinysh_char_in(pc.getc());
tom_astranis 31:ebd464b0d0d8 411
tom_astranis 28:19b25daa7777 412 } // end main loop
tom_astranis 30:a23204ee2be2 413 }