Justin Carr / NavigationTest

Tools Section Navigation Calibrated slightly

Dependencies:   HCSR04 PID PololuQik2 QEI mbed-rtos

Fork of NavigationTest by Paolina Povolotskaya

Files at this revision

API Documentation at this revision

Revision 8:32ba0fad1689, committed 2014-03-22

Comitter:
jjcarr2
Date:
Sat Mar 22 16:24:59 2014 +0000
Parent:
7:0b7897232e93
Commit message:
Modular function blah;

Changed in this revision

main.cpp Show annotated file Show diff for this revision Revisions of this file 
--- a/main.cpp	Fri Mar 21 00:02:51 2014 +0000
+++ b/main.cpp	Sat Mar 22 16:24:59 2014 +0000
@@ -6,7 +6,7 @@
 #include "HCSR04.h"
 #include "stdio.h"
 #include "LPC17xx.h"
- 
+
 #define PIN_TRIGGERL    (p12)
 #define PIN_ECHOL       (p11)
 #define PIN_TRIGGERR    (p29)
@@ -20,328 +20,233 @@
 #define LEFT           (1)
 #define RIGHT          (0)
 #define FORWARD        (1)
-#define BACKWARD       (0) 
+#define BACKWARD       (0)
 #define TOOLS          (0)
 #define MID            (1)
-#define RIGS           (2)  
- 
-float range, pid_return;
+#define FIRST_WAVE     (0)
+#define FAR            (1)
+
+
+float range, pid_return, usValue;
 void errFunction(void);
 bool cRc;
- 
+
 //Hardware Initialization
 Serial bt(p13,p14);
 Serial pc(USBTX,USBRX);
 HCSR04 rangeFinderLeft( PIN_TRIGGERL, PIN_ECHOL );
 HCSR04 rangeFinderRight( PIN_TRIGGERR, PIN_ECHOR );
-PID pid1(15.0,0.0,10.0,0.02);
+PID pid1(15.0,0.0,4.0,0.02);
 PololuQik2 motors(p9, p10, p8, p15, errFunction, cRc);
 QEI rightEncoder(p17,p18,NC,PPR,QEI::X4_ENCODING);
 QEI leftEncoder(p16,p15,NC,PPR,QEI::X4_ENCODING);
 //InterruptIn encoder(p29);
- 
- 
+
+
 //Functions
- 
+
 float wall_follow(int side, int direction, int section);
-void wall_follow2(int side, int direction, int section);
+void wall_follow2(int side, int direction, int section, float location);
 void wall_follow3(int &currentLocation, int &WaveOpening);
 void leftTurn(void);
 void rightTurn(void);
-void us_distance(void);
-void tools_section(float *location, float &current);
-void overBump(void);
-void alignWithWall(void);
- 
+void us_distance(int side);
+void tools_section(float* location, float &current);
+void mid_section(float* location, float &current, int* direction);
+void mid_section2(float* location, float &current, int* direction);
+void rig_section(float* location, float &current, int* direction, int rig);
+void overBump(int wave);
+void alignWithWall(int section);
+void alignParallel(int side, int turn, float limit);
+
 //Variables
- 
+
 int main(void)
 {
     float location[3], current=0;
     int direction[3];
- 
+
     pc.baud(115200);
     bt.baud(115200);
     motors.begin();
-    
-    /*****************************OLD CODE FOR GOING THROUGH WAVE SECTION****************/
-    /*
-    leftEncoder.reset();
-    rightEncoder.reset();
-    wall_follow2(LEFT,FORWARD,TOOLS);
-    
-    motors.stopBothMotors();
-    wait_ms(500);
-    leftEncoder.reset();
-    rightEncoder.reset();
-    
-    motors.setMotor0Speed(0.6*MAX_SPEED); //right
-    motors.setMotor1Speed(0.6*MAX_SPEED); //left
-    
-    wait_ms(200);
-    motors.stopBothMotors();
-    
-    leftEncoder.reset();
-    rightEncoder.reset();
-    bt.printf("1st turn left\r\n");
-    
-    motors.setMotor0Speed(MAX_SPEED); //right
-    motors.setMotor1Speed(-MAX_SPEED); //left
-    
-    while(((abs(leftEncoder.getPulses())/(PPR) + abs(rightEncoder.getPulses())/(PPR))/2) < 1);
-    
-    leftEncoder.reset();
-    rightEncoder.reset();
-    
-    motors.setMotor0Speed(MAX_SPEED); //right
-    motors.setMotor1Speed(MAX_SPEED); //left
-    
-    while(((abs(leftEncoder.getPulses())/(PPR) + abs(rightEncoder.getPulses())/PPR)/2) < 2);
-    
-    leftEncoder.reset();
-    rightEncoder.reset();
-    motors.setMotor0Speed(-MAX_SPEED); //right
-    motors.setMotor1Speed(MAX_SPEED); //left
-    float usValue = 0;
-    while(1){
-        rangeFinderLeft.startMeas();
-        wait_ms(100);
-        rangeFinderLeft.getMeas(range);
-        bt.printf("Range %f \t OldValue %f\n\r",range, usValue);
-        if(range > usValue && usValue != 0 && range < 10){
-            break;
-        } else {
-            usValue = range;    
-        }
-    }
-    motors.stopBothMotors();
-    
-    leftEncoder.reset();
-    rightEncoder.reset();
-    
-    wall_follow2(LEFT,FORWARD,TOOLS);
-    
-    while(1){}
-    */
-    /*
-    //  Very Consistent Turn
-    leftEncoder.reset();
-    rightEncoder.reset();
-    
-    motors.setMotor0Speed(-MAX_SPEED); //right
-    motors.setMotor1Speed(MAX_SPEED); //left
-    float usValue = 0;
-    while(1){
-        rangeFinderLeft.startMeas();
-        wait_ms(100);
-        rangeFinderLeft.getMeas(range);
-        bt.printf("Range %f \t OldValue %f\n\r",range, usValue);
-        if(range > usValue && usValue != 0 && range < 10){
-            break;
-        } else {
-            usValue = range;    
-        }
-    }
-    */
-    motors.stopBothMotors();
-    /*
-    
-    leftEncoder.reset();
-    rightEncoder.reset();
-    motors.setMotor0Speed(MAX_SPEED); //right
-    motors.setMotor1Speed(MAX_SPEED); //left
 
-    while(leftEncoder.getPulses()/(PPR) < 3);
-    */
-    
-    //leftEncoder.reset();
-    //rightEncoder.reset();
-    //motors.setMotor0Speed(MAX_SPEED); //right
-    //motors.setMotor1Speed(MAX_SPEED); //left
-    
-    //while((abs(leftEncoder.getPulses())/(PPR) + abs(rightEncoder.getPulses())/(PPR))/2 < 3)
- 
-    
-    
+    bt.printf("START\r\n");
     //Go to tools
     tools_section(location, current);
-    bt.printf("Location 0 = %f", location[0]);
-    
- /*   
-    //////////////////////////////// without predefined wavegaps//////////////////////////////////////////////
-    current=0;
-    if(location[0]< 75){
-        turnRight();
-        current=wall_follow(LEFT,FORWARD);
-        if(current == 0)turnLeft();
-        else{
-            direction[0]= RIGHT;
-            turnLeft();
-            overBump();
-        }
-    }
-    else if(location[0]>=75 || current == 0){  
-        turnLeft();
-        wall_follow2(RIGHT,FORWARD);  
-    }
-        
-    
-    
-    
-    
-    
-    // left or right
-    
-    
-    location[1]=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
-    
-    
-    
-    
-     
-    
-    
-    leftTurn();
-    //wall_follow2(RIGHT);
-    rightTurn();
-    
- 
- 
-    bt.printf("LOCATION %f\n\r",location);
- 
-    motors.stopBothMotors();
-//   leftTurn();
-//   wait(1);
-//   rightTurn();
-*/
- 
+    mid_section(location, current, direction);
+    mid_section2(location, current, direction);
+
+
+
 }
- 
+
 void errFunction(void)
 {
     //Nothing
 }
- 
-void us_distance(void)
+
+void us_distance(int side)
+{
+    if(side == LEFT) {
+        rangeFinderLeft.startMeas();
+        wait_us(20);
+        rangeFinderLeft.getMeas(range);
+    } else {
+
+        rangeFinderRight.startMeas();
+        wait_us(20);
+        rangeFinderRight.getMeas(range);
+    }
+
+}
+
+void alignParallel(int side, int turn, float limit)
 {
- 
-    rangeFinderLeft.startMeas();
-    wait_us(20);
-    if ( (rangeFinderLeft.getMeas(range) == RANGE_MEAS_VALID)) {
-        pc.printf("Range = %f\n\r", range);
+    leftEncoder.reset();
+    rightEncoder.reset();
+
+    if(turn == LEFT) {
+        motors.setMotor0Speed(MAX_SPEED); //right
+        motors.setMotor1Speed(-MAX_SPEED); //left
+        while(rightEncoder.getPulses() < 100);
+
+        motors.stopBothMotors();
+
+        motors.setMotor0Speed(0.7*MAX_SPEED); //right
+        motors.setMotor1Speed(-0.7*MAX_SPEED); //left
+    } else {
+        motors.setMotor0Speed(-MAX_SPEED); //right
+        motors.setMotor1Speed(MAX_SPEED); //left
+        while(leftEncoder.getPulses() < 100);
+
+        motors.stopBothMotors();
+
+        motors.setMotor0Speed(-0.7*MAX_SPEED); //right
+        motors.setMotor1Speed(0.7*MAX_SPEED); //left
     }
+    
+    usValue = 0;
+    
+    if(side == LEFT) {
+        while(1) {
+            us_distance(LEFT);
+            //bt.printf("Range %f \t OldValue %f\n\r",range, usValue);
+            if(range > usValue && usValue != 0 && range < limit) {
+                break;
+            } else {
+                usValue = range;
+            }
+        }
+    } else {
+        while(1) {
+            us_distance(RIGHT);
+            //bt.printf("Range %f \t OldValue %f\n\r",range, usValue);
+            if(range > usValue && usValue != 0 && range < limit) {
+                break;
+            } else {
+                usValue = range;
+            }
+        }
+    }
+    motors.stopBothMotors();
 }
- 
+
 float wall_follow(int side, int direction, int section)
 {
-    float location, wavegap;
-    int dir=1, set=5;
-    
+    float location, wavegap=0, set=4;
+    int dir=1;
+
     pid1.reset();
-    
+
     if(direction == BACKWARD) dir=-1;
     if(section == TOOLS)set= 10;
-    
+
     leftEncoder.reset();
     rightEncoder.reset();
-    
+
     location=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
-    
-    while(location< 75) {
+
+    while(location< 78) {
         location=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
-        
+
         pid1.setInputLimits(0, set);
         pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED);
         pid1.setSetPoint(set);
-        if(side){   //Side = 1 for Left
-            rangeFinderLeft.startMeas();
-            wait_ms(20);
-            rangeFinderLeft.getMeas(range);
+        if(side) {
+            us_distance(LEFT);
+        } else {
+            us_distance(RIGHT);
         }
-        else{       //Use right Ultrasonic
-            rangeFinderRight.startMeas();
-            wait_ms(20);
-            rangeFinderRight.getMeas(range);
-            pc.printf("%d\r\n",range);
-        }
-        
+
         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*MAX_SPEED);//left
-            motors.setMotor0Speed(dir*MAX_SPEED);//right
-
+            motors.setMotor1Speed(dir*0.3*127);//left
+            motors.setMotor0Speed(dir*0.3*127);//right
         } else {
-        
-        
-        pid1.setProcessValue(range);
-        pid_return = pid1.compute();
- 
-        if(pid_return > 0) {
-            if(side){
-                motors.setMotor0Speed(dir*MAX_SPEED - dir*pid_return);//right
+
+            pid1.setProcessValue(range);
+            pid_return = pid1.compute();
+
+            if(pid_return > 0) {
+                if(side) {
+                    motors.setMotor0Speed(dir*MAX_SPEED - dir*pid_return);//right
+                    motors.setMotor1Speed(dir*MAX_SPEED);//left
+                } else {
+                    motors.setMotor1Speed(dir*MAX_SPEED - dir*pid_return);//left
+                    motors.setMotor0Speed(dir*MAX_SPEED);//right
+                }
+            } else if(pid_return < 0) {
+                if(side) {
+                    motors.setMotor0Speed(dir*MAX_SPEED);//right
+                    motors.setMotor1Speed(dir*MAX_SPEED + dir*pid_return);//left
+                } else {
+                    motors.setMotor1Speed(dir*MAX_SPEED);//left
+                    motors.setMotor0Speed(dir*MAX_SPEED + dir*pid_return);//right
+                }
+            } else {
+                motors.setMotor0Speed(dir*MAX_SPEED);//right
                 motors.setMotor1Speed(dir*MAX_SPEED);//left
             }
-            else{
-                motors.setMotor1Speed(dir*MAX_SPEED - dir*pid_return);//left
-                motors.setMotor0Speed(dir*MAX_SPEED);//right
-            }
-        }else if(pid_return < 0) {
-            if(side){
-                motors.setMotor0Speed(dir*MAX_SPEED);//right
-                motors.setMotor1Speed(dir*MAX_SPEED + dir*pid_return);//left
-            }
-            else{
-                motors.setMotor1Speed(dir*MAX_SPEED);//left
-                motors.setMotor0Speed(dir*MAX_SPEED + dir*pid_return);//right
-            }
-        }else {
-            motors.setMotor0Speed(dir*MAX_SPEED);//right
-            motors.setMotor1Speed(dir*MAX_SPEED);//left
         }
-        }
-        }
+    }
     return wavegap;
 }
- 
+
 /* MODIFIED WALL_FOLLOW FOR NAVIGATION */
- 
-void wall_follow2(int side, int direction, int section)
+
+void wall_follow2(int side, int direction, int section, float location)
 {
-    int SeeWaveGap = false, count=0, set=5;
-    float dir=1.0; 
-    
+    int SeeWaveGap = false, dir=1;
+    float set=4, loc=0;
+
     pid1.reset();
-    
+
     if(direction == BACKWARD) dir=-1;
     if(section == TOOLS)set= 5;
-    
+
     leftEncoder.reset();
     rightEncoder.reset();
- 
-    while(1) {
- 
+
+    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);
         pid1.setSetPoint(set);
- 
-        if(side){
-            rangeFinderLeft.startMeas();
-            wait_ms(20);
-            rangeFinderLeft.getMeas(range);
+
+        if(side) {
+            us_distance(LEFT);
+        } else {
+            us_distance(RIGHT);
         }
-        else{
-            rangeFinderRight.startMeas();
-            wait_ms(20);
-            rangeFinderRight.getMeas(range);
-        }
- 
- 
+
+
         /*************CHECK FOR WAVE OPENING*****************/
         /* If after 20 ms the ultrasonic still sees 20+ cm */
         /*      then robot is at wave opening               */
- 
+
         pc.printf("range %f\r\n",range);
         if(range > 20) {
             motors.stopBothMotors();
@@ -349,26 +254,24 @@
             // AT WAVE OPENING!!!!
             break;
         }
- 
+
         pid1.setProcessValue(range);
         pid_return = pid1.compute();
         //bt.printf("Range: %f\n      PID:   %f\r\n", range, pid_return);
- 
-               if(pid_return > 0) {
-            if(side){
+
+        if(pid_return > 0) {
+            if(side) {
                 motors.setMotor0Speed(dir*MAX_SPEED - dir*pid_return);//right
                 motors.setMotor1Speed(dir*MAX_SPEED);//left
-            }
-            else{
+            } else {
                 motors.setMotor1Speed(dir*MAX_SPEED - dir*pid_return);//left
                 motors.setMotor0Speed(dir*MAX_SPEED);//right
             }
-        }else if(pid_return < 0) {
-            if(side){
+        } else if(pid_return < 0) {
+            if(side) {
                 motors.setMotor0Speed(dir*MAX_SPEED);//right
                 motors.setMotor1Speed(dir*MAX_SPEED + dir*pid_return);//left
-            }
-            else{
+            } else {
                 motors.setMotor1Speed(dir*MAX_SPEED);//left
                 motors.setMotor0Speed(dir*MAX_SPEED + dir*pid_return);//right
             }
@@ -377,214 +280,210 @@
             motors.setMotor1Speed(dir*MAX_SPEED);
         }
     }
+    motors.stopBothMotors();
 }
- 
- 
-/* MODIFIED WALL_FOLLOW FOR NAVIGATION WITH WAVE OPENINGS PASSED IN */
-/* MEANT FOR RETURNING FROM OIL RIGS */
- 
-void wall_follow3(int &currentLocation, int &WaveOpening)
+
+
+void alignWithWall(int section)
 {
-    while(1) {
- 
- 
-        pid1.setInputLimits(0, 5);
-        pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED);
-        pid1.setSetPoint(5.0);
- 
-        rangeFinderLeft.startMeas();
-        wait_ms(100);
-        if ( (rangeFinderLeft.getMeas(range) == RANGE_MEAS_VALID) && (range < 100.0) && (range > 3.0)) {
-            //bt.printf("Range = %f\n", range);
-        }
- 
-        /*************CHECK FOR WAVE OPENING*****************/
-        /* If after 100 ms the ultrasonic still sees 20+ cm */
-        /*      then robot is at wave opening               */
- 
- 
-        if(range > 20 ) {
-            currentLocation--;
-        }
- 
-        if( currentLocation == WaveOpening) {
-            // AT WAVE OPENING!!!!
- 
-            break;
-        }
- 
- 
-        pid1.setProcessValue(range);
-        pid_return = pid1.compute();
-        bt.printf("Range: %f\n      PID:   %f", range, pid_return);
- 
-        if(pid_return > 0) {
-            motors.setMotor0Speed(MAX_SPEED - pid_return);
-            motors.setMotor1Speed(MAX_SPEED);
-        } else if(pid_return < 0) {
-            motors.setMotor0Speed(MAX_SPEED);
-            motors.setMotor1Speed(MAX_SPEED + pid_return);
-        } else {
-            motors.setMotor0Speed(MAX_SPEED);
-            motors.setMotor1Speed(MAX_SPEED);
-        }
+    usValue = 0;
+
+    if(section == TOOLS) {
+        // turn at an angle
+        leftEncoder.reset();
+        rightEncoder.reset();
+        motors.setMotor0Speed(-MAX_SPEED); //right
+        motors.setMotor1Speed(0.4*MAX_SPEED); //left
+        while(rightEncoder.getPulses()>-1000);
+        motors.stopBothMotors();
+
+        //go backwards toward wall
+        leftEncoder.reset();
+        rightEncoder.reset();
+        motors.setMotor0Speed(-MAX_SPEED); //right
+        motors.setMotor1Speed(-MAX_SPEED); //left
+        while(abs(leftEncoder.getPulses()) < 300 || abs(rightEncoder.getPulses()) < 300);
+
+        motors.stopBothMotors();
+
+        // turn left towards wall
+        alignParallel(LEFT,LEFT,100);
+    } else {
+        // turn right towards wall
+        alignParallel(LEFT,RIGHT, 60);    
     }
 }
- 
+
 void rightTurn(void)
 {
-    float speedL, speedR;
- 
-    speedL=speedR= 0.4;
- 
     leftEncoder.reset();
     rightEncoder.reset();
-    motors.setMotor0Speed(-speedR*127);//right
-    motors.setMotor1Speed(speedL*127);//left
-    while(leftEncoder.getPulses()<900 || rightEncoder.getPulses()>-900);
+    motors.setMotor0Speed(-0.4*127);//right
+    motors.setMotor1Speed(0.4*127);//left
+    while(leftEncoder.getPulses()<1000 || rightEncoder.getPulses()>-1000);
     motors.stopBothMotors();
 }
- 
+
 void leftTurn(void)
 {
     leftEncoder.reset();
     rightEncoder.reset();
     motors.setMotor0Speed(0.4*127);// right
     motors.setMotor1Speed(-0.4*127);// left
-    while(leftEncoder.getPulses()>-940 || rightEncoder.getPulses()<940);
-    motors.stopBothMotors();
-}
- 
-void overBump(void){
-    
-    leftEncoder.reset();
-    rightEncoder.reset();
-    motors.setMotor0Speed(MAX_SPEED); //right
-    motors.setMotor1Speed(MAX_SPEED); //left
-    while(abs(leftEncoder.getPulses()/PPR) + abs(rightEncoder.getPulses()/PPR)/2 < 1);
-    /*
-    leftEncoder.reset();
-    rightEncoder.reset();
-    motors.setMotor0Speed(MAX_SPEED); //right
-    motors.setMotor1Speed(MAX_SPEED); //left
-    while(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR)/2 <3);
-    */
+    while(abs(leftEncoder.getPulses())<1075 || rightEncoder.getPulses()<1075);
     motors.stopBothMotors();
 }
-void alignWithWall(void){
-    
-    leftEncoder.reset();
-    rightEncoder.reset();
-    
-    motors.setMotor0Speed(-MAX_SPEED); //right
-    motors.setMotor1Speed(0.4*MAX_SPEED); //left
-    while(rightEncoder.getPulses()>-1000);
-    motors.stopBothMotors();
-    
+
+
+void overBump(int wave)
+{
+    int preLeft=5000, preRight=5000 ;
+
     leftEncoder.reset();
     rightEncoder.reset();
-    
-    motors.setMotor0Speed(-MAX_SPEED); //right
-    motors.setMotor1Speed(-MAX_SPEED); //left
-    while(abs(leftEncoder.getPulses()*11.12/PPR) < 3);
-    
-    motors.setMotor0Speed(0.1*MAX_SPEED); //right
-    motors.setMotor1Speed(0.1*MAX_SPEED); //left
-    
-    wait_ms(200);
-    
-    motors.stopBothMotors();
-    
+    motors.setMotor0Speed(0.15*127); //right
+    motors.setMotor1Speed(0.15*127); //left
+    while((abs(leftEncoder.getPulses()) < 700 || abs(rightEncoder.getPulses())< 700) && preLeft!=0) {
+        preLeft=leftEncoder.getPulses();
+        preRight=rightEncoder.getPulses();
+        wait_ms(20);
+        if(leftEncoder.getPulses() == preLeft && rightEncoder.getPulses()== preRight) preLeft=preRight=0;
+    }
+
+    if(wave == FAR) {
+        while(leftEncoder.getPulses() != preLeft && rightEncoder.getPulses()!= preRight) {
+            preLeft=leftEncoder.getPulses();
+            preRight=rightEncoder.getPulses();
+            wait_ms(20);
+        }
+
+        motors.stopBothMotors();
+    }
+
     leftEncoder.reset();
     rightEncoder.reset();
-    
-    motors.setMotor0Speed(MAX_SPEED); //right
-    motors.setMotor1Speed(-MAX_SPEED); //left
-    while(rightEncoder.getPulses() < 260);
+    motors.setMotor0Speed(0.4*127); //right
+    motors.setMotor1Speed(0.4*127); //left
+    while(abs(leftEncoder.getPulses()) < 200 || abs(rightEncoder.getPulses()) < 200);
+
+    motors.stopBothMotors();
+}
+
+void tools_section(float* location, float &current)
+{
+
+    wall_follow(LEFT,FORWARD, TOOLS);
+    // current position in reference to the starting position
+    current=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
+    bt.printf("current %f \r\n",current);
+
+    motors.stopBothMotors();
+
+    //Tool aquiring
+    wait(2);
+    // After tool is aquired
+
+    alignWithWall(TOOLS);
+    current-=8;
+    wait_ms(100);
+
+    us_distance(LEFT);
     
-    motors.setMotor0Speed(0.7*MAX_SPEED); //right
-    motors.setMotor1Speed(-0.7*MAX_SPEED); //left
-    float usValue = 0;
-    while(1){
-        rangeFinderLeft.startMeas();
-        wait_ms(100);
-        rangeFinderLeft.getMeas(range);
-        bt.printf("Range %f \t OldValue %f\n\r",range, usValue);
-        if(range > usValue && usValue != 0 && range < 25){
-            break;
-        } else {
-            usValue = range;    
-        }
+    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];
+
+        /*      // go backwards
+              leftEncoder.reset();
+              rightEncoder.reset();
+              motors.setMotor0Speed(-0.3*127); //right
+              motors.setMotor1Speed(-0.3*127); //left
+              while(abs(leftEncoder.getPulses()) < 150 || abs(rightEncoder.getPulses()) < 150);
+
+              motors.stopBothMotors();
+              */
+
+        leftTurn();
+        overBump(FAR);
+    } else {
+        location[0]= 77;
+        leftTurn();
+        wait_ms(20);
+        overBump(FIRST_WAVE);
     }
-    
-    
-    
-    
+
+    bt.printf("wavegap = %f\r\n",location[0]);
 }
 
- 
-void tools_section(float *location, float &current){
-    
-    //location[0]=wall_follow(LEFT,FORWARD, TOOLS); //location from the left edge of the field
-    //Too Far from wall to see gap :(
-    wall_follow(LEFT,FORWARD,TOOLS);
-    
-    current=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
-    
-    bt.printf("wavegap %f \t current %f \r\n",location[0],current);
-    
-     motors.stopBothMotors();
-    ////////////////////////////////////////// determine shape and pick up tool ///////////////////////////////////////////////////////
-        alignWithWall();
-        wait_ms(200);
-        
-        rangeFinderLeft.startMeas();
-        wait_ms(100);
-        rangeFinderLeft.getMeas(range);
-        if(range < 10){
-        
-        wall_follow2(LEFT,BACKWARD,TOOLS);
-        
-        leftEncoder.reset();
-        rightEncoder.reset();
-        motors.setMotor0Speed(-0.9*MAX_SPEED); //right
-        motors.setMotor1Speed(-0.9*MAX_SPEED); //left
-        while(abs(leftEncoder.getPulses()*11.12/PPR) < 2);
-        motors.stopBothMotors();
-        
-        // forward 
-        
- /*     leftEncoder.reset();
-        rightEncoder.reset();
-        motors.setMotor0Speed(-MAX_SPEED); //right
-        motors.setMotor1Speed(-MAX_SPEED); //left
-        while(abs(leftEncoder.getPulses())<300 || abs(rightEncoder.getPulses())<300);
-        motors.stopBothMotors();
-        wait_ms(500);*/
-        current = (abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; 
-        // REMOVED += because current should just be current pulses?
-        /*
-        wall_follow2(LEFT,FORWARD);   
-        // backward 
-        leftEncoder.reset();
-        rightEncoder.reset();
-        motors.setMotor0Speed(MAX_SPEED); //right
-        motors.setMotor1Speed(MAX_SPEED); //left
-        while(abs(leftEncoder.getPulses())<300 || abs(rightEncoder.getPulses())<300);
-        motors.stopBothMotors();
-        current-=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;*/
-    
-    
+void mid_section(float* location, float &current, int* direction)
+{
+
+    motors.begin();
+    alignWithWall(MID);
+    /*
+       leftEncoder.reset();
+       rightEncoder.reset();
+       motors.setMotor0Speed(MAX_SPEED); //right
+       motors.setMotor1Speed(MAX_SPEED); //left
+       while(abs(leftEncoder.getPulses())<75 || abs(rightEncoder.getPulses())<75);
+       motors.stopBothMotors();
+       */
+    bt.printf("mid section current = %f\r\n",current);
+    wall_follow2(LEFT,FORWARD,MID, current);
+    current=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
+    bt.printf("after wf2 current = %f\r\n",current);
+
+    if(current != 0) {
+        direction[0]= RIGHT;
+        current+= location[0];
+        location[1]= current;
+    } else {
+        current=location[0];
+        direction[0]= LEFT;
+        wall_follow2(LEFT,BACKWARD,MID,current);
+        location[1]= location[0]- ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
+    }
+
+    bt.printf("wavegap2 = %f\r\n",location[1]);
     leftTurn();
-    
-    //Go over 
-    overBump();
-    } else { 
-    
+    overBump(FAR);
+    // go forward
+    leftEncoder.reset();
+    rightEncoder.reset();
+    motors.setMotor0Speed(0.2*127); //right
+    motors.setMotor1Speed(0.2*127); //left
+    while(abs(leftEncoder.getPulses())<300 || abs(rightEncoder.getPulses())<300);
+    motors.stopBothMotors();
+
+}
+
+void mid_section2(float* location, float &current, int* direction)
+{
+
+    motors.begin();
+    rightTurn();
+    alignWithWall(MID);
+    wall_follow2(LEFT,FORWARD,MID, current);
+    current=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
+
+    if(current != 0) {
+        direction[1]= RIGHT;
+        current+= location[1];
+        location[2]= current;
+    } else {
+        current=location[1];
+        direction[1]= LEFT;
+        wall_follow2(LEFT,BACKWARD,MID,current);
+        location[2]= location[1]- ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
+    }
+
     leftTurn();
-    //Go over 
-    overBump();
-    }
-    
+    overBump(FAR);
 }
- 
\ No newline at end of file
+
+void rig_section(float* location, float &current, int* direction, int rig)
+{
+
+
+}
\ No newline at end of file