Diff: scanner.cpp

Revision:

17:ec04d60f38bd

Parent:

15:3967cb2d93f5

Child:

18:f5c46fb2cd31

--- a/scanner.cpp	Fri Apr 01 17:25:02 2016 +0000
+++ b/scanner.cpp	Wed Apr 06 22:05:34 2016 +0000
@@ -195,7 +195,7 @@
         }
         else
         {
-            drive.move(0.0, (5.0*(3.14159 / 180.0));
+            drive.move(0.0, (5.0*(3.14159 / 180.0)));
             while(!drive.isMoveDone())
             {
                 wait(1e-6);
@@ -321,11 +321,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 +334,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 +345,14 @@
             wait(1e-6);
         }
         drive.resumeMove();
-        pitEnable = 1;
     }
+
+    // wait for move to complete
+    while(!drive.isMoveDone())
+    {
+        wait(1e-6);
+    }
+    pitEnable = 1;
 }
 
 // FUNCTION:
@@ -385,15 +389,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 +402,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 +413,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 +443,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;
-        }*/
     }
 }