Diff: main.cpp

Revision:

15:a467af795e57

Parent:

14:3c8c4efe4786

Child:

16:0888096bff60

--- a/main.cpp	Sun Mar 30 18:21:06 2014 +0000
+++ b/main.cpp	Tue Apr 01 15:42:03 2014 +0000
@@ -147,7 +147,7 @@
 
     location=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
 
-    while(location< 69) {
+    while(location< 66.5) {
         location=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
 
         pid1.setInputLimits(0, set);
@@ -196,6 +196,12 @@
             }
         }
     }
+    
+    //STOP
+    motors.setMotor0Speed(dir*-0.3*127); //right
+    motors.setMotor1Speed(dir*-0.3*127); //left
+    wait_ms(10);
+    motors.stopBothMotors();
 }
 
 /* MODIFIED WALL_FOLLOW FOR NAVIGATION */
@@ -643,18 +649,18 @@
 void tools_section(float* location, float &current)
 {
     wall_follow(LEFT,FORWARD, TOOLS);
-    //HARD STOP
-    motors.setMotor0Speed(-.3*127); //right
-    motors.setMotor1Speed(-.3*127); //left
-    wait_ms(5);
-    motors.stopBothMotors();
     // current position in reference to the starting position
     current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
-    pc.printf("current %f \r\n",current);
+   
+    //////////////////////////////// determine tool 
+    wait(2);
+    ///////////////////////////////////////////////////////////////////////////////////////
+    // Move Forward
+    slightMove(FORWARD, 100);
 
-    //Tool aquiring
+    //////////////////////////////////////////Tool aquiring
     wait(2);
-    // After tool is aquired
+    //////////////////////////////////////////////////////////////////// After tool is aquired
 
     alignWithWall(TOOLS);
     pc.printf("align\r\n");