Diff: main.cpp

Revision:

14:3c8c4efe4786

Parent:

13:9bad7f74833a

Child:

15:a467af795e57

--- a/main.cpp	Sun Mar 30 00:29:49 2014 +0000
+++ b/main.cpp	Sun Mar 30 18:21:06 2014 +0000
@@ -7,7 +7,7 @@
 #include "stdio.h"
 #include "LPC17xx.h"
 #include "Sharp.h"
- 
+
 #define PIN_TRIGGERL    (p12)
 #define PIN_ECHOL       (p11)
 #define PIN_TRIGGERR    (p29)
@@ -26,13 +26,15 @@
 #define TOOLS          (0)
 #define MID            (1)
 #define RIGS           (2)
+#define MID2           (3)
+#define RETURN         (4)
 #define FAR            (1)
- 
- 
+
+
 float range, range2, pid_return;
 void errFunction(void);
 bool cRc;
- 
+
 //Hardware Initialization
 Serial bt(p13,p14);
 Serial pc(USBTX,USBRX);
@@ -44,14 +46,15 @@
 QEI leftEncoder(p16,p15,NC,PPR,QEI::X4_ENCODING);
 Sharp IR(p20);
 //InterruptIn encoder(p29);
- 
- 
+
+
 //Functions
- 
+
 void wall_follow(int side, int direction, int section);
 void wall_follow2(int side, int direction, int section, float location, int rig);
 void leftTurn(void);
 void slightleft(void);
+void slightright(void);
 void rightTurn(void);
 void slightMove(int direction, float pulses);
 void us_distance(void);
@@ -59,54 +62,66 @@
 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 tools_section_return(float* location, float &current);
+void mid_section_return(float* location, float &current, int* direction);
+void mid_section2_return(float* location, float &current, int* direction);
+void rig_section_return(float* location, float &current, int* direction);
 void overBump(int section);
 void alignWithWall(int section);
 void UntilWall(int dir);
 
- 
+
 //Variables
- 
+
 int main(void)
 {
     float location[3], current=4;
     int direction[3];
     double distance;
- 
+
     pc.baud(115200);
     pc.baud(115200);
     motors.begin();
- 
+
     pc.printf("START\r\n");
     //Go to tools
     tools_section(location, current);
     mid_section(location, current, direction);
     mid_section2(location, current, direction);
     rig_section(location, current, direction, 3);
+    // fire putting out
+    wait(2);
+    //
+    rig_section_return(location, current, direction);
+    mid_section2_return(location, current, direction);
+    mid_section_return(location, current, direction);
+    tools_section_return(location,current);
+
     /*while(1) {
         pc.printf("IR %f\r\n", IR.getDistance());
         /*rangeFinderLeft.startMeas();
         rangeFinderRight.startMeas();
         wait_ms(38);
         rangeFinderLeft.getMeas(range);
-        rangeFinderRight.getMeas(range); 
+        rangeFinderRight.getMeas(range);
         pc.printf("leftRange = %f\n\r", range);
         pc.printf("rightRange = %f\n\r", range);
- 
+
         wait_ms(200);
     }*/
- 
- //wall_follow2(LEFT,FORWARD,MID,0);
+
+//wall_follow2(LEFT,FORWARD,MID,0);
     //leftTurn();
     //rightTurn();
- 
- 
+
+
 }
- 
+
 void errFunction(void)
 {
     //Nothing
 }
- 
+
 void us_distance(void)
 {
     pc.printf("Ultra Sonic\n\r");
@@ -116,25 +131,25 @@
         pc.printf("Range = %f\n\r", range);
     }
 }
- 
+
 void wall_follow(int side, int direction, int section)
 {
     float location, set=6;
     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< 69) {
         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);
@@ -148,17 +163,17 @@
             rangeFinderRight.getMeas(range);
             pc.printf("%d\r\n",range);
         }
- 
-        if(range > 20) {
+
+        if(range > 15) {
             //pc.printf("wavegap %f\r\n",wavegap);
             // AT WAVE OPENING!!!!
             motors.setMotor1Speed(dir*0.25*127);//left
             motors.setMotor0Speed(dir*0.25*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
@@ -182,62 +197,65 @@
         }
     }
 }
- 
+
 /* MODIFIED WALL_FOLLOW FOR NAVIGATION */
