Revision 20:55dcff40c5d9, committed 2014-04-05

Comitter:

tashworth

Date:

Sat Apr 05 07:26:15 2014 +0000

Parent:

19:d4d967a885dc

Child:

21:0907e1f5e16c

Commit message:

4-5-14

Changed in this revision

--- a/PololuQik2.lib	Fri Apr 04 01:09:49 2014 +0000
+++ b/PololuQik2.lib	Sat Apr 05 07:26:15 2014 +0000
@@ -1,1 +1,1 @@
-http://mbed.org/users/jjcarr2/code/PololuQik2/#25c41766d768
+http://mbed.org/users/tashworth/code/PololuQik2/#01272a06e922

--- a/ShapeDetect.cpp	Fri Apr 04 01:09:49 2014 +0000
+++ b/ShapeDetect.cpp	Sat Apr 05 07:26:15 2014 +0000
@@ -281,23 +281,26 @@
 {
 
     centerMass(&xcoord_val, &ycoord_val, &s_area_val);
+    
     pc.printf("\ncentriod calculated\n\r");
     pc.printf("Center of Mass is at X: %d    Y: %d\n\r", xcoord_val, ycoord_val, s_area_val);
     pc.printf("The area of the Mass is: %d\n\r", s_area_val);
     
-    //if( (s_area_val > (SQUARE_AREA - AREA_TOLERANCE)) && (s_area_val < (SQUARE_AREA + AREA_TOLERANCE)) ) {
-    if( (s_area_val > 3150) ) {
-        pc.printf("\nSQUARE DETECTECD\n\r");
+/*
+    if( (s_area_val > SQUARE_AREA) ) {
+        pc.printf("\nSQUARE DETECTECD\n\r");   
         return 1;
     } 
-    //else if ((s_area_val > (TRIANGLE_AREA - AREA_TOLERANCE)) && (s_area_val < (TRIANGLE_AREA + AREA_TOLERANCE)) ) {
-        else if (s_area_val < 2600) {
+        else if (s_area_val < TRIANGLE_AREA) {
         pc.printf("\nTRIANGLE DETECTECD\n\r");
         return 2;
     } else {
         pc.printf("\nCIRCLE DETECTECD\n\r");
         return 3;
     }
+    */
+    
+    return s_area_val;
 }
 
 

--- a/ShapeDetect.h	Fri Apr 04 01:09:49 2014 +0000
+++ b/ShapeDetect.h	Sat Apr 05 07:26:15 2014 +0000
@@ -2,11 +2,12 @@
 #define SHAPEDETECT_H_
 
 /* theshold for setting binary output */
-#define THRESHOLD 80 
+#define THRESHOLD 56 
 
 //areas from camera 11" from ground
-#define TRIANGLE_AREA       2100
-#define SQUARE_AREA         3400  
+#define TRIANGLE_AREA       2356   //2356   
+#define SQUARE_AREA         3200   //3247
+#define CIRCLE_AREA         2800   //3036
 #define AREA_TOLERANCE      100         
 
 /* modes for image processing */

--- a/main.cpp	Fri Apr 04 01:09:49 2014 +0000
+++ b/main.cpp	Sat Apr 05 07:26:15 2014 +0000
@@ -10,6 +10,11 @@
 #include "LPC17xx.h"
 #include "Sharp.h"
 
+
+#define PI 3.14159
+
+
+
 /* Navigation Definitions */
 #define PIN_TRIGGERL    (p12)
 #define PIN_ECHOL       (p11)
@@ -20,6 +25,7 @@
 #define DIST_PER_PULSE (0.01054225722682)
 #define MTRS_TO_INCH   (39.3701)
 #define MAX_SPEED      (0.3*127)
+#define MAX_SPEED1     (0.25*127)
 #define PPR            (4331/4)
 #define LEFT           (1)
 #define RIGHT          (0)
@@ -33,6 +39,7 @@
 #define RETURN         (4)
 #define FAR            (1)
 
+
 //States
 #define START                       0
 #define OILRIG1_POS                 1
@@ -107,7 +114,6 @@
 
 Serial pc(USBTX,USBRX);                     //USB Comm
 Adafruit_PWMServoDriver pwm(p28,p27);       //pwm(SDA,SCL) - Servo Control PWM
-DigitalOut ServoOutputDisable(p8);          //Servo Control Output Enable/Disable
 extern Serial lrf;                         //Laser Range Finder    lrf(p13,p14)
 //Hardware Initialization
 //Serial bt(p13,p14);
@@ -158,6 +164,10 @@
 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();
 
 /************
@@ -165,7 +175,7 @@
 *************/
 int state = START;
 int fire = 0;
-int tool_needed = 0;
+int tool_needed = 1;
 int shape_detected = 0;
 float range, range2, pid_return;
 int num, input;
@@ -199,8 +209,8 @@
 //increase in number 5 rotates gripper
 
     {STORE_POSITION, 85, 10, 0, 165, 175, 0},              // storing position
-    {OIL_RIG1, 162, 20, 60, 50, 175, 0},                 // point laser at oilrig1
-    {OIL_RIG2, 165, 20, 60, 50, 175, 0},                // point laser at oilrig2
+    {OIL_RIG1, 156, 20, 60, 47, 175, 0},                 // point laser at oilrig1
+    {OIL_RIG2, 162, 20, 60, 47, 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
@@ -246,11 +256,6 @@
 
 
 
-
-
-
-
-
 int main()
 {
 
@@ -282,6 +287,8 @@
     MAIN WHILE LOOP FOR COMPETITION
     *********************************/
 
+
+
     while(1) {
         if(button_start == 1) {
 
@@ -380,7 +387,7 @@
                     setServoPulse(5, Arm_Table[DRIVE_POSITION_NOTOOL].claw_open);
                     wait(3);                                //wait for servos to settle
 
-                    slightMove(FORWARD,3100);
+                    slightMove(FORWARD,3350);
                     current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
 
                     state = IDENTIFY_TOOLS;
@@ -397,7 +404,7 @@
                     **************************************************/
                 case IDENTIFY_TOOLS:
 
-                    wait(5);
+                    //wait(5);
                     servoPosition(TOOL_2);              //arm/camera looks over tool
                     wait(5);                            //wait for servos to settle
 
@@ -428,51 +435,17 @@
                         }
                     }*/
 
-
-                    for(int i = 0; i < 4; i++) {
-                        shape_detected = shapeDetection();
-                        wait(2);
-                        if(get_com_x() > X_CENTER ) {
-                            deltaX = get_com_x()-X_CENTER;
-                            setServoPulse2(0,Arm_Table[TOOL_2].base_rotate += (deltaX/60.0) );
-                            Arm_Table[PU_TOOL_2].base_rotate = Arm_Table[TOOL_2].base_rotate;
-                        }
-                        if(get_com_x() < X_CENTER) {
-                            deltaX = get_com_x()-X_CENTER;
-                            setServoPulse2(0,Arm_Table[TOOL_2].base_rotate += (deltaX/60.0) );
-                            Arm_Table[PU_TOOL_2].base_rotate = Arm_Table[TOOL_2].base_rotate;
-                        }
-                    }
-
-
-                    /*
-
-                    while( shape_alignX_done == 0) {
-                        wait(1);
-                        shape_detected = shapeDetection();
-
-                        pc.printf("X - Adjust to center tool\n\r");
-                        if(get_com_x() > 95) {
-                            setServoPulse(0,Arm_Table[TOOL_2].base_rotate+=1);
-
-                        } else if(get_com_x() < 65) {
-                            setServoPulse(0,Arm_Table[TOOL_2].base_rotate-=1);
-
-                        } else {
-                            shape_alignX_done = 1;
-                        }
-                    }*/
-
+                    // aveArea = sumArea/count;
                     //printImageToFile(BINARY);
                     //either goes to aquire the tool or look at the next shape
-                    if(shape_detected == tool_needed) {
+                    if(Xadjust(TOOL_2) == tool_needed) {
+                        //printImageToFile(BINARY);
                         state = AQUIRE_TOOL2;
                         break;
                     } else {
-                        slightMove(FORWARD,40);
+                        //printImageToFile(BINARY);
+                        slightMove(FORWARD,60);
                         current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
-
-
                         servoPosition(TOOL_1);
                         wait(5);                        //wait for servos to settle
                         shape_alignX_done = 0;
@@ -499,41 +472,7 @@
                             }
                         }*/
 
-
-                        for(int i = 0; i < 4; i++) {
-                            shape_detected = shapeDetection();
-                            wait(2);
-                            if (get_com_x() > X_CENTER) {
-                                deltaX = get_com_x()-X_CENTER;
-                                setServoPulse2(0,Arm_Table[TOOL_1].base_rotate += (deltaX/60.0) );
-                                Arm_Table[PU_TOOL_1].base_rotate = Arm_Table[TOOL_1].base_rotate;
-                            }
-                            if (get_com_x() < X_CENTER) {
-                                deltaX = get_com_x()-X_CENTER;
-                                setServoPulse2(0,Arm_Table[TOOL_1].base_rotate += (deltaX/60.0) );
-                                Arm_Table[PU_TOOL_1].base_rotate = Arm_Table[TOOL_1].base_rotate;
-                            }
-                        }
-
-
-                        /*
-                                                while( shape_alignX_done == 0) {
-                                                    wait(1);
-                                                    shape_detected = shapeDetection();
-
-                                                    pc.printf("X - Adjust to center tool\n\r");
-                                                    if(get_com_x() > 100) {
-                                                        setServoPulse(0,Arm_Table[TOOL_1].base_rotate+=1);
-
-                                                    } else if(get_com_x() < 60) {
-                                                        setServoPulse(0,Arm_Table[TOOL_1].base_rotate-=1);
-
-                                                    } else {
-                                                        shape_alignX_done = 1;
-                                                    }
-                                                }*/
-
-                        if (shape_detected == tool_needed) {
+                        if (Xadjust(TOOL_1) == tool_needed) {
                             state = AQUIRE_TOOL1;
                             break;
                         } else {
@@ -550,13 +489,10 @@
                     servoPosition(PU_TOOL_1);
                     setServoPulse(4, 175);
                     wait(5);
-                    setServoPulse(0, Arm_Table[PU_TOOL_1].base_rotate );
+                    setServoPulse(0, Arm_Table[PU_TOOL_1].base_rotate - 1);
                     wait(1);
                     setServoPulse(1, Arm_Table[PU_TOOL_1].base_arm - 5);
                     wait(1);
-                    setServoPulse(5, 105);
-                    wait(.5);
-                    setServoPulse(5, 00);
                     setServoPulse(1, Arm_Table[PU_TOOL_1].base_arm - 6);
                     wait(.5);
                     setServoPulse(5, 105);
@@ -584,15 +520,12 @@
                     servoPosition(PU_TOOL_2);
                     setServoPulse(4, 175);
                     wait(5);
-                    setServoPulse(0, Arm_Table[PU_TOOL_2].base_rotate );
+                    setServoPulse(0, Arm_Table[PU_TOOL_2].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 - 2);
                     wait(1);
-                    setServoPulse(5, 105);
-                    wait(2);
-                    setServoPulse(5, 00);
                     setServoPulse(1, Arm_Table[PU_TOOL_2].base_arm - 6);
                     wait(2);
                     setServoPulse(5, 105);
@@ -644,18 +577,7 @@
                         }
 
                     */
-                    for(int i = 0; i < 6; i++) {
-                        shape_detected = shapeDetection();
-                        wait(2);
-                        if (get_com_x() > X_CENTER) {
-                            deltaX = get_com_x()-X_CENTER;
-                            setServoPulse2(0,Arm_Table[PU_TOOL_3].base_rotate += (deltaX/60.0) );
-                        }
-                        if (get_com_x() < X_CENTER) {
-                            deltaX = get_com_x()-X_CENTER;
-                            setServoPulse2(0,Arm_Table[PU_TOOL_3].base_rotate += (deltaX/60.0) );
-                        }
-                    }
+                    Xadjust(TOOL_3);
 
                     setServoPulse(4, 175);
                     wait(5);
@@ -665,9 +587,6 @@
                     wait(1);
                     setServoPulse(3, Arm_Table[PU_TOOL_2].claw_arm - 3);
                     wait(1);
-                    setServoPulse(5, 105);
-                    wait(.5);
-                    setServoPulse(5, 00);
                     setServoPulse(1, Arm_Table[PU_TOOL_3].base_arm - 6);
                     wait(.5);
                     setServoPulse(5, 105);
@@ -824,7 +743,7 @@
     pulse = MIN_SERVO_PULSE + (( angle * (MAX_SERVO_PULSE -  MIN_SERVO_PULSE)) / SERVO_MAX_ANGLE);
     float pulselength = 20000;   // 20,000 us per second
     int i = currentPosition[n];
-    pc.printf("ServoNumber: %d  Begining Pulse: %d\n\r",n,currentPosition[n]);
+    //pc.printf("ServoNumber: %d  Begining Pulse: %d\n\r",n,currentPosition[n]);
     int pulse2;
     if(currentPosition[n] < pulse) {
         for(i; i < pulse; i++) {
@@ -840,7 +759,7 @@
         }
     }
     currentPosition[n] = i;
-    pc.printf("END: pulse: %d,  angle: %d\n\r", i, angle);
+    //pc.printf("END: pulse: %d,  angle: %d\n\r", i, angle);
 }
 
 void initServoDriver(void)
@@ -896,6 +815,12 @@
 }
 
 
+
+
+
+
+
+
 int fire_checker(int rig)
 {
     switch (rig) {
@@ -936,11 +861,16 @@
 
 void errFunction(void)
 {
-    //Nothing
+    pc.printf("\n\nERROR: %d", motors.getError() );
+
 }
+
+
+
+
 void wall_follow(int side, int direction, int section)
 {
-    float location, set=6;
+    float location, set=4;
     int dir=1;
 
     pid1.reset();
@@ -967,7 +897,7 @@
             rangeFinderRight.startMeas();
             wait_ms(20);
             rangeFinderRight.getMeas(range);
-            pc.printf("%d\r\n",range);
+            //pc.printf("%d\r\n",range);
         }
 
         if(range > 15) {
@@ -1014,8 +944,8 @@
 
 void wall_follow2(int side, int direction, int section, float location, int rig)
 {
-    int dir=1, limit=86, lowlim=5;
-    float set=6, loc=0, Rigloc=0;
+    int dir=1, limit=80, lowlim=4;
+    float set=9, loc=0, Rigloc=0;
     bool SeeWaveGap = false;
 
     if(rig == 1) Rigloc= 16;
@@ -1027,28 +957,31 @@
     if(direction == BACKWARD) {
         dir=-1;
         limit = 100;
-    }
+    } else if(direction == FORWARD) lowlim=-20;
     if(section == TOOLS) {
-        set= 6;
+        set= 9;
         limit = 86;
-    }
-    if(section == RETURN) lowlim=15;
+    } else if(section == RIGS) set = 9;
+    else if(section == RETURN) lowlim=4;
+    else if(section == MID2) limit =85;
+
+    if(location <4) limit=80;
 
     leftEncoder.reset();
     rightEncoder.reset();
 
-    //pc.printf("before %f\r\n", location);
+    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);
+    //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);
+        //pc.printf("loc %f \r\n", loc);
 
         pid1.setInputLimits(0.0, set);
-        pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED);
+        pid1.setOutputLimits( -MAX_SPEED1, MAX_SPEED1);
         pid1.setSetPoint(set);
 
         if(side) {
@@ -1066,7 +999,7 @@
             wait_ms(20);
             rangeFinderLeft.getMeas(range2);
 
-            if(range2< 20) {
+            if(range2< 15) {
                 if( abs(dir*loc + location - Rigloc) < 10) {
                     //STOP
                     motors.stopBothMotors(127);
@@ -1078,18 +1011,19 @@
 
         //pc.printf("wall follow 2 range %f\r\n",range);
         //pc.printf("loc+location = %f\r\n", loc+location);
-        if(range > 20 ) {
+        if(range > 15 ) {
             if(section == RIGS || section == RETURN) {
                 motors.setMotor0Speed(dir*0.25*127); //right
                 motors.setMotor1Speed(dir*0.25*127); //left
             } else {
                 if(!SeeWaveGap) {
+                    wait_ms(40);
                     SeeWaveGap=true;
                 } else {
                     //STOP
                     motors.stopBothMotors(127);
 
-                    pc.printf("wavegap\r\n");
+                    //pc.printf("wavegap\r\n");
                     // AT WAVE OPENING!!!!
                     break;
                 }
@@ -1098,7 +1032,7 @@
             SeeWaveGap = false;
             pid1.setProcessValue(range);
             pid_return = pid1.compute();
-            //pc.printf("Range: %f\n      PID:   %f\r\n", range, pid_return);
+            pc.printf("Range: %f\n      PID:   %f\r\n", range, pid_return);
 
             if(pid_return > 0) {
                 if(side) {
@@ -1128,15 +1062,12 @@
 }
 
 
-
-
-
 void alignWithWall(int section)
 {
     float usValue = 0;
 
     if(section == TOOLS) {
-        pc.printf("tools section align\r\n");
+        //pc.printf("tools section align\r\n");
         // turn at an angle
         leftEncoder.reset();
         rightEncoder.reset();
@@ -1160,11 +1091,28 @@
         motors.setMotor1Speed(-MAX_SPEED); //left
         while(rightEncoder.getPulses() < 20 || abs(leftEncoder.getPulses()) < 20);
 
-        motors.stopBothMotors(0);
+        motors.stopBothMotors(127);
+        wait_ms(300);
+        return;
+        /*
+               rangeFinderRight.startMeas();
+               wait_ms(20);
+               rangeFinderRight.getMeas(range);
 
+               if(range>15){
+                   // turning left
+                   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);
+                   return;
+               }
+        */
         // turning left
-        motors.setMotor0Speed(0.9*MAX_SPEED); //right
-        motors.setMotor1Speed(-0.9*MAX_SPEED); //left
+        //motors.setMotor0Speed(0.9*MAX_SPEED); //right
+        //motors.setMotor1Speed(-0.9*MAX_SPEED); //left
 
     } else if(section == RIGS) {
         // check distance to wall
@@ -1172,7 +1120,7 @@
         wait_ms(20);
         rangeFinderRight.getMeas(range);
 
-        if(range < 3 || range > 20) return;
+        if(range < 3) return;
 
         // turn at an angle
         leftEncoder.reset();
@@ -1206,34 +1154,61 @@
                 motors.setMotor0Speed(-0.9*MAX_SPEED); //right
                 motors.setMotor1Speed(0.9*MAX_SPEED); //left
         */
-    } else {// MID
-        pc.printf("in mid section align\r\n");
+    } else if(section == MID2) {
+        leftEncoder.reset();
+        rightEncoder.reset();
+        motors.setMotor0Speed(-1.2*MAX_SPEED); //right
+        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();
+        motors.setMotor0Speed(0.4*127); //right
+        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
         rightTurn();
         // 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)
-            rangeFinderRight.startMeas();
-            wait_ms(20);
-            rangeFinderRight.getMeas(range);
-        } else {
-            rangeFinderLeft.startMeas();
-            wait_ms(20);
-            rangeFinderLeft.getMeas(range);
+    /*    while(1) {
+            if(section == 10) { // CURENTLY NOT USED (WAS 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;
+            }
         }
-        pc.printf("Range %f \t OldValue %f\n\r",range, usValue);
-        if(range > usValue && usValue != 0 && range < 25) {
-            break;
-        } else {
-            usValue = range;
-        }
-    }
-    motors.stopBothMotors(0);
+        motors.stopBothMotors(0);*/
 }
 
 void rightTurn(void)
@@ -1243,7 +1218,7 @@
     rightEncoder.reset();
     motors.setMotor0Speed(-0.5*127);//right
     motors.setMotor1Speed(0.5*127);//left
-    while(abs(leftEncoder.getPulses())<1050 || abs(rightEncoder.getPulses())<1050);
+    while(abs(leftEncoder.getPulses())<850 || abs(rightEncoder.getPulses())<850);
     motors.stopBothMotors(127);
 }
 
@@ -1254,7 +1229,7 @@
     rightEncoder.reset();
     motors.setMotor0Speed(0.5*127);// right
     motors.setMotor1Speed(-0.5*127);// left
-    while(abs(leftEncoder.getPulses())<1100 || rightEncoder.getPulses()<1100);
+    while(abs(leftEncoder.getPulses())<1075 || rightEncoder.getPulses()<1075);
     motors.stopBothMotors(127);
 }
 
@@ -1265,7 +1240,7 @@
     rightEncoder.reset();
     motors.setMotor0Speed(0.5*127);// right
     motors.setMotor1Speed(-0.5*127);// left
-    while(abs(leftEncoder.getPulses())<90 || rightEncoder.getPulses()<90);
+    while(abs(leftEncoder.getPulses())<70 || rightEncoder.getPulses()<70);
     motors.stopBothMotors(127);
 }
 
@@ -1276,7 +1251,7 @@
     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())<200 || abs(rightEncoder.getPulses())<200);
     motors.stopBothMotors(127);
 }
 
@@ -1329,7 +1304,7 @@
     while(abs(leftEncoder.getPulses()) < 50 || abs(rightEncoder.getPulses()) < 50);
     motors.stopBothMotors(127);
 
-    pc.printf("slight backwards\r\n");
+    //pc.printf("slight backwards\r\n");
     wait_ms(200);
 
     // Over bump
@@ -1337,16 +1312,11 @@
     rightEncoder.reset();
     motors.setMotor0Speed(0.3*127); //right
     motors.setMotor1Speed(0.3*127); //left
-    while((abs(leftEncoder.getPulses()) < 800 || abs(rightEncoder.getPulses()) < 800) /*&& preLeft!=0*/ && IR.getIRDistance() >15 ) {
-        /*
-        preLeft=leftEncoder.getPulses();
-        preRight=rightEncoder.getPulses();
-        wait_ms(200);
-        if(leftEncoder.getPulses() == preLeft || rightEncoder.getPulses()== preRight) preLeft=preRight=0;
-        */
-    }
+    while((abs(leftEncoder.getPulses()) < 800 || abs(rightEncoder.getPulses()) < 800) /*&& preLeft!=0*/ && IR.getIRDistance() >15 );
+
 
-    pc.printf("forward \r\n");
+
+    //pc.printf("forward \r\n");
     wait_ms(200);
 
     motors.stopBothMotors(0);
@@ -1368,8 +1338,9 @@
             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()) < 300 || abs(rightEncoder.getPulses()) < 300) && (leftEncoder.getPulses() != preLeft || rightEncoder.getPulses() != preRight)) {
+        if(section == MID2) while(IR.getIRDistance() > 15 && (abs(leftEncoder.getPulses()) < 100 || abs(rightEncoder.getPulses()) < 100));
+
+        while(IR.getIRDistance() > 15 && (abs(leftEncoder.getPulses()) < 100 || abs(rightEncoder.getPulses()) < 100) && (leftEncoder.getPulses() != preLeft || rightEncoder.getPulses() != preRight)) {
 
             if(IR.getIRDistance() > 38) break;
 
@@ -1394,6 +1365,7 @@
             if(leftEncoder.getPulses() == preLeft || rightEncoder.getPulses()== preRight) preLeft=preRight=0;
         }
         motors.stopBothMotors(127);
+        motors.begin();
 
         return;
     }
@@ -1403,16 +1375,18 @@
     motors.begin();
 
 }
+
+
 void to_tools_section1(float* location, float &current)
 {
-    slightMove(FORWARD,6600);
+    slightMove(FORWARD,6800);
     current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
 
 }
 
 void to_tools_section2(float* location, float &current)
 {
-    slightMove(FORWARD,3200);
+    slightMove(FORWARD,3250);
     current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
 
 }
@@ -1420,43 +1394,44 @@
 void from_tools_section(float* location, float &current)
 {
 
-    alignWithWall(TOOLS);
-    pc.printf("align\r\n");
-    wait_ms(100);
+    //alignWithWall(TOOLS);
+    //current-=4;
+
+    //slightMove(FORWARD,150);
+    //current+=1;
+    //pc.printf("align\r\n");
+    //wait_ms(200);
 
     //wall_follow2(LEFT,FORWARD,MID, current);
-    //current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
 
+    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 < 20) {
-        wall_follow2(LEFT,BACKWARD,TOOLS, current,0);
-        pc.printf("wall follow\r\n");
+    if(range < 15) {
+        wall_follow2(LEFT,BACKWARD,MID, 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);
+        //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
-        motors.stopBothMotors(127);
+        //slightMove(BACKWARD,200);
 
         wait_ms(100);
         leftTurn();
+        slightleft();
         overBump(TOOLS);
     } else {
-        pc.printf("else greater than 20\r\n");
+        //pc.printf("else greater than 20\r\n");
         location[0]= current;
         leftTurn();
         overBump(TOOLS);
     }
 
-    pc.printf("First Wavegap = %f\r\n",location[0]);
+    //pc.printf("First Wavegap = %f\r\n",location[0]);
 
 }
 void tools_section(float* location, float &current)
@@ -1470,14 +1445,15 @@
     ///////////////////////////////////////////////////////////////////////////////////////
     // 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);
+    //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);
@@ -1488,10 +1464,10 @@
 
     if(range < 20) {
         wall_follow2(LEFT,BACKWARD,TOOLS, current,0);
-        pc.printf("wall follow\r\n");
+        //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);
+        //pc.printf("current %f \r\n",current);
         // go backwards
         leftEncoder.reset();
         rightEncoder.reset();
@@ -1506,32 +1482,36 @@
         leftTurn();
         overBump(TOOLS);
     } else {
-        pc.printf("else greater than 20\r\n");
+        //pc.printf("else greater than 20\r\n");
         location[0]= current;
         leftTurn();
         overBump(TOOLS);
     }
 
-    pc.printf("First Wavegap = %f\r\n",location[0]);
+    //pc.printf("First Wavegap = %f\r\n",location[0]);
 }
 
 void mid_section(float* location, float &current, int* direction)
 {
-    motors.begin();
-
     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("before align with wall \r\n");
+    //alignWithWall(MID);
+    //current-=4;
+    //wait_ms(200);
 
-    pc.printf("mid section current = %f\r\n",current);
+    //if(current > 20){
+    //alignWithWall(MID2);
+    //current-=4;
+    //}
+    rightTurn();
+    //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);
+    //pc.printf("after wf2 current = %f\r\n",current);
 
     wait_ms(500);
     rangeFinderLeft.startMeas();
@@ -1541,8 +1521,9 @@
     if(range > 20 ) {
         direction[0]= RIGHT;
         location[1]= current;
-        slightMove(FORWARD,75);
-        //current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
+        wait_ms(300);
+        slightMove(FORWARD,100);
+        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);
@@ -1550,14 +1531,23 @@
         current= location[1];
 
         if(location[1] < 18) {
-            slightMove(FORWARD, 50);
+            slightMove(FORWARD, 75);
             current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
         }
+        //slightMove(BACKWARD,100);
 
     }
 
