GroupA / Mbed 2 deprecated Lab_6_WaG

Lab 6 code.

Dependencies:   mbed

Fork of WaG by GroupA

stepper.cpp@47:0547118e348e, 2018-03-30 (annotated)

Committer:
phn10
Date:
Fri Mar 30 20:26:19 2018 +0000
Revision:
47:0547118e348e
Parent:
45:54d18ab80fd1
Child:
48:d612de6880b0
finish function stp_calibrate() in part 9 of lab 6. Haven't tested yet. The constant PTTHRESH is undefined because the laser power is weak and cannot impose 2.2V on sensor

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 18:0e281922212c 20
spm71 21:88f9f280931b 21 extern DigitalIn jog_ccw;
spm71 21:88f9f280931b 22 extern DigitalIn jog_cw;
spm71 36:ad2b3d6f0e5a 23 extern DigitalIn my_button;
spm71 36:ad2b3d6f0e5a 24 extern DigitalIn cal_button;
spm71 41:9b293b14b845 25 extern DigitalIn home_sensor;
spm71 22:09dd6977576b 26 extern Serial pc;
spm71 20:d23bcd97f2c5 27
spm71 20:d23bcd97f2c5 28 int stp_cur_pos;
phn10 44:4c2ba5bbba67 29 int stp_sensor_pos[TGT_SENSOR_QUAN];
spm71 18:0e281922212c 30
spm71 18:0e281922212c 31 extern spi_cfg drv8806 {
spm71 18:0e281922212c 32 SPI_DRV8806_ID,
spm71 18:0e281922212c 33 STP_DRV8806_NCS,
spm71 20:d23bcd97f2c5 34 DRV8806_SPI_MODE,
spm71 20:d23bcd97f2c5 35 DRV8806_SPI_FREQ,
spm71 20:d23bcd97f2c5 36 DRV8806_SPI_NO_BITS,
spm71 18:0e281922212c 37 };
spm71 18:0e281922212c 38
spm71 18:0e281922212c 39 /*
spm71 18:0e281922212c 40 * void stp_init();
spm71 18:0e281922212c 41 * Description: initializes stepper values to unkown
spm71 18:0e281922212c 42 *
spm71 18:0e281922212c 43 * Inputs:
spm71 18:0e281922212c 44 * Parameters: void
spm71 18:0e281922212c 45 * Globals:
spm71 18:0e281922212c 46 *
spm71 18:0e281922212c 47 * Outputs:
spm71 18:0e281922212c 48 * Returns: void
spm71 18:0e281922212c 49 */
spm71 18:0e281922212c 50 void stp_init() {
spm71 20:d23bcd97f2c5 51 stp_cur_pos = STP_POS_UNKN;
spm71 43:3b7f326aa096 52 jog_cw.mode(PullUp);
spm71 43:3b7f326aa096 53 jog_ccw.mode(PullUp);
spm71 43:3b7f326aa096 54 cal_button.mode(PullUp);
spm71 43:3b7f326aa096 55 home_sensor.mode(PullUp);
spm71 18:0e281922212c 56 for (int i = 1; i <= NUM_SENSORS; i++) {
spm71 43:3b7f326aa096 57 //stp_sensor_pos[i] = STP_POS_UNKN;
spm71 18:0e281922212c 58 }
spm71 18:0e281922212c 59 }
spm71 18:0e281922212c 60
spm71 18:0e281922212c 61 /*
spm71 18:0e281922212c 62 * void stp_step(int direction);
spm71 18:0e281922212c 63 * Description: turns the stepper motor clockwise or counter-clockwise
spm71 18:0e281922212c 64 *
spm71 18:0e281922212c 65 * Inputs:
spm71 18:0e281922212c 66 * Parameters:
phn10 47:0547118e348e 67 * int direction: STP_CW for clock wise and STP_CCW for counter clock wise
spm71 18:0e281922212c 68 * Globals:
spm71 18:0e281922212c 69 *
spm71 18:0e281922212c 70 * Outputs:
spm71 18:0e281922212c 71 * Returns: void
spm71 18:0e281922212c 72 */
spm71 18:0e281922212c 73 void stp_step(int direction) {
spm71 20:d23bcd97f2c5 74
spm71 20:d23bcd97f2c5 75 //static int cur_pos = stp_cur_pos;
spm71 20:d23bcd97f2c5 76 static int turn[4] = {0x03, 0x06, 0x0c, 0x09};
spm71 20:d23bcd97f2c5 77 if (direction == STP_CW) {
spm71 22:09dd6977576b 78 for (int i = 0; i < 4; i++) {
spm71 41:9b293b14b845 79 wait(MOTOR_DELAY);
spm71 22:09dd6977576b 80 //pc.printf("i = %d\n", i);
spm71 20:d23bcd97f2c5 81 spi_send(drv8806, turn[i]);
spm71 22:09dd6977576b 82 }
spm71 43:3b7f326aa096 83 wait(0.015);
spm71 20:d23bcd97f2c5 84 }
spm71 20:d23bcd97f2c5 85 else if (direction == STP_CCW) {
spm71 22:09dd6977576b 86 for (int i = 3; i >= 0; i--) {
spm71 41:9b293b14b845 87 wait(MOTOR_DELAY);
spm71 22:09dd6977576b 88 //pc.printf("i = %d\n", i);
spm71 20:d23bcd97f2c5 89 spi_send(drv8806, turn[i]);
spm71 22:09dd6977576b 90 }
spm71 43:3b7f326aa096 91 wait(0.015);
spm71 20:d23bcd97f2c5 92 }
spm71 41:9b293b14b845 93 wait(MOTOR_DELAY);
spm71 36:ad2b3d6f0e5a 94 }
spm71 36:ad2b3d6f0e5a 95
spm71 36:ad2b3d6f0e5a 96 /*
spm71 36:ad2b3d6f0e5a 97 * void step_test();
spm71 36:ad2b3d6f0e5a 98 * Description: tests the stepper motor
spm71 36:ad2b3d6f0e5a 99 *
spm71 36:ad2b3d6f0e5a 100 * Inputs:
spm71 36:ad2b3d6f0e5a 101 * Parameters:
spm71 36:ad2b3d6f0e5a 102 * Globals:
spm71 36:ad2b3d6f0e5a 103 *
spm71 36:ad2b3d6f0e5a 104 * Outputs:
spm71 36:ad2b3d6f0e5a 105 * Returns: void
spm71 36:ad2b3d6f0e5a 106 */
spm71 36:ad2b3d6f0e5a 107 void step_test() {
spm71 43:3b7f326aa096 108 stp_init();
spm71 41:9b293b14b845 109 while (uti_chk_ubutton() == 0);
spm71 41:9b293b14b845 110 pc.printf("test begin\n");
spm71 36:ad2b3d6f0e5a 111 while(1) {
spm71 36:ad2b3d6f0e5a 112 if (jog_ccw == 0) {
spm71 42:6cba679a4ee4 113 if (stp_cur_pos != 0) {
spm71 42:6cba679a4ee4 114 stp_step(STP_CCW);
spm71 42:6cba679a4ee4 115 if (stp_cur_pos != STP_POS_UNKN)
spm71 42:6cba679a4ee4 116 stp_cur_pos--;
spm71 42:6cba679a4ee4 117 }
spm71 43:3b7f326aa096 118
spm71 36:ad2b3d6f0e5a 119 }
spm71 36:ad2b3d6f0e5a 120 if (jog_cw == 0) {
spm71 42:6cba679a4ee4 121 if (stp_cur_pos <= 400) {
spm71 42:6cba679a4ee4 122 stp_step(STP_CW);
spm71 42:6cba679a4ee4 123 if (stp_cur_pos != STP_POS_UNKN)
spm71 42:6cba679a4ee4 124 stp_cur_pos++;
spm71 42:6cba679a4ee4 125 }
spm71 42:6cba679a4ee4 126 else
spm71 42:6cba679a4ee4 127 while(1);
spm71 36:ad2b3d6f0e5a 128 }
spm71 36:ad2b3d6f0e5a 129 if (cal_button == 0) {
spm71 36:ad2b3d6f0e5a 130 stp_find_home();
spm71 36:ad2b3d6f0e5a 131 }
spm71 36:ad2b3d6f0e5a 132 }
spm71 36:ad2b3d6f0e5a 133 }
spm71 36:ad2b3d6f0e5a 134
spm71 41:9b293b14b845 135 /*
spm71 41:9b293b14b845 136 * void stp_find_home();
spm71 41:9b293b14b845 137 * Description: uses the stepper motor and home sensor to find home
spm71 41:9b293b14b845 138 *
spm71 41:9b293b14b845 139 * Inputs:
spm71 41:9b293b14b845 140 * Parameters:
spm71 41:9b293b14b845 141 * Globals:
spm71 41:9b293b14b845 142 *
spm71 41:9b293b14b845 143 * Outputs:
spm71 41:9b293b14b845 144 * Returns: void
spm71 41:9b293b14b845 145 */
spm71 36:ad2b3d6f0e5a 146 void stp_find_home() {
spm71 41:9b293b14b845 147 int count = 0;
spm71 41:9b293b14b845 148 int half_count = 0;
spm71 41:9b293b14b845 149 stp_cur_pos = STP_POS_UNKN;
spm71 41:9b293b14b845 150 //pc.printf("Home sensor is currently %d\n", home_sensor.read());
spm71 41:9b293b14b845 151 if (home_sensor == 0) {
spm71 41:9b293b14b845 152 for(int i = 0; i < 100; i++)
spm71 41:9b293b14b845 153 stp_step(STP_CW);
spm71 41:9b293b14b845 154 if (home_sensor == 0) {
spm71 41:9b293b14b845 155 pc.printf("Error, home sensor not functioning. Home sensor is currently %d\n", home_sensor.read());
spm71 41:9b293b14b845 156 while(1);
spm71 41:9b293b14b845 157 }
spm71 41:9b293b14b845 158 }
spm71 41:9b293b14b845 159 while (home_sensor.read() != 0) {
spm71 41:9b293b14b845 160 stp_step(STP_CCW);
spm71 41:9b293b14b845 161 }
spm71 41:9b293b14b845 162 while (home_sensor.read() != 1) {
spm71 41:9b293b14b845 163 stp_step(STP_CCW);
spm71 41:9b293b14b845 164 count++;
spm71 41:9b293b14b845 165 }
spm71 41:9b293b14b845 166 half_count = count/2;
spm71 41:9b293b14b845 167 for(int i = 0; i < half_count; i++)
spm71 41:9b293b14b845 168 stp_step(STP_CW);
spm71 41:9b293b14b845 169 stp_cur_pos = 0;
spm71 41:9b293b14b845 170 pc.printf("Home found.\n");
phn10 44:4c2ba5bbba67 171 }
phn10 44:4c2ba5bbba67 172
phn10 44:4c2ba5bbba67 173
phn10 44:4c2ba5bbba67 174 /*
phn10 44:4c2ba5bbba67 175 * void stp_calibrate(int station, float * sensor_values, int * cal_status);
phn10 44:4c2ba5bbba67 176 * Description: uses the stepper motor and home sensor to find home
phn10 44:4c2ba5bbba67 177 *
phn10 44:4c2ba5bbba67 178 * Inputs:
phn10 44:4c2ba5bbba67 179 * Parameters:
phn10 47:0547118e348e 180 * int station: STATION_A or STATION_B
phn10 47:0547118e348e 181 * float *sensor_value: array of float holds 16 sensor values
phn10 47:0547118e348e 182 * int *cal_status: pointer to int variable that will hold calibration status
phn10 44:4c2ba5bbba67 183 * Globals:
phn10 44:4c2ba5bbba67 184 *
phn10 44:4c2ba5bbba67 185 * Outputs:
phn10 44:4c2ba5bbba67 186 * Returns: void
phn10 44:4c2ba5bbba67 187 */
phn10 47:0547118e348e 188 void stp_calibrate(int station, float * sensor_values, int * cal_status){
phn10 47:0547118e348e 189 int sensor_no = 0;
phn10 47:0547118e348e 190 if (station == STATION_B) sensor = 8;
phn10 47:0547118e348e 191
phn10 47:0547118e348e 192 // find home position
phn10 47:0547118e348e 193 stp_find_home();
phn10 47:0547118e348e 194
phn10 47:0547118e348e 195 // turn laser on
phn10 47:0547118e348e 196 lzr_on();
phn10 47:0547118e348e 197
phn10 47:0547118e348e 198 for (int i = 0; i < TGT_SENSOR_QUAN; i++) {
phn10 47:0547118e348e 199 // scan all 16 sensors into sensor_values array
phn10 47:0547118e348e 200 ana_scan_mux(sensor_values, TGT_SENSOR_QUAN * 2);
phn10 47:0547118e348e 201
phn10 47:0547118e348e 202 // keep turning stepper motor clock wise until it points to a PT sensor
phn10 47:0547118e348e 203 while (sensor_values[sensor_no + i] < PTTHRESH) {
phn10 47:0547118e348e 204 // turn CW one step
phn10 47:0547118e348e 205 stp_step(STP_CW);
phn10 47:0547118e348e 206
phn10 47:0547118e348e 207 wait(0.005);
phn10 47:0547118e348e 208
phn10 47:0547118e348e 209 // scan all PT sensors again
phn10 47:0547118e348e 210 ana_scan_mux(sensor_values, TGT_SENSOR_QUAN * 2);
phn10 47:0547118e348e 211 }
phn10 47:0547118e348e 212 // found the sensor, save it's position
phn10 47:0547118e348e 213 stp_sensor_pos[i] = stp_cur_pos;
phn10 47:0547118e348e 214 }
phn10 47:0547118e348e 215 // found position of all 8 sensors
phn10 47:0547118e348e 216
phn10 47:0547118e348e 217 // go back home
phn10 47:0547118e348e 218 stp_find_home();
phn10 47:0547118e348e 219
phn10 47:0547118e348e 220 *cal_status = CALIBRATED;
phn10 47:0547118e348e 221
phn10 47:0547118e348e 222 // turn laser off
phn10 47:0547118e348e 223 lzr_off();
phn10 47:0547118e348e 224
phn10 47:0547118e348e 225 // for debugging: print positions of all 8 sensors here
phn10 47:0547118e348e 226 for (int i = 0; i < TGT_SENSOR_QUAN; i++) {
phn10 47:0547118e348e 227 pc.printf("PT %d: %d ", i, stp_sensor_pos[i]);
phn10 47:0547118e348e 228 }
phn10 47:0547118e348e 229 }