- 
+
 void wall_follow2(int side, int direction, int section, float location, int rig)
 {
-    int dir=1, limit=90;
+    int dir=1, limit=86, lowlim=5;
     float set=6, loc=0, Rigloc=0;
-    
+    bool SeeWaveGap = false;
+
     if(rig == 1) Rigloc= 16;
     else if(rig == 2) Rigloc= 45;
     else if(rig== 3) Rigloc = 70;
- 
+
     pid1.reset();
- 
-    if(direction == BACKWARD){
-         dir=-1;
-         limit = 100;
+
+    if(direction == BACKWARD) {
+        dir=-1;
+        limit = 100;
     }
-    if(section == TOOLS){
+    if(section == TOOLS) {
         set= 6;
-        limit = 89;
+        limit = 86;
     }
+    if(section == RETURN) lowlim=15;
 
     leftEncoder.reset();
     rightEncoder.reset();
-    
+
     //pc.printf("before %f\r\n", location);
-    
+
     pc.printf("dir*loc+location %f\r\n",dir*loc + location );
     pc.printf("limit %d \r\n", limit);
-    
-    while((dir*loc + location <= limit) && (dir*loc + location >= 5)) {
+
+    while((dir*loc + location <= limit) && (dir*loc + location >= lowlim)) {
+        
         loc=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
         pc.printf("loc %f \r\n", loc);
- 
+
         pid1.setInputLimits(0.0, set);
         pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED);
         pid1.setSetPoint(set);
- 
+
         if(side) {
             rangeFinderLeft.startMeas();
             wait_ms(20);
             rangeFinderLeft.getMeas(range);
-        } else{
+        } else {
             rangeFinderRight.startMeas();
             wait_ms(20);
             rangeFinderRight.getMeas(range);
         }
-        
-        if(section == RIGS){
+
+        if(section == RIGS) {
             rangeFinderLeft.startMeas();
             wait_ms(20);
             rangeFinderLeft.getMeas(range2);
-            
-            if(range2< 20){
-                if( abs(dir*loc + location - Rigloc) < 10){
+
+            if(range2< 20) {
+                if( abs(dir*loc + location - Rigloc) < 10) {
                     //STOP
                     motors.setMotor0Speed(dir*-0.25*127); //right
                     motors.setMotor1Speed(dir*-0.25*127); //left
@@ -247,66 +265,70 @@
                 }
             }
         }
- 
- 
+
+
         //pc.printf("wall follow 2 range %f\r\n",range);
         //pc.printf("loc+location = %f\r\n", loc+location);
         if(range > 20 ) {
-            if(section == RIGS){
+            if(section == RIGS || section == RETURN) {
                 motors.setMotor0Speed(dir*0.25*127); //right
                 motors.setMotor1Speed(dir*0.25*127); //left
-            }
-            else{
-                //STOP
-                motors.setMotor0Speed(dir*-0.25*127); //right
-                motors.setMotor1Speed(dir*-0.25*127); //left
-                wait_ms(5);
-                motors.stopBothMotors();
-                
-                pc.printf("wavegap\r\n");
-                // AT WAVE OPENING!!!!
-                break;
+            }else {
+                if(!SeeWaveGap) {
+                    SeeWaveGap=true;
+                }else {
+                    //STOP
+                    motors.setMotor0Speed(dir*-0.25*127); //right
+                    motors.setMotor1Speed(dir*-0.25*127); //left
+                    wait_ms(5);
+                    motors.stopBothMotors();
+
+                    pc.printf("wavegap\r\n");
+                    // AT WAVE OPENING!!!!
+                    break;
+                }
             }
         } else {
- 
-        pid1.setProcessValue(range);
-        pid_return = pid1.compute();
-        //pc.printf("Range: %f\n      PID:   %f\r\n", range, pid_return);
- 
-        if(pid_return > 0) {
-            if(side) {
-                motors.setMotor0Speed(dir*MAX_SPEED - dir*pid_return);//right
-                motors.setMotor1Speed(dir*MAX_SPEED);//left
+            SeeWaveGap = false;
+            pid1.setProcessValue(range);
+            pid_return = pid1.compute();
+            //pc.printf("Range: %f\n      PID:   %f\r\n", range, pid_return);
+
+            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.setMotor1Speed(dir*MAX_SPEED - dir*pid_return);//left
-                motors.setMotor0Speed(dir*MAX_SPEED);//right
+                motors.setMotor0Speed(dir*MAX_SPEED);
+                motors.setMotor1Speed(dir*MAX_SPEED);
             }
-        } 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);
-            motors.setMotor1Speed(dir*MAX_SPEED);
         }
-    }}
-    
+    }
+
     //STOP
-    motors.setMotor0Speed(dir*-0.25*127); //right
-    motors.setMotor1Speed(dir*-0.25*127); //left
+    motors.setMotor0Speed(dir*-0.3*127); //right
+    motors.setMotor1Speed(dir*-0.3*127); //left
     wait_ms(5);
     motors.stopBothMotors();
 }
- 
- 
+
+
 void alignWithWall(int section)
 {
     float usValue = 0;
- 
+
     if(section == TOOLS) {
         pc.printf("tools section align\r\n");
         // turn at an angle
@@ -316,7 +338,7 @@
         motors.setMotor1Speed(0.4*MAX_SPEED); //left
         while(rightEncoder.getPulses()>-1000);
         motors.stopBothMotors();
- 
+
         //go backwards toward wall
         leftEncoder.reset();
         rightEncoder.reset();
@@ -324,31 +346,60 @@
         motors.setMotor1Speed(-0.25*127); //left
         while(abs(leftEncoder.getPulses()) < 300 || abs(rightEncoder.getPulses()) < 300);
         motors.stopBothMotors();
- 
+
         // turn left towards wall
         leftEncoder.reset();
         rightEncoder.reset();
         motors.setMotor0Speed(MAX_SPEED); //right
         motors.setMotor1Speed(-MAX_SPEED); //left
-        while(rightEncoder.getPulses() < 100 || abs(leftEncoder.getPulses()) < 100);
- 
+        while(rightEncoder.getPulses() < 20 || abs(leftEncoder.getPulses()) < 20);
+
         motors.stopBothMotors();
-        
+
         // turning left
         motors.setMotor0Speed(0.9*MAX_SPEED); //right
         motors.setMotor1Speed(-0.9*MAX_SPEED); //left
-           
-    }else if( section == RIGS){
-        rightTurn();
-        // turning right towards wall
+
+    } else if( section == RIGS) {
+        // check distance to wall
+        rangeFinderRight.startMeas();
+        wait_ms(20);
+        rangeFinderRight.getMeas(range);
+
+        if(range < 4 || range > 20) return;
+
+        // turn at an angle
+        leftEncoder.reset();
+        rightEncoder.reset();
+        motors.setMotor1Speed(-1.2*MAX_SPEED); //left
+        motors.setMotor0Speed(0.4*MAX_SPEED); //right
+        while(abs(leftEncoder.getPulses())<1000);
+        motors.stopBothMotors();
+
+        //go backwards toward wall
+        leftEncoder.reset();
+        rightEncoder.reset();
+        motors.setMotor0Speed(-0.25*127); //right
+        motors.setMotor1Speed(-0.25*127); //left
+        while(abs(leftEncoder.getPulses()) < 150 || abs(rightEncoder.getPulses()) < 150);
+        motors.stopBothMotors();
+
+        // turn left towards wall
+        leftEncoder.reset();
+        rightEncoder.reset();
+        motors.setMotor0Speed(-MAX_SPEED); //right
+        motors.setMotor1Speed(MAX_SPEED); //left
+        while(abs(rightEncoder.getPulses()) < 20 || abs(leftEncoder.getPulses()) < 20);
+
+        motors.stopBothMotors();
+
+        // turning left
         motors.setMotor0Speed(-0.9*MAX_SPEED); //right
         motors.setMotor1Speed(0.9*MAX_SPEED); //left
-    }
-    else {
+    } else {
         pc.printf("in mid section align\r\n");
         // turn right towards wall
         rightTurn();
-        
         // turning left towards wall
         motors.setMotor0Speed(0.9*MAX_SPEED); //right
         motors.setMotor1Speed(-0.9*MAX_SPEED); //left
@@ -356,12 +407,11 @@
 
     usValue = 0;
     while(1) {
-        if(section == RIGS){
+        if(section == RIGS) {
             rangeFinderRight.startMeas();
             wait_ms(20);
             rangeFinderRight.getMeas(range);
-        }
-        else{
+        } else {
             rangeFinderLeft.startMeas();
             wait_ms(20);
             rangeFinderLeft.getMeas(range);
@@ -375,7 +425,7 @@
     }
     motors.stopBothMotors();
 }
- 
+
 void rightTurn(void)
 {
     motors.begin();
@@ -386,7 +436,7 @@
     while(abs(leftEncoder.getPulses())<950 || abs(rightEncoder.getPulses())<950);
     motors.stopBothMotors();
 }
- 
+
 void leftTurn(void)
 {
     motors.begin();
@@ -397,9 +447,10 @@
     while(abs(leftEncoder.getPulses())<1100 || rightEncoder.getPulses()<1100);
     motors.stopBothMotors();
 }
- 
-void slightleft(void){
-    
+
+void slightleft(void)
+{
+
     leftEncoder.reset();
     rightEncoder.reset();
     motors.setMotor0Speed(0.5*127);// right
@@ -407,178 +458,215 @@
     while(abs(leftEncoder.getPulses())<90 || rightEncoder.getPulses()<90);
     motors.stopBothMotors();
 }
-void slightMove(int direction, float pulses){
+
+void slightright(void)
+{
+
+    leftEncoder.reset();
+    rightEncoder.reset();
+    motors.setMotor0Speed(-0.4*127);// right
+    motors.setMotor1Speed(0.4*127);// left
+    while(abs(leftEncoder.getPulses())<90 || abs(rightEncoder.getPulses())<90);
+    motors.stopBothMotors();
+}
+
+void slightMove(int direction, float pulses)
+{
     int dir=1;
-    
+
     if(direction == BACKWARD) dir= -1;
-    
+
     leftEncoder.reset();
     rightEncoder.reset();
     motors.setMotor0Speed(dir*0.25*127); //right
     motors.setMotor1Speed(dir*0.25*127); //left
     while(abs(leftEncoder.getPulses()) < pulses || abs(rightEncoder.getPulses()) < pulses);
-    
+
     motors.setMotor0Speed(dir*-0.25*127); //right
     motors.setMotor1Speed(dir*-0.25*127); //left
     wait_ms(10);
     motors.stopBothMotors();
 }
 
-void UntilWall(int dir){
-    
-    if(dir == BACKWARD) dir=-1; 
-    
+void UntilWall(int dir)
+{
+
+    if(dir == BACKWARD) dir=-1;
+
     leftEncoder.reset();
     rightEncoder.reset();
     motors.setMotor0Speed(dir*0.2*127); //right
     motors.setMotor1Speed(dir*0.2*127); //left
-    
+
     range = 30;
-    
-    while(range > 20){
+
+    while(range > 20) {
         rangeFinderRight.startMeas();
         wait_ms(20);
         rangeFinderRight.getMeas(range);
     }
-    
+
     motors.setMotor0Speed(dir*-0.2*127); //right
     motors.setMotor1Speed(dir*-0.2*127); //left
     wait_ms(5);
     motors.stopBothMotors();
-} 
- 
+}
+
 void overBump(int section)
 {
     int preLeft=5000, preRight=5000, out=0;
-    
+
     motors.begin();
-    // slight backwards 
+    // slight backwards
     leftEncoder.reset();
     rightEncoder.reset();
     motors.setMotor0Speed(-0.25*127); //right
     motors.setMotor1Speed(-0.25*127); //left
     while(abs(leftEncoder.getPulses()) < 50 || abs(rightEncoder.getPulses()) < 50);
     motors.stopBothMotors();
-    
+
     pc.printf("slight backwards\r\n");
     wait_ms(200);
-    
-    leftEncoder.reset();
-    rightEncoder.reset();
-    motors.setMotor0Speed(0.35*127); //right
-    motors.setMotor1Speed(0.35*127); //left
-    while((abs(leftEncoder.getPulses()) < 1000 || abs(rightEncoder.getPulses()) < 1000) && preLeft!=0 && IR.getDistance() >15 ){
-       preLeft=leftEncoder.getPulses();
-       preRight=rightEncoder.getPulses();
-       wait_ms(200);
-       if(leftEncoder.getPulses() == preLeft || rightEncoder.getPulses()== preRight) preLeft=preRight=0;
-    }
-    
-    pc.printf("forward \r\n");
-    wait_ms(200);
- /*   
-    motors.stopBothMotors();
-    motors.begin();
-        
+
     leftEncoder.reset();
     rightEncoder.reset();
     motors.setMotor0Speed(0.3*127); //right
     motors.setMotor1Speed(0.3*127); //left
- 
-    while(!out) {
-        preLeft=leftEncoder.getPulses();
+    while((abs(leftEncoder.getPulses()) < 800 || abs(rightEncoder.getPulses()) < 800) /*&& preLeft!=0*/ && IR.getDistance() >15 ) {
+        /*preLeft=leftEncoder.getPulses();
         preRight=rightEncoder.getPulses();
- 
-        rangeFinderLeft.startMeas();
-        rangeFinderRight.startMeas();
-        wait_ms(20);
-        rangeFinderLeft.getMeas(range);
-        rangeFinderRight.getMeas(range2);
-        if(range < 10 || range2 < 10) out=1;
- 
-        if(leftEncoder.getPulses() == preLeft || rightEncoder.getPulses()== preRight) {
-            motors.setMotor0Speed(0.4*127); //right
-            motors.setMotor1Speed(0.4*127); //left
-            wait_ms(50);
-            out=1;
-        }
-        if(abs(leftEncoder.getPulses()) >1000 || abs(leftEncoder.getPulses())>1000) out=1;
+        wait_ms(200);
+        if(leftEncoder.getPulses() == preLeft || rightEncoder.getPulses()== preRight) preLeft=preRight=0;*/
     }
-    */
-    
+
+    pc.printf("forward \r\n");
+    wait_ms(200);
+    /*
+       motors.stopBothMotors();
+       motors.begin();
+
+       leftEncoder.reset();
+       rightEncoder.reset();
+       motors.setMotor0Speed(0.3*127); //right
+       motors.setMotor1Speed(0.3*127); //left
+
+       while(!out) {
+           preLeft=leftEncoder.getPulses();
+           preRight=rightEncoder.getPulses();
+
+           rangeFinderLeft.startMeas();
+           rangeFinderRight.startMeas();
+           wait_ms(20);
+           rangeFinderLeft.getMeas(range);
+           rangeFinderRight.getMeas(range2);
+           if(range < 10 || range2 < 10) out=1;
+
+           if(leftEncoder.getPulses() == preLeft || rightEncoder.getPulses()== preRight) {
+               motors.setMotor0Speed(0.4*127); //right
+               motors.setMotor1Speed(0.4*127); //left
+               wait_ms(50);
+               out=1;
+           }
+           if(abs(leftEncoder.getPulses()) >1000 || abs(leftEncoder.getPulses())>1000) out=1;
+       }
+       */
+
     motors.stopBothMotors();
     motors.begin();
-    
+
     preLeft=preRight=5000 ;
     leftEncoder.reset();
     rightEncoder.reset();
     motors.setMotor0Speed(.25*127); //right
     motors.setMotor1Speed(.25*127); //left
-    
-    if(section == TOOLS){
-        while(IR.getDistance() > 10 && (abs(leftEncoder.getPulses()) < 300 || abs(rightEncoder.getPulses()) < 300) && (leftEncoder.getPulses() != preLeft || rightEncoder.getPulses() != preRight)){
-            
+
+    if(section == TOOLS) {
+        while(IR.getDistance() > 10 && (abs(leftEncoder.getPulses()) < 300 || abs(rightEncoder.getPulses()) < 300) && (leftEncoder.getPulses() != preLeft || rightEncoder.getPulses() != preRight)) {
+
             if(IR.getDistance() > 38) break;
-            
+
             preLeft=leftEncoder.getPulses();
             preRight=rightEncoder.getPulses();
             wait_ms(200);
         }
-    }
-    else if( section == MID ){
-        while(IR.getDistance() > 15 && (abs(leftEncoder.getPulses()) < 300 || abs(rightEncoder.getPulses()) < 300) && (leftEncoder.getPulses() != preLeft || rightEncoder.getPulses() != preRight)){
-            
+    } else if(section == MID || section == MID2) {
+        if(section == MID2) while(IR.getDistance() > 15 && (abs(leftEncoder.getPulses()) < 400 || abs(rightEncoder.getPulses()) < 400));
+        while(IR.getDistance() > 15 && (abs(leftEncoder.getPulses()) < 400 || abs(rightEncoder.getPulses()) < 400) && (leftEncoder.getPulses() != preLeft || rightEncoder.getPulses() != preRight)) {
+
             if(IR.getDistance() > 38) break;
-            
+
             preLeft=leftEncoder.getPulses();
             preRight=rightEncoder.getPulses();
             wait_ms(200);
         }
-            
+
+    } else {
+        while(abs(leftEncoder.getPulses()) < 100 || abs(rightEncoder.getPulses()) < 100);
+
+        leftEncoder.reset();
+        rightEncoder.reset();
+
+        motors.setMotor0Speed(-.15*127); //right
+        motors.setMotor1Speed(-.15*127); //left
+        while((abs(leftEncoder.getPulses()) < 100 || abs(rightEncoder.getPulses()) < 100) && preLeft!=0 ) {
+            preLeft = leftEncoder.getPulses();
+            preRight = rightEncoder.getPulses();
+            wait_ms(200);
+            if(leftEncoder.getPulses() == preLeft || rightEncoder.getPulses()== preRight) preLeft=preRight=0;
+        }
+
+        leftEncoder.reset();
+        rightEncoder.reset();
+
+        motors.setMotor0Speed(0.25*127); //right
+        motors.setMotor1Speed(0.25*127); //left
+        while((abs(leftEncoder.getPulses()) < 10 || abs(rightEncoder.getPulses()) < 10));
+
+        motors.stopBothMotors();
+
+        return;
     }
-    else while(abs(leftEncoder.getPulses()) < 100 || abs(rightEncoder.getPulses()) < 100);
- 
-    motors.stopBothMotors();
-    
+
     leftEncoder.reset();
     rightEncoder.reset();
-    
+
     motors.setMotor0Speed(-.25*127); //right
     motors.setMotor1Speed(-.25*127); //left
     while((abs(leftEncoder.getPulses()) < 10 || abs(rightEncoder.getPulses()) < 10));
-    
+
     motors.stopBothMotors();
-    wait_ms(50);
+    wait_ms(20);
     motors.begin();
- 
+
 }
- 
+
 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(10);
+    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);
-     
+
     //Tool aquiring
     wait(2);
     // After tool is aquired
- 
+
     alignWithWall(TOOLS);
     pc.printf("align\r\n");
     wait_ms(100);
-    
+
     //wall_follow2(LEFT,FORWARD,MID, current);
     //current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
-    
+
     rangeFinderLeft.startMeas();
     wait_ms(20);
     rangeFinderLeft.getMeas(range);
- 
+
     if(range < 20) {
         wall_follow2(LEFT,BACKWARD,TOOLS, current,0);
         pc.printf("wall follow\r\n");
@@ -598,7 +686,7 @@
         motors.setMotor1Speed(MAX_SPEED); //left
         while(abs(leftEncoder.getPulses()) < 10 || abs(rightEncoder.getPulses())< 10);
         motors.stopBothMotors();
- 
+
         wait_ms(100);
         leftTurn();
         overBump(TOOLS);
@@ -608,15 +696,15 @@
         leftTurn();
         overBump(TOOLS);
     }
- 
+
     pc.printf("First Wavegap = %f\r\n",location[0]);
 }
- 
+
 void mid_section(float* location, float &current, int* direction)
 {
     motors.begin();
-    
-    if(IR.getDistance() > 38){
+
+    if(IR.getDistance() > 38) {
         direction[0]= STRAIGHT;
         overBump(MID);
         return;
@@ -624,72 +712,74 @@
     pc.printf("before align with wall \r\n");
     alignWithWall(MID);
     wait_ms(100);
- 
+
     pc.printf("mid section current = %f\r\n",current);
     wall_follow2(LEFT,FORWARD,MID, current,0);
     current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
     pc.printf("after wf2 current = %f\r\n",current);
-    
+
     wait_ms(500);
     rangeFinderLeft.startMeas();
     wait_ms(20);
     rangeFinderLeft.getMeas(range);
-    
+
     if(range > 20 ) {
         direction[0]= RIGHT;
         location[1]= current;
-        //slightMove(FORWARD,200);
-        current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
+        slightMove(FORWARD,75);
+        //current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
     } else {
         direction[0]= LEFT;
         wall_follow2(LEFT,BACKWARD,MID,current,0);
         location[1]= current- ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
         current= location[1];
-        
-        if(location[1] < 18){
+
+        if(location[1] < 18) {
             slightMove(FORWARD, 50);
             current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
-        }  
-        
+        }
+
     }
-    
+
     pc.printf("wavegap2 = %f\r\n",location[1]);
     leftTurn();
-    
+
     wait_ms(100);
-    
+
     overBump(MID);
- 
+
 }
- 
+
 void mid_section2(float* location, float &current, int* direction)
 {
     motors.begin();
-    
+
     pc.printf("mid section 2\r\n");
-    
-    if(IR.getDistance() > 38){
-        direction[0]= STRAIGHT;
+
+    if(IR.getDistance() > 38) {
+        direction[1]= STRAIGHT;
         overBump(RIGS);
         return;
     }
     
     alignWithWall(MID);
     pc.printf("midsection 2 alignt with wall mid \r\n");
-    
+
     wall_follow2(LEFT,FORWARD,MID, current,0);
     current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
-    
-    pc.printf("midseection 2 after wf2");
+
+    wait_ms(500);
+
+    pc.printf("midseection 2 after wf2 %f",current);
     rangeFinderLeft.startMeas();
     wait_ms(20);
     rangeFinderLeft.getMeas(range);
-    
+
     if(range > 20 ) {
         direction[1]= RIGHT;
         location[2]= current;
-        slightMove(FORWARD,500);
-        current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
+        slightMove(FORWARD,75);
+        //current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
     } else {
         direction[1]= LEFT;
         wall_follow2(LEFT,BACKWARD,MID,current,0);
@@ -697,30 +787,89 @@
         current=location[2];
         //slightMove(FORWARD,500);
     }
- 
+
     leftTurn();
     overBump(RIGS);
     pc.printf("overbump rigs\r\n");
 }
- 
+
 void rig_section(float* location, float &current, int* direction, int rig)
 {
     float loc;
-    
+
     if(rig == 1) loc= 15;
     else if(rig == 2) loc= 45;
     else loc = 75;
-    
+
     rightTurn();
-    
-    if(current > loc){
+    slightright();
+
+    if(current > loc) {
         pc.printf("RIG section %f\r\n",current);
         wall_follow2(RIGHT, BACKWARD, RIGS, current, rig);
         current-=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
-    }
-    else{
+    } else {
         pc.printf("RIG section %f\r\n",current);
         wall_follow2(RIGHT, FORWARD, RIGS, current, rig);
-        current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); 
+        current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
+    }
+}
+
+void tools_section_return(float* location, float &current)
+{
+    if(location[0] > 16) {
+        leftTurn();
+        wall_follow2(LEFT, BACKWARD, RETURN, location[0], 0);
+    }
+    motors.stopBothMotors();
+
+}
+
+void mid_section_return(float* location, float &current, int* direction)
+{
+    if(direction[0] == RIGHT) {
+        leftTurn();
+        alignWithWall(MID);
+        wall_follow2(LEFT, BACKWARD, MID, current,0);
+        current-=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
+        rightTurn();
+    } else if(direction[0] == LEFT) {
+        leftTurn();
+        wall_follow2(RIGHT, FORWARD, MID, current,0);
+        current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
+        rightTurn();
     }
+    //ELSE and GO FORWARD
+    overBump(RIGS);
+}
+
+void mid_section2_return(float* location, float &current, int* direction)
+{
+    if(direction[1] == RIGHT) {
+        leftTurn();
+        wall_follow2(LEFT, BACKWARD, MID, current,0);
+        current-=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
+        rightTurn();
+    } else if(direction[1] == LEFT) {
+        leftTurn();
+        wall_follow2(RIGHT, FORWARD, MID, current,0);
+        current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
+        rightTurn();
+    }
+    //ELSE and GO FORWARD
+    overBump(MID);
+}
+
+void rig_section_return(float* location, float &current, int* direction)
+{
+    alignWithWall(RIGS);
+    if(location[2] > current) {
+        wall_follow2(RIGHT, FORWARD, MID, current,0);
+        current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
+    } else {
+        wall_follow2(RIGHT, BACKWARD, MID, current,0);
+        current-=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
+    }
+    rightTurn();
+    overBump(MID2);
 }
\ No newline at end of file