Diff: navigation.cpp

Revision:

7:f7489797746b

Parent:

6:d2da4d4b5112

Child:

8:290a110bcf0e

diff -r d2da4d4b5112 -r f7489797746b navigation.cpp
--- a/navigation.cpp	Wed Apr 06 22:01:59 2016 +0000
+++ b/navigation.cpp	Fri Apr 08 01:35:28 2016 +0000
@@ -215,12 +215,12 @@
             {
                 wait(1e-6);
             }
-            
+            /*
             distLocalL = longRangeL.distInchesLOne();
             distLocalR = longRangeR.distInchesROne();
             pc.printf("\n    distLocalL = %f", distLocalL);
             pc.printf("\n    distLocalR = %f", distLocalR);
-            if ((distLocalL <= 8.0) && (distLocalL >= 3.0)) // wall in range
+            if ((distLocalL <= 8.7) && (distLocalL >= 3.0)) // wall in range
                 {
                     localizeRight();
                 }
@@ -228,7 +228,7 @@
             else if ((distLocalR <= 10.5) && (distLocalR >= 5.0)) // wall in range
                 {
                     localizeLeft();
-                }
+                }*/
         }
 
         /* send targetDist to motors */
@@ -248,15 +248,20 @@
         distLocalR = longRangeR.distInchesROne();
         pc.printf("\n    distLocalL = %f", distLocalL);
         pc.printf("\n    distLocalR = %f", distLocalR);
-        if ((distLocalL <= 8.5) && (distLocalL >= 3.0)) // wall in range
-            {
-                localizeRight();
-            }
+        if ((distLocalL <= 8.7) && (distLocalR <= 10.5)) // both walls in range
+        {
+            newLocalize();
+        }
+        
+        else if ((distLocalL <= 8.7) && (distLocalL >= 3.0)) // wall in range
+        {
+            localizeRight();
+        }
             
         else if ((distLocalR <= 10.5) && (distLocalR >= 5.0)) // wall in range
-            {
-                localizeLeft();
-            }
+        {
+            localizeLeft();
+        }
     }
 }
 
@@ -300,7 +305,18 @@
 //      to the right
 void Navigation::localizeRight()
 {   
-    if (distLocalL >= 7.1) // too close left, turn right
+    if (distLocalL >= 7.8) // very close left, turn more right
+    {
+        drive.move(0, (5.0)*(3.14159 / 180.0));
+        // wait for move to complete
+        while(!drive.isMoveDone())
+        {
+            wait(1e-6);
+        }
+        pc.printf("\n    Adjusted angle more right, in localizeRight");
+    }
+        
+    else if (distLocalL >= 6.9) // too close left, turn right
     {
         drive.move(0, (2.0)*(3.14159 / 180.0));
         // wait for move to complete
@@ -311,7 +327,7 @@
         pc.printf("\n    Adjusted angle right, in localizeRight");
     }
     
-    else if (distLocalL <= 5.9) // too close right, turn left
+    else if (distLocalL <= 5.5) // too close right, turn left
     {
         drive.move(0, (-2.0)*(3.14159 / 180.0));
         // wait for move to complete
@@ -450,3 +466,25 @@
         pc.printf("\n    No need to adjust, in localizeLeftReverse");
     }
 }
+
+// FUNCTION:
+//      void newLocalize()
+// IN-PARAMETERS:
+//      None
+// OUT-PARAMETERS:
+//      None
+// DESCRIPTION:
+//      Uses difference between sensors to adjust angle
+void Navigation::newLocalize()
+{   
+    float k = 2.5;
+    float angleAdjust = k*(distLocalL - distLocalR);
+    
+    drive.move(0, angleAdjust);
+    // 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