Diff: main.cpp

Revision:

10:2aa70a504c18

Parent:

9:f34700716f1d

Child:

11:12ce7600f2f9

--- a/main.cpp	Thu Mar 27 17:49:30 2014 +0000
+++ b/main.cpp	Thu Mar 27 18:32:27 2014 +0000
@@ -194,7 +194,7 @@
     leftEncoder.reset();
     rightEncoder.reset();
 
-    while(dir*loc + location < 78) {
+    while(dir*loc + location <= 78) {
         loc=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
 
         pid1.setInputLimits(0.0, set);
@@ -216,8 +216,8 @@
         /* If after 20 ms the ultrasonic still sees 20+ cm */
         /*      then robot is at wave opening               */
 
-        bt.printf("wall follow 2 range %f\r\n",range);
-        bt.printf("loc+location = %f\r\n", loc+location);
+        //bt.printf("wall follow 2 range %f\r\n",range);
+        //bt.printf("loc+location = %f\r\n", loc+location);
         if(range > 20) {
             motors.stopBothMotors();
             bt.printf("wavegap\r\n");
@@ -369,7 +369,7 @@
        preLeft=leftEncoder.getPulses();
        preRight=rightEncoder.getPulses();
        wait_ms(100);
-       bt.printf(" first while left %d right %d \r\n", preLeft, preRight);
+       //bt.printf(" first while left %d right %d \r\n", preLeft, preRight);
        if(leftEncoder.getPulses() == preLeft || rightEncoder.getPulses()== preRight) preLeft=preRight=0;
     }
     
@@ -427,8 +427,8 @@
 
     if(section == TOOLS || section == MID) {
         while(IR.getDistance() > 20 ) {
-            bt.printf("IR %f\r\n", IR.getDistance());
-            bt.printf("third while left %d right %d \r\n", preLeft, preRight);
+            //bt.printf("IR %f\r\n", IR.getDistance());
+            //bt.printf("third while left %d right %d \r\n", preLeft, preRight);
         }
     } else while((abs(leftEncoder.getPulses()) < 200 || abs(rightEncoder.getPulses())< 200));
 
@@ -452,6 +452,7 @@
 
     //Tool aquiring
     wait(2);
+    while(1){ }
     // After tool is aquired
 
     alignWithWall(TOOLS);
@@ -483,7 +484,6 @@
         wait_ms(500);
         location[0]= current- ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
         current= location[0];
-
         leftTurn();
         slightleft();
         overBump(TOOLS);
@@ -494,7 +494,7 @@
         overBump(FIRST_WAVE);
     }
 
-    bt.printf("wavegap = %f\r\n",location[0]);
+    bt.printf("First Wavegap = %f\r\n",location[0]);
 }
 
 void mid_section(float* location, float &current, int* direction)