Diff: scanner.cpp

Revision:

16:4e76b53cdef2

Parent:

15:3967cb2d93f5

--- a/scanner.cpp	Fri Apr 01 17:25:02 2016 +0000
+++ b/scanner.cpp	Fri Apr 01 22:21:27 2016 +0000
@@ -1,9 +1,7 @@
 #include "scanner.h"
 #include "mbed.h"
 #include "LongRangeSensor.h"
-#include "ShortRangeSensor.h"
 #include "StepperDrive.h"
-#include "Gripper.h"
 #include <stack>
 
 // FUNCTION:
@@ -17,11 +15,9 @@
 // DESCRIPTION:
 //      Constructor.
 Scanner::Scanner(Serial &pc1, StepperDrive &_drive, PinName _servoL, PinName _servoR,
-    ShortRangeSensor &_shortRangeL, ShortRangeSensor &_shortRangeR, LongRangeSensor &_longRangeL,
-    LongRangeSensor &_longRangeR, Gripper &_robotGrip, float _period) : pc(pc1), drive(_drive), servoL(_servoL),
-    servoR(_servoR), shortRangeL(_shortRangeL),
-    shortRangeR(_shortRangeR), longRangeL(_longRangeL),
-    longRangeR(_longRangeR), robotGrip(_robotGrip), period(_period)
+    LongRangeSensor &_longRangeL, LongRangeSensor &_longRangeR, float _period) : pc(pc1), 
+    drive(_drive), servoL(_servoL), servoR(_servoR), longRangeL(_longRangeL),
+    longRangeR(_longRangeR), period(_period)
 {
     servoL.period(1.0/50.0);
     servoR.period(1.0/50.0);
@@ -41,7 +37,7 @@
     // initializing to localizeRight status
     avoidFlag = 0;
     huntFlag = 0;
-    localizeRightFlag = 1;
+    localizeRightFlag = 0;
     localizeLeftFlag = 0;
     invertL = -1;
     invertR = 1;
@@ -56,239 +52,6 @@
 }
 
 // FUNCTION:
-//      huntMode()
-// IN-PARAMETERS:
-//      None
-//      None
-// DESCRIPTION:
-//      Hunts for victim.
-void Scanner::huntMode()
-{
-    pitEnable = 0;
-    avoidFlag = 0;
-    huntFlag = 1;
-    invertL = -1;
-    invertR = 1;
-    dutyL = MAX_DUTY;
-    dutyR = MIN_DUTY;
-    servoL.write(DUTY[dutyL]);
-    servoR.write(DUTY[dutyR]);
-    wait(0.1);
-    pitEnable = 1;
-}
-
-// FUNCTION:
-//      avoidMode()
-// IN-PARAMETERS:
-//      None
-// OUT-PARAMETERS:
-//      None
-// DESCRIPTION:
-//      Scans to avoid obstacles.
-void Scanner::avoidMode()
-{
-    pitEnable = 0;
-    avoidFlag = 1;
-    huntFlag = 0;
-    invertL = -1;
-    invertR = 1;
-    dutyL = MAX_DUTY;
-    dutyR = MIN_DUTY;
-    servoL.write(DUTY[dutyL]);
-    servoR.write(DUTY[dutyR]);
-    wait(0.1);
-    pitEnable = 1;
-}
-// FUNCTION:
-//      
-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]);
-    
-    // This initializes the temporary distance variables
-    // with the data collected from the VL6180X sensors.
-    tempDistShortL = shortRangeL.getRange();
-    tempDistShortR = shortRangeR.getRange();
-    
-    // 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);
-        while(!drive.isMoveDone())
-        {
-            wait(1e-6);
-        }
-        
-        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 ideally checks the short range
-    // sensors and if they are not equal to 
-    // each other, the code will center the
-    // 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(distShortL > (distShortR + tolerance))
-        {
-            drive.move(0.0, (-5.0)*(3.14159 / 180.0));
-            while(!drive.isMoveDone())
-            {
-                wait(1e-6);
-            }
-            
-            moveRobot.distance = 0.0;
-            moveRobot.angle = ((-5.0)*(3.14159 / 180.0));
-        
-            myStack.push(moveRobot);
-        }
-        else
-        {
-            drive.move(0.0, (5.0*(3.14159 / 180.0));
-            while(!drive.isMoveDone())
-            {
-                wait(1e-6);
-            }
-            
-            moveRobot.distance = 0.0;
-            moveRobot.angle = ((5.0)*(3.14159 / 180.0));
-            
-            myStack.push(moveRobot);
-        }
-    }
-        
-    servoL.write(DUTY[dutyR]);
-    servoR.write(DUTY[dutyL]);
-    
-    // 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 vertically with regards 
-    // to the peg in order to pick up the peg. The tolerance
-    while(((distShortL + toleranceSHP) <= distShortR) && ((distShortL + toleranceAHP) >= distShortR)) 
-    {
-        drive.move(0.125, 0.0);
-        while(!drive.isMoveDone())
-        {
-            wait(1e-6);
-        }
-        
-        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)));
-        while(!drive.isMoveDone())
-        {
-            wait(1e-6);
-        }
-    }
-}
-
-// FUNCTION:
-//      localize()
-// IN-PARAMETERS:
-//      None
-// OUT-PARAMETERS:
-//      None
-// DESCRIPTION:
-//      Checks localization points.
-void Scanner::localize()
-{
-    pitEnable = 0;
-    dutyL = MIN_DUTY;
-    dutyR = MAX_DUTY;
-    servoL.write(DUTY[dutyL]);
-    servoR.write(DUTY[dutyR]);
-    wait(0.1);
-    distLeft = longRangeL.distInchesL();
-    distRight = longRangeR.distInchesR();
-    dutyL = MAX_DUTY;
-    dutyR = MIN_DUTY;
-    servoL.write(DUTY[dutyL]);
-    servoR.write(DUTY[dutyR]);
-    wait(0.1);
-    distForwardL = longRangeL.distInchesL();
-    distForwardR = longRangeR.distInchesR();
-    pitEnable = 1;
-}
-
-// FUNCTION:
 //      void localizeLeftMode()
 // IN-PARAMETERS:
 //      None
