GroupA / Mbed 2 deprecated Lab_6_WaG

Lab 6 code.

Dependencies:   mbed

Fork of WaG by GroupA

stepper.cpp@52:8987e38851e5, 2018-04-03 (annotated)

Committer:
spm71
Date:
Tue Apr 03 22:32:41 2018 +0000
Revision:
52:8987e38851e5
Parent:
51:1eb60f0d2f03
Child:
53:389fe53b2642
add repeatability_test() function and part 10 test

Who changed what in which revision?

UserRevisionLine numberNew contents of line
spm71 18:0e281922212c 1 /******************************************************************************
spm71 18:0e281922212c 2 * EECS 397
spm71 18:0e281922212c 3 *
spm71 42:6cba679a4ee4 4 * Assignment Name: Lab 6: WaG
spm71 18:0e281922212c 5 *
spm71 18:0e281922212c 6 * Authors: Sam Morrison and Phong Nguyen
spm71 18:0e281922212c 7 * File name: stepper.cpp
spm71 18:0e281922212c 8 * Purpose: Driver for stepper motor
spm71 18:0e281922212c 9 *
spm71 18:0e281922212c 10 * Created: 03/02/2018
spm71 42:6cba679a4ee4 11 * Last Modified: 03/29/2018
spm71 18:0e281922212c 12 *
spm71 18:0e281922212c 13 ******************************************************************************/
spm71 18:0e281922212c 14
spm71 18:0e281922212c 15 #include "mbed.h"
spm71 18:0e281922212c 16 #include "io_pins.h"
spm71 18:0e281922212c 17 #include "spi.h"
spm71 18:0e281922212c 18 #include "stepper.h"
spm71 41:9b293b14b845 19 #include "utility.h"
spm71 49:80d4ffabec16 20 #include "laser.h"
spm71 49:80d4ffabec16 21 #include "analog.h"
spm71 49:80d4ffabec16 22 #include "wag.h"
spm71 18:0e281922212c 23
spm71 21:88f9f280931b 24 extern DigitalIn jog_ccw;
spm71 21:88f9f280931b 25 extern DigitalIn jog_cw;
spm71 36:ad2b3d6f0e5a 26 extern DigitalIn my_button;
spm71 36:ad2b3d6f0e5a 27 extern DigitalIn cal_button;
spm71 41:9b293b14b845 28 extern DigitalIn home_sensor;
spm71 22:09dd6977576b 29 extern Serial pc;
spm71 49:80d4ffabec16 30 extern DigitalOut laser;
spm71 52:8987e38851e5 31 extern int stp_sensor_pos[TGT_SENSOR_QUAN];
spm71 20:d23bcd97f2c5 32
spm71 49:80d4ffabec16 33 int stp_cur_pos = STP_POS_UNKN;
spm71 18:0e281922212c 34
spm71 18:0e281922212c 35 extern spi_cfg drv8806 {
spm71 18:0e281922212c 36 SPI_DRV8806_ID,
spm71 18:0e281922212c 37 STP_DRV8806_NCS,
spm71 20:d23bcd97f2c5 38 DRV8806_SPI_MODE,
spm71 20:d23bcd97f2c5 39 DRV8806_SPI_FREQ,
spm71 20:d23bcd97f2c5 40 DRV8806_SPI_NO_BITS,
spm71 18:0e281922212c 41 };
spm71 18:0e281922212c 42
spm71 18:0e281922212c 43 /*
spm71 18:0e281922212c 44 * void stp_init();
spm71 18:0e281922212c 45 * Description: initializes stepper values to unkown
spm71 18:0e281922212c 46 *
spm71 18:0e281922212c 47 * Inputs:
spm71 18:0e281922212c 48 * Parameters: void
spm71 18:0e281922212c 49 * Globals:
spm71 18:0e281922212c 50 *
spm71 18:0e281922212c 51 * Outputs:
spm71 18:0e281922212c 52 * Returns: void
spm71 18:0e281922212c 53 */
spm71 18:0e281922212c 54 void stp_init() {
spm71 20:d23bcd97f2c5 55 stp_cur_pos = STP_POS_UNKN;
spm71 43:3b7f326aa096 56 jog_cw.mode(PullUp);
spm71 43:3b7f326aa096 57 jog_ccw.mode(PullUp);
spm71 43:3b7f326aa096 58 cal_button.mode(PullUp);
spm71 43:3b7f326aa096 59 home_sensor.mode(PullUp);
spm71 46:85c4b722baa7 60 for (int i = 1; i <= TGT_SENSOR_QUAN; i++) {
spm71 43:3b7f326aa096 61 //stp_sensor_pos[i] = STP_POS_UNKN;
spm71 18:0e281922212c 62 }
spm71 18:0e281922212c 63 }
spm71 18:0e281922212c 64
spm71 18:0e281922212c 65 /*
spm71 18:0e281922212c 66 * void stp_step(int direction);
spm71 18:0e281922212c 67 * Description: turns the stepper motor clockwise or counter-clockwise
spm71 18:0e281922212c 68 *
spm71 18:0e281922212c 69 * Inputs:
spm71 18:0e281922212c 70 * Parameters:
phn10 47:0547118e348e 71 * int direction: STP_CW for clock wise and STP_CCW for counter clock wise
spm71 18:0e281922212c 72 * Globals:
spm71 18:0e281922212c 73 *
spm71 18:0e281922212c 74 * Outputs:
spm71 18:0e281922212c 75 * Returns: void
spm71 18:0e281922212c 76 */
spm71 18:0e281922212c 77 void stp_step(int direction) {
spm71 20:d23bcd97f2c5 78
spm71 20:d23bcd97f2c5 79 //static int cur_pos = stp_cur_pos;
spm71 20:d23bcd97f2c5 80 static int turn[4] = {0x03, 0x06, 0x0c, 0x09};
spm71 20:d23bcd97f2c5 81 if (direction == STP_CW) {
spm71 49:80d4ffabec16 82
spm71 50:e3a03bc1e1a6 83 if (stp_cur_pos <= 200) {
spm71 50:e3a03bc1e1a6 84 if (stp_cur_pos != STP_POS_UNKN){
spm71 50:e3a03bc1e1a6 85 //pc.printf("current position = %d\n", stp_cur_pos);
spm71 49:80d4ffabec16 86 stp_cur_pos++;
spm71 50:e3a03bc1e1a6 87 }
spm71 49:80d4ffabec16 88 }
spm71 49:80d4ffabec16 89 else {
spm71 52:8987e38851e5 90 pc.printf("Error: Cannot turn past maximum position. See stp_step() function.\n");
spm71 49:80d4ffabec16 91 while(1);
spm71 49:80d4ffabec16 92 }
spm71 49:80d4ffabec16 93
spm71 49:80d4ffabec16 94
spm71 22:09dd6977576b 95 for (int i = 0; i < 4; i++) {
spm71 41:9b293b14b845 96 wait(MOTOR_DELAY);
spm71 22:09dd6977576b 97 //pc.printf("i = %d\n", i);
spm71 20:d23bcd97f2c5 98 spi_send(drv8806, turn[i]);
spm71 22:09dd6977576b 99 }
spm71 50:e3a03bc1e1a6 100 wait(TURN_DELAY);
spm71 20:d23bcd97f2c5 101 }
spm71 20:d23bcd97f2c5 102 else if (direction == STP_CCW) {
spm71 49:80d4ffabec16 103
spm71 49:80d4ffabec16 104 if (stp_cur_pos != 0) {
spm71 49:80d4ffabec16 105 if (stp_cur_pos != STP_POS_UNKN)
spm71 49:80d4ffabec16 106 stp_cur_pos--;
spm71 49:80d4ffabec16 107 }
spm71 49:80d4ffabec16 108 else {
spm71 52:8987e38851e5 109 pc.printf("Error: Cannot turn past home position. See stp_step() function.\n");
spm71 49:80d4ffabec16 110 wait(0.5);
spm71 49:80d4ffabec16 111 return;
spm71 49:80d4ffabec16 112 }
spm71 49:80d4ffabec16 113
spm71 22:09dd6977576b 114 for (int i = 3; i >= 0; i--) {
spm71 41:9b293b14b845 115 wait(MOTOR_DELAY);
spm71 22:09dd6977576b 116 //pc.printf("i = %d\n", i);
spm71 20:d23bcd97f2c5 117 spi_send(drv8806, turn[i]);
spm71 22:09dd6977576b 118 }
spm71 50:e3a03bc1e1a6 119 wait(TURN_DELAY);
spm71 20:d23bcd97f2c5 120 }
spm71 41:9b293b14b845 121 wait(MOTOR_DELAY);
spm71 36:ad2b3d6f0e5a 122 }
spm71 36:ad2b3d6f0e5a 123
spm71 36:ad2b3d6f0e5a 124 /*
spm71 36:ad2b3d6f0e5a 125 * void step_test();
spm71 36:ad2b3d6f0e5a 126 * Description: tests the stepper motor
spm71 36:ad2b3d6f0e5a 127 *
spm71 36:ad2b3d6f0e5a 128 * Inputs:
spm71 36:ad2b3d6f0e5a 129 * Parameters:
spm71 36:ad2b3d6f0e5a 130 * Globals:
spm71 36:ad2b3d6f0e5a 131 *
spm71 36:ad2b3d6f0e5a 132 * Outputs:
spm71 36:ad2b3d6f0e5a 133 * Returns: void
spm71 36:ad2b3d6f0e5a 134 */
spm71 36:ad2b3d6f0e5a 135 void step_test() {
spm71 43:3b7f326aa096 136 stp_init();
spm71 41:9b293b14b845 137 while (uti_chk_ubutton() == 0);
spm71 46:85c4b722baa7 138 pc.printf("step motor test begin\n");
spm71 36:ad2b3d6f0e5a 139 while(1) {
spm71 36:ad2b3d6f0e5a 140 if (jog_ccw == 0) {
spm71 49:80d4ffabec16 141 stp_step(STP_CCW);
spm71 36:ad2b3d6f0e5a 142 }
spm71 36:ad2b3d6f0e5a 143 if (jog_cw == 0) {
spm71 50:e3a03bc1e1a6 144 stp_step(STP_CW);
spm71 36:ad2b3d6f0e5a 145 }
spm71 36:ad2b3d6f0e5a 146 if (cal_button == 0) {
spm71 36:ad2b3d6f0e5a 147 stp_find_home();
spm71 36:ad2b3d6f0e5a 148 }
spm71 46:85c4b722baa7 149 if (uti_chk_ubutton() == 1)
spm71 46:85c4b722baa7 150 break;
spm71 36:ad2b3d6f0e5a 151 }
spm71 36:ad2b3d6f0e5a 152 }
spm71 36:ad2b3d6f0e5a 153
spm71 41:9b293b14b845 154 /*
spm71 41:9b293b14b845 155 * void stp_find_home();
spm71 41:9b293b14b845 156 * Description: uses the stepper motor and home sensor to find home
spm71 41:9b293b14b845 157 *
spm71 41:9b293b14b845 158 * Inputs:
spm71 41:9b293b14b845 159 * Parameters:
spm71 41:9b293b14b845 160 * Globals:
spm71 41:9b293b14b845 161 *
spm71 41:9b293b14b845 162 * Outputs:
spm71 41:9b293b14b845 163 * Returns: void
spm71 41:9b293b14b845 164 */
spm71 36:ad2b3d6f0e5a 165 void stp_find_home() {
spm71 41:9b293b14b845 166 int count = 0;
spm71 41:9b293b14b845 167 int half_count = 0;
spm71 41:9b293b14b845 168 stp_cur_pos = STP_POS_UNKN;
spm71 41:9b293b14b845 169 //pc.printf("Home sensor is currently %d\n", home_sensor.read());
spm71 41:9b293b14b845 170 if (home_sensor == 0) {
spm71 41:9b293b14b845 171 for(int i = 0; i < 100; i++)
spm71 41:9b293b14b845 172 stp_step(STP_CW);
spm71 41:9b293b14b845 173 if (home_sensor == 0) {
spm71 52:8987e38851e5 174 pc.printf("Error: Home sensor not functioning. See stp_find_home() function.\n", home_sensor.read());
spm71 41:9b293b14b845 175 while(1);
spm71 41:9b293b14b845 176 }
spm71 41:9b293b14b845 177 }
spm71 41:9b293b14b845 178 while (home_sensor.read() != 0) {
spm71 41:9b293b14b845 179 stp_step(STP_CCW);
spm71 41:9b293b14b845 180 }
spm71 41:9b293b14b845 181 while (home_sensor.read() != 1) {
spm71 41:9b293b14b845 182 stp_step(STP_CCW);
spm71 41:9b293b14b845 183 count++;
spm71 41:9b293b14b845 184 }
spm71 41:9b293b14b845 185 half_count = count/2;
spm71 41:9b293b14b845 186 for(int i = 0; i < half_count; i++)
spm71 41:9b293b14b845 187 stp_step(STP_CW);
spm71 41:9b293b14b845 188 stp_cur_pos = 0;
spm71 41:9b293b14b845 189 pc.printf("Home found.\n");
phn10 44:4c2ba5bbba67 190 }
phn10 44:4c2ba5bbba67 191
phn10 44:4c2ba5bbba67 192
phn10 44:4c2ba5bbba67 193 /*
phn10 44:4c2ba5bbba67 194 * void stp_calibrate(int station, float * sensor_values, int * cal_status);
phn10 44:4c2ba5bbba67 195 * Description: uses the stepper motor and home sensor to find home
phn10 44:4c2ba5bbba67 196 *
phn10 44:4c2ba5bbba67 197 * Inputs:
phn10 44:4c2ba5bbba67 198 * Parameters:
phn10 47:0547118e348e 199 * int station: STATION_A or STATION_B
phn10 47:0547118e348e 200 * float *sensor_value: array of float holds 16 sensor values
phn10 47:0547118e348e 201 * int *cal_status: pointer to int variable that will hold calibration status
phn10 44:4c2ba5bbba67 202 * Globals:
phn10 44:4c2ba5bbba67 203 *
phn10 44:4c2ba5bbba67 204 * Outputs:
phn10 44:4c2ba5bbba67 205 * Returns: void
phn10 44:4c2ba5bbba67 206 */
phn10 47:0547118e348e 207 void stp_calibrate(int station, float * sensor_values, int * cal_status){
spm71 49:80d4ffabec16 208 while (uti_chk_ubutton() == 0);
spm71 49:80d4ffabec16 209 pc.printf("step 9 test begin\n");
spm71 49:80d4ffabec16 210
phn10 47:0547118e348e 211 int sensor_no = 0;
spm71 49:80d4ffabec16 212 if (station == STATION_B) sensor_no = 8;
phn10 47:0547118e348e 213
spm71 49:80d4ffabec16 214 // find home position
phn10 47:0547118e348e 215 stp_find_home();
phn10 47:0547118e348e 216
spm71 51:1eb60f0d2f03 217
phn10 47:0547118e348e 218 // turn laser on
spm71 49:80d4ffabec16 219 wait(1);
phn10 47:0547118e348e 220 lzr_on();
phn10 47:0547118e348e 221
phn10 47:0547118e348e 222 for (int i = 0; i < TGT_SENSOR_QUAN; i++) {
phn10 47:0547118e348e 223 // scan all 16 sensors into sensor_values array
phn10 47:0547118e348e 224 ana_scan_mux(sensor_values, TGT_SENSOR_QUAN * 2);
phn10 47:0547118e348e 225
phn10 47:0547118e348e 226 // keep turning stepper motor clock wise until it points to a PT sensor
phn10 47:0547118e348e 227 while (sensor_values[sensor_no + i] < PTTHRESH) {
phn10 47:0547118e348e 228 // turn CW one step
phn10 47:0547118e348e 229 stp_step(STP_CW);
phn10 47:0547118e348e 230
phn10 47:0547118e348e 231 wait(0.005);
phn10 47:0547118e348e 232
phn10 47:0547118e348e 233 // scan all PT sensors again
phn10 47:0547118e348e 234 ana_scan_mux(sensor_values, TGT_SENSOR_QUAN * 2);
spm71 51:1eb60f0d2f03 235
spm71 51:1eb60f0d2f03 236 for (int j = 0; j < TGT_SENSOR_QUAN; j++) {
spm71 51:1eb60f0d2f03 237 if (sensor_values[sensor_no + j] > PTTHRESH)
spm71 51:1eb60f0d2f03 238 pc.printf("PT %d: %f ", sensor_no + j, sensor_values[sensor_no + j]);
spm71 51:1eb60f0d2f03 239 }
spm71 51:1eb60f0d2f03 240 pc.printf("\n");
phn10 47:0547118e348e 241 }
phn10 47:0547118e348e 242 // found the sensor, save it's position
phn10 47:0547118e348e 243 stp_sensor_pos[i] = stp_cur_pos;
spm71 51:1eb60f0d2f03 244 //pc.printf("Sensor %d = %d\n", i, stp_sensor_pos[i]);
phn10 47:0547118e348e 245 }
phn10 47:0547118e348e 246 // found position of all 8 sensors
phn10 47:0547118e348e 247
phn10 47:0547118e348e 248 // go back home
phn10 47:0547118e348e 249 stp_find_home();
phn10 47:0547118e348e 250
phn10 47:0547118e348e 251 *cal_status = CALIBRATED;
phn10 47:0547118e348e 252
phn10 47:0547118e348e 253 // turn laser off
phn10 47:0547118e348e 254 lzr_off();
phn10 47:0547118e348e 255
phn10 47:0547118e348e 256 // for debugging: print positions of all 8 sensors here
phn10 47:0547118e348e 257 for (int i = 0; i < TGT_SENSOR_QUAN; i++) {
phn10 47:0547118e348e 258 pc.printf("PT %d: %d ", i, stp_sensor_pos[i]);
phn10 47:0547118e348e 259 }
spm71 52:8987e38851e5 260 }
spm71 52:8987e38851e5 261
spm71 52:8987e38851e5 262 /*
spm71 52:8987e38851e5 263 * void repeatability_test(itn sensor_position, int cal_status);
spm71 52:8987e38851e5 264 * Description: repeatability test in part 10 of lab 6. The function will point to laser to
spm71 52:8987e38851e5 265 * the location of the specified sensor_position arguemnt. The function will
spm71 52:8987e38851e5 266 * return error when the calibration status is NOT_CALIBRATED
spm71 52:8987e38851e5 267 *
spm71 52:8987e38851e5 268 * Inputs:
spm71 52:8987e38851e5 269 * Parameters:
spm71 52:8987e38851e5 270 * int sensor_position: the position of sensor that the laser will point to (range from 0 to 15)
spm71 52:8987e38851e5 271 * int cal_status: calibration status.
spm71 52:8987e38851e5 272 * Globals:
spm71 52:8987e38851e5 273 *
spm71 52:8987e38851e5 274 * Outputs:
spm71 52:8987e38851e5 275 * Returns: void
spm71 52:8987e38851e5 276 */
spm71 52:8987e38851e5 277 void repeatability_test(int sensor_position, int cal_status) {
spm71 52:8987e38851e5 278 int num_steps = 0; // number of steps the stepper motor needed to move from current position
spm71 52:8987e38851e5 279
spm71 52:8987e38851e5 280 // if the system is not calibrated
spm71 52:8987e38851e5 281 if (cal_status == NOT_CALIBRATED)
spm71 52:8987e38851e5 282 pc.printf("Error: The system is not calibrated. See repeatibility_test() function .\n");
spm71 52:8987e38851e5 283 else {
spm71 52:8987e38851e5 284 // if the current position is less than the position of sepcified sensor
spm71 52:8987e38851e5 285 if (stp_cur_pos < stp_sensor_pos[sensor_position]) {
spm71 52:8987e38851e5 286 num_steps = stp_sensor_pos[sensor_position] - stp_cur_pos;
spm71 52:8987e38851e5 287
spm71 52:8987e38851e5 288 for (int i = 0; num_steps; i++) {
spm71 52:8987e38851e5 289 stp_step(STP_CW);
spm71 52:8987e38851e5 290 }
spm71 52:8987e38851e5 291 } else {
spm71 52:8987e38851e5 292 num_steps = stp_cur_pos - stp_sensor_pos[sensor_position];
spm71 52:8987e38851e5 293
spm71 52:8987e38851e5 294 for (int i = 0; num_steps; i++) {
spm71 52:8987e38851e5 295 stp_step(STP_CCW);
spm71 52:8987e38851e5 296 }
spm71 52:8987e38851e5 297 }
spm71 52:8987e38851e5 298 }
phn10 47:0547118e348e 299 }