Diff: main.cpp

Revision:

32:4a42f61f64a6

Parent:

31:99cf9c25b0f2

Child:

33:a41981e18a7d

diff -r 99cf9c25b0f2 -r 4a42f61f64a6 main.cpp
--- a/main.cpp	Mon Apr 21 18:27:12 2014 +0000
+++ b/main.cpp	Mon Apr 21 19:23:26 2014 +0000
@@ -213,8 +213,8 @@
 //increase in number 5 rotates gripper
 
     {STORE_POSITION, 85, 8, 0, 170, 60, 0},              // storing position
-    {OIL_RIG1, 164, 20, 60, 100, 175, 0},                 // point laser at oilrig1
-    {OIL_RIG2, 164, 20, 60, 100, 175, 0},                // point laser at oilrig2
+    {OIL_RIG1, 158, 20, 61, 100, 175, 0},                 // point laser at oilrig1
+    {OIL_RIG2, 158, 20, 61, 100, 175, 0},                // point laser at oilrig2
     {OIL_RIG3, 130, 90, 90, 100, 175, 0},                // NOT USED!!!!!    point laser at oilrig2
     {DRIVE_POSITION_NOTOOL, 85, 10, 0, 165, 60, 0},    // Drive through course
     {TOOL_1, 101, 50, 80, 133, 60, 0},                  // Look over first tool
@@ -222,9 +222,9 @@
     {TOOL_3, 62, 50, 80, 132, 60, 0},                  // Look over third tool
     {DRIVE_POSITION_TOOL, 85, 10, 0, 165, 140, 120},     // Drive with tool loaded
     {ORIENT_TOOL, 135, 60, 75, 60, 90, 90},             // position tool to be inserted
-    {PU_TOOL_1, 99, 46, 78, 110, 170, 0},               // STATIC Pickup tool POSITION
-    {PU_TOOL_2, 76, 47, 80, 112, 170, 0},                // STATIC Pickup tool POSITION
-    {PU_TOOL_3, 59, 44, 76, 110, 170, 0},                // STATIC Pickup tool POSITION
+    {PU_TOOL_1, 99, 46, 78, 110, 138, 0},               // STATIC Pickup tool POSITION
+    {PU_TOOL_2, 76, 47, 80, 112, 138, 0},                // STATIC Pickup tool POSITION
+    {PU_TOOL_3, 59, 44, 76, 110, 138, 0},                // STATIC Pickup tool POSITION
     {INSERT_TOOL_1, 170, 50, 127, 52, 140, 120},           // InsertToolIntoRig
 
 };
@@ -618,9 +618,9 @@
                     wait(5);
                     setServoPulse(0, Arm_Table[PU_TOOL_3].base_rotate - 1);
                     wait(1);
-                    setServoPulse(1, Arm_Table[PU_TOOL_2].base_arm - 2);
+                    setServoPulse(1, Arm_Table[PU_TOOL_3].base_arm - 2);
                     wait(1);
-                    setServoPulse(3, Arm_Table[PU_TOOL_2].claw_arm - 3);
+                    setServoPulse(3, Arm_Table[PU_TOOL_3].claw_arm - 2);
                     wait(1);
                     setServoPulse(4, 175);
                     wait(2);
@@ -1287,7 +1287,7 @@
     leftEncoder.reset();
     rightEncoder.reset();
     motors.setMotor0Speed(dir*0.25*127); //right
-    motors.setMotor1Speed(dir*0.25*127); //left
+    motors.setMotor1Speed(dir*0.23*127); //left
     while(abs(leftEncoder.getPulses()) < pulses || abs(rightEncoder.getPulses()) < pulses);
 
     motors.stopBothMotors(127);
@@ -1428,7 +1428,7 @@
 
     pc.printf("IR front %f \t back %f \r\n", IRLeftFront.getIRDistance(), IRLeftBack.getIRDistance());
 
-    if(IRLeftFront.getIRDistance() < 40 || IRLeftBack.getIRDistance() < 40) {
+    if(IRLeftFront.getIRDistance() < 40 && IRLeftBack.getIRDistance() < 40) {
         wall_follow2(LEFT,BACKWARD,TOOLS, current,0);
         
         pc.printf("IR front %f \t back %f \r\n", IRLeftFront.getIRDistance(), IRLeftBack.getIRDistance());