Diff: main.cpp

Revision:

19:d4d967a885dc

Parent:

18:a0ea7ecaf4fe

Child:

20:55dcff40c5d9

--- a/main.cpp	Thu Apr 03 22:41:24 2014 +0000
+++ b/main.cpp	Fri Apr 04 01:09:49 2014 +0000
@@ -168,6 +168,7 @@
 int tool_needed = 0;
 int shape_detected = 0;
 float range, range2, pid_return;
+int num, input;
 
 
 /************
@@ -198,18 +199,18 @@
 //increase in number 5 rotates gripper
 
     {STORE_POSITION, 85, 10, 0, 165, 175, 0},              // storing position
-    {OIL_RIG1, 160, 20, 60, 45, 175, 0},                 // point laser at oilrig1
-    {OIL_RIG2, 163, 20, 60, 45, 175, 0},                // point laser at oilrig2
+    {OIL_RIG1, 162, 20, 60, 50, 175, 0},                 // point laser at oilrig1
+    {OIL_RIG2, 165, 20, 60, 50, 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, 96, 50, 80, 109, 90, 0},                  // Look over first tool
-    {TOOL_2, 79, 50, 80, 110, 90, 0},                  // Look over second tool
-    {TOOL_3, 59, 50, 80, 109, 90, 0},                  // Look over third tool
+    {TOOL_1, 101, 50, 80, 113, 90, 0},                  // Look over first tool
+    {TOOL_2, 82, 50, 80, 113, 90, 0},                  // Look over second tool
+    {TOOL_3, 62, 50, 80, 112, 90, 0},                  // Look over third tool
     {DRIVE_POSITION_TOOL, 85, 10, 0, 165, 90, 105},     // Drive with tool loaded
     {ORIENT_TOOL, 135, 60, 75, 60, 90, 90},             // position tool to be inserted
-    {PU_TOOL_1, 96, 46, 78, 102, 170, 0},               // STATIC Pickup tool POSITION
-    {PU_TOOL_2, 74, 47, 80, 104, 170, 0},                // STATIC Pickup tool POSITION
-    {PU_TOOL_3, 57, 44, 76, 102, 170, 0},                // STATIC Pickup tool POSITION
+    {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_STAB, 98, 50, 90, 118, 90, 0},           // STAB TOOL 1
     {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
@@ -244,9 +245,16 @@
 }
 
 
+
+
+
+
+
+
 int main()
 {
 
+
     /*****************
      INITIALIZATIONS
     *******************/
@@ -273,6 +281,7 @@
     /********************************
     MAIN WHILE LOOP FOR COMPETITION
     *********************************/
+
     while(1) {
         if(button_start == 1) {
 
@@ -1006,7 +1015,7 @@
 void wall_follow2(int side, int direction, int section, float location, int rig)
 {
     int dir=1, limit=86, lowlim=5;
-    float set=7, loc=0, Rigloc=0;
+    float set=6, loc=0, Rigloc=0;
     bool SeeWaveGap = false;
 
     if(rig == 1) Rigloc= 16;
@@ -1119,6 +1128,9 @@
 }
 
 
+
+
+
 void alignWithWall(int section)
 {
     float usValue = 0;
@@ -1180,19 +1192,20 @@
         motors.stopBothMotors(0);
         wait(2);
 
-        // turn left towards wall
+        // turn right towards wall
         leftEncoder.reset();
         rightEncoder.reset();
         motors.setMotor0Speed(-MAX_SPEED); //right
         motors.setMotor1Speed(MAX_SPEED); //left
-        while(abs(rightEncoder.getPulses()) < 20 || abs(leftEncoder.getPulses()) < 20);
+        while(abs(rightEncoder.getPulses()) < 50 || abs(leftEncoder.getPulses()) < 50);
+
+        motors.stopBothMotors(127);
+        /*        wait(2);
 
-        motors.stopBothMotors(0);
-        wait(2);
-
-        // turning left
-        motors.setMotor0Speed(-0.9*MAX_SPEED); //right
-        motors.setMotor1Speed(0.9*MAX_SPEED); //left
+                // turning left
+                motors.setMotor0Speed(-0.9*MAX_SPEED); //right
+                motors.setMotor1Speed(0.9*MAX_SPEED); //left
+        */
     } else {// MID
         pc.printf("in mid section align\r\n");
         // turn right towards wall
@@ -1204,7 +1217,7 @@
 
     usValue = 0;
     while(1) {
-        if(section == RIGS) {
+        if(section == 10) { // CURENTLY NOT USED (WAS RIGS)
             rangeFinderRight.startMeas();
             wait_ms(20);
             rangeFinderRight.getMeas(range);
@@ -1392,7 +1405,7 @@
 }
 void to_tools_section1(float* location, float &current)
 {
-    slightMove(FORWARD,6700);
+    slightMove(FORWARD,6600);
     current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
 
 }
@@ -1621,6 +1634,11 @@
     }
 
     alignWithWall(RIGS);
+    // Go forward until Rig
+    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);
 }
 
 void tools_section_return(float* location, float &current)