Diff: main.cpp

Revision:

11:12ce7600f2f9

Parent:

10:2aa70a504c18

Child:

12:081b8fc654af

--- a/main.cpp	Thu Mar 27 18:32:27 2014 +0000
+++ b/main.cpp	Thu Mar 27 21:09:55 2014 +0000
@@ -183,18 +183,22 @@
 
 void wall_follow2(int side, int direction, int section, float location)
 {
-    int SeeWaveGap = false, dir=1;
+    bool SeeWaveGap = false, dir=1, limit;
     float set=5, loc=0;
 
     pid1.reset();
 
     if(direction == BACKWARD) dir=-1;
-    if(section == TOOLS)set= 5;
-
+    if(section == TOOLS){
+        set= 5;
+        limit = 78;
+    }
+    if(section == TOOLS) limit = 83; 
+    
     leftEncoder.reset();
     rightEncoder.reset();
 
-    while(dir*loc + location <= 78) {
+    while(dir*loc + location <= limit) {
         loc=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
 
         pid1.setInputLimits(0.0, set);
@@ -212,18 +216,19 @@
         }
 
 
-        /*************CHECK FOR WAVE OPENING*****************/
-        /* 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);
-        if(range > 20) {
-            motors.stopBothMotors();
+        if(range > 20 && !SeeWaveGap) {
+                motors.setMotor0Speed(dir*MAX_SPEED - dir*pid_return);//right
+                motors.setMotor1Speed(dir*MAX_SPEED);//left
+                wait_ms(30);
+                SeeWaveGap = true;
+         } else if(range > 20 && SeeWaveGap){
+            motors.stopBothMotors(); 
             bt.printf("wavegap\r\n");
             // AT WAVE OPENING!!!!
             break;
-        }
+        } else {
 
         pid1.setProcessValue(range);
         pid_return = pid1.compute();
@@ -249,7 +254,7 @@
             motors.setMotor0Speed(dir*MAX_SPEED);
             motors.setMotor1Speed(dir*MAX_SPEED);
         }
-    }
+    }}
     motors.stopBothMotors();
 }
 
@@ -334,16 +339,17 @@
     rightEncoder.reset();
     motors.setMotor0Speed(0.5*127);// right
     motors.setMotor1Speed(-0.5*127);// left
-    while(abs(leftEncoder.getPulses())<1000 || rightEncoder.getPulses()<1000);
+    while(abs(leftEncoder.getPulses())<1100 || rightEncoder.getPulses()<1100);
     motors.stopBothMotors();
 }
+
 void slightleft(void){
     
     leftEncoder.reset();
     rightEncoder.reset();
     motors.setMotor0Speed(0.5*127);// right
     motors.setMotor1Speed(-0.5*127);// left
-    while(abs(leftEncoder.getPulses())<50 || rightEncoder.getPulses()<50);
+    while(abs(leftEncoder.getPulses())<100 || rightEncoder.getPulses()<100);
     motors.stopBothMotors();
 }
 
@@ -452,7 +458,6 @@
 
     //Tool aquiring
     wait(2);
-    while(1){ }
     // After tool is aquired
 
     alignWithWall(TOOLS);
@@ -468,6 +473,10 @@
 
     if(range < 20) {
         wall_follow2(LEFT,BACKWARD,TOOLS, current);
+        
+        location[0]= current - ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
+        current= location[0];
+        
         leftEncoder.reset();
         rightEncoder.reset();
         motors.setMotor0Speed(-MAX_SPEED); //right
@@ -482,8 +491,6 @@
         motors.stopBothMotors();
 
         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);