GroupA / Mbed 2 deprecated WaG_final

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

Fork of Lab_6_WaG by GroupA

stepper.cpp@69:1b7271bd4a75, 2018-04-23 (annotated)

Committer:
spm71
Date:
Mon Apr 23 19:52:06 2018 +0000
Revision:
69:1b7271bd4a75
Parent:
62:b73067127fd6
Child:
70:a1990ee177b3
Timer fix

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
phn10 59:6b3a52d87465 5 *
phn10 59:6b3a52d87465 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 56:048b30c9f2a1 11 * Last Modified: 04/06/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 35:ad2b3d6f0e5a 26 extern DigitalIn my_button;
spm71 35: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 *
phn10 59:6b3a52d87465 47 * Inputs:
spm71 18:0e281922212c 48 * Parameters: void
spm71 18:0e281922212c 49 * Globals:
phn10 59:6b3a52d87465 50 *
spm71 18:0e281922212c 51 * Outputs:
spm71 18:0e281922212c 52 * Returns: void
spm71 18:0e281922212c 53 */
phn10 59:6b3a52d87465 54 void stp_init()
phn10 59:6b3a52d87465 55 {
spm71 20:d23bcd97f2c5 56 stp_cur_pos = STP_POS_UNKN;
spm71 43:3b7f326aa096 57 jog_cw.mode(PullUp);
spm71 43:3b7f326aa096 58 jog_ccw.mode(PullUp);
spm71 43:3b7f326aa096 59 cal_button.mode(PullUp);
spm71 43:3b7f326aa096 60 home_sensor.mode(PullUp);
spm71 46:85c4b722baa7 61 for (int i = 1; i <= TGT_SENSOR_QUAN; i++) {
spm71 62:b73067127fd6 62 stp_sensor_pos[i] = STP_POS_UNKN;
spm71 18:0e281922212c 63 }
spm71 18:0e281922212c 64 }
spm71 18:0e281922212c 65
spm71 18:0e281922212c 66 /*
spm71 18:0e281922212c 67 * void stp_step(int direction);
spm71 18:0e281922212c 68 * Description: turns the stepper motor clockwise or counter-clockwise
spm71 18:0e281922212c 69 *
phn10 59:6b3a52d87465 70 * Inputs:
spm71 18:0e281922212c 71 * Parameters:
phn10 47:0547118e348e 72 * int direction: STP_CW for clock wise and STP_CCW for counter clock wise
spm71 18:0e281922212c 73 * Globals:
phn10 59:6b3a52d87465 74 *
spm71 18:0e281922212c 75 * Outputs:
spm71 18:0e281922212c 76 * Returns: void
spm71 18:0e281922212c 77 */
phn10 59:6b3a52d87465 78 void stp_step(int direction)
phn10 59:6b3a52d87465 79 {
phn10 59:6b3a52d87465 80
spm71 20:d23bcd97f2c5 81 //static int cur_pos = stp_cur_pos;
spm71 20:d23bcd97f2c5 82 static int turn[4] = {0x03, 0x06, 0x0c, 0x09};
spm71 53:389fe53b2642 83 static int index = 0;
spm71 20:d23bcd97f2c5 84 if (direction == STP_CW) {
phn10 59:6b3a52d87465 85
spm71 53:389fe53b2642 86 if (stp_cur_pos <= 400) {
phn10 59:6b3a52d87465 87 if (stp_cur_pos != STP_POS_UNKN) {
phn10 59:6b3a52d87465 88 //pc.printf("current position = %d\n", stp_cur_pos);
phn10 59:6b3a52d87465 89 stp_cur_pos++;
phn10 59:6b3a52d87465 90 }
phn10 59:6b3a52d87465 91 } else {
spm71 52:8987e38851e5 92 pc.printf("Error: Cannot turn past maximum position. See stp_step() function.\n");
spm71 49:80d4ffabec16 93 while(1);
spm71 49:80d4ffabec16 94 }
phn10 59:6b3a52d87465 95
spm71 53:389fe53b2642 96 if (index == 3)
spm71 53:389fe53b2642 97 index = 0;
phn10 59:6b3a52d87465 98 else
spm71 53:389fe53b2642 99 index++;
spm71 53:389fe53b2642 100 wait(MOTOR_DELAY);
spm71 53:389fe53b2642 101 spi_send(drv8806, turn[index]);
spm71 50:e3a03bc1e1a6 102 wait(TURN_DELAY);
phn10 59:6b3a52d87465 103 } else if (direction == STP_CCW) {
phn10 59:6b3a52d87465 104
spm71 49:80d4ffabec16 105 if (stp_cur_pos != 0) {
spm71 49:80d4ffabec16 106 if (stp_cur_pos != STP_POS_UNKN)
spm71 49:80d4ffabec16 107 stp_cur_pos--;
phn10 59:6b3a52d87465 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 }
phn10 59:6b3a52d87465 113
spm71 53:389fe53b2642 114 if (index == 0)
spm71 53:389fe53b2642 115 index = 3;
spm71 53:389fe53b2642 116 else
spm71 53:389fe53b2642 117 index--;
spm71 53:389fe53b2642 118 wait(MOTOR_DELAY);
spm71 53:389fe53b2642 119 spi_send(drv8806, turn[index]);
spm71 50:e3a03bc1e1a6 120 wait(TURN_DELAY);
spm71 20:d23bcd97f2c5 121 }
spm71 41:9b293b14b845 122 wait(MOTOR_DELAY);
spm71 35:ad2b3d6f0e5a 123 }
spm71 35:ad2b3d6f0e5a 124
spm71 35:ad2b3d6f0e5a 125 /*
spm71 35:ad2b3d6f0e5a 126 * void step_test();
spm71 35:ad2b3d6f0e5a 127 * Description: tests the stepper motor
spm71 35:ad2b3d6f0e5a 128 *
phn10 59:6b3a52d87465 129 * Inputs:
spm71 35:ad2b3d6f0e5a 130 * Parameters:
spm71 35:ad2b3d6f0e5a 131 * Globals:
phn10 59:6b3a52d87465 132 *
spm71 35:ad2b3d6f0e5a 133 * Outputs:
spm71 35:ad2b3d6f0e5a 134 * Returns: void
spm71 35:ad2b3d6f0e5a 135 */
phn10 59:6b3a52d87465 136 void step_test()
phn10 59:6b3a52d87465 137 {
spm71 43:3b7f326aa096 138 stp_init();
spm71 41:9b293b14b845 139 while (uti_chk_ubutton() == 0);
spm71 46:85c4b722baa7 140 pc.printf("step motor test begin\n");
spm71 35:ad2b3d6f0e5a 141 while(1) {
phn10 59:6b3a52d87465 142 if (jog_ccw == 0) {
phn10 59:6b3a52d87465 143 stp_step(STP_CCW);
phn10 59:6b3a52d87465 144 }
phn10 59:6b3a52d87465 145 if (jog_cw == 0) {
phn10 59:6b3a52d87465 146 stp_step(STP_CW);
spm71 35:ad2b3d6f0e5a 147 }
phn10 59:6b3a52d87465 148 if (cal_button == 0) {
phn10 59:6b3a52d87465 149 stp_find_home();
phn10 59:6b3a52d87465 150 }
phn10 59:6b3a52d87465 151 if (uti_chk_ubutton() == 1)
phn10 59:6b3a52d87465 152 break;
phn10 59:6b3a52d87465 153 }
spm71 35:ad2b3d6f0e5a 154 }
spm71 35:ad2b3d6f0e5a 155
spm71 41:9b293b14b845 156 /*
spm71 41:9b293b14b845 157 * void stp_find_home();
spm71 41:9b293b14b845 158 * Description: uses the stepper motor and home sensor to find home
spm71 41:9b293b14b845 159 *
phn10 59:6b3a52d87465 160 * Inputs:
spm71 41:9b293b14b845 161 * Parameters:
spm71 41:9b293b14b845 162 * Globals:
phn10 59:6b3a52d87465 163 *
spm71 41:9b293b14b845 164 * Outputs:
spm71 41:9b293b14b845 165 * Returns: void
spm71 41:9b293b14b845 166 */
phn10 59:6b3a52d87465 167 void stp_find_home()
phn10 59:6b3a52d87465 168 {
spm71 69:1b7271bd4a75 169 stp_init();
spm71 41:9b293b14b845 170 int count = 0;
spm71 41:9b293b14b845 171 int half_count = 0;
spm71 41:9b293b14b845 172 stp_cur_pos = STP_POS_UNKN;
spm71 41:9b293b14b845 173 //pc.printf("Home sensor is currently %d\n", home_sensor.read());
spm71 41:9b293b14b845 174 if (home_sensor == 0) {
spm71 41:9b293b14b845 175 for(int i = 0; i < 100; i++)
spm71 41:9b293b14b845 176 stp_step(STP_CW);
spm71 41:9b293b14b845 177 if (home_sensor == 0) {
spm71 52:8987e38851e5 178 pc.printf("Error: Home sensor not functioning. See stp_find_home() function.\n", home_sensor.read());
spm71 41:9b293b14b845 179 while(1);
spm71 41:9b293b14b845 180 }
spm71 41:9b293b14b845 181 }
spm71 41:9b293b14b845 182 while (home_sensor.read() != 0) {
spm71 41:9b293b14b845 183 stp_step(STP_CCW);
spm71 41:9b293b14b845 184 }
spm71 41:9b293b14b845 185 while (home_sensor.read() != 1) {
spm71 41:9b293b14b845 186 stp_step(STP_CCW);
spm71 41:9b293b14b845 187 count++;
spm71 41:9b293b14b845 188 }
spm71 41:9b293b14b845 189 half_count = count/2;
spm71 41:9b293b14b845 190 for(int i = 0; i < half_count; i++)
spm71 41:9b293b14b845 191 stp_step(STP_CW);
spm71 41:9b293b14b845 192 stp_cur_pos = 0;
spm71 41:9b293b14b845 193 pc.printf("Home found.\n");
phn10 44:4c2ba5bbba67 194 }
phn10 44:4c2ba5bbba67 195
phn10 44:4c2ba5bbba67 196
phn10 44:4c2ba5bbba67 197 /*
phn10 44:4c2ba5bbba67 198 * void stp_calibrate(int station, float * sensor_values, int * cal_status);
phn10 44:4c2ba5bbba67 199 * Description: uses the stepper motor and home sensor to find home
phn10 44:4c2ba5bbba67 200 *
phn10 59:6b3a52d87465 201 * Inputs:
phn10 44:4c2ba5bbba67 202 * Parameters:
phn10 47:0547118e348e 203 * int station: STATION_A or STATION_B
phn10 47:0547118e348e 204 * float *sensor_value: array of float holds 16 sensor values
phn10 47:0547118e348e 205 * int *cal_status: pointer to int variable that will hold calibration status
phn10 44:4c2ba5bbba67 206 * Globals:
phn10 59:6b3a52d87465 207 *
phn10 44:4c2ba5bbba67 208 * Outputs:
phn10 44:4c2ba5bbba67 209 * Returns: void
phn10 44:4c2ba5bbba67 210 */
phn10 59:6b3a52d87465 211 void stp_calibrate(int station, float * sensor_values, int * cal_status)
phn10 59:6b3a52d87465 212 {
spm71 49:80d4ffabec16 213 while (uti_chk_ubutton() == 0);
spm71 58:69f9a4607a16 214 pc.printf("Calibration test begin...\n");
spm71 49:80d4ffabec16 215 pc.printf("step 9 test begin\n");
phn10 47:0547118e348e 216 int sensor_no = 0;
spm71 56:048b30c9f2a1 217 if (station == STATION_A) sensor_no = 8;
spm71 69:1b7271bd4a75 218 if (station == STATION_B) sensor_no = 8;
phn10 59:6b3a52d87465 219
phn10 59:6b3a52d87465 220
spm71 53:389fe53b2642 221 pc.printf("sensor_no: %d\n", sensor_no);
phn10 59:6b3a52d87465 222
spm71 49:80d4ffabec16 223 // find home position
phn10 47:0547118e348e 224 stp_find_home();
phn10 59:6b3a52d87465 225
phn10 59:6b3a52d87465 226
phn10 47:0547118e348e 227 // turn laser on
spm71 49:80d4ffabec16 228 wait(1);
phn10 47:0547118e348e 229 lzr_on();
phn10 59:6b3a52d87465 230
phn10 47:0547118e348e 231 for (int i = 0; i < TGT_SENSOR_QUAN; i++) {
phn10 47:0547118e348e 232 // scan all 16 sensors into sensor_values array
phn10 47:0547118e348e 233 ana_scan_mux(sensor_values, TGT_SENSOR_QUAN * 2);
phn10 59:6b3a52d87465 234
phn10 47:0547118e348e 235 // keep turning stepper motor clock wise until it points to a PT sensor
spm71 53:389fe53b2642 236 while (sensor_values[sensor_no + i] * 3.3f < PTTHRESH) {
phn10 47:0547118e348e 237 // turn CW one step
phn10 47:0547118e348e 238 stp_step(STP_CW);
phn10 59:6b3a52d87465 239
phn10 47:0547118e348e 240 wait(0.005);
phn10 59:6b3a52d87465 241
phn10 47:0547118e348e 242 // scan all PT sensors again
phn10 47:0547118e348e 243 ana_scan_mux(sensor_values, TGT_SENSOR_QUAN * 2);
phn10 59:6b3a52d87465 244
spm71 51:1eb60f0d2f03 245 for (int j = 0; j < TGT_SENSOR_QUAN; j++) {
spm71 53:389fe53b2642 246 if (sensor_values[sensor_no + j] * 3.3f > PTTHRESH) {
phn10 59:6b3a52d87465 247 pc.printf("PT %d: %f\n", sensor_no + j, sensor_values[sensor_no + j] * 3.3f);
phn10 59:6b3a52d87465 248 wait(0.02);
spm71 53:389fe53b2642 249 }
spm71 51:1eb60f0d2f03 250 }
phn10 47:0547118e348e 251 }
spm71 69:1b7271bd4a75 252 // found the sensor, save its position
phn10 47:0547118e348e 253 stp_sensor_pos[i] = stp_cur_pos;
phn10 47:0547118e348e 254 }
phn10 47:0547118e348e 255 // found position of all 8 sensors
phn10 59:6b3a52d87465 256
phn10 47:0547118e348e 257 // go back home
phn10 47:0547118e348e 258 stp_find_home();
phn10 59:6b3a52d87465 259
phn10 47:0547118e348e 260 *cal_status = CALIBRATED;
phn10 59:6b3a52d87465 261
phn10 47:0547118e348e 262 // turn laser off
phn10 47:0547118e348e 263 lzr_off();
phn10 59:6b3a52d87465 264
phn10 47:0547118e348e 265 // for debugging: print positions of all 8 sensors here
phn10 47:0547118e348e 266 for (int i = 0; i < TGT_SENSOR_QUAN; i++) {
phn10 47:0547118e348e 267 pc.printf("PT %d: %d ", i, stp_sensor_pos[i]);
phn10 59:6b3a52d87465 268 }
spm71 58:69f9a4607a16 269 pc.printf("\nCalibration complete.\n");
spm71 52:8987e38851e5 270 }
spm71 52:8987e38851e5 271
spm71 52:8987e38851e5 272 /*
spm71 52:8987e38851e5 273 * void repeatability_test(itn sensor_position, int cal_status);
phn10 59:6b3a52d87465 274 * Description: repeatability test in part 10 of lab 6. The function will point to laser to
phn10 59:6b3a52d87465 275 * the location of the specified sensor_position arguemnt. The function will
spm71 52:8987e38851e5 276 * return error when the calibration status is NOT_CALIBRATED
spm71 52:8987e38851e5 277 *
phn10 59:6b3a52d87465 278 * Inputs:
spm71 52:8987e38851e5 279 * Parameters:
spm71 54:d5a95bc8ffb0 280 * int sensor_position: the position of sensor that the laser will point to (range from 0 to 7)
phn10 59:6b3a52d87465 281 * int cal_status: calibration status.
spm71 52:8987e38851e5 282 * Globals:
phn10 59:6b3a52d87465 283 *
spm71 52:8987e38851e5 284 * Outputs:
spm71 52:8987e38851e5 285 * Returns: void
spm71 52:8987e38851e5 286 */
phn10 59:6b3a52d87465 287 void repeatability_test(int sensor_position, int cal_status)
phn10 59:6b3a52d87465 288 {
spm71 52:8987e38851e5 289 int num_steps = 0; // number of steps the stepper motor needed to move from current position
phn10 59:6b3a52d87465 290
phn10 55:1a25dd75e309 291 //pc.printf("stp_cur_pos: %d\t stp_sensor_pos[sensor_position]: %d\t sensor_position: %d\n", stp_cur_pos, stp_sensor_pos[sensor_position], sensor_position);
phn10 55:1a25dd75e309 292 pc.printf("cal_status: %d\n", cal_status);
phn10 59:6b3a52d87465 293 // if the system is not calibrated
phn10 55:1a25dd75e309 294 if (cal_status == NOT_CALIBRATED) {
spm71 52:8987e38851e5 295 pc.printf("Error: The system is not calibrated. See repeatibility_test() function .\n");
phn10 55:1a25dd75e309 296 while (1);
phn10 59:6b3a52d87465 297 } else {
spm71 52:8987e38851e5 298 // if the current position is less than the position of sepcified sensor
spm71 52:8987e38851e5 299 if (stp_cur_pos < stp_sensor_pos[sensor_position]) {
spm71 52:8987e38851e5 300 num_steps = stp_sensor_pos[sensor_position] - stp_cur_pos;
phn10 59:6b3a52d87465 301
spm71 54:d5a95bc8ffb0 302 for (int i = 0; i < num_steps; i++) {
phn10 59:6b3a52d87465 303 stp_step(STP_CW);
spm71 52:8987e38851e5 304 }
spm71 52:8987e38851e5 305 } else {
spm71 52:8987e38851e5 306 num_steps = stp_cur_pos - stp_sensor_pos[sensor_position];
phn10 59:6b3a52d87465 307
spm71 54:d5a95bc8ffb0 308 for (int i = 0; i < num_steps; i++) {
phn10 59:6b3a52d87465 309 stp_step(STP_CCW);
spm71 52:8987e38851e5 310 }
spm71 52:8987e38851e5 311 }
spm71 52:8987e38851e5 312 }
spm71 58:69f9a4607a16 313 }
spm71 58:69f9a4607a16 314
spm71 58:69f9a4607a16 315 /*
spm71 58:69f9a4607a16 316 * void turn_to_target(int target);
spm71 58:69f9a4607a16 317 * Description: turns the stepper to the designated target
spm71 58:69f9a4607a16 318 *
spm71 58:69f9a4607a16 319 * Inputs:
spm71 58:69f9a4607a16 320 * Parameters:
spm71 58:69f9a4607a16 321 * int target: location of target
spm71 58:69f9a4607a16 322 * Globals:
spm71 58:69f9a4607a16 323 *
spm71 58:69f9a4607a16 324 * Outputs:
spm71 58:69f9a4607a16 325 * Returns: void
spm71 58:69f9a4607a16 326 */
spm71 58:69f9a4607a16 327 void turn_to_target(int target) {
spm71 58:69f9a4607a16 328 if (target < stp_cur_pos)
spm71 69:1b7271bd4a75 329 while (target <= stp_cur_pos)
spm71 69:1b7271bd4a75 330 stp_step(STP_CCW);
spm71 58:69f9a4607a16 331 else if (target > stp_cur_pos)
spm71 69:1b7271bd4a75 332 while (target >= stp_cur_pos)
spm71 69:1b7271bd4a75 333 stp_step(STP_CW);
phn10 47:0547118e348e 334 }