For IEEE Robotics

Dependencies:   Adafruit-16-Ch-PWM-Servo-Driver HCSR04 PID PololuQik2 QEI Sharp mbed-rtos

Files at this revision

API Documentation at this revision

Comitter:
tashworth
Date:
Fri Apr 11 21:56:18 2014 +0000
Parent:
21:0907e1f5e16c
Commit message:
Fixed to pick up 3rd tool

Changed in this revision

ShapeDetect.h Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
diff -r 0907e1f5e16c -r 79c5871543b5 ShapeDetect.h
--- a/ShapeDetect.h	Tue Apr 08 16:20:40 2014 +0000
+++ b/ShapeDetect.h	Fri Apr 11 21:56:18 2014 +0000
@@ -5,9 +5,9 @@
 #define THRESHOLD 56 
 
 //areas from camera 11" from ground
-#define TRIANGLE_AREA       2356   //2356   
-#define SQUARE_AREA         3000   //3247
-#define CIRCLE_AREA         2600   //3036
+#define TRIANGLE_AREA       2356      
+#define SQUARE_AREA         3000   
+#define CIRCLE_AREA         2600   
 #define AREA_TOLERANCE      100         
 
 /* modes for image processing */
diff -r 0907e1f5e16c -r 79c5871543b5 main.cpp
--- a/main.cpp	Tue Apr 08 16:20:40 2014 +0000
+++ b/main.cpp	Fri Apr 11 21:56:18 2014 +0000
@@ -9,12 +9,12 @@
 #include "stdio.h"
 #include "LPC17xx.h"
 #include "Sharp.h"
-
-
+ 
+ 
 #define PI 3.14159
-
-
-
+ 
+ 
+ 
 /* Navigation Definitions */
 #define PIN_TRIGGERL    (p12)
 #define PIN_ECHOL       (p11)
@@ -38,8 +38,8 @@
 #define MID2           (3)
 #define RETURN         (4)
 #define FAR            (1)
-
-
+ 
+ 
 //States
 #define START                       0
 #define OILRIG1_POS                 1
@@ -59,9 +59,10 @@
 #define INSERT_TOOL                 15
 #define END                         16
 #define GOTO_TOOLS2                 17
-
-
-
+#define RETURN_TO_START             18
+ 
+ 
+ 
 //Servo Static Positions
 #define STORE_POSITION          0
 #define OIL_RIG1                1
@@ -79,13 +80,13 @@
 #define PU_TOOL_1_STAB          13
 #define PU_TOOL_2_STAB          14
 #define PU_TOOL_3_STAB          15
-
+ 
 //Rig definitions
 #define SQUARE                  1
 #define TRIANGLE                2
 #define CIRCLE                  3
-
-
+ 
+ 
 //Oil Rig distance thresholds
 #define OILRIG1_MAX     1800
 #define OILRIG1_MIN     1000
@@ -93,25 +94,25 @@
 #define OILRIG2_MIN     1000
 #define OILRIG3_MAX     1800
 #define OILRIG3_MIN     1000
-
+ 
 //for servo normalization
 #define MIN_SERVO_PULSE     900
 #define MAX_SERVO_PULSE     2100
 #define SERVO_MAX_ANGLE     180
-
+ 
 #define X_CENTER    80
 #define Y_CENTER    60
-
-
+ 
+ 
 DigitalOut myled1(LED1);
 DigitalOut myled2(LED2);
 DigitalOut myled3(LED3);
 DigitalOut myled4(LED4);
 InterruptIn startBtn(p7);
-
+ 
 void errFunction(void);
 bool cRc;
-
+ 
 Serial pc(USBTX,USBRX);                     //USB Comm
 Adafruit_PWMServoDriver pwm(p28,p27);       //pwm(SDA,SCL) - Servo Control PWM
 extern Serial lrf;                         //Laser Range Finder    lrf(p13,p14)
@@ -125,10 +126,10 @@
 QEI leftEncoder(p16,p15,NC,PPR,QEI::X4_ENCODING);
 Sharp IR(p20);
 //InterruptIn encoder(p29);
-
-
-
-
+ 
+ 
+ 
+ 
 /***************
 local servo functions
 ****************/
@@ -140,8 +141,8 @@
 void servoPosition(int set);
 int fire_checker(int rig);
 int button_start = 0;
-
-
+ 
+ 
 //Navigation Functions
 void wall_follow(int side, int direction, int section);
 void wall_follow2(int side, int direction, int section, float location, int rig);
@@ -163,13 +164,13 @@
 void overBump(int section);
 void alignWithWall(int section);
 void UntilWall(int dir);
-
-
+ 
+ 
 float normd(int* pop, int count, int threshold);
 int Xadjust(int tool);
-
+ 
 extern "C" void mbed_reset();
-
+ 
 /************
 Main Variables
 *************/
@@ -179,14 +180,14 @@
 int shape_detected = 0;
 float range, range2, pid_return;
 int num, input;
-
-
+ 
+ 
 /************
 Variables for Servos
 *************/
 int servoNum, servoAngle, outputDisabled, posNum, testVal;
 int currentPosition[7];
-
+ 
 typedef struct {
     int arm_position_name;      //for organization only (STORE, OILRIG1, OILRIG2...)
     int base_rotate;
@@ -196,21 +197,21 @@
     int claw_rotate;
     int claw_open;
 } Coord;
-
+ 
 /********************
 Static Arm Positions
 *********************/
