Diff: navigation.cpp

Revision:

8:290a110bcf0e

Parent:

7:f7489797746b

Child:

9:e24b50c1741b

diff -r f7489797746b -r 290a110bcf0e navigation.cpp
--- a/navigation.cpp	Fri Apr 08 01:35:28 2016 +0000
+++ b/navigation.cpp	Fri Apr 08 04:35:44 2016 +0000
@@ -247,20 +247,15 @@
         distLocalL = longRangeL.distInchesLOne();
         distLocalR = longRangeR.distInchesROne();
         pc.printf("\n    distLocalL = %f", distLocalL);
-        pc.printf("\n    distLocalR = %f", distLocalR);
-        if ((distLocalL <= 8.7) && (distLocalR <= 10.5)) // both walls in range
+        pc.printf("\n    distLocalR = %f", distLocalR);        
+        if (distLocalL <= 18.0) // wall in range
         {
-            newLocalize();
-        }
-        
-        else if ((distLocalL <= 8.7) && (distLocalL >= 3.0)) // wall in range
-        {
-            localizeRight();
+            newLocalizeRight();
         }
             
-        else if ((distLocalR <= 10.5) && (distLocalR >= 5.0)) // wall in range
+        else if (distLocalR <= 13.0) // wall in range
         {
-            localizeLeft();
+            newLocalizeLeft();
         }
     }
 }
@@ -480,11 +475,107 @@
     float k = 2.5;
     float angleAdjust = k*(distLocalL - distLocalR);
     
-    drive.move(0, angleAdjust);
+    drive.move(0, angleAdjust*(PI / 180.0));
+    // wait for move to complete
+    while(!drive.isMoveDone())
+    {
+        wait(1e-6);
+    }
+    angle += angleAdjust;
+    pc.printf("\n    Adjusted angle by %f in newLocalize", angleAdjust);
+}
+
+// FUNCTION:
+//      void newLocalizeRight()
+// IN-PARAMETERS:
+//      None
+// OUT-PARAMETERS:
+//      None
+// DESCRIPTION:
+//      Left sensor to adjust off right wall
+void Navigation::newLocalizeRight()
+{   
+    float k = 2.0;
+        
+    float angleAdjust = k*(distLocalL - 13.0);
+    
+    drive.move(0, angleAdjust*(PI / 180.0));
     // wait for move to complete
     while(!drive.isMoveDone())
     {
         wait(1e-6);
     }
-    pc.printf("\n    Adjusted angle by %f in newLocalize", angleAdjust);
-}
\ No newline at end of file
+    //angle += (-1*angleAdjust);
+    pc.printf("\n    Adjusted angle by %f in newLocalizeRight", -1*angleAdjust);
+}
+
+// FUNCTION:
+//      void newLocalizeLeft()
+// IN-PARAMETERS:
+//      None
+// OUT-PARAMETERS:
+//      None
+// DESCRIPTION:
+//      Right sensor to adjust off Left wall
+void Navigation::newLocalizeLeft()
+{   
+    float k = 3.5;
+    float angleAdjust = k*(9.1 - distLocalR);
+    
+    drive.move(0, angleAdjust*(PI / 180.0));
+    // wait for move to complete
+    while(!drive.isMoveDone())
+    {
+        wait(1e-6);
+    }
+    //angle += (-1*angleAdjust);
+    pc.printf("\n    Adjusted angle by %f in newLocalizeLeft", -1*angleAdjust);
+}
+
+// FUNCTION:
+//      void newLocalizeLeftReverse()
+// IN-PARAMETERS:
+//      None
+// OUT-PARAMETERS:
+//      None
+// DESCRIPTION:
+//      Right sensor to adjust off Left wall
+void Navigation::newLocalizeLeftReverse()
+{   
+    float k = 3.5;
+    float angleAdjust = k*(9.1 - distLocalR);
+    
+    drive.move(0, -1*angleAdjust*(PI / 180.0));
+    // wait for move to complete
+    while(!drive.isMoveDone())
+    {
+        wait(1e-6);
+    }
+    //angle += (-1*angleAdjust);
+    pc.printf("\n    Adjusted angle by %f in newLocalizeLeft", angleAdjust);
+}
+
+// FUNCTION:
+//      void newLocalizeRightReverse()
+// IN-PARAMETERS:
+//      None
+// OUT-PARAMETERS:
+//      None
+// DESCRIPTION:
+//      Left sensor to adjust off right wall
+void Navigation::newLocalizeRightReverse()
+{   
+    float k = 2.0;
+        
+    float angleAdjust = k*(distLocalL - 13.0);
+    
+    drive.move(0, -1*angleAdjust*(PI / 180.0));
+    // wait for move to complete
+    while(!drive.isMoveDone())
+    {
+        wait(1e-6);
+    }
+    //angle += (-1*angleAdjust);
+    pc.printf("\n    Adjusted angle by %f in newLocalizeRight", angleAdjust);
+}
+