Diff: scanner.cpp

Revision:

9:bae63bc84829

Parent:

8:32a445ae1d72

Child:

12:514544a4014f

--- a/scanner.cpp	Sat Mar 26 05:26:17 2016 +0000
+++ b/scanner.cpp	Sat Mar 26 23:01:49 2016 +0000
@@ -3,6 +3,7 @@
 #include "LongRangeSensor.h"
 #include "ShortRangeSensor.h"
 #include "StepperDrive.h"
+#include "Gripper.h"
 #include <stack>
 
 // FUNCTION:
@@ -17,10 +18,10 @@
 //      Constructor.
 Scanner::Scanner(Serial &pc1, StepperDrive &_drive, PinName _servoL, PinName _servoR,
     ShortRangeSensor &_shortRangeL, ShortRangeSensor &_shortRangeR, LongRangeSensor &_longRangeL,
-    LongRangeSensor &_longRangeR, float _period) : pc(pc1), drive(_drive), servoL(_servoL),
+    LongRangeSensor &_longRangeR, Gripper &_robotGrip, float _period) : pc(pc1), drive(_drive), servoL(_servoL),
     servoR(_servoR), shortRangeL(_shortRangeL),
     shortRangeR(_shortRangeR), longRangeL(_longRangeL),
-    longRangeR(_longRangeR), period(_period)
+    longRangeR(_longRangeR), grip(_robotGrip), period(_period)
 {
     servoL.period(1.0/50.0);
     servoR.period(1.0/50.0);
@@ -100,71 +101,141 @@
 //      
 void Scanner::hunt()
 {
+    // This creates an instance of a structure
+    // call huntMove. This structure is used to
+    // store the distance value and angle value
+    // when implementing the Navigation drive 
+    // code.
+    huntMove moveRobot;
+    
     // Used to store temporary distance 
     // recorded from VL6180x short range
     // sensor.
     float tempDistShortL;
     float tempDistShortR;
+    
+    float distShortL;
+    float distShortR;
 
     // Used to temporary store the number 
     // of stepper motor increments.
     int tempStepCount = 0;
     
+    // This variable is half of the available 
+    // tolerance of the gripper with regards 
+    // to the peg.
+    float tolerance = 0.1875;
+    
+    // These variables represent the tolerance
+    // and plus/minus half the diameter of the
+    // peg measured in inches.
+    float toleranceAHP = 0.9375;
+    float toleranceSHP = 0.5625;
+    
     pitEnable = 0;
     
+    // This positions both of the servos to 
+    // be 60 degrees inwards.
     servoL.write(DUTY[4]);
     servoR.write(DUTY[2]);
     
-    // Update this code using the new VL6180x code
-    // from Victor.
-    // tempDistShortL = 
-    // tempDistShortR =
+    // This initializes the temporary distance variables
+    // with the data collected from the VL6180X sensors.
+    tempDistShortL = shortRangeL.getRange();
+    tempDistShortR = shortRangeR.getRange();
     
-    // This if statement moves the robot forward in order
-    // to continue the hunt of the peg.
-/*    while(distShortL == 0 && distShortR == 0 && tempStepCount < 34)
+    // This code converts the measurements from the short
+    // range sensor from millimeters to inches. 
+    distShortL = (tempDistShortL / 25.0);
+    distShortR = (tempDistShortR / 25.0);
+    
+    // This while loop statement moves the robot forward in order
+    // to continue the hunt of the peg. The equals to comparsion
+    // statements need to be updated to reflect what the short
+    // range sensors output when there is nothing within the 
+    // range of the short distance sensors.
+    while(distShortL == 0.0 && distShortR == 0.0 && tempStepCount < 34)
     {
         drive.move(0.25, 0.0);
-    }*/  
+        
+        moveRobot.distance = 0.25;
+        moveRobot.angle = 0.0;
+        
+        myStack.push(moveRobot);
+    }  
     
     // Adjust the comparsion for floats with
-    // regards to the sensor data.
-    // This while loop checks the short range
+    // regards to the sensor data. This
+    // while loop ideally checks the short range
     // sensors and if they are not equal to 
     // each other, the code will center the
-    // robot.
-    while(tempDistShortL != tempDistShortR)
+    // robot according to the short range sensor.
+    while((distShortL <= distShortR + tolerance) && (distShortL >= distShortR + tolerance))
     {            
         // Adjust this comparsion for floats
         // with regards to the sensor data.
-        if(tempDistShortL > tempDistShortR)
+        if(distShortL > (distShortR + tolerance))
+        {
             drive.move(0.0, -5.0);
+            
+            moveRobot.distance = 0.0;
+            moveRobot.angle = -5.0;
+        
+            myStack.push(moveRobot);
+        }
         else
+        {
             drive.move(0.0, 5.0);
+            
+            moveRobot.distance = 0.0;
+            moveRobot.angle = 5.0;
+            
+            myStack.push(moveRobot);
+        }
     }
         
     servoL.write(DUTY[dutyR]);
     servoR.write(DUTY[dutyL]);
     
-    // Update this code with the new VL6180x sensor
-    // code from Victor.
-    // tempDistShortL =
-    // tempDistShortR = 
+    // This code reinitializes the short distance 
+    // variable now that the short distance sensors
+    // are pointing inward. 
+    distShortL = shortRangeL.getRange();
+    distShortR = shortRangeR.getRange();
     
     // This while loop is used to position the robot
-    // at the correct location in order to pick up
-    // the peg.
-    while(tempDistShortL == tempDistShortR)
+    // at the correct location vertically with regards 
+    // to the peg in order to pick up the peg. The tolerance
+    while(((distShortL + toleranceSHP) <= distShortR) && ((distShortL + toleranceAHP) >= distShortR)) 
     {
-        /* 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
-        */   
-    }    
+        drive.move(0.125, 0.0);
+        
+        moveRobot.distance = 0.125;
+        moveRobot.angle = 0.0;
+        
+        myStack.push(moveRobot);
+    }
+    
+    // This is the code to enable the gripper to grip the peg. 
+    robotGrip.grip()
+    
+    /* pseudocode
+    implement the code for reading a value from the color sensor
+    store the value that is returned from the color sensor
+    */       
+    
+    // This is the code to enable the gripper to lift the peg.
+    robotGrip.lift();
+    
+    // This code uses the stack to reverse all the movement
+    // of the robot during this function.
+    while(!myStack.empty())
+    {   
+        myStack.top();
+        myStack.pop();
+        
+        drive.move((-(moveRobot.distance)),(-(moveRobot.angle)));  
+    }
 }
 
 // FUNCTION: