Diff: stepper.cpp

Revision:

48:d612de6880b0

Parent:

46:85c4b722baa7

Parent:

47:0547118e348e

Child:

49:80d4ffabec16

--- a/stepper.cpp	Fri Mar 30 20:25:25 2018 +0000
+++ b/stepper.cpp	Fri Mar 30 20:28:41 2018 +0000
@@ -64,7 +64,7 @@
  *
  * Inputs: 
  *      Parameters:
- *          int direction
+ *          int direction: STP_CW for clock wise and STP_CCW for counter clock wise
  *      Globals:
  *      
  * Outputs:
@@ -185,9 +185,53 @@
  *
  * 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);
\ No newline at end of file
+void stp_calibrate(int station, float * sensor_values, int * cal_status){
+    int sensor_no = 0;
+    if (station == STATION_B) sensor = 8;
+    
+    // find home position 
+    stp_find_home();
+    
+    // turn laser on
+    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);
+        }
+        // found the sensor, save it's position
+        stp_sensor_pos[i] = stp_cur_pos;
+    }
+    // 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]);     
+    }    
+}
\ No newline at end of file