stepper.cpp

00001 /******************************************************************************
00002 * EECS 397
00003 *
00004 * Assignment Name: Lab 6: WaG
00005 *
00006 * Authors: Sam Morrison and Phong Nguyen
00007 * File name: stepper.cpp
00008 * Purpose: Driver for stepper motor
00009 *
00010 * Created: 03/02/2018
00011 * Last Modified: 04/06/2018
00012 *
00013 ******************************************************************************/
00014 
00015 #include "mbed.h"
00016 #include "io_pins.h"
00017 #include "spi.h"
00018 #include "stepper.h"
00019 #include "utility.h"
00020 #include "laser.h"
00021 #include "analog.h"
00022 #include "wag.h"
00023 
00024 extern DigitalIn jog_ccw;
00025 extern DigitalIn jog_cw;
00026 extern DigitalIn my_button;
00027 extern DigitalIn cal_button;
00028 extern DigitalIn home_sensor;
00029 extern Serial pc;
00030 extern DigitalOut laser;
00031 extern int stp_sensor_pos[TGT_SENSOR_QUAN];
00032 
00033 int stp_cur_pos = STP_POS_UNKN;
00034 
00035 extern spi_cfg drv8806 {
00036     SPI_DRV8806_ID,
00037     STP_DRV8806_NCS,
00038     DRV8806_SPI_MODE,
00039     DRV8806_SPI_FREQ,
00040     DRV8806_SPI_NO_BITS,
00041 };
00042 
00043 /*
00044  * void stp_init();
00045  * Description: initializes stepper values to unkown
00046  *
00047  * Inputs:
00048  *      Parameters: void
00049  *      Globals:
00050  *
00051  * Outputs:
00052  *      Returns: void
00053 */
00054 void stp_init()
00055 {
00056     stp_cur_pos = STP_POS_UNKN;
00057     jog_cw.mode(PullUp);
00058     jog_ccw.mode(PullUp);
00059     cal_button.mode(PullUp);
00060     home_sensor.mode(PullUp);
00061     for (int i = 1; i <= TGT_SENSOR_QUAN; i++) {
00062         stp_sensor_pos[i] = STP_POS_UNKN;
00063     }
00064 }
00065 
00066 /*
00067  * void stp_step(int direction);
00068  * Description: turns the stepper motor clockwise or counter-clockwise
00069  *
00070  * Inputs:
00071  *      Parameters:
00072  *          int direction: STP_CW for clock wise and STP_CCW for counter clock wise
00073  *      Globals:
00074  *
00075  * Outputs:
00076  *      Returns: void
00077 */
00078 void stp_step(int direction)
00079 {
00080 
00081     //static int cur_pos = stp_cur_pos;
00082     static int turn[4] = {0x03, 0x06, 0x0c, 0x09};
00083     static int index = 0;
00084     if (direction == STP_CW) {
00085 
00086         if (stp_cur_pos <= 400) {
00087             if (stp_cur_pos != STP_POS_UNKN) {
00088                 //pc.printf("current position = %d\n", stp_cur_pos);
00089                 stp_cur_pos++;
00090             }
00091         } else {
00092             pc.printf("Error: Cannot turn past maximum position. See stp_step() function.\n");
00093             while(1);
00094         }
00095 
00096         if (index == 3)
00097             index = 0;
00098         else
00099             index++;
00100         wait(MOTOR_DELAY);
00101         spi_send(drv8806, turn[index]);
00102         //wait(TURN_DELAY);
00103     } else if (direction == STP_CCW) {
00104 
00105         if (stp_cur_pos != 0) {
00106             if (stp_cur_pos != STP_POS_UNKN)
00107                 stp_cur_pos--;
00108         } else {
00109             pc.printf("Error: Cannot turn past home position. See stp_step() function.\n");
00110             wait(0.5);
00111             return;
00112         }
00113 
00114         if (index == 0)
00115             index = 3;
00116         else
00117             index--;
00118         wait(MOTOR_DELAY);
00119         spi_send(drv8806, turn[index]);
00120         //wait(TURN_DELAY);
00121     }
00122     wait(MOTOR_DELAY);
00123 }
00124 
00125 /*
00126  * void step_test();
00127  * Description: tests the stepper motor
00128  *
00129  * Inputs:
00130  *      Parameters:
00131  *      Globals:
00132  *
00133  * Outputs:
00134  *      Returns: void
00135 */
00136 void step_test()
00137 {
00138     stp_init();
00139     while (uti_chk_ubutton() == 0);
00140     pc.printf("step motor test begin\n");
00141     while(1) {
00142         if (jog_ccw == 0) {
00143             stp_step(STP_CCW);
00144         }
00145         if (jog_cw == 0) {
00146             stp_step(STP_CW);
00147         }
00148         if (cal_button == 0) {
00149             stp_find_home();
00150         }
00151         if (uti_chk_ubutton() == 1)
00152             break;
00153     }
00154 }
00155 
00156 /*
00157  * void stp_find_home();
00158  * Description: uses the stepper motor and home sensor to find home
00159  *
00160  * Inputs:
00161  *      Parameters:
00162  *      Globals:
00163  *
00164  * Outputs:
00165  *      Returns: void
00166 */
00167 void stp_find_home()
00168 {
00169     int count = 0;
00170     int half_count = 0;
00171     stp_cur_pos = STP_POS_UNKN;
00172     //pc.printf("Home sensor is currently %d\n", home_sensor.read());
00173     if (home_sensor == 0) {
00174         for(int i = 0; i < 100; i++)
00175             stp_step(STP_CW);
00176         if (home_sensor == 0) {
00177             pc.printf("Error: Home sensor not functioning. See stp_find_home() function.\n", home_sensor.read());
00178             while(1);
00179         }
00180     }
00181     while (home_sensor.read() != 0) {
00182         stp_step(STP_CCW);
00183     }
00184     while (home_sensor.read() != 1) {
00185         stp_step(STP_CCW);
00186         count++;
00187     }
00188     half_count = count/2;
00189     for(int i = 0; i < half_count; i++)
00190         stp_step(STP_CW);
00191     stp_cur_pos = 0;
00192     pc.printf("Home found.\n");
00193 }
00194 
00195 
00196 /*
00197  * void stp_calibrate(int station, float * sensor_values, int * cal_status);
00198  * Description: uses the stepper motor and home sensor to find home
00199  *
00200  * Inputs:
00201  *      Parameters:
00202  *          int station: STATION_A or STATION_B
00203  *          float *sensor_value: array of float holds 16 sensor values
00204  *          int *cal_status: pointer to int variable that will hold calibration status
00205  *      Globals:
00206  *
00207  * Outputs:
00208  *      Returns: void
00209 */
00210 void stp_calibrate(int station, float * sensor_values, int * cal_status)
00211 {
00212     while (cal_button.read() == 1);
00213     pc.printf("Calibration test begin...\n");
00214     pc.printf("step 9 test begin\n");
00215     int sensor_no = 0;
00216     if (station == STATION_A) sensor_no = 0;
00217     if (station == STATION_B) sensor_no = 8;
00218 
00219 
00220     pc.printf("sensor_no: %d\n", sensor_no);
00221 
00222     // find home position
00223     stp_find_home();
00224 
00225 
00226     // turn laser on
00227     wait(1);
00228     lzr_on();
00229 
00230     for (int i = 0; i < TGT_SENSOR_QUAN; i++) {
00231         // scan all 16 sensors into sensor_values array
00232         ana_scan_mux(sensor_values, TGT_SENSOR_QUAN * 2);
00233 
00234         // keep turning stepper motor clock wise until it points to a PT sensor
00235         while (sensor_values[sensor_no + i] * 3.3f < PTTHRESH) {
00236             // turn CW one step
00237             stp_step(STP_CW);
00238 
00239             wait(0.005);
00240 
00241             // scan all PT sensors again
00242             ana_scan_mux(sensor_values, TGT_SENSOR_QUAN * 2);
00243             
00244             wait(0.005);
00245             
00246             for (int j = 0; j < TGT_SENSOR_QUAN; j++) {
00247                 if (sensor_values[sensor_no + j] * 3.3f > PTTHRESH) {
00248                     pc.printf("PT %d: %f %d\n", sensor_no + j, sensor_values[sensor_no + j] * 3.3f, stp_cur_pos);
00249                     wait(0.02);
00250                 }
00251             }
00252         }
00253         // found the sensor, save its position
00254         stp_sensor_pos[i] = stp_cur_pos;
00255     }
00256     // found position of all 8 sensors
00257 
00258     // go back home
00259     stp_find_home();
00260 
00261     *cal_status = CALIBRATED;
00262 
00263     // turn laser off
00264     lzr_off();
00265 
00266     // for debugging: print positions of all 8 sensors here
00267     for (int i = 0; i < TGT_SENSOR_QUAN; i++) {
00268         pc.printf("PT %d: %d  ", i, stp_sensor_pos[i]);     
00269     }
00270     pc.printf("\nCalibration complete.\n");    
00271 }
00272 
00273 /*
00274  * void repeatability_test(itn sensor_position, int cal_status);
00275  * Description: repeatability test in part 10 of lab 6. The function will point to laser to
00276  *              the location of the specified sensor_position arguemnt. The function will
00277  *              return error when the calibration status is NOT_CALIBRATED
00278  *
00279  * Inputs:
00280  *      Parameters:
00281  *          int sensor_position: the position of sensor that the laser will point to (range from 0 to 7)
00282  *          int cal_status: calibration status.
00283  *      Globals:
00284  *
00285  * Outputs:
00286  *      Returns: void
00287 */
00288 void repeatability_test(int sensor_position, int cal_status)
00289 {
00290     int num_steps = 0;      // number of steps the stepper motor needed to move from current position
00291 
00292     //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);
00293     pc.printf("cal_status: %d\n", cal_status);
00294     // if the system is not calibrated
00295     if (cal_status == NOT_CALIBRATED) {
00296         pc.printf("Error: The system is not calibrated. See repeatibility_test() function .\n");
00297         while (1);
00298     } else {
00299         // if the current position is less than the position of sepcified sensor
00300         if (stp_cur_pos < stp_sensor_pos[sensor_position]) {
00301             num_steps = stp_sensor_pos[sensor_position] - stp_cur_pos;
00302 
00303             for (int i = 0; i < num_steps; i++) {
00304                 stp_step(STP_CW);
00305             }
00306         } else {
00307             num_steps = stp_cur_pos - stp_sensor_pos[sensor_position];
00308 
00309             for (int i = 0; i < num_steps; i++) {
00310                 stp_step(STP_CCW);
00311             }
00312         }
00313     }
00314 }
00315 
00316 /*
00317  * void turn_to_target(int target);
00318  * Description: turns the stepper to the designated target
00319  *
00320  * Inputs: 
00321  *      Parameters:
00322  *          int target: location of target
00323  *      Globals:
00324  *      
00325  * Outputs:
00326  *      Returns: void
00327 */
00328 void turn_to_target(int target) {
00329     if (target < stp_cur_pos) 
00330         while (target < stp_cur_pos) 
00331             stp_step(STP_CCW); 
00332     else if (target > stp_cur_pos) 
00333         while (target > stp_cur_pos) 
00334             stp_step(STP_CW); 
00335     pc.printf("current_position: %d\n", stp_cur_pos);
00336 }