-
+ 
 Coord Arm_Table[] = {
-
+ 
     // POSITION ODER:
     // base_rotate, base_arm, int big_arm, int claw_arm, int claw_rotate, int claw_open
-
+ 
 //increase in number 5 rotates gripper
-
+ 
     {STORE_POSITION, 85, 10, 0, 165, 175, 0},              // storing position
-    {OIL_RIG1, 160, 20, 60, 47, 175, 0},                 // point laser at oilrig1
-    {OIL_RIG2, 164, 20, 60, 47, 175, 0},                // point laser at oilrig2
+    {OIL_RIG1, 164, 20, 60, 44, 175, 0},                 // point laser at oilrig1
+    {OIL_RIG2, 164, 20, 60, 44, 175, 0},                // point laser at oilrig2
     {OIL_RIG3, 130, 90, 90, 52, 175, 0},                // point laser at oilrig2
     {DRIVE_POSITION_NOTOOL, 85, 10, 0, 165, 175, 0},    // Drive through course
     {TOOL_1, 101, 50, 80, 113, 90, 0},                  // Look over first tool
@@ -225,8 +226,8 @@
     {PU_TOOL_2_STAB, 78, 50, 90, 108, 90, 0},           // STAB TOOL 2
     {PU_TOOL_3_STAB, 53, 50, 90, 118, 90, 0},           // STAB TOOL 3
 };
-
-
+ 
+ 
 /* Variables for imageprocessing and distance */
 int x_coord;
 int y_coord;
@@ -235,13 +236,13 @@
 int shape_alignX_done = 0;
 int shape_alignY_done = 0;
 float deltaX = 0;
-
-
+ 
+ 
 /* Variables for distance sensor*/
 int distLaser;
 int fire_detected = 0;
 int fire_not_detected = 0;
-
+ 
 void button_int(void)
 {
     if(!button_start) {
@@ -253,47 +254,47 @@
     }
     return;
 }
