Diff: main.cpp

Revision:

21:0907e1f5e16c

Parent:

20:55dcff40c5d9

Child:

22:072dabdf905a

--- a/main.cpp	Sat Apr 05 07:26:15 2014 +0000
+++ b/main.cpp	Tue Apr 08 16:20:40 2014 +0000
@@ -87,11 +87,11 @@
 
 
 //Oil Rig distance thresholds
-#define OILRIG1_MAX     2200
+#define OILRIG1_MAX     1800
 #define OILRIG1_MIN     1000
-#define OILRIG2_MAX     2200
+#define OILRIG2_MAX     1800
 #define OILRIG2_MIN     1000
-#define OILRIG3_MAX     2200
+#define OILRIG3_MAX     1800
 #define OILRIG3_MIN     1000
 
 //for servo normalization
@@ -209,8 +209,8 @@
 //increase in number 5 rotates gripper
 
     {STORE_POSITION, 85, 10, 0, 165, 175, 0},              // storing position
-    {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_RIG1, 160, 20, 60, 47, 175, 0},                 // point laser at oilrig1
+    {OIL_RIG2, 164, 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
@@ -288,7 +288,6 @@
     *********************************/
 
 
-
     while(1) {
         if(button_start == 1) {
 
@@ -387,7 +386,7 @@
                     setServoPulse(5, Arm_Table[DRIVE_POSITION_NOTOOL].claw_open);
                     wait(3);                                //wait for servos to settle
 
-                    slightMove(FORWARD,3350);
+                    slightMove(FORWARD,3250);
                     current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
 
                     state = IDENTIFY_TOOLS;
@@ -444,7 +443,7 @@
                         break;
                     } else {
                         //printImageToFile(BINARY);
-                        slightMove(FORWARD,60);
+                        slightMove(FORWARD,70);
                         current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
                         servoPosition(TOOL_1);
                         wait(5);                        //wait for servos to settle
@@ -495,12 +494,12 @@
                     wait(1);
                     setServoPulse(1, Arm_Table[PU_TOOL_1].base_arm - 6);
                     wait(.5);
-                    setServoPulse(5, 105);
+                    setServoPulse(5, 140);
                     wait(.5);
                     setServoPulse(5, 00);
                     setServoPulse(1, Arm_Table[PU_TOOL_1].base_arm - 10);
                     wait(1);
-                    setServoPulse(5, 105);
+                    setServoPulse(5, 140);
                     wait(1);
                     setServoPulse(1, Arm_Table[PU_TOOL_1].base_arm);
                     wait(2);
@@ -528,12 +527,12 @@
                     wait(1);
                     setServoPulse(1, Arm_Table[PU_TOOL_2].base_arm - 6);
                     wait(2);
-                    setServoPulse(5, 105);
+                    setServoPulse(5, 140);
                     wait(2);
                     setServoPulse(5, 00);
                     setServoPulse(1, Arm_Table[PU_TOOL_2].base_arm - 10);
                     wait(2);
-                    setServoPulse(5, 105);
+                    setServoPulse(5, 140);
                     wait(2);
                     setServoPulse(1, Arm_Table[PU_TOOL_2].base_arm);
 
@@ -589,12 +588,12 @@
                     wait(1);
                     setServoPulse(1, Arm_Table[PU_TOOL_3].base_arm - 6);
                     wait(.5);
-                    setServoPulse(5, 105);
+                    setServoPulse(5, 140);
                     wait(.5);
                     setServoPulse(5, 00);
                     setServoPulse(1, Arm_Table[PU_TOOL_3].base_arm - 10);
                     wait(1);
-                    setServoPulse(5, 105);
+                    setServoPulse(5, 140);
                     wait(1);
                     setServoPulse(1, Arm_Table[PU_TOOL_3].base_arm);
 
@@ -1312,7 +1311,7 @@
     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 );
+    while((abs(leftEncoder.getPulses()) < 900 || abs(rightEncoder.getPulses()) < 900) /*&& preLeft!=0*/ && IR.getIRDistance() >15 );
 
 
 
@@ -1338,7 +1337,11 @@
             wait_ms(200);
         }
     } else if(section == MID || section == MID2) {
-        if(section == MID2) while(IR.getIRDistance() > 15 && (abs(leftEncoder.getPulses()) < 100 || abs(rightEncoder.getPulses()) < 100));
+        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)) {
 
@@ -1379,7 +1382,7 @@
 
 void to_tools_section1(float* location, float &current)
 {
-    slightMove(FORWARD,6800);
+    slightMove(FORWARD,6850);
     current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
 
 }
@@ -1406,7 +1409,7 @@
 
     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);
@@ -1508,6 +1511,12 @@
     //current-=4;
     //}
     rightTurn();
+    leftEncoder.reset();
+    rightEncoder.reset();
+    motors.setMotor0Speed(-0.4*127);// right
+    motors.setMotor1Speed(0.4*127);// left
+    while(abs(leftEncoder.getPulses())<50 || abs(rightEncoder.getPulses())<50);
+    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);
@@ -1586,7 +1595,7 @@
     if(range > 20 ) {
         direction[1]= RIGHT;
         location[2]= current;
-        slightMove(FORWARD,50);
+        slightMove(FORWARD,100);
         current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
     } else {
         direction[1]= LEFT;
@@ -1731,32 +1740,32 @@
     }
 
     C = normd(areaArray, 10, CIRCLE_AREA);
-   // S = normd(areaArray, 10, SQUARE_AREA);
-   // T = normd(areaArray, 10, TRIANGLE_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");
-        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;
-    }*/
+    }
+
+
+    /*
+        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)