Diff: scanner.cpp

Revision:

15:3967cb2d93f5

Parent:

14:d327852dafd1

Child:

16:4e76b53cdef2

Child:

17:ec04d60f38bd

--- a/scanner.cpp	Fri Apr 01 15:55:52 2016 +0000
+++ b/scanner.cpp	Fri Apr 01 17:25:02 2016 +0000
@@ -159,6 +159,10 @@
     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;
@@ -178,19 +182,27 @@
         // with regards to the sensor data.
         if(distShortL > (distShortR + tolerance))
         {
-            drive.move(0.0, -5.0);
+            drive.move(0.0, (-5.0)*(3.14159 / 180.0));
+            while(!drive.isMoveDone())
+            {
+                wait(1e-6);
+            }
             
             moveRobot.distance = 0.0;
-            moveRobot.angle = -5.0;
+            moveRobot.angle = ((-5.0)*(3.14159 / 180.0));
         
             myStack.push(moveRobot);
         }
         else
         {
-            drive.move(0.0, 5.0);
+            drive.move(0.0, (5.0*(3.14159 / 180.0));
+            while(!drive.isMoveDone())
+            {
+                wait(1e-6);
+            }
             
             moveRobot.distance = 0.0;
-            moveRobot.angle = 5.0;
+            moveRobot.angle = ((5.0)*(3.14159 / 180.0));
             
             myStack.push(moveRobot);
         }
@@ -211,6 +223,10 @@
     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;
@@ -236,7 +252,11 @@
         myStack.top();
         myStack.pop();
         
-        drive.move((-(moveRobot.distance)),(-(moveRobot.angle)));  
+        drive.move((-(moveRobot.distance)),(-(moveRobot.angle)));
+        while(!drive.isMoveDone())
+        {
+            wait(1e-6);
+        }
     }
 }
 
@@ -307,7 +327,7 @@
     {
         pitEnable = 0;
         drive.pauseMove();
-        drive.move(0.0, ((-0.5)*(3.14159 / 180.0)));
+        drive.move(0.0, ((-5.0)*(3.14159 / 180.0)));
         // wait for move to complete
         while(!drive.isMoveDone())
         {
@@ -320,7 +340,7 @@
     {
         pitEnable = 0;
         drive.pauseMove();
-        drive.move(0.0, ((0.5)*(3.14159 / 180.0)));
+        drive.move(0.0, ((5.0)*(3.14159 / 180.0)));
         // wait for move to complete
         while(!drive.isMoveDone())
         {
@@ -339,7 +359,7 @@
 //      None
 // DESCRIPTION:
 //      Initializes to localizeRight()
-void Scanner::localizeRight()
+void Scanner::localizeRightMode()
 {
     localizeLeftFlag = 0;
     
@@ -375,7 +395,7 @@
     {
         pitEnable = 0;
         drive.pauseMove();
-        drive.move(0, ((2.0)*(3.14159 / 180.0)));
+        drive.move(0, ((5.0)*(3.14159 / 180.0)));
         // wait for move to complete
         while(!drive.isMoveDone())
         {
@@ -388,7 +408,7 @@
     {
         pitEnable = 0;
         drive.pauseMove();
-        drive.move(0, ((-2.0)*(3.14159 / 180.0)));
+        drive.move(0, ((-5.0)*(3.14159 / 180.0)));
         // wait for move to complete
         while(!drive.isMoveDone())
         {