-    pc.printf("wavegap2 = %f\r\n",location[1]);
-    leftTurn();
+    wait_ms(200);
+    //pc.printf("wavegap2 = %f\r\n",location[1]);
+    //left turn
+    motors.begin();
+    leftEncoder.reset();
+    rightEncoder.reset();
+    motors.setMotor0Speed(0.5*127);// right
+    motors.setMotor1Speed(-0.5*127);// left
+    while(abs(leftEncoder.getPulses())<1045 || rightEncoder.getPulses()<1045);
+    motors.stopBothMotors(127);
 
     wait_ms(100);
 
@@ -1567,9 +1557,7 @@
 
 void mid_section2(float* location, float &current, int* direction)
 {
-    motors.begin();
-
-    pc.printf("mid section 2\r\n");
+    //pc.printf("mid section 2\r\n");
 
     if(IR.getIRDistance() > 38) {
         direction[1]= STRAIGHT;
@@ -1577,15 +1565,20 @@
         return;
     }
 
-    alignWithWall(MID);
-    pc.printf("midsection 2 alignt with wall mid \r\n");
+    //alignWithWall(MID);
+    wait_ms(100);
+
+    rightTurn();
+    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);
+    //pc.printf("midseection 2 after wf2 %f",current);
     rangeFinderLeft.startMeas();
     wait_ms(20);
     rangeFinderLeft.getMeas(range);
