nav fixed

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

Fork of theRobot by Thomas Ashworth

Files at this revision

API Documentation at this revision

Comitter:
jjcarr2
Date:
Fri Apr 11 20:56:52 2014 +0000
Parent:
21:0907e1f5e16c
Commit message:
cleaned up navigation;

Changed in this revision

main.cpp Show annotated file Show diff for this revision Revisions of this file
diff -r 0907e1f5e16c -r 072dabdf905a main.cpp
--- a/main.cpp	Tue Apr 08 16:20:40 2014 +0000
+++ b/main.cpp	Fri Apr 11 20:56:52 2014 +0000
@@ -59,6 +59,7 @@
 #define INSERT_TOOL                 15
 #define END                         16
 #define GOTO_TOOLS2                 17
+#define RETURN_TO_START             18
 
 
 
@@ -259,7 +260,7 @@
 int main()
 {
 
-
+    
     /*****************
      INITIALIZATIONS
     *******************/
@@ -272,7 +273,7 @@
 
     pc.baud(115200);
     //Laser Range Finder Initialization
-    lrf_baudCalibration();
+    //lrf_baudCalibration();
 
     motors.begin();
 
@@ -302,7 +303,8 @@
                     **************************************************/
                 case START :
                     myled1 = 1;
-                    state = OILRIG1_POS;
+                    state = GOTO_TOOLS1;
+                    //state = OILRIG1_POS;
                     break;
 
 
@@ -365,15 +367,17 @@
                     *
                     **************************************************/
                 case GOTO_TOOLS1:
-                    setServoPulse(2, Arm_Table[DRIVE_POSITION_NOTOOL].big_arm);
+                    /*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);
                     setServoPulse(0, Arm_Table[DRIVE_POSITION_NOTOOL].base_rotate);
                     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 = IDENTIFY_TOOLS;
+                    state  = NAVIGATE_WAVES_ROW1;
+                    //state = IDENTIFY_TOOLS;
                     break;
 
                 case GOTO_TOOLS2:
@@ -631,19 +635,20 @@
                     break;
 
                 case NAVIGATE_WAVES_ROW3:
-
+                    shape_detected = 2;
                     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;
 
@@ -703,6 +708,25 @@
                     /**************************************************
                     *           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
                     *
                     **************************************************/
@@ -953,10 +977,6 @@
 
     pid1.reset();
 
-    if(direction == BACKWARD) {
-        dir=-1;
-        limit = 100;
-    } else if(direction == FORWARD) lowlim=-20;
     if(section == TOOLS) {
         set= 9;
         limit = 86;
@@ -964,6 +984,11 @@
     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();
@@ -1016,7 +1041,7 @@
                 motors.setMotor1Speed(dir*0.25*127); //left
             } else {
                 if(!SeeWaveGap) {
-                    wait_ms(40);
+                    wait_ms(75);
                     SeeWaveGap=true;
                 } else {
                     //STOP
@@ -1301,6 +1326,7 @@
     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");
@@ -1337,11 +1363,11 @@
             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)) {
 
@@ -1497,6 +1523,11 @@
 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;
@@ -1515,7 +1546,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())<100 || abs(rightEncoder.getPulses())<100);
     motors.stopBothMotors(127);
     //pc.printf("mid section current = %f\r\n",current);
     wall_follow2(LEFT,FORWARD,MID, current,0);
@@ -1528,10 +1559,11 @@
     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;
@@ -1561,7 +1593,6 @@
     wait_ms(100);
 
     overBump(MID);
-
 }
 
 void mid_section2(float* location, float &current, int* direction)
@@ -1569,6 +1600,11 @@
     //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;
@@ -1578,7 +1614,7 @@
     wait_ms(100);
 
     rightTurn();
-    slightright();
+    //slightright();
     wait_ms(100);
 
 
@@ -1611,7 +1647,7 @@
     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);
@@ -1643,18 +1679,13 @@
         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)