Justin Carr
nav fixed
Dependencies: Adafruit-16-Ch-PWM-Servo-Driver HCSR04 PID PololuQik2 QEI Sharp mbed-rtos
Fork of
theRobot
by 
Thomas Ashworth
Revision 22:072dabdf905a, committed 2014-04-11
- 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 |
--- 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 ¤t, 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 ¤t, 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 ¤t)