Revision 12:284be46593ae, committed 2014-04-02

Comitter:

tashworth

Date:

Wed Apr 02 00:30:30 2014 +0000

Parent:

11:8d2455e383ce

Child:

13:529323807361

Commit message:

4-01-14

Changed in this revision

--- a/PololuQik2.lib	Tue Apr 01 02:00:01 2014 +0000
+++ b/PololuQik2.lib	Wed Apr 02 00:30:30 2014 +0000
@@ -1,1 +1,1 @@
-http://mbed.org/users/Fairy_Paolina/code/PololuQik2/#df9964aaa00d
+http://mbed.org/users/jjcarr2/code/PololuQik2/#25c41766d768

--- a/ShapeDetect.cpp	Tue Apr 01 02:00:01 2014 +0000
+++ b/ShapeDetect.cpp	Wed Apr 02 00:30:30 2014 +0000
@@ -248,7 +248,7 @@
 ************************************************/
 void clearBounds(void){
         //top 20 pixels
-        for(int i=0; i<10; i++){
+        for(int i=0; i<5; i++){
                  for(int j=0; j<160; j++){
                      image[i][j] = 0;
                         
@@ -256,20 +256,20 @@
                  }// i
                  
         //bottom 20 pixels         
-        for(int i=109; i<120; i++){
+        for(int i=114; i<120; i++){
                  for(int j=0; j<160; j++){
                      image[i][j] = 0;        
                     }// j
                  }// i
         //left 20 pixels         
         for(int i=0; i<120; i++){
-                 for(int j=0; j<10; j++){
+                 for(int j=0; j<20; j++){
                      image[i][j] = 0;     
                     }// j
                  }// i
          //right 20 pixels         
         for(int i=0; i<120; i++){
-                 for(int j=149; j<160; j++){
+                 for(int j=139; j<160; j++){
                      image[i][j] = 0;    
                     }// j
                  }// i
@@ -296,6 +296,7 @@
     }
 }
 
+
 int getDistance(void){
         lrf.putc('B'); //Take Binary range reading
         // read in the four bytes for the range in mm (MSB first)

--- a/ShapeDetect.h	Tue Apr 01 02:00:01 2014 +0000
+++ b/ShapeDetect.h	Wed Apr 02 00:30:30 2014 +0000
@@ -2,11 +2,11 @@
 #define SHAPEDETECT_H_
 
 /* theshold for setting binary output */
-#define THRESHOLD 100 
+#define THRESHOLD 90 
 
 //areas from camera 11" from ground
-#define TRIANGE_AREA_TRESHOLD   3350
-#define SQUARE_AREA_TRESHOLD    3800           
+#define TRIANGE_AREA_TRESHOLD   3000
+#define SQUARE_AREA_TRESHOLD    3400           
 
 /* modes for image processing */
 #define BINARY    1         

--- a/main.cpp	Tue Apr 01 02:00:01 2014 +0000
+++ b/main.cpp	Wed Apr 02 00:30:30 2014 +0000
@@ -23,19 +23,21 @@
 #define PPR            (4331/4)
 #define LEFT           (1)
 #define RIGHT          (0)
+#define STRAIGHT       (2)
 #define FORWARD        (1)
 #define BACKWARD       (0)
 #define TOOLS          (0)
 #define MID            (1)
 #define RIGS           (2)
-#define FIRST_WAVE     (0)
+#define MID2           (3)
+#define RETURN         (4)
 #define FAR            (1)
 
 //States
 #define START                       0
 #define OILRIG1_POS                 1
 #define OILRIG2_POS                 2
-#define GOTO_TOOLS                  3
+#define GOTO_TOOLS1                 3
 #define IDENTIFY_TOOLS              4
 #define AQUIRE_TOOL1                5
 #define AQUIRE_TOOL2                6
@@ -49,6 +51,7 @@
 #define RIG_ALIGN                   14
 #define INSERT_TOOL                 15
 #define END                         16
+#define GOTO_TOOLS2                 17
 
 
 
@@ -131,21 +134,26 @@
 
 
 //Navigation Functions
-float wall_follow(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 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 us_distance(void);
-void to_tools_section(float* location, float &current);
+void slightMove(int direction, float pulses);
+void to_tools_section1(float* location, float &current);
+void to_tools_section2(float* location, float &current);
 void from_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 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 ledtoggle(void);
+void UntilWall(int dir);
 
 
 /************
@@ -186,13 +194,13 @@
 //increase in number 5 rotates gripper
 
     {STORE_POSITION, 85, 5, 0, 165, 175, 0},              // storing position
-    {OIL_RIG1, 162, 20, 60, 55, 100, 0},                 // point laser at oilrig1
-    {OIL_RIG2, 145, 20, 60, 55, 100, 0},                // point laser at oilrig2
-    {OIL_RIG3, 130, 90, 90, 57, 100, 0},                // point laser at oilrig2
-    {DRIVE_POSITION_NOTOOL, 85, 5, 0, 175, 175, 0},    // Drive through course
-    {TOOL_1, 95, 40, 70, 128, 175, 0},                  // Look over first tool
-    {TOOL_2, 77, 40, 70, 128, 175, 0},                  // Look over second tool
-    {TOOL_3, 57, 40, 70, 128, 175, 0},                  // Look over third tool
+    {OIL_RIG1, 160, 20, 60, 45, 175, 0},                 // point laser at oilrig1
+    {OIL_RIG2, 163, 20, 60, 45, 175, 0},                // point laser at oilrig2
+    {OIL_RIG3, 130, 90, 90, 52, 175, 0},                // point laser at oilrig2
+    {DRIVE_POSITION_NOTOOL, 85, 5, 0, 165, 175, 0},    // Drive through course
+    {TOOL_1, 94, 50, 80, 109, 175, 0},                  // Look over first tool
+    {TOOL_2, 77, 50, 80, 110, 175, 0},                  // Look over second tool
+    {TOOL_3, 57, 50, 80, 109, 175, 0},                  // Look over third tool
     {DRIVE_POSITION_TOOL, 55, 70, 102, 74, 0, 0},     // Drive with tool loaded
     {ORIENT_TOOL, 135, 60, 75, 60, 90, 90},             // position tool to be inserted
     {PU_TOOL_1, 98, 50, 82, 118, 90, 0},                // STATIC Pickup tool POSITION
@@ -222,7 +230,7 @@
     /*****************
      INITIALIZATIONS
     *******************/
-    float location[3], current=6;
+    float location[3], current=4;
     int direction[3];
     double distance;
 
@@ -246,8 +254,8 @@
     /*******************
     WHILE LOOP FOR TESTING
     ********************/
-    while(1) {
-
+    //while(1) {
+/*
         pc.scanf("%d %d", &servoNum, &servoAngle);
         if(servoAngle > 175) {
             servoAngle = 175;
@@ -259,7 +267,7 @@
         setServoPulse(servoNum, servoAngle);
         distLaser = getDistance();
         pc.printf("Distance %d", distLaser);
-
+*/
         /*
                 while(pc.getc() != 'g');
                 servoPosition(TOOL_2);
@@ -349,7 +357,7 @@
                 }
                 printImageToFile(BINARY);*/
 
-    }
+   // }
 
 
 
@@ -370,7 +378,7 @@
                 **************************************************/
             case START :
                 myled1 = 1;
-                wait(1);
+                while(pc.getc() != 'g');
                 myled1 = 0;
                 state = OILRIG1_POS;
                 break;
@@ -389,15 +397,17 @@
                 setServoPulse(0, Arm_Table[OIL_RIG1].base_rotate);
                 setServoPulse(4, Arm_Table[OIL_RIG1].claw_rotate);
                 setServoPulse(5, Arm_Table[OIL_RIG1].claw_open);
-
-                wait(3);                        //wait for servos to settle
+                wait(3);
+                //lrf.putc('E');                  // lighting calculation
+                //wait(15);
 
                 fire = fire_checker(OIL_RIG1);  //determines if oil rig is on fire
-                pc.printf("FIRE: %d", fire);
+
+                pc.printf("FIRE: SQUARE %d", fire);
                 //determines what tool is needed
                 if (fire == 1) {
                     tool_needed = SQUARE;
-                    state = GOTO_TOOLS;
+                    state = GOTO_TOOLS1;
                 } else {
                     state = OILRIG2_POS;
                 }
@@ -405,16 +415,31 @@
                 break;
 
             case OILRIG2_POS:
+
+
+                setServoPulse(2, Arm_Table[DRIVE_POSITION_NOTOOL].big_arm);
+                setServoPulse(3, Arm_Table[DRIVE_POSITION_NOTOOL].claw_arm);
+                setServoPulse(1, Arm_Table[DRIVE_POSITION_NOTOOL].base_arm);
+                setServoPulse(0, Arm_Table[DRIVE_POSITION_NOTOOL].base_rotate);
+                setServoPulse(4, Arm_Table[DRIVE_POSITION_NOTOOL].claw_rotate);
+                setServoPulse(5, Arm_Table[DRIVE_POSITION_NOTOOL].claw_open);
+
+                wait(3);                                //wait for servos to settle
+
+                to_tools_section2(location, current);
+
                 servoPosition(OIL_RIG2);    //position arm to point at first oilrig
-                wait(1);                    //wait for servos to settle
+                wait(2);
+                //lrf.putc('E');                  // lighting calculation
+                //wait(15);
                 fire = fire_checker(OIL_RIG2);
-                pc.printf("FIRE: %d", fire);
+                pc.printf("FIRE: TRIANGLE %d", fire);
                 if (fire == 1) {
                     tool_needed = TRIANGLE;
-                    state = GOTO_TOOLS;
+                    state = GOTO_TOOLS2;
                 } else {
                     tool_needed = CIRCLE;
-                    state = GOTO_TOOLS;
+                    state = GOTO_TOOLS2;
                 }
                 pc.printf("tool needed: %d", tool_needed);
                 break;
@@ -425,10 +450,25 @@
                 *          - TRAVEL TO TOOLS
                 *
                 **************************************************/
-            case GOTO_TOOLS:
+            case GOTO_TOOLS1:
+
+                setServoPulse(2, Arm_Table[DRIVE_POSITION_NOTOOL].big_arm);
+                setServoPulse(3, Arm_Table[DRIVE_POSITION_NOTOOL].claw_arm);
+                setServoPulse(1, Arm_Table[DRIVE_POSITION_NOTOOL].base_arm);
+                setServoPulse(0, Arm_Table[DRIVE_POSITION_NOTOOL].base_rotate);
+                setServoPulse(4, Arm_Table[DRIVE_POSITION_NOTOOL].claw_rotate);
+                setServoPulse(5, Arm_Table[DRIVE_POSITION_NOTOOL].claw_open);
 
+                wait(5);                                //wait for servos to settle
+                to_tools_section1(location, current);
+
+                state = IDENTIFY_TOOLS;
+                break;
+
+            case GOTO_TOOLS2:
+
+                setServoPulse(2, Arm_Table[DRIVE_POSITION_NOTOOL].big_arm);
                 setServoPulse(3, Arm_Table[DRIVE_POSITION_NOTOOL].claw_arm);
-                setServoPulse(2, Arm_Table[DRIVE_POSITION_NOTOOL].big_arm);
                 setServoPulse(1, Arm_Table[DRIVE_POSITION_NOTOOL].base_arm);
                 setServoPulse(0, Arm_Table[DRIVE_POSITION_NOTOOL].base_rotate);
                 setServoPulse(4, Arm_Table[DRIVE_POSITION_NOTOOL].claw_rotate);
@@ -436,12 +476,12 @@
 
                 wait(5);                                //wait for servos to settle
 
-                to_tools_section(location, current);
+                slightMove(FORWARD,3100);
+                current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
 
                 state = IDENTIFY_TOOLS;
-                pc.printf("YES!!!!!");
-                while(1);
                 break;
+
                 /**************************************************
                 *           STAGE 3
                 *
@@ -450,21 +490,24 @@
                 *
                 **************************************************/
             case IDENTIFY_TOOLS:
-                servoPosition(TOOL_1);              //arm/camera looks over tool
-                wait(1);                            //wait for servos to settle
-                centerCamWithTool();                //centers camera over tool
+
+                servoPosition(TOOL_2);              //arm/camera looks over tool
+                wait(5);                            //wait for servos to settle
+                //centerCamWithTool();                //centers camera over tool
                 shape_detected = shapeDetection();  //determines the shape
+                clearBounds();
+                //printImageToFile(BINARY);
 
                 //either goes to aquire the tool or look at the next shape
                 if(shape_detected == tool_needed) {
-                    state = AQUIRE_TOOL1;
+                    state = AQUIRE_TOOL2;
                 } else {
-                    servoPosition(TOOL_2);
+                    servoPosition(TOOL_1);
                     wait(1);                        //wait for servos to settle
-                    centerCamWithTool();
+                    //centerCamWithTool();
                     shape_detected = shapeDetection();
                     if (shape_detected == tool_needed) {
-                        state = AQUIRE_TOOL2;
+                        state = AQUIRE_TOOL1;
                     } else {
                         servoPosition(TOOL_3);
                         wait(1);                            //wait for servos to settle
@@ -472,28 +515,29 @@
                         state = AQUIRE_TOOL3;
                     }
                 }
+                while(1);
                 break;
             case AQUIRE_TOOL1:
-                //*********************//
-                //******* TODO ********//
-                //*********************//
-                // CODE TO GRAB TOOL1
+
+                servoPosition(PU_TOOL_1_STAB);
+                wait(2);
+
                 servoPosition(DRIVE_POSITION_TOOL);     //arm position for driving with the tool
                 state  = NAVIGATE_WAVES_ROW1;
                 break;
             case AQUIRE_TOOL2:
-                //*********************//
-                //******* TODO ********//
-                //*********************//
-                // CODE TO GRAB TOOL2
+                servoPosition(PU_TOOL_2_STAB);
+                wait(2);
+
                 servoPosition(DRIVE_POSITION_TOOL);     //arm position for driving with the tool
                 state  = NAVIGATE_WAVES_ROW1;
                 break;
+
             case AQUIRE_TOOL3:
-                //*********************//
-                //******* TODO ********//
-                //*********************//
-                // CODE TO GRAB TOOL3
+                servoPosition(PU_TOOL_3_STAB);
+                wait(2);
+
+
                 servoPosition(DRIVE_POSITION_TOOL);     //arm position for driving with the tool
                 state  = NAVIGATE_WAVES_ROW1;
                 break;
@@ -711,6 +755,7 @@
 
         case 1:
             for (int i = 0; i<5; i++) {
+                pc.printf("1");
                 distLaser = getDistance();
                 pc.printf("L DISTANCE: %d \n\r", distLaser);
                 if ((distLaser < OILRIG1_MAX)
@@ -769,20 +814,9 @@
 {
     //Nothing
 }
-
-void us_distance(void)
+void wall_follow(int side, int direction, int section)
 {
-    pc.printf("Ultra Sonic\n\r");
-    rangeFinderLeft.startMeas();
-    wait_us(20);
-    if ( (rangeFinderLeft.getMeas(range) == RANGE_MEAS_VALID)) {
-        pc.printf("Range = %f\n\r", range);
-    }
-}
-
-float wall_follow(int side, int direction, int section)
-{
-    float location, wavegap=0, set=5;
+    float location, set=6;
     int dir=1;
 
     pid1.reset();
@@ -795,7 +829,7 @@
 
     location=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
 
-    while(location< 70) {
+    while(location< 66.5) {
         location=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
 
         pid1.setInputLimits(0, set);
@@ -803,21 +837,20 @@
         pid1.setSetPoint(set);
         if(side) {
             rangeFinderLeft.startMeas();
-            wait_ms(38);
+            wait_ms(20);
             rangeFinderLeft.getMeas(range);
         } else {
             rangeFinderRight.startMeas();
-            wait_ms(38);
+            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;
+        if(range > 15) {
             //pc.printf("wavegap %f\r\n",wavegap);
             // AT WAVE OPENING!!!!
-            motors.setMotor1Speed(dir*0.4*127);//left
-            motors.setMotor0Speed(dir*0.4*127);//right
+            motors.setMotor1Speed(dir*0.25*127);//left
+            motors.setMotor0Speed(dir*0.25*127);//right
         } else {
 
             pid1.setProcessValue(range);
@@ -845,26 +878,50 @@
             }
         }
     }
-    return wavegap;
+
+    //STOP
+    motors.setMotor0Speed(dir*-0.3*127); //right
+    motors.setMotor1Speed(dir*-0.3*127); //left
+    wait_ms(10);
+    motors.stopBothMotors(0);
 }
 
 /* MODIFIED WALL_FOLLOW FOR NAVIGATION */
 
-void wall_follow2(int side, int direction, int section, float location)
+void wall_follow2(int side, int direction, int section, float location, int rig)
 {
-    int SeeWaveGap = false, dir=1;
-    float set=5, loc=0;
+    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;
-    if(section == TOOLS)set= 5;
+    if(direction == BACKWARD) {
+        dir=-1;
+        limit = 100;
+    }
+    if(section == TOOLS) {
+        set= 6;
+        limit = 86;
+    }
+    if(section == RETURN) lowlim=15;
 
     leftEncoder.reset();
     rightEncoder.reset();
 
-    while(dir*loc + location <= 78) {
+    //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 >= 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);
@@ -872,54 +929,87 @@
 
         if(side) {
             rangeFinderLeft.startMeas();
-            wait_ms(38);
+            wait_ms(20);
             rangeFinderLeft.getMeas(range);
         } else {
             rangeFinderRight.startMeas();
-            wait_ms(38);
+            wait_ms(20);
             rangeFinderRight.getMeas(range);
         }
 
+        if(section == RIGS) {
+            rangeFinderLeft.startMeas();
+            wait_ms(20);
+            rangeFinderLeft.getMeas(range2);
 
-        /*************CHECK FOR WAVE OPENING*****************/
-        /* If after 20 ms the ultrasonic still sees 20+ cm */
-        /*      then robot is at wave opening               */
+            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
+                    wait_ms(5);
+                    motors.stopBothMotors(0);
+                    break;
+                }
+            }
+        }
+
 
         //pc.printf("wall follow 2 range %f\r\n",range);
         //pc.printf("loc+location = %f\r\n", loc+location);
-        if(range > 20) {
-            motors.stopBothMotors();
-            pc.printf("wavegap\r\n");
-            // AT WAVE OPENING!!!!
-            break;
-        }
-
-        pid1.setProcessValue(range);
-        pid_return = pid1.compute();
-        //pc.printf("Range: %f\n      PID:   %f\r\n", range, pid_return);
+        if(range > 20 ) {
+            if(section == RIGS || section == RETURN) {
+                motors.setMotor0Speed(dir*0.25*127); //right
+                motors.setMotor1Speed(dir*0.25*127); //left
+            } 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(0);
 
-        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
+                    pc.printf("wavegap\r\n");
+                    // AT WAVE OPENING!!!!
+                    break;
+                }
             }
         } else {
-            motors.setMotor0Speed(dir*MAX_SPEED);
-            motors.setMotor1Speed(dir*MAX_SPEED);
+            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.setMotor0Speed(dir*MAX_SPEED);
+                motors.setMotor1Speed(dir*MAX_SPEED);
+            }
         }
     }
-    motors.stopBothMotors();
+
+    //STOP
+    motors.setMotor0Speed(dir*-0.3*127); //right
+    motors.setMotor1Speed(dir*-0.3*127); //left
+    wait_ms(5);
+    motors.stopBothMotors(0);
 }
 
 
@@ -928,53 +1018,100 @@
     float usValue = 0;
 
     if(section == TOOLS) {
+        pc.printf("tools section align\r\n");
         // turn at an angle
         leftEncoder.reset();
         rightEncoder.reset();
         motors.setMotor0Speed(-1.2*MAX_SPEED); //right
         motors.setMotor1Speed(0.4*MAX_SPEED); //left
         while(rightEncoder.getPulses()>-1000);
-        motors.stopBothMotors();
+        motors.stopBothMotors(0);
 
         //go backwards toward wall
         leftEncoder.reset();
         rightEncoder.reset();
-        motors.setMotor0Speed(-MAX_SPEED); //right
-        motors.setMotor1Speed(-MAX_SPEED); //left
+        motors.setMotor0Speed(-0.25*127); //right
+        motors.setMotor1Speed(-0.25*127); //left
         while(abs(leftEncoder.getPulses()) < 300 || abs(rightEncoder.getPulses()) < 300);
-
-        motors.stopBothMotors();
+        motors.stopBothMotors(0);
 
         // turn left towards wall
         leftEncoder.reset();
         rightEncoder.reset();
         motors.setMotor0Speed(MAX_SPEED); //right
         motors.setMotor1Speed(-MAX_SPEED); //left
-        while(rightEncoder.getPulses() < 10 || abs(leftEncoder.getPulses()) < 10);
+        while(rightEncoder.getPulses() < 20 || abs(leftEncoder.getPulses()) < 20);
+
+        motors.stopBothMotors(0);
+
+        // turning left
+        motors.setMotor0Speed(0.9*MAX_SPEED); //right
+        motors.setMotor1Speed(-0.9*MAX_SPEED); //left
 
-        motors.stopBothMotors();
+    } 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(0);
 
-        motors.setMotor0Speed(0.7*MAX_SPEED); //right
-        motors.setMotor1Speed(-0.7*MAX_SPEED); //left
+        //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(0);
+
+        // 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(0);
+
+        // turning left
+        motors.setMotor0Speed(-0.9*MAX_SPEED); //right
+        motors.setMotor1Speed(0.9*MAX_SPEED); //left
     } else {
+        pc.printf("in mid section align\r\n");
+        // turn right towards wall
         rightTurn();
-        motors.setMotor0Speed(-0.7*MAX_SPEED); //right
-        motors.setMotor1Speed(0.7*MAX_SPEED); //left
+        // turning left towards wall
+        motors.setMotor0Speed(0.9*MAX_SPEED); //right
+        motors.setMotor1Speed(-0.9*MAX_SPEED); //left
     }
 
     usValue = 0;
     while(1) {
-        rangeFinderLeft.startMeas();
-        wait_ms(20);
-        rangeFinderLeft.getMeas(range);
-        //pc.printf("Range %f \t OldValue %f\n\r",range, usValue);
+        if(section == RIGS) {
+            rangeFinderRight.startMeas();
+            wait_ms(20);
+            rangeFinderRight.getMeas(range);
+        } else {
+            rangeFinderLeft.startMeas();
+            wait_ms(20);
+            rangeFinderLeft.getMeas(range);
+        }
+        pc.printf("Range %f \t OldValue %f\n\r",range, usValue);
         if(range > usValue && usValue != 0 && range < 25) {
             break;
         } else {
             usValue = range;
         }
     }
-    motors.stopBothMotors();
+    motors.stopBothMotors(0);
 }
 
 void rightTurn(void)
@@ -984,28 +1121,21 @@
     rightEncoder.reset();
     motors.setMotor0Speed(-0.5*127);//right
     motors.setMotor1Speed(0.5*127);//left
-    while(leftEncoder.getPulses()<900 || rightEncoder.getPulses()>-900);
-    motors.stopBothMotors();
+    while(abs(leftEncoder.getPulses())<950 || abs(rightEncoder.getPulses())<950);
+    motors.stopBothMotors(0);
 }
 
 void leftTurn(void)
 {
-    /*
-    leftEncoder.reset();
-    rightEncoder.reset();
-    motors.setMotor0Speed(0.4*MAX_SPEED); //right
-    motors.setMotor1Speed(-MAX_SPEED); //left
-    while(abs(leftEncoder.getPulses())<2500);
-    motors.stopBothMotors();
-    */
     motors.begin();
     leftEncoder.reset();
     rightEncoder.reset();
     motors.setMotor0Speed(0.5*127);// right
     motors.setMotor1Speed(-0.5*127);// left
-    while(abs(leftEncoder.getPulses())<1000 || rightEncoder.getPulses()<1000);
-    motors.stopBothMotors();
+    while(abs(leftEncoder.getPulses())<1100 || rightEncoder.getPulses()<1100);
+    motors.stopBothMotors(0);
 }
+
 void slightleft(void)
 {
 
@@ -1013,80 +1143,123 @@
     rightEncoder.reset();
     motors.setMotor0Speed(0.5*127);// right
     motors.setMotor1Speed(-0.5*127);// left
-    while(abs(leftEncoder.getPulses())<50 || rightEncoder.getPulses()<50);
-    motors.stopBothMotors();
+    while(abs(leftEncoder.getPulses())<90 || rightEncoder.getPulses()<90);
+    motors.stopBothMotors(0);
+}
+
+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(0);
 }
 