@@ -321,11 +84,11 @@
 //      should remain xxx distance from the left wall. 
 void Scanner::localizeLeft()
 {   
+    pitEnable = 0;
     float distLocalR = longRangeR.distInchesR();
     
-    if (distLocalR >= 8.5)
+    if (distLocalR >= 9.2)
     {
-        pitEnable = 0;
         drive.pauseMove();
         drive.move(0.0, ((-5.0)*(3.14159 / 180.0)));
         // wait for move to complete
@@ -334,11 +97,9 @@
             wait(1e-6);
         }
         drive.resumeMove();
-        pitEnable = 1;
     }
-    else if (distLocalR <= 7.5)
+    else if (distLocalR <= 8.2)
     {
-        pitEnable = 0;
         drive.pauseMove();
         drive.move(0.0, ((5.0)*(3.14159 / 180.0)));
         // wait for move to complete
@@ -347,8 +108,14 @@
             wait(1e-6);
         }
         drive.resumeMove();
-        pitEnable = 1;
     }
+
+    // wait for move to complete
+    while(!drive.isMoveDone())
+    {
+        wait(1e-6);
+    }
+    pitEnable = 1;
 }
 
 // FUNCTION:
@@ -385,15 +152,11 @@
 //      will be called from scan when the localizeRightFlag is set.
 void Scanner::localizeRight()
 {
-    dutyL = MIN_DUTY;
-    dutyR = MIN_DUTY;
-    servoL.write(DUTY[dutyL]);
-    servoR.write(DUTY[dutyR]);
+    pitEnable = 0;
     float distLocalL = longRangeL.distInchesL();
     
-    if (distLocalL >= 8.5)
+    if (distLocalL >= 9.2)
     {
-        pitEnable = 0;
         drive.pauseMove();
         drive.move(0, ((5.0)*(3.14159 / 180.0)));
         // wait for move to complete
@@ -402,11 +165,9 @@
             wait(1e-6);
         }
         drive.resumeMove();
-        pitEnable = 1;
     }
-    else if (distLocalL <= 7.5)
+    else if (distLocalL <= 8.2)
     {
-        pitEnable = 0;
         drive.pauseMove();
         drive.move(0, ((-5.0)*(3.14159 / 180.0)));
         // wait for move to complete
@@ -415,12 +176,16 @@
             wait(1e-6);
         }
         drive.resumeMove();
-        pitEnable = 1;
     }
+    
+    // wait for move to complete
+    while(!drive.isMoveDone())
+    {
+        wait(1e-6);
+    }
+    pitEnable = 1;
 }
 
-void Scanner
-
 // FUNCTION:
 //      scan()
 // IN-PARAMETERS:
@@ -441,35 +206,5 @@
         
         /*if (localizeLeftFlag == 1)
             localizeLeft();*/
-        
-        /*// obtain distance measures from sensors
-        distLeft = longRangeL.distInchesL();
-        distRight = longRangeR.distInchesR();
-        
-        //check localize flags and make necessary method calls
-        
-        // check avoid flag
-        if (avoidFlag == 1)
-        {
-            // add obstacle avoid code
-        }
-        
-        // check hunt flag
-        if (huntFlag == 1)
-        {
-            // add hunt code
-        }*/
-
-/*        // increment/decrement servo position
-        dutyL += invertL;
-        dutyR += invertR;
-        servoL.write(DUTY[dutyL]);
-        servoR.write(DUTY[dutyR]);
-        if(((dutyL == MIN_DUTY) && (dutyR == MAX_DUTY)) ||
-            ((dutyL == MAX_DUTY) && (dutyR == MIN_DUTY)))
-        {
-            invertL *= -1;
-            invertR *= -1;
-        }*/
     }
 }