Diff: scanner.cpp

Revision:

14:d327852dafd1

Parent:

13:8767be1c5cc2

Child:

15:3967cb2d93f5

diff -r 8767be1c5cc2 -r d327852dafd1 scanner.cpp
--- a/scanner.cpp	Sat Mar 26 23:28:14 2016 +0000
+++ b/scanner.cpp	Fri Apr 01 15:55:52 2016 +0000
@@ -38,12 +38,14 @@
     distForwardL = 0.0;
     distForwardR = 0.0;
 
-    // initializing to avoid status
-    avoidFlag = 1;
+    // initializing to localizeRight status
+    avoidFlag = 0;
     huntFlag = 0;
+    localizeRightFlag = 1;
+    localizeLeftFlag = 0;
     invertL = -1;
     invertR = 1;
-    dutyL = MAX_DUTY;
+    dutyL = MIN_DUTY;
     dutyR = MIN_DUTY;
     servoL.write(DUTY[dutyL]);
     servoR.write(DUTY[dutyR]);
@@ -267,6 +269,27 @@
 }
 
 // FUNCTION:
+//      void localizeLeftMode()
+// IN-PARAMETERS:
+//      None
+// OUT-PARAMETERS:
+//      None
+// DESCRIPTION:
+//      This method initializes to localizeLeft()
+void Scanner::localizeLeftMode()
+{   
+    localizeRightFlag = 0;
+    
+    dutyL = MAX_DUTY;
+    dutyR = MAX_DUTY;
+    servoL.write(DUTY[dutyL]);
+    servoR.write(DUTY[dutyR]);
+    wait(0.2);
+    
+    localizeLeftFlag = 1;
+}
+
+// FUNCTION:
 //      void localizeLeft()
 // IN-PARAMETERS:
 //      None
@@ -280,25 +303,56 @@
 {   
     float distLocalR = longRangeR.distInchesR();
     
-    if (distLocalR > 8.5)
+    if (distLocalR >= 8.5)
     {
         pitEnable = 0;
         drive.pauseMove();
-        drive.move(0.0, -1.0);
+        drive.move(0.0, ((-0.5)*(3.14159 / 180.0)));
+        // wait for move to complete
+        while(!drive.isMoveDone())
+        {
+            wait(1e-6);
+        }
         drive.resumeMove();
         pitEnable = 1;
     }
-    else if (distLocalR < 7.5)
+    else if (distLocalR <= 7.5)
     {
         pitEnable = 0;
         drive.pauseMove();
-        drive.move(0.0, 1.0);
+        drive.move(0.0, ((0.5)*(3.14159 / 180.0)));
+        // wait for move to complete
+        while(!drive.isMoveDone())
+        {
+            wait(1e-6);
+        }
         drive.resumeMove();
         pitEnable = 1;
     }
 }
 
 // FUNCTION:
+//      void localizeRightMode()
+// IN-PARAMETERS:
+//      None
+// OUT-PARAMETERS:
+//      None
+// DESCRIPTION:
+//      Initializes to localizeRight()
+void Scanner::localizeRight()
+{
+    localizeLeftFlag = 0;
+    
+    dutyL = MIN_DUTY;
+    dutyR = MIN_DUTY;
+    servoL.write(DUTY[dutyL]);
+    servoR.write(DUTY[dutyR]);
+    wait(0.2);
+    
+    localizeRightFlag = 1;
+}
+
+// FUNCTION:
 //      void localizeRight()
 // IN-PARAMETERS:
 //      None
@@ -311,26 +365,42 @@
 //      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]);
     float distLocalL = longRangeL.distInchesL();
     
-    if (distLocalL > 8.5)
+    if (distLocalL >= 8.5)
     {
         pitEnable = 0;
         drive.pauseMove();
-        drive.move(0.0, 1.0);
+        drive.move(0, ((2.0)*(3.14159 / 180.0)));
+        // wait for move to complete
+        while(!drive.isMoveDone())
+        {
+            wait(1e-6);
+        }
         drive.resumeMove();
         pitEnable = 1;
     }
-    else if (distLocalL < 7.5)
+    else if (distLocalL <= 7.5)
     {
         pitEnable = 0;
         drive.pauseMove();
-        drive.move(0.0, -1.0);
+        drive.move(0, ((-2.0)*(3.14159 / 180.0)));
+        // wait for move to complete
+        while(!drive.isMoveDone())
+        {
+            wait(1e-6);
+        }
         drive.resumeMove();
         pitEnable = 1;
     }
 }
 
+void Scanner
+
 // FUNCTION:
 //      scan()
 // IN-PARAMETERS:
@@ -349,8 +419,8 @@
         if (localizeRightFlag == 1)
             localizeRight();
         
-        if (localizeLeftFlag == 1)
-            localizeLeft();
+        /*if (localizeLeftFlag == 1)
+            localizeLeft();*/
         
         /*// obtain distance measures from sensors
         distLeft = longRangeL.distInchesL();