+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(127);
+}
+
+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) {
+        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(0);
+}
 
 void overBump(int section)
 {
     int preLeft=5000, preRight=5000, out=0;
 
     motors.begin();
-
-    leftEncoder.reset();
-    rightEncoder.reset();
-    motors.setMotor0Speed(-0.2*127); //right
-    motors.setMotor1Speed(-0.2*127); //left
-    while(abs(leftEncoder.getPulses()) < 50 || abs(rightEncoder.getPulses())< 50);
-    motors.stopBothMotors();
-
+    // slight backwards
     leftEncoder.reset();
     rightEncoder.reset();
-    motors.setMotor0Speed(0.2*127); //right
-    motors.setMotor1Speed(0.2*127); //left
-    while(/*(abs(leftEncoder.getPulses()) < 1000 || abs(rightEncoder.getPulses())< 1000)*/ IR.getIRDistance() >20 && preLeft!=0) {
-        preLeft=leftEncoder.getPulses();
-        preRight=rightEncoder.getPulses();
-        wait_ms(100);
-        //pc.printf(" first while left %d right %d \r\n", preLeft, preRight);
-        if(leftEncoder.getPulses() == preLeft || rightEncoder.getPulses()== preRight) preLeft=preRight=0;
-    }
+    motors.setMotor0Speed(-0.25*127); //right
+    motors.setMotor1Speed(-0.25*127); //left
+    while(abs(leftEncoder.getPulses()) < 50 || abs(rightEncoder.getPulses()) < 50);
+    motors.stopBothMotors(0);
 
-    motors.stopBothMotors();
-    motors.begin();
-    wait(2);
-    /*
-        motors.stopBothMotors();
-        motors.setMotor0Speed(0.15*127); //right
-        motors.setMotor1Speed(0.15*127); //left
-        preLeft=preRight=5000 ;
-        leftEncoder.reset();
-        rightEncoder.reset();
-     */
-//   while(/*(abs(leftEncoder.getPulses()) < 1000 || abs(rightEncoder.getPulses())< 1000)*/ IR.getIRDistance() >20 && preLeft!=0){
-    /*        preLeft=leftEncoder.getPulses();
-            preRight=rightEncoder.getPulses();
-            pc.printf("second while left %d right %d \r\n", preLeft, preRight);
-            wait_ms(200);
-            if(leftEncoder.getPulses() == preLeft || rightEncoder.getPulses()== preRight) preLeft=preRight=0;
-        }*/
+    pc.printf("slight backwards\r\n");
+    wait_ms(200);
 
     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.getIRDistance() >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
-        }
-        if(abs(leftEncoder.getPulses()) <1000 || abs(leftEncoder.getPulses())<1000) out=1;
+        wait_ms(200);
+        if(leftEncoder.getPulses() == preLeft || rightEncoder.getPulses()== preRight) preLeft=preRight=0;*/
     }
 
-    motors.stopBothMotors();
-    wait(2);
+    pc.printf("forward \r\n");
+    wait_ms(200);
+    /*
+       motors.stopBothMotors(0);
+       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(0);
     motors.begin();
 
     preLeft=preRight=5000 ;
@@ -1095,159 +1268,303 @@
     motors.setMotor0Speed(.25*127); //right
     motors.setMotor1Speed(.25*127); //left
 
-    if(section == TOOLS || section == MID) {
-        while(IR.getIRDistance() > 20 ) {
-            //pc.printf("IR %f\r\n", IR.getIRDistance());
-            //pc.printf("third while left %d right %d \r\n", preLeft, preRight);
+    if(section == TOOLS) {
+        while(IR.getIRDistance() > 10 && (abs(leftEncoder.getPulses()) < 300 || abs(rightEncoder.getPulses()) < 300) && (leftEncoder.getPulses() != preLeft || rightEncoder.getPulses() != preRight)) {
+
+            if(IR.getIRDistance() > 38) break;
+
+            preLeft=leftEncoder.getPulses();
+            preRight=rightEncoder.getPulses();
+            wait_ms(200);
+        }
+    } else if(section == MID || section == MID2) {
+        if(section == MID2) while(IR.getIRDistance() > 15 && (abs(leftEncoder.getPulses()) < 400 || abs(rightEncoder.getPulses()) < 400));
+        while(IR.getIRDistance() > 15 && (abs(leftEncoder.getPulses()) < 400 || abs(rightEncoder.getPulses()) < 400) && (leftEncoder.getPulses() != preLeft || rightEncoder.getPulses() != preRight)) {
+
+            if(IR.getIRDistance() > 38) break;
+
+            preLeft=leftEncoder.getPulses();
+            preRight=rightEncoder.getPulses();
+            wait_ms(200);
         }
-    } else while((abs(leftEncoder.getPulses()) < 200 || abs(rightEncoder.getPulses())< 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(0);
+
+        return;
+    }
+
+    leftEncoder.reset();
+    rightEncoder.reset();
 
     motors.setMotor0Speed(-.25*127); //right
     motors.setMotor1Speed(-.25*127); //left
-    wait_ms(10);
-    motors.stopBothMotors();
-    wait(2);
+    while((abs(leftEncoder.getPulses()) < 10 || abs(rightEncoder.getPulses()) < 10));
+
+    motors.stopBothMotors(0);
+    wait_ms(20);
     motors.begin();
 
 }
 
-void to_tools_section(float* location, float &current)
+void to_tools_section2(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;
-    pc.printf("current %f \r\n",current);
+    slightMove(FORWARD,3200);
+    current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
 
-    motors.stopBothMotors();
-
-    wait(2);
-
-    motors.setMotor0Speed(-.2*127); //right
-    motors.setMotor1Speed(-.2*127); //left
-    wait_ms(5);
-    motors.stopBothMotors();
+}
+void to_tools_section1(float* location, float &current)
+{
+    slightMove(FORWARD,6450);
+    current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
 
 }
 
 void from_tools_section(float* location, float &current)
 {
+
+
     alignWithWall(TOOLS);
-
+    pc.printf("align\r\n");
     wait_ms(100);
 
-    wall_follow2(LEFT,FORWARD,MID, current);
-    current= 78;
+    //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);
+        wall_follow2(LEFT,BACKWARD,TOOLS, current,0);
+        pc.printf("wall follow\r\n");
+        location[0]= current - ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
+        current= location[0];
+        pc.printf("current %f \r\n",current);
+        // go backwards
         leftEncoder.reset();
         rightEncoder.reset();
         motors.setMotor0Speed(-MAX_SPEED); //right
         motors.setMotor1Speed(-MAX_SPEED); //left
         while(abs(leftEncoder.getPulses()) < 120 || abs(rightEncoder.getPulses())< 120);
-
+        // hard stop
         leftEncoder.reset();
         rightEncoder.reset();
         motors.setMotor0Speed(MAX_SPEED); //right
         motors.setMotor1Speed(MAX_SPEED); //left
-        while(abs(leftEncoder.getPulses()) < 40 || abs(rightEncoder.getPulses())< 40);
-        motors.stopBothMotors();
+        while(abs(leftEncoder.getPulses()) < 10 || abs(rightEncoder.getPulses())< 10);
+        motors.stopBothMotors(0);
 
-        wait_ms(500);
-        location[0]= current- ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
-        current= location[0];
+        wait_ms(100);
         leftTurn();
-        slightleft();
         overBump(TOOLS);
     } else {
-        location[0]= 77;
+        pc.printf("else greater than 20\r\n");
+        location[0]= current;
         leftTurn();
-        wait_ms(20);
-        overBump(FIRST_WAVE);
+        overBump(TOOLS);
     }
 
     pc.printf("First Wavegap = %f\r\n",location[0]);
+
 }
+
 void mid_section(float* location, float &current, int* direction)
 {
-
     motors.begin();
 
-    if(IR.getIRDistance() > 20) return;
-
+    if(IR.getIRDistance() > 38) {
+        direction[0]= STRAIGHT;
+        overBump(MID);
+        return;
+    }
+    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);
-    current=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
+    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);
 
-    if(current != 0) {
+    wait_ms(500);
+    rangeFinderLeft.startMeas();
+    wait_ms(20);
+    rangeFinderLeft.getMeas(range);
+
+    if(range > 20 ) {
         direction[0]= RIGHT;
-        current+= location[0];
         location[1]= current;
+        slightMove(FORWARD,75);
+        //current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
     } 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);
+        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) {
+            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();
-    overBump(TOOLS);
-    // 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();
+
+    wait_ms(100);
+
+    overBump(MID);
 
 }
 
 void mid_section2(float* location, float &current, int* direction)
 {
-
     motors.begin();
 
-    if(IR.getIRDistance() > 20) return;
+    pc.printf("mid section 2\r\n");
+
+    if(IR.getIRDistance() > 38) {
+        direction[1]= STRAIGHT;
+        overBump(RIGS);
+        return;
+    }
 
     alignWithWall(MID);
-    wall_follow2(LEFT,FORWARD,MID, current);
-    current=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
+    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);
+
+    wait_ms(500);
 
-    if(current != 0) {
+    pc.printf("midseection 2 after wf2 %f",current);
+    rangeFinderLeft.startMeas();
+    wait_ms(20);
+    rangeFinderLeft.getMeas(range);
+
+    if(range > 20 ) {
         direction[1]= RIGHT;
-        current+= location[1];
         location[2]= current;
+        slightMove(FORWARD,75);
+        //current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
     } 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);
+        wall_follow2(LEFT,BACKWARD,MID,current,0);
+        location[2]= current- ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
+        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();
+    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 {
+        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);
+    }
+}
+
+void tools_section_return(float* location, float &current)
+{
+    if(location[0] > 16) {
+        leftTurn();
+        wall_follow2(LEFT, BACKWARD, RETURN, location[0], 0);
+    }
+    motors.stopBothMotors(0);
 
 }
 
-void ledtoggle(void)
+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)
 {
-    pwm.setPWM(9, 0, 4094);
-    wait(2);
-    pwm.setPWM(9, 0, 0);
+    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);
 }
 
 
 
+
+