-
-
-
+ 
+ 
+ 
 int main()
 {
-
-
+ 
+    
     /*****************
      INITIALIZATIONS
     *******************/
     float location[3], current=4;
     int direction[3];
     double distance;
-
+ 
     int pu, num, input;
-
-
+ 
+ 
     pc.baud(115200);
     //Laser Range Finder Initialization
     lrf_baudCalibration();
-
+ 
     motors.begin();
-
+ 
     startBtn.rise(&button_int);
-
+ 
     //Servo initialization
     initServoDriver();
     servoBegin();   //initiates servos to start position
     //ServoOutputDisable = 0;
-
+ 
     /********************************
     MAIN WHILE LOOP FOR COMPETITION
     *********************************/
-
-
+ 
+ 
     while(1) {
         if(button_start == 1) {
-
-
+ 
+ 
             switch (state) {
-
+ 
                     /**************************************************
                     *           STAGE 0
                     *
@@ -302,24 +303,25 @@
                     **************************************************/
                 case START :
                     myled1 = 1;
+                    //state = GOTO_TOOLS1;
                     state = OILRIG1_POS;
                     break;
-
-
+ 
+ 
                     /**************************************************
                     *           STAGE 1
                     *
                     *          - DETERMINE OIL RIG ON FIRE
                     *
                     **************************************************/
-
+ 
                 case OILRIG1_POS:                   //aims arm at square oil rig
-
+ 
                     servoPosition(OIL_RIG1);
                     wait(3); //wait for servo to settle before laser distance
-
+ 
                     fire = fire_checker(OIL_RIG1);  //determines if oil rig is on fire
-
+ 
                     //determines what tool is needed
                     if (fire == 1) {
                         pc.printf("FIRE FOUND!!!!!!!!\n\r");
@@ -330,9 +332,9 @@
                         state = OILRIG2_POS;
                     }
                     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);
@@ -340,12 +342,12 @@
                     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);   // moves to second rig
-
+ 
                     servoPosition(OIL_RIG2);    //position arm to point at first oilrig
                     wait(3);
-
+ 
                     fire = fire_checker(OIL_RIG2);
                     if (fire == 1) {
                         pc.printf("FIRE FOUND!!!!!!!!");
@@ -357,7 +359,7 @@
                         state = GOTO_TOOLS2;
                     }
                     break;
-
+ 
                     /**************************************************
                     *           STAGE 2
                     *
@@ -372,12 +374,14 @@
                     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_section1(location, current);
+                    //state  = NAVIGATE_WAVES_ROW1;
                     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(1, Arm_Table[DRIVE_POSITION_NOTOOL].base_arm);
@@ -385,15 +389,15 @@
                     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
-
+ 
                     slightMove(FORWARD,3250);
                     current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
-
+ 
                     state = IDENTIFY_TOOLS;
                     break;
-
-
-
+ 
+ 
+ 
                     /**************************************************
                     *           STAGE 3
                     *
@@ -402,11 +406,11 @@
                     *
                     **************************************************/
                 case IDENTIFY_TOOLS:
-
+ 
                     //wait(5);
                     servoPosition(TOOL_2);              //arm/camera looks over tool
                     wait(5);                            //wait for servos to settle
-
+ 
                     //shape_detected = shapeDetection();  //determines the shape
                     //clearBounds();
                     //printImageToFile(BINARY);
@@ -416,28 +420,30 @@
                     while( shape_alignY_done == 0) {
                         wait(1);
                         shape_detected = shapeDetection();
-
+ 
                         pc.printf("Y - Adjust to center tool\n\r");
-
+ 
                         if(get_com_y() < 50) {
                             wait(1);
                             slightMove(FORWARD,25);
                             current-=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
-
+ 
                         } else if(get_com_y() > 70) {
                             wait(1);
                             slightMove(BACKWARD,25);
                             current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
-
+ 
                         } else {
                             shape_alignY_done = 1;
                         }
                     }*/
-
+ 
                     // aveArea = sumArea/count;
                     //printImageToFile(BINARY);
                     //either goes to aquire the tool or look at the next shape
-                    if(Xadjust(TOOL_2) == tool_needed) {
+                    
+//****************//if(Xadjust(TOOL_2) == tool_needed) {
+                    if(Xadjust(TOOL_2) == 162) {
                         //printImageToFile(BINARY);
                         state = AQUIRE_TOOL2;
                         break;
@@ -453,25 +459,25 @@
                         while( shape_alignY_done == 0) {
                             wait(1);
                             shape_detected = shapeDetection();
-
+ 
                             pc.printf("Y - Adjust to center tool\n\r");
-
+ 
                             if(get_com_y() < 50) {
                                 wait(1);
                                 slightMove(FORWARD,25);
                                 current-=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
-
+ 
                             } else if(get_com_y() > 70) {
                                 wait(1);
                                 slightMove(BACKWARD,25);
                                 current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
-
+ 
                             } else {
                                 shape_alignY_done = 1;
                             }
                         }*/
-
-                        if (Xadjust(TOOL_1) == tool_needed) {
+//****************//    if (Xadjust(TOOL_1) == tool_needed) {
+                        if (Xadjust(TOOL_1) == 169) {
                             state = AQUIRE_TOOL1;
                             break;
                         } else {
@@ -480,11 +486,11 @@
                             state = AQUIRE_TOOL3;
                         }
                     }
-
+ 
                     break;
-
+ 
                 case AQUIRE_TOOL1:
-
+ 
                     servoPosition(PU_TOOL_1);
                     setServoPulse(4, 175);
                     wait(5);
@@ -503,18 +509,18 @@
                     wait(1);
                     setServoPulse(1, Arm_Table[PU_TOOL_1].base_arm);
                     wait(2);
-
+ 
                     setServoPulse(2, Arm_Table[DRIVE_POSITION_TOOL].big_arm);
                     setServoPulse(3, Arm_Table[DRIVE_POSITION_TOOL].claw_arm);
                     setServoPulse(1, Arm_Table[DRIVE_POSITION_TOOL].base_arm);
                     setServoPulse(0, Arm_Table[DRIVE_POSITION_TOOL].base_rotate);
                     setServoPulse(4, Arm_Table[DRIVE_POSITION_TOOL].claw_rotate);
                     setServoPulse(5, Arm_Table[DRIVE_POSITION_TOOL].claw_open);
-
-
+ 
+ 
                     state  = NAVIGATE_WAVES_ROW1;
                     break;
-
+ 
                 case AQUIRE_TOOL2:
                     servoPosition(PU_TOOL_2);
                     setServoPulse(4, 175);
@@ -535,118 +541,119 @@
                     setServoPulse(5, 140);
                     wait(2);
                     setServoPulse(1, Arm_Table[PU_TOOL_2].base_arm);
-
+ 
                     wait(2);
-
+ 
                     setServoPulse(2, Arm_Table[DRIVE_POSITION_TOOL].big_arm);
                     setServoPulse(3, Arm_Table[DRIVE_POSITION_TOOL].claw_arm);
                     setServoPulse(1, Arm_Table[DRIVE_POSITION_TOOL].base_arm);
                     setServoPulse(0, Arm_Table[DRIVE_POSITION_TOOL].base_rotate);
                     setServoPulse(4, Arm_Table[DRIVE_POSITION_TOOL].claw_rotate);
                     setServoPulse(5, Arm_Table[DRIVE_POSITION_TOOL].claw_open);
-
-
+ 
+ 
                     state  = NAVIGATE_WAVES_ROW1;
                     break;
-
+ 
                 case AQUIRE_TOOL3:
                     /*
                         while( shape_alignY_done == 0) {
                             wait(1);
-
+ 
                             servoPosition(PU_TOOL_3);
                             shape_detected = shapeDetection();
                             wait(2);
-
+ 
                             pc.printf("Y - Adjust to center tool\n\r");
-
+ 
                             if(get_com_y() < 50) {
                                 wait(1);
                                 slightMove(FORWARD,25);
                                 current-=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
-
+ 
                             } else if(get_com_y() > 70) {
                                 wait(1);
                                 slightMove(BACKWARD,25);
                                 current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
-
+ 
                             } else {
                                 shape_alignY_done = 1;
                             }
                         }
-
+ 
                     */
                     Xadjust(TOOL_3);
-
+ 
                     setServoPulse(4, 175);
                     wait(5);
-                    setServoPulse(0, Arm_Table[PU_TOOL_3].base_rotate );
+                    setServoPulse(0, Arm_Table[PU_TOOL_3].base_rotate - 1);
                     wait(1);
                     setServoPulse(1, Arm_Table[PU_TOOL_2].base_arm - 2);
                     wait(1);
                     setServoPulse(3, Arm_Table[PU_TOOL_2].claw_arm - 3);
                     wait(1);
-                    setServoPulse(1, Arm_Table[PU_TOOL_3].base_arm - 6);
+                    setServoPulse(1, Arm_Table[PU_TOOL_3].base_arm - 9);
                     wait(.5);
-                    setServoPulse(5, 140);
+                    setServoPulse(5, 110);
                     wait(.5);
                     setServoPulse(5, 00);
                     setServoPulse(1, Arm_Table[PU_TOOL_3].base_arm - 10);
                     wait(1);
-                    setServoPulse(5, 140);
+                    setServoPulse(5, 115);
                     wait(1);
                     setServoPulse(1, Arm_Table[PU_TOOL_3].base_arm);
-
+ 
                     wait(2);
-
+ 
                     setServoPulse(2, Arm_Table[DRIVE_POSITION_TOOL].big_arm);
                     setServoPulse(3, Arm_Table[DRIVE_POSITION_TOOL].claw_arm);
                     setServoPulse(1, Arm_Table[DRIVE_POSITION_TOOL].base_arm);
                     setServoPulse(0, Arm_Table[DRIVE_POSITION_TOOL].base_rotate);
                     setServoPulse(4, Arm_Table[DRIVE_POSITION_TOOL].claw_rotate);
                     setServoPulse(5, Arm_Table[DRIVE_POSITION_TOOL].claw_open);
-
+ 
                     state  = NAVIGATE_WAVES_ROW1;
                     break;
-
-
+ 
+ 
                     /**************************************************
                     *           STAGE 4
                     *
                     *           - Navigate through the ocean
                     *
                     **************************************************/
-
+ 
                 case NAVIGATE_WAVES_ROW1:
                     from_tools_section(location,current);
-
+ 
                     mid_section(location, current, direction);
-
+ 
                     state = NAVIGATE_WAVES_ROW2;
                     break;
-
+ 
                 case NAVIGATE_WAVES_ROW2:
                     mid_section2(location, current, direction);
                     state = NAVIGATE_WAVES_ROW3;
                     break;
-
+ 
                 case NAVIGATE_WAVES_ROW3:
-
+                    shape_detected = 1;
                     if(shape_detected == 1) {
                         rig_section(location, current, direction, 1);
-                        while(1);
-                        state = NAVIGATE_TO_SQUARE_RIG;
+                        //state = NAVIGATE_TO_SQUARE_RIG;
+                        state = RETURN_TO_START;
                     } else if(shape_detected == 2) {
                         rig_section(location, current, direction, 2);
-                        while(1);
-                        state = NAVIGATE_TO_TRIANGLE_RIG;
+                        //state = NAVIGATE_TO_TRIANGLE_RIG;
+                        state = RETURN_TO_START;
                     } else {
                         rig_section(location, current, direction, 3);
-                        while(1);
-                        state = NAVIGATE_TO_CIRCLE_RIG;
+                        //state = NAVIGATE_TO_CIRCLE_RIG;
+                        state = RETURN_TO_START;
+ 
                     }
                     break;
-
+ 
                     /**************************************************
                     *           STAGE 5
                     *
@@ -665,7 +672,7 @@
                     //NAVIGATION CODE HERE
                     state = RIG_ALIGN;
                     break;
-
+ 
                     /**************************************************
                     *           STAGE 6
                     *
@@ -673,17 +680,17 @@
                     *
                     **************************************************/
                 case RIG_ALIGN:
-
+ 
                     //*********************//
                     //******* TODO ********//
                     //*********************//
                     // CODE TO ALIGN ROBOT WITH RIG
-
+ 
                     servoPosition(ORIENT_TOOL);
                     wait(1);                        //wait for servos to settle
                     state = INSERT_TOOL;
                     break;
-
+ 
                     /**************************************************
                     *           STAGE 7
                     *
@@ -692,17 +699,36 @@
                     *           - win contest
                     *
                     **************************************************/
-
+ 
                 case INSERT_TOOL:
                     //*********************//
                     //******* TODO ********//
                     //*********************//
                     // CODE TO INSERT TOOL
                     break;
-
+ 
                     /**************************************************
                     *           STAGE 8
                     *
+                    *           - Return to start zone
+                    *
+                    **************************************************/
+                case RETURN_TO_START:
+                    wait(3);
+                    rig_section_return(location, current, direction);
+                    wait(3);
+                    mid_section2_return(location, current, direction);
+                    wait(3);
+                    mid_section_return(location, current, direction);
+                    wait(3);
+                    tools_section_return(location,current);
+                    while(1);
+                    state = END;
+                    break;
+ 
+                    /**************************************************
+                    *           STAGE 9
+                    *
                     *           - END COMPETITION
                     *
                     **************************************************/
@@ -718,24 +744,24 @@
                     wait(.2);
                     break;
                 default:
-
+ 
                     break;
             }
         }   // End while loop
-
+ 
     }   // End if for start button
-
-
+ 
+ 
 }   // main loop
-
-
-
+ 
+ 
+ 
 /************
-
+ 
 Servo Functions
-
+ 
 **************/
-
+ 
 void setServoPulse(uint8_t n, int angle)
 {
     int pulse;
@@ -760,16 +786,16 @@
     currentPosition[n] = i;
     //pc.printf("END: pulse: %d,  angle: %d\n\r", i, angle);
 }
-
+ 
 void initServoDriver(void)
 {
     pwm.begin();
     //pwm.setPWMFreq(100);  //This dosen't work well because of uncertain clock speed. Use setPrescale().
     pwm.setPrescale(140);    //This value is decided for 20ms interval.
     pwm.setI2Cfreq(400000); //400kHz
-
+ 
 }
-
+ 
 void servoBegin(void)
 {
     pc.printf("Setting Initial Servo Position\n\r");
@@ -781,7 +807,7 @@
     setServoPulseNo_delay(4, Arm_Table[STORE_POSITION].claw_rotate);
     setServoPulseNo_delay(5, Arm_Table[STORE_POSITION].claw_open);
 }
-
+ 
 void setServoPulseNo_delay(uint8_t n, int angle)
 {
     int pulse = MIN_SERVO_PULSE + (( angle * (MAX_SERVO_PULSE -  MIN_SERVO_PULSE)) / SERVO_MAX_ANGLE);
@@ -790,7 +816,7 @@
     //pc.printf("ServoNumber: %d    Pulsewidth: %d  Angle: %d \n\r",n,pulse, angle);
     pulse = 4094 * pulse / pulselength;
     pwm.setPWM(n, 0, pulse);
-
+ 
 }
 void setServoPulse2(uint8_t n, float angle)
 {
@@ -799,9 +825,9 @@
     pulse = 4094 * pulse / pulselength;
     pwm.setPWM(n, 0, pulse);
 }
-
-
-
+ 
+ 
+ 
 void servoPosition(int set)
 {
     //moves to current position
@@ -812,18 +838,18 @@
     setServoPulse(4, Arm_Table[set].claw_rotate);
     setServoPulse(5, Arm_Table[set].claw_open);
 }
-
-
-
-
-
-
-
-
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
 int fire_checker(int rig)
 {
     switch (rig) {
-
+ 
         case 1:
             for (int i = 0; i<10; i++) {
                 distLaser = getDistance();
@@ -848,43 +874,43 @@
                 }
             }
             break;
-
+ 
     }
-
+ 
     if (fire_detected > 0) {
         return 1;
     } else {
         return 0;
     }
 }
-
+ 
 void errFunction(void)
 {
     pc.printf("\n\nERROR: %d", motors.getError() );
-
+ 
 }
-
-
-
-
+ 
+ 
+ 
+ 
 void wall_follow(int side, int direction, int section)
 {
     float location, 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< 66.5) {
         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);
@@ -898,17 +924,17 @@
             rangeFinderRight.getMeas(range);
             //pc.printf("%d\r\n",range);
         }
-
+ 
         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
@@ -931,58 +957,59 @@
             }
         }
     }
-
+ 
     //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, int rig)
 {
     int dir=1, limit=80, lowlim=4;
     float set=9, 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;
-    } else if(direction == FORWARD) lowlim=-20;
+ 
     if(section == TOOLS) {
         set= 9;
         limit = 86;
     } else if(section == RIGS) set = 9;
     else if(section == RETURN) lowlim=4;
     else if(section == MID2) limit =85;
-
+ 
+    if(direction == BACKWARD) {
+        dir=-1;
+        limit = 100;
+    } else if(direction == FORWARD) lowlim=-20;
+ 
     if(location <4) limit=80;
-
+ 
     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 >= 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_SPEED1, MAX_SPEED1);
         pid1.setSetPoint(set);
-
+ 
         if(side) {
             rangeFinderLeft.startMeas();
             wait_ms(20);
@@ -992,12 +1019,12 @@
             wait_ms(20);
             rangeFinderRight.getMeas(range);
         }
-
+ 
         if(section == RIGS) {
             rangeFinderLeft.startMeas();
             wait_ms(20);
             rangeFinderLeft.getMeas(range2);
-
+ 
             if(range2< 15) {
                 if( abs(dir*loc + location - Rigloc) < 10) {
                     //STOP
@@ -1006,8 +1033,8 @@
                 }
             }
         }
-
-
+ 
+ 
         //pc.printf("wall follow 2 range %f\r\n",range);
         //pc.printf("loc+location = %f\r\n", loc+location);
         if(range > 15 ) {
@@ -1016,12 +1043,12 @@
                 motors.setMotor1Speed(dir*0.25*127); //left
             } else {
                 if(!SeeWaveGap) {
-                    wait_ms(40);
+                    wait_ms(75);
                     SeeWaveGap=true;
                 } else {
                     //STOP
                     motors.stopBothMotors(127);
-
+ 
                     //pc.printf("wavegap\r\n");
                     // AT WAVE OPENING!!!!
                     break;
@@ -1032,7 +1059,7 @@
             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
@@ -1055,16 +1082,16 @@
             }
         }
     }
-
+ 
     //STOP
     motors.stopBothMotors(127);
 }
-
-
+ 
+ 
 void alignWithWall(int section)
 {
     float usValue = 0;
-
+ 
     if(section == TOOLS) {
         //pc.printf("tools section align\r\n");
         // turn at an angle
@@ -1074,7 +1101,7 @@
         motors.setMotor1Speed(0.4*MAX_SPEED); //left
         while(rightEncoder.getPulses()>-1000);
         motors.stopBothMotors(0);
-
+ 
         //go backwards toward wall
         leftEncoder.reset();
         rightEncoder.reset();
@@ -1082,14 +1109,14 @@
         motors.setMotor1Speed(-0.25*127); //left
         while(abs(leftEncoder.getPulses()) < 300 || abs(rightEncoder.getPulses()) < 300);
         motors.stopBothMotors(0);
-
+ 
         // turn left towards wall
         leftEncoder.reset();
         rightEncoder.reset();
         motors.setMotor0Speed(MAX_SPEED); //right
         motors.setMotor1Speed(-MAX_SPEED); //left
         while(rightEncoder.getPulses() < 20 || abs(leftEncoder.getPulses()) < 20);
-
+ 
         motors.stopBothMotors(127);
         wait_ms(300);
         return;
@@ -1097,7 +1124,7 @@
                rangeFinderRight.startMeas();
                wait_ms(20);
                rangeFinderRight.getMeas(range);
-
+ 
                if(range>15){
                    // turning left
                    leftEncoder.reset();
@@ -1112,15 +1139,15 @@
         // turning left
         //motors.setMotor0Speed(0.9*MAX_SPEED); //right
         //motors.setMotor1Speed(-0.9*MAX_SPEED); //left
-
+ 
     } else if(section == RIGS) {
         // check distance to wall
         rangeFinderRight.startMeas();
         wait_ms(20);
         rangeFinderRight.getMeas(range);
-
+ 
         if(range < 3) return;
-
+ 
         // turn at an angle
         leftEncoder.reset();
         rightEncoder.reset();
@@ -1129,7 +1156,7 @@
         while(abs(leftEncoder.getPulses())<500);
         motors.stopBothMotors(0);
         wait(2);
-
+ 
         //go backwards toward wall
         leftEncoder.reset();
         rightEncoder.reset();
@@ -1138,17 +1165,17 @@
         while(abs(leftEncoder.getPulses()) < 200 || abs(rightEncoder.getPulses()) < 200);
         motors.stopBothMotors(0);
         wait(2);
-
+ 
         // turn right towards wall
         leftEncoder.reset();
         rightEncoder.reset();
         motors.setMotor0Speed(-MAX_SPEED); //right
         motors.setMotor1Speed(MAX_SPEED); //left
         while(abs(rightEncoder.getPulses()) < 50 || abs(leftEncoder.getPulses()) < 50);
-
+ 
         motors.stopBothMotors(127);
         /*        wait(2);
-
+ 
                 // turning left
                 motors.setMotor0Speed(-0.9*MAX_SPEED); //right
                 motors.setMotor1Speed(0.9*MAX_SPEED); //left
@@ -1160,14 +1187,14 @@
         motors.setMotor1Speed(0.4*MAX_SPEED); //left
         while(rightEncoder.getPulses()>-1000);
         motors.stopBothMotors(0);
-
+ 
         //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);
-
+ 
         // turn left towards wall
         leftEncoder.reset();
         rightEncoder.reset();
@@ -1175,10 +1202,10 @@
         motors.setMotor1Speed(-0.4*127); //left
         while(abs(rightEncoder.getPulses()) < 65 || abs(leftEncoder.getPulses()) < 65);
         motors.stopBothMotors(127);
-
+ 
         slightMove(FORWARD,100);
         return;
-
+ 
     } else { // MID
         //pc.printf("in mid section align\r\n");
         // turn right towards wall
@@ -1186,9 +1213,9 @@
         // turning left towards wall
         motors.setMotor0Speed(0.9*MAX_SPEED); //right
         motors.setMotor1Speed(-0.9*MAX_SPEED); //left
-
+ 
     }
-
+ 
     usValue = 0;
     /*    while(1) {
             if(section == 10) { // CURENTLY NOT USED (WAS RIGS)
@@ -1209,7 +1236,7 @@
         }
         motors.stopBothMotors(0);*/
 }
-
+ 
 void rightTurn(void)
 {
     motors.begin();
@@ -1220,7 +1247,7 @@
     while(abs(leftEncoder.getPulses())<850 || abs(rightEncoder.getPulses())<850);
     motors.stopBothMotors(127);
 }
-
+ 
 void leftTurn(void)
 {
     motors.begin();
@@ -1231,10 +1258,10 @@
     while(abs(leftEncoder.getPulses())<1075 || rightEncoder.getPulses()<1075);
     motors.stopBothMotors(127);
 }
-
+ 
 void slightleft(void)
 {
-
+ 
     leftEncoder.reset();
     rightEncoder.reset();
     motors.setMotor0Speed(0.5*127);// right
@@ -1242,10 +1269,10 @@
     while(abs(leftEncoder.getPulses())<70 || rightEncoder.getPulses()<70);
     motors.stopBothMotors(127);
 }
-
+ 
 void slightright(void)
 {
-
+ 
     leftEncoder.reset();
     rightEncoder.reset();
     motors.setMotor0Speed(-0.4*127);// right
@@ -1253,47 +1280,47 @@
     while(abs(leftEncoder.getPulses())<200 || abs(rightEncoder.getPulses())<200);
     motors.stopBothMotors(127);
 }
-
+ 
 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.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.stopBothMotors(127);
 }
-
+ 
 void overBump(int section)
 {
     int preLeft=5000, preRight=5000, out=0;
-
+ 
     motors.begin();
     // slight backwards
     leftEncoder.reset();
@@ -1301,64 +1328,65 @@
     motors.setMotor0Speed(-0.25*127); //right
     motors.setMotor1Speed(-0.25*127); //left
     while(abs(leftEncoder.getPulses()) < 50 || abs(rightEncoder.getPulses()) < 50);
+ 
     motors.stopBothMotors(127);
-
+ 
     //pc.printf("slight backwards\r\n");
     wait_ms(200);
-
+ 
     // Over bump
     leftEncoder.reset();
     rightEncoder.reset();
     motors.setMotor0Speed(0.3*127); //right
     motors.setMotor1Speed(0.3*127); //left
     while((abs(leftEncoder.getPulses()) < 900 || abs(rightEncoder.getPulses()) < 900) /*&& preLeft!=0*/ && IR.getIRDistance() >15 );
-
-
-
+ 
+ 
+ 
     //pc.printf("forward \r\n");
     wait_ms(200);
-
+ 
     motors.stopBothMotors(0);
     motors.begin();
-
+ 
     preLeft=preRight=5000 ;
     leftEncoder.reset();
     rightEncoder.reset();
     motors.setMotor0Speed(.25*127); //right
     motors.setMotor1Speed(.25*127); //left
-
+ 
     if(section == TOOLS) {
         while(IR.getIRDistance() > 10 && (abs(leftEncoder.getPulses()) < 200 || abs(rightEncoder.getPulses()) < 200) && (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){
-                motors.setMotor0Speed(.3*127); //right
-                motors.setMotor1Speed(.3*127); //left
-             while(IR.getIRDistance() > 15 && (abs(leftEncoder.getPulses()) < 300 || abs(rightEncoder.getPulses()) < 300));
-             }
-
+        if(section == MID2) {
+            motors.setMotor0Speed(.3*127); //right
+            motors.setMotor1Speed(.3*127); //left
+            while(IR.getIRDistance() > 15 && (abs(leftEncoder.getPulses()) < 300 || abs(rightEncoder.getPulses()) < 300));
+        }
+ 
         while(IR.getIRDistance() > 15 && (abs(leftEncoder.getPulses()) < 100 || abs(rightEncoder.getPulses()) < 100) && (leftEncoder.getPulses() != preLeft || rightEncoder.getPulses() != preRight)) {
-
+ 
             if(IR.getIRDistance() > 38) break;
-
+ 
             preLeft=leftEncoder.getPulses();
             preRight=rightEncoder.getPulses();
             wait_ms(200);
         }
-
+ 
     } else {// RIGS
         while(abs(leftEncoder.getPulses()) < 220 || abs(rightEncoder.getPulses()) < 220);
-
+ 
         // go backwards to line up with bump
         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 ) {
@@ -1369,51 +1397,51 @@
         }
         motors.stopBothMotors(127);
         motors.begin();
-
+ 
         return;
     }
-
+ 
     motors.stopBothMotors(127);
     wait_ms(20);
     motors.begin();
-
+ 
 }
-
-
+ 
+ 
 void to_tools_section1(float* location, float &current)
 {
-    slightMove(FORWARD,6850);
+    slightMove(FORWARD,6650);
     current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
-
+ 
 }
-
+ 
 void to_tools_section2(float* location, float &current)
 {
     slightMove(FORWARD,3250);
     current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
-
+ 
 }
-
+ 
 void from_tools_section(float* location, float &current)
 {
-
+ 
     //alignWithWall(TOOLS);
     //current-=4;
-
+ 
     //slightMove(FORWARD,150);
     //current+=1;
     //pc.printf("align\r\n");
     //wait_ms(200);
-
+ 
     //wall_follow2(LEFT,FORWARD,MID, current);
-
+ 
     slightMove(BACKWARD,400);
     current-=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
-
+ 
     rangeFinderLeft.startMeas();
     wait_ms(20);
     rangeFinderLeft.getMeas(range);
-
+ 
     if(range < 15) {
         wall_follow2(LEFT,BACKWARD,MID, current,0);
         //pc.printf("wall follow\r\n");
@@ -1422,7 +1450,7 @@
         //pc.printf("current %f \r\n",current);
         // go backwards
         //slightMove(BACKWARD,200);
-
+ 
         wait_ms(100);
         leftTurn();
         slightleft();
@@ -1433,38 +1461,38 @@
         leftTurn();
         overBump(TOOLS);
     }
-
+ 
     //pc.printf("First Wavegap = %f\r\n",location[0]);
-
+ 
 }
 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;
-
+ 
     //////////////////////////////// determine tool
     wait(2);
     ///////////////////////////////////////////////////////////////////////////////////////
     // Move Forward
     slightMove(FORWARD, 100);
     current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
-
+ 
     //////////////////////////////////////////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");
@@ -1478,9 +1506,9 @@
         motors.setMotor1Speed(-MAX_SPEED); //left
         while(abs(leftEncoder.getPulses()) < 120 || abs(rightEncoder.getPulses())< 120);
         // hard stop
-
+ 
         motors.stopBothMotors(127);
-
+ 
         wait_ms(100);
         leftTurn();
         overBump(TOOLS);
@@ -1490,13 +1518,18 @@
         leftTurn();
         overBump(TOOLS);
     }
