Diff: scanner.cpp

Revision:

7:52c4be3bfc1b

Parent:

6:5e24ff86b743

--- a/scanner.cpp	Fri Mar 25 19:51:11 2016 +0000
+++ b/scanner.cpp	Sat Mar 26 05:09:29 2016 +0000
@@ -2,20 +2,22 @@
 #include "mbed.h"
 #include "LongRangeSensor.h"
 #include "VL6180x.h"
+#include "StepperDrive.h"
+#include <stack>
 
 // FUNCTION:
 //      Scanner()
 // IN-PARAMETERS:
-//      Serial &pc1, PinName _servoL, PinName _servoR,
+//      Serial &pc1, StepperDrive &_drive, PinName _servoL, PinName _servoR,
 //      VL6180x &_shortRangeL, VL6180x &_shortRangeR, Gp2x &_longRangeL,
 //      Gp2x &_longRangeR, float _period = 10e-3
 // OUT-PARAMETERS:
 //      None
 // DESCRIPTION:
 //      Constructor.
-Scanner::Scanner(Serial &pc1, PinName _servoL, PinName _servoR,
+Scanner::Scanner(Serial &pc1, StepperDrive &_drive, PinName _servoL, PinName _servoR,
     VL6180x &_shortRangeL, VL6180x &_shortRangeR, LongRangeSensor &_longRangeL,
-    LongRangeSensor &_longRangeR, float _period) : pc(pc1), servoL(_servoL),
+    LongRangeSensor &_longRangeR, float _period) : pc(pc1), drive(_drive), servoL(_servoL),
     servoR(_servoR), shortRangeL(_shortRangeL),
     shortRangeR(_shortRangeR), longRangeL(_longRangeL),
     longRangeR(_longRangeR), period(_period)
@@ -94,6 +96,76 @@
     wait(0.1);
     pitEnable = 1;
 }
+// FUNCTION:
+//      
+void Scanner::hunt()
+{
+    // Used to store temporary distance 
+    // recorded from VL6180x short range
+    // sensor.
+    float tempDistShortL;
+    float tempDistShortR;
+
+    // Used to temporary store the number 
+    // of stepper motor increments.
+    int tempStepCount = 0;
+    
+    pitEnable = 0;
+    
+    servoL.write(DUTY[4]);
+    servoR.write(DUTY[2]);
+    
+    // Update this code using the new VL6180x code
+    // from Victor.
+    // tempDistShortL = 
+    // tempDistShortR =
+    
+    // This if statement moves the robot forward in order
+    // to continue the hunt of the peg.
+    while(distShortL == 0 && distShortR == 0 && tempStepCount < 34)
+    {
+        drive.move(0.25, 0.0);
+    }  
+    
+    // Adjust the comparsion for floats with
+    // regards to the sensor data.
+    // This while loop checks the short range
+    // sensors and if they are not equal to 
+    // each other, the code will center the
+    // robot.
+    while(tempDistShortL != tempDistShortR)
+    {            
+        // Adjust this comparsion for floats
+        // with regards to the sensor data.
+        if(tempDistShortL > tempDistShortR)
+            drive.move(0.0, -5.0);
+        else
+            drive.move(0.0, 5.0);
+    }
+        
+    servoL.write(DUTY[dutyR]);
+    servoR.write(DUTY[dutyL]);
+    
+    // Update this code with the new VL6180x sensor
+    // code from Victor.
+    // tempDistShortL =
+    // tempDistShortR = 
+    
+    // This while loop is used to position the robot
+    // at the correct location in order to pick up
+    // the peg.
+    while(tempDistShortL == tempDistShortR)
+    {
+        /* pseudocode
+        implement the code for the gripper to pick up the peg
+        implement the code for reading a value from the color sensor
+        store the value that is returned from the color sensor
+        implement the code for the gripper to angle up
+        pop the stack and reverse the moves stored in the stack
+        return a done value
+        */   
+    }    
+}
 
 // FUNCTION:
 //      localize()
@@ -134,26 +206,25 @@
 //      to the left during stretches of forward motion where robot 
 //      should remain xxx distance from the left wall. 
 void Scanner::localizeLeft()
-{
-    /****** pseudocode *****
-    
-    dist = longRangeR dist measure
+{   
+    float distLocalR = longRangeR.distInchesR();
     
-    if (dist > xxx dist)
-        pitEnable = 0
-        pause current route
-        adjust angle to left
-        unpause route
-        pitEnable = 1
-        
-        
-    else if (dist < xxx dist)
-        pitEnable = 0
-        pause current route
-        adjust angle to right
-        unpause route
-        pitEnable = 1
-    */
+    if (distLocalR > 8.5)
+    {
+        pitEnable = 0;
+        drive.pauseMove();
+        drive.move(0.0, -1.0);
+        drive.resumeMove();
+        pitEnable = 1;
+    }
+    else if (distLocalR < 7.5)
+    {
+        pitEnable = 0;
+        drive.pauseMove();
+        drive.move(0.0, 1.0);
+        drive.resumeMove();
+        pitEnable = 1;
+    }
 }
 
 // FUNCTION:
@@ -167,26 +238,26 @@
 //      to the left during stretches of forward motion where robot 
 //      should remain xxx distance from the left wall. This function
 //      will be called from scan when the localizeRightFlag is set.
-void localizeRight()
+void Scanner::localizeRight()
 {
-    /****** pseudocode *****
-    
-    dist = longRangeL dist measure
+    float distLocalL = longRangeL.distInchesL();
     
-    if (dist > xxx dist)
-        pitEnable = 0
-        pause current route
-        adjust angle to right
-        unpause route
-        pitEnable = 1
-        
-    else if (dist < xxx dist)
-        pitEnable = 0
-        pause current route
-        adjust angle to left
-        unpause route
-        pitEnable = 1
-    */
+    if (distLocalL > 8.5)
+    {
+        pitEnable = 0;
+        drive.pauseMove();
+        drive.move(0.0, 1.0);
+        drive.resumeMove();
+        pitEnable = 1;
+    }
+    else if (distLocalL < 7.5)
+    {
+        pitEnable = 0;
+        drive.pauseMove();
+        drive.move(0.0, -1.0);
+        drive.resumeMove();
+        pitEnable = 1;
+    }
 }
 
 // FUNCTION: