Diff: main.cpp

Revision:

7:78745a518957

Parent:

6:f5c26372b2d0

Child:

8:11ef93eebe07

--- a/main.cpp	Fri Mar 21 21:26:50 2014 +0000
+++ b/main.cpp	Fri Mar 21 22:19:51 2014 +0000
@@ -69,6 +69,7 @@
     bt.baud(115200);
     motors.begin();
     
+    bt.printf("START\r\n");
     //Go to tools
     tools_section(location, current);
     mid_section(location, current, direction);
@@ -128,7 +129,7 @@
         
         if(range > 20) {
             wavegap=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
-            bt.printf("wavegap %f\r\n",wavegap);
+            //bt.printf("wavegap %f\r\n",wavegap);
             // AT WAVE OPENING!!!!
             motors.setMotor1Speed(dir*0.3*127);//left
             motors.setMotor0Speed(dir*0.3*127);//right
@@ -170,7 +171,7 @@
 void wall_follow2(int side, int direction, int section, float location)
 {
     int SeeWaveGap = false, dir=1;
-    float set=4, loc;
+    float set=4, loc=0;
     
     pid1.reset();
     
@@ -179,9 +180,9 @@
     
     leftEncoder.reset();
     rightEncoder.reset();
- 
-    while(loc - location ) {
-        location=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
+        
+    while(loc + location < 80) {
+        loc=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
         
         pid1.setInputLimits(0.0, set);
         pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED);
@@ -259,7 +260,7 @@
         rightEncoder.reset(); 
         motors.setMotor0Speed(-MAX_SPEED); //right
         motors.setMotor1Speed(-MAX_SPEED); //left
-        while(abs(leftEncoder.getPulses()) < 350 || abs(rightEncoder.getPulses()) < 350);
+        while(abs(leftEncoder.getPulses()) < 300 || abs(rightEncoder.getPulses()) < 300);
         
         motors.stopBothMotors();
     
@@ -271,28 +272,29 @@
         while(rightEncoder.getPulses() < 100);
         
         motors.stopBothMotors();   
-    }
-    else{
-        // turn right towards wall 
+        
+        motors.setMotor0Speed(0.7*MAX_SPEED); //right
+        motors.setMotor1Speed(-0.7*MAX_SPEED); //left
+    } else {
+        // turn right towards wall
         leftEncoder.reset();
         rightEncoder.reset();
         motors.setMotor0Speed(-MAX_SPEED); //right
         motors.setMotor1Speed(MAX_SPEED); //left
-        while(abs(rightEncoder.getPulses()) < 300);
-        
+        while(abs(rightEncoder.getPulses()) < 100);
+
         motors.stopBothMotors();
+
+        motors.setMotor0Speed(-0.7*MAX_SPEED); //right
+        motors.setMotor1Speed(0.7*MAX_SPEED); //left
     }
     
-    // align using the distance of the wall   
-    motors.setMotor0Speed(0.7*MAX_SPEED); //right
-    motors.setMotor1Speed(-0.7*MAX_SPEED); //left
-    
     usValue = 0;
     while(1){
         rangeFinderLeft.startMeas();
         wait_ms(20);
         rangeFinderLeft.getMeas(range);
-        bt.printf("Range %f \t OldValue %f\n\r",range, usValue);
+        //bt.printf("Range %f \t OldValue %f\n\r",range, usValue);
         if(range > usValue && usValue != 0 && range < 25){
             break;
         } else {
@@ -408,7 +410,6 @@
 void mid_section(float* location, float &current, int* direction){
     
     motors.begin();
-    rightTurn();
     alignWithWall(MID);
  /*   
     leftEncoder.reset();