File content as of revision 51:1eb60f0d2f03:

/******************************************************************************
* EECS 397
*
* Assignment Name: Lab 6: WaG
* 
* Authors: Sam Morrison and Phong Nguyen 
* File name: stepper.cpp
* Purpose: Driver for stepper motor
*
* Created: 03/02/2018
* Last Modified: 03/29/2018
*
******************************************************************************/

#include "mbed.h"
#include "io_pins.h"
#include "spi.h"
#include "stepper.h"
#include "utility.h"
#include "laser.h"
#include "analog.h"
#include "wag.h"

extern DigitalIn jog_ccw;
extern DigitalIn jog_cw;
extern DigitalIn my_button;
extern DigitalIn cal_button;
extern DigitalIn home_sensor;
extern Serial pc;
extern DigitalOut laser;

int stp_cur_pos = STP_POS_UNKN;
int stp_sensor_pos[TGT_SENSOR_QUAN];

extern spi_cfg drv8806 {
    SPI_DRV8806_ID,
    STP_DRV8806_NCS,
    DRV8806_SPI_MODE,
    DRV8806_SPI_FREQ,
    DRV8806_SPI_NO_BITS,
};

/*
 * void stp_init();
 * Description: initializes stepper values to unkown
 *
 * Inputs: 
 *      Parameters: void
 *      Globals:
 *      
 * Outputs:
 *      Returns: void
*/
void stp_init() {
    stp_cur_pos = STP_POS_UNKN;
    jog_cw.mode(PullUp);
    jog_ccw.mode(PullUp);
    cal_button.mode(PullUp);
    home_sensor.mode(PullUp);
    for (int i = 1; i <= TGT_SENSOR_QUAN; i++) {
        //stp_sensor_pos[i] = STP_POS_UNKN;
    }
}

/*
 * void stp_step(int direction);
 * Description: turns the stepper motor clockwise or counter-clockwise
 *
 * Inputs: 
 *      Parameters:
 *          int direction: STP_CW for clock wise and STP_CCW for counter clock wise
 *      Globals:
 *      
 * Outputs:
 *      Returns: void
*/
void stp_step(int direction) {
    
    //static int cur_pos = stp_cur_pos;
    static int turn[4] = {0x03, 0x06, 0x0c, 0x09};
    if (direction == STP_CW) {
        
        if (stp_cur_pos <= 200) {
                if (stp_cur_pos != STP_POS_UNKN){
                    //pc.printf("current position = %d\n", stp_cur_pos);
                    stp_cur_pos++;
                }
        }
        else {
            pc.printf("Cannot turn past maximum position. Fatal error.\n");
            while(1);
        }
        
                
        for (int i = 0; i < 4; i++) {
            wait(MOTOR_DELAY);
            //pc.printf("i = %d\n", i);
            spi_send(drv8806, turn[i]);
        }
        wait(TURN_DELAY);
    }
    else if (direction == STP_CCW) {
        
        if (stp_cur_pos != 0) {
            if (stp_cur_pos != STP_POS_UNKN)
                stp_cur_pos--;
        }
        else {
            pc.printf("Cannot turn past home position.\n");
            wait(0.5);
            return;
        }
        
        for (int i = 3; i >= 0; i--) {
            wait(MOTOR_DELAY);
            //pc.printf("i = %d\n", i);
            spi_send(drv8806, turn[i]);
        }
        wait(TURN_DELAY);
    }
    wait(MOTOR_DELAY);
}

/*
 * void step_test();
 * Description: tests the stepper motor
 *
 * Inputs: 
 *      Parameters:
 *      Globals:
 *      
 * Outputs:
 *      Returns: void
*/
void step_test() {
    stp_init();
    while (uti_chk_ubutton() == 0);
    pc.printf("step motor test begin\n");
    while(1) {
            if (jog_ccw == 0) {
                stp_step(STP_CCW);
            }
            if (jog_cw == 0) {
                stp_step(STP_CW);
            } 
            if (cal_button == 0) {
                stp_find_home();
            }
            if (uti_chk_ubutton() == 1)
                break;
        }
}

/*
 * void stp_find_home();
 * Description: uses the stepper motor and home sensor to find home
 *
 * Inputs: 
 *      Parameters:
 *      Globals:
 *      
 * Outputs:
 *      Returns: void
*/
void stp_find_home() {
    int count = 0;
    int half_count = 0;
    stp_cur_pos = STP_POS_UNKN;
    //pc.printf("Home sensor is currently %d\n", home_sensor.read());
    if (home_sensor == 0) {
        for(int i = 0; i < 100; i++)
            stp_step(STP_CW);
        if (home_sensor == 0) {
            pc.printf("Error, home sensor not functioning. Fatal error.\n", home_sensor.read());
            while(1);
        }
    }
    while (home_sensor.read() != 0) {
        stp_step(STP_CCW);
    }
    while (home_sensor.read() != 1) {
        stp_step(STP_CCW);
        count++;
    }
    half_count = count/2;
    for(int i = 0; i < half_count; i++)
        stp_step(STP_CW);
    stp_cur_pos = 0;
    pc.printf("Home found.\n");
}


/*
 * void stp_calibrate(int station, float * sensor_values, int * cal_status);
 * Description: uses the stepper motor and home sensor to find home
 *
 * Inputs: 
 *      Parameters:
 *          int station: STATION_A or STATION_B
 *          float *sensor_value: array of float holds 16 sensor values
 *          int *cal_status: pointer to int variable that will hold calibration status
 *      Globals:
 *      
 * Outputs:
 *      Returns: void
*/
void stp_calibrate(int station, float * sensor_values, int * cal_status){
    while (uti_chk_ubutton() == 0);
    pc.printf("step 9 test begin\n");
    
    int sensor_no = 0;
    if (station == STATION_B) sensor_no = 8;
    
    // find home position
    stp_find_home();
    
    
    // turn laser on
    wait(1);
    lzr_on();
    
    for (int i = 0; i < TGT_SENSOR_QUAN; i++) {
        // scan all 16 sensors into sensor_values array
        ana_scan_mux(sensor_values, TGT_SENSOR_QUAN * 2);
        
        // keep turning stepper motor clock wise until it points to a PT sensor
        while (sensor_values[sensor_no + i] < PTTHRESH) {
            // turn CW one step
            stp_step(STP_CW);
            
            wait(0.005);
            
            // scan all PT sensors again
            ana_scan_mux(sensor_values, TGT_SENSOR_QUAN * 2);
            
            for (int j = 0; j < TGT_SENSOR_QUAN; j++) {
                if (sensor_values[sensor_no + j] > PTTHRESH)
                    pc.printf("PT %d: %f ", sensor_no + j, sensor_values[sensor_no + j]);     
            }
            pc.printf("\n");
        }
        // found the sensor, save it's position
        stp_sensor_pos[i] = stp_cur_pos;
        //pc.printf("Sensor %d = %d\n", i, stp_sensor_pos[i]);
    }
    // found position of all 8 sensors
    
    // go back home
    stp_find_home();
    
    *cal_status = CALIBRATED;
    
    // turn laser off
    lzr_off();
    
    // for debugging: print positions of all 8 sensors here
    for (int i = 0; i < TGT_SENSOR_QUAN; i++) {
        pc.printf("PT %d: %d  ", i, stp_sensor_pos[i]);     
    }    
}