-
+ 
     //pc.printf("First Wavegap = %f\r\n",location[0]);
 }
-
+ 
 void mid_section(float* location, float &current, int* direction)
 {
     if(IR.getIRDistance() > 38) {
+        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);
         direction[0]= STRAIGHT;
         overBump(MID);
         return;
@@ -1505,7 +1538,7 @@
     //alignWithWall(MID);
     //current-=4;
     //wait_ms(200);
-
+ 
     //if(current > 20){
     //alignWithWall(MID2);
     //current-=4;
@@ -1515,38 +1548,39 @@
     rightEncoder.reset();
     motors.setMotor0Speed(-0.4*127);// right
     motors.setMotor1Speed(0.4*127);// left
-    while(abs(leftEncoder.getPulses())<50 || abs(rightEncoder.getPulses())<50);
+    while(abs(leftEncoder.getPulses())<100 || abs(rightEncoder.getPulses())<100);
     motors.stopBothMotors(127);
     //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 ) {
+        wait(3);
         direction[0]= RIGHT;
         location[1]= current;
         wait_ms(300);
-        slightMove(FORWARD,100);
+        slightMove(FORWARD,200);
         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) {
             slightMove(FORWARD, 75);
             current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
         }
         //slightMove(BACKWARD,100);
-
+ 
     }
-
+ 
     wait_ms(200);
     //pc.printf("wavegap2 = %f\r\n",location[1]);
     //left turn
@@ -1557,41 +1591,45 @@
     motors.setMotor1Speed(-0.5*127);// left
     while(abs(leftEncoder.getPulses())<1045 || rightEncoder.getPulses()<1045);
     motors.stopBothMotors(127);
-
+ 
     wait_ms(100);
-
+ 
     overBump(MID);
-
 }
-
+ 
 void mid_section2(float* location, float &current, int* direction)
 {
     //pc.printf("mid section 2\r\n");
-
+ 
     if(IR.getIRDistance() > 38) {
+        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);
         direction[1]= STRAIGHT;
         overBump(RIGS);
         return;
     }
-
+ 
     //alignWithWall(MID);
     wait_ms(100);
-
+ 
     rightTurn();
-    slightright();
+    //slightright();
     wait_ms(100);
-
-
+ 
+ 
     wall_follow2(LEFT,FORWARD,MID, current,0);
     current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
-
+ 
     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;
@@ -1604,36 +1642,36 @@
         current=location[2];
         //slightMove(BACKWARD,100);
     }
-
+ 
     //LEFT turn
     motors.begin();
     leftEncoder.reset();
     rightEncoder.reset();
     motors.setMotor0Speed(0.5*127);// right
     motors.setMotor1Speed(-0.5*127);// left
-    while(abs(leftEncoder.getPulses())<950 || rightEncoder.getPulses()<950);
+    while(abs(leftEncoder.getPulses())<1050 || rightEncoder.getPulses()<1050);
     motors.stopBothMotors(127);
-
+ 
     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;
-
+ 
     // Slight forward for turn
     slightMove(FORWARD,150);
     current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
     wait_ms(100);
     rightTurn();
     //slightright();
-
-
+ 
+ 
     if(current > loc) {
         //pc.printf("RIG section %f\r\n",current);
         wall_follow2(RIGHT, BACKWARD, RIGS, current, rig);
@@ -1643,20 +1681,15 @@
         wall_follow2(RIGHT, FORWARD, RIGS, current, rig);
         current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
     }
-
+    wait(2);
     alignWithWall(MID2);
     current-=4;
     wall_follow2(RIGHT, FORWARD, RIGS, current, rig);
     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);
-
-
-
-
-
 }
-
+ 
 void tools_section_return(float* location, float &current)
 {
     if(location[0] > 16) {
@@ -1664,9 +1697,9 @@
         wall_follow2(LEFT, BACKWARD, RETURN, location[0], 0);
     }
     motors.stopBothMotors(0);
-
+ 
 }
-
+ 
 void mid_section_return(float* location, float &current, int* direction)
 {
     if(direction[0] == RIGHT) {
@@ -1684,7 +1717,7 @@
     //ELSE and GO FORWARD
     overBump(RIGS);
 }
-
+ 
 void mid_section2_return(float* location, float &current, int* direction)
 {
     if(direction[1] == RIGHT) {
@@ -1701,7 +1734,7 @@
     //ELSE and GO FORWARD
     overBump(MID);
 }
-
+ 
 void rig_section_return(float* location, float &current, int* direction)
 {
     if(location[2] > current) {
@@ -1714,15 +1747,15 @@
     rightTurn();
     overBump(MID2);
 }
-
-
-
-
+ 
+ 
+ 
+ 
 int Xadjust(int tool)
 {
     int areaArray[10];
     float C, T, S;
-    for(int i = 0; i < 10; i++) {
+    for(int i = 0; i < 5; i++) {
         areaArray[i] = shapeDetection();
         wait(2);
         if(get_com_x() > X_CENTER ) {
@@ -1735,26 +1768,26 @@
             setServoPulse2(0,Arm_Table[tool].base_rotate += (deltaX/60.0) );
             Arm_Table[PU_TOOL_2].base_rotate = Arm_Table[TOOL_2].base_rotate;
         }
-
-
+ 
+ 
     }
-
+ 
     C = normd(areaArray, 10, CIRCLE_AREA);
     // S = normd(areaArray, 10, SQUARE_AREA);
     // T = normd(areaArray, 10, TRIANGLE_AREA);
-
+ 
     if((C < SQUARE_AREA) && (C > CIRCLE_AREA)) {
+        pc.printf("\n\nCIRCLE DETECTED\n\r");
+        return CIRCLE;
+    } else if( ( C > SQUARE_AREA) ) {
         pc.printf("\n\nSQUARE DETECTED\n\r");
         return SQUARE;
-    } else if( ( C > SQUARE_AREA) ) {
-        pc.printf("\n\nCIRCLE DETECTED\n\r");
-        return CIRCLE;
     } else {
         pc.printf("\n\nTRIANGLE DETECTED\n\r");
         return TRIANGLE;
     }
-
-
+ 
+ 
     /*
         if((C < S) && (C < T)) {
             pc.printf("\n\nCIRCLE DETECTED\n\r");
@@ -1767,7 +1800,7 @@
             return TRIANGLE;
         }*/
 }
-
+ 
 float normd(int* pop, int count, int threshold)
 {
     int i = 0;
@@ -1777,19 +1810,19 @@
     }
     mean /= (float)count;
     pc.printf("\n\nMean: %f\n\r", mean);
-
+ 
     for(i=0; i<count; i++) {
         std += pow(((float)pop[i]-mean),2);
     }
     std /= (float)count;
     std = sqrt(std);
     //pc.printf("\n\nStd: %f\n\r", std);
-
+ 
     //pc.printf("\n\nNorm: %f\n\r", (1/(std*sqrt(2*PI)))*exp(-pow(((float)threshold-mean),2)/(2*pow(std,2))));
-
-
+ 
+ 
     //return abs(mean - threshold);
     return mean;
     //return (1/(std*sqrt(2*PI)))*exp(-pow(((float)threshold-mean),2)/(2*pow(std,2)));
-
-}
+ 
+}
\ No newline at end of file