@@ -1593,19 +1586,27 @@
     if(range > 20 ) {
         direction[1]= RIGHT;
         location[2]= current;
-        slightMove(FORWARD,100);
+        slightMove(FORWARD,50);
         current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
     } else {
         direction[1]= LEFT;
         wall_follow2(LEFT,BACKWARD,MID,current,0);
         location[2]= current- ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
         current=location[2];
-        //slightMove(FORWARD,100);
+        //slightMove(BACKWARD,100);
     }
 
-    leftTurn();
+    //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);
+    motors.stopBothMotors(127);
+
     overBump(RIGS);
-    pc.printf("overbump rigs\r\n");
+    //pc.printf("overbump rigs\r\n");
 }
 
 void rig_section(float* location, float &current, int* direction, int rig)
@@ -1617,28 +1618,34 @@
     else loc = 75;
 
     // Slight forward for turn
-    slightMove(FORWARD,100);
+    slightMove(FORWARD,150);
+    current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
     wait_ms(100);
     rightTurn();
-    slightright();
-    wait(5);
+    //slightright();
+
 
     if(current > loc) {
-        pc.printf("RIG section %f\r\n",current);
+        //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);
+        //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);
     }
 
-    alignWithWall(RIGS);
-    // Go forward until Rig
+    alignWithWall(MID2);
+    current-=4;
     wall_follow2(RIGHT, FORWARD, RIGS, current, rig);
-    // line back wheel up with side of rig
-    slightMove(FORWARD,300);
+    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)
@@ -1697,4 +1704,83 @@
     }
     rightTurn();
     overBump(MID2);
-}
\ No newline at end of file
+}
+
+
+
+
+int Xadjust(int tool)
+{
+    int areaArray[10];
+    float C, T, S;
+    for(int i = 0; i < 10; i++) {
+        areaArray[i] = shapeDetection();
+        wait(2);
+        if(get_com_x() > X_CENTER ) {
+            deltaX = get_com_x()-X_CENTER;
+            setServoPulse2(0,Arm_Table[tool].base_rotate += (deltaX/60.0) );
+            Arm_Table[PU_TOOL_2].base_rotate = Arm_Table[TOOL_2].base_rotate;
+        }
+        if(get_com_x() < X_CENTER) {
+            deltaX = get_com_x()-X_CENTER;
+            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 {
+        pc.printf("\n\nTRIANGLE DETECTED\n\r");
+        return TRIANGLE;
+    
+    
+    
+/*
+    if((C < S) && (C < T)) {
+        pc.printf("\n\nCIRCLE DETECTED\n\r");
+        return CIRCLE;    
+    } else if( ( S<C ) && ( S<T ) ) {
+        pc.printf("\n\nSQUARE DETECTED\n\r");
+        return SQUARE;
+    } else {
+        pc.printf("\n\nTRIANGLE DETECTED\n\r");
+        return TRIANGLE;
+    }*/
+}
+
+float normd(int* pop, int count, int threshold)
+{
+    int i = 0;
+    float mean=0, std=0;
+    for(i=0; i<count; i++) {
+        mean += pop[i];
+    }
+    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)));
+
+}