nav fixed
Dependencies: Adafruit-16-Ch-PWM-Servo-Driver HCSR04 PID PololuQik2 QEI Sharp mbed-rtos
Fork of theRobot by
Diff: main.cpp
- Revision:
- 9:1b29cd9ed1ba
- Parent:
- 8:77a57909aa15
- Child:
- 10:1a1d52207f59
diff -r 77a57909aa15 -r 1b29cd9ed1ba main.cpp --- a/main.cpp Thu Mar 27 23:16:02 2014 +0000 +++ b/main.cpp Fri Mar 28 15:32:16 2014 +0000 @@ -75,9 +75,9 @@ //Oil Rig distance thresholds #define OILRIG1_MAX 3000 #define OILRIG1_MIN 1000 -#define OILRIG2_MAX 3000 +#define OILRIG2_MAX 5000 #define OILRIG2_MIN 1000 -#define OILRIG3_MAX 3000 +#define OILRIG3_MAX 5000 #define OILRIG3_MIN 1000 //for servo normalization @@ -179,11 +179,11 @@ //increase in number 5 rotates gripper - {STORE_POSITION, 85, 5, 5, 175, 100, 0}, // storing position - {OIL_RIG1, 163, 90, 90, 48, 100, 0}, // point laser at oilrig1 - {OIL_RIG2, 144, 90, 90, 47, 100, 0}, // point laser at oilrig2 + {STORE_POSITION, 85, 10, 0, 175, 100, 0}, // storing position + {OIL_RIG1, 164, 90, 90, 52, 100, 0}, // point laser at oilrig1 + {OIL_RIG2, 145, 90, 90, 51, 100, 0}, // point laser at oilrig2 {OIL_RIG3, 130, 90, 90, 50, 100, 0}, // point laser at oilrig2 - {DRIVE_POSITION_NOTOOL, 55, 70, 102, 74, 0, 0}, // Drive through course + {DRIVE_POSITION_NOTOOL, 85, 10, 0, 175, 100, 0}, // Drive through course {TOOL_1, 95, 64, 97, 79, 0, 0}, // Look over first tool {TOOL_2, 75, 70, 102, 74, 0, 0}, // Look over second tool {TOOL_3, 55, 70, 102, 74, 0, 0}, // Look over third tool @@ -214,6 +214,7 @@ int direction[3]; double distance; + pc.baud(115200); //Laser Range Finder Initialization lrf_baudCalibration(); @@ -227,7 +228,7 @@ /******************* WHILE LOOP FOR TESTING ********************/ - while(1) { + /*while(1) { pc.scanf("%d %d", &servoNum, &servoAngle); if(servoAngle > 175) { servoAngle = 175; @@ -237,22 +238,26 @@ servoAngle = 90; } setServoPulse(servoNum, servoAngle); - - //ledtoggle(); - //pc.scanf("%d", &posNum); - //servoPosition(posNum); - //wait(3); //shape_detected = shapeDetection(); //distLaser = getDistance(); //pc.printf("Distance %d", distLaser); + //ledtoggle(); - } + pc.scanf("%d", &posNum); + servoPosition(posNum); + wait(5); + //shape_detected = shapeDetection(); + distLaser = getDistance(); + pc.printf("Distance %d", distLaser); + + }*/ /******************************** MAIN WHILE LOOP FOR COMPETITION *********************************/ while(1) { + switch (state) { /************************************************** @@ -277,7 +282,7 @@ **************************************************/ case OILRIG1_POS: //aims arm at square oil rig servoPosition(OIL_RIG1); //position arm to point at first oilrig - wait(2); //wait for servos to settle + wait(5); //wait for servos to settle fire = fire_checker(OIL_RIG1); //determines if oil rig is on fire //determines what tool is needed @@ -287,6 +292,7 @@ } else { state = OILRIG2_POS; } + break; case OILRIG2_POS: @@ -300,6 +306,7 @@ tool_needed = CIRCLE; state = GOTO_TOOLS; } + pc.printf("tool needed: %d", tool_needed); break; /************************************************** @@ -310,15 +317,14 @@ **************************************************/ case GOTO_TOOLS: servoPosition(DRIVE_POSITION_NOTOOL); //drive position without a tool - wait(1); //wait for servos to settle - -//****************************************************// + wait(2); //wait for servos to settle to_tools_section(location, current); state = IDENTIFY_TOOLS; + pc.printf("YES!!!!!"); + while(1); break; - while(1) {} /************************************************** * STAGE 3 * @@ -594,33 +600,39 @@ case 1: for (int i = 0; i<5; i++) { distLaser = getDistance(); + pc.printf("L DISTANCE: %d", distLaser); if ((distLaser < OILRIG1_MAX) - || (distLaser > OILRIG1_MIN)) { + && (distLaser > OILRIG1_MIN)) { fire_detected++; } else { fire_not_detected++; } } + break; case 2: for (int i = 0; i<5; i++) { distLaser = getDistance(); + pc.printf("L DISTANCE: %d", distLaser); if ((distLaser < OILRIG2_MAX) - || (distLaser > OILRIG2_MIN)) { + && (distLaser > OILRIG2_MIN)) { fire_detected++; } else { fire_not_detected++; } } + break; case 3: for (int i = 0; i<5; i++) { distLaser = getDistance(); if ((distLaser < OILRIG3_MAX) - || (distLaser > OILRIG3_MIN)) { + && (distLaser > OILRIG3_MIN)) { fire_detected++; } else { fire_not_detected++; } } + break; + default: for (int i = 0; i<5; i++) { distLaser = getDistance(); @@ -631,6 +643,7 @@ fire_not_detected++; } } + break; } if (fire_detected > fire_not_detected) { @@ -670,7 +683,7 @@ location=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; - while(location< 73) { + while(location< 68) { location=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; pid1.setInputLimits(0, set); @@ -910,7 +923,7 @@ rightEncoder.reset(); motors.setMotor0Speed(0.2*127); //right motors.setMotor1Speed(0.2*127); //left - while(/*(abs(leftEncoder.getPulses()) < 1000 || abs(rightEncoder.getPulses())< 1000)*/ IR.getDistance() >20 && preLeft!=0) { + while(/*(abs(leftEncoder.getPulses()) < 1000 || abs(rightEncoder.getPulses())< 1000)*/ IR.getIRDistance() >20 && preLeft!=0) { preLeft=leftEncoder.getPulses(); preRight=rightEncoder.getPulses(); wait_ms(100); @@ -929,7 +942,7 @@ leftEncoder.reset(); rightEncoder.reset(); */ -// while(/*(abs(leftEncoder.getPulses()) < 1000 || abs(rightEncoder.getPulses())< 1000)*/ IR.getDistance() >20 && preLeft!=0){ +// while(/*(abs(leftEncoder.getPulses()) < 1000 || abs(rightEncoder.getPulses())< 1000)*/ IR.getIRDistance() >20 && preLeft!=0){ /* preLeft=leftEncoder.getPulses(); preRight=rightEncoder.getPulses(); pc.printf("second while left %d right %d \r\n", preLeft, preRight); @@ -971,8 +984,8 @@ motors.setMotor1Speed(.25*127); //left if(section == TOOLS || section == MID) { - while(IR.getDistance() > 20 ) { - //pc.printf("IR %f\r\n", IR.getDistance()); + while(IR.getIRDistance() > 20 ) { + //pc.printf("IR %f\r\n", IR.getIRDistance()); //pc.printf("third while left %d right %d \r\n", preLeft, preRight); } } else while((abs(leftEncoder.getPulses()) < 200 || abs(rightEncoder.getPulses())< 200)); @@ -992,12 +1005,19 @@ // current position in reference to the starting position current=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; pc.printf("current %f \r\n",current); - + motors.stopBothMotors(); - //Tool aquiring - //wait(2); - // After tool is aquired + wait(2); + + motors.setMotor0Speed(.2*127); //right + motors.setMotor1Speed(.2*127); //left + while(IR.getIRDistance()>16); + + motors.setMotor0Speed(-.2*127); //right + motors.setMotor1Speed(-.2*127); //left + wait_ms(5); + motors.stopBothMotors(); } @@ -1049,7 +1069,7 @@ motors.begin(); - if(IR.getDistance() > 20) return; + if(IR.getIRDistance() > 20) return; alignWithWall(MID); @@ -1087,7 +1107,7 @@ motors.begin(); - if(IR.getDistance() > 20) return; + if(IR.getIRDistance() > 20) return; alignWithWall(MID); wall_follow2(LEFT,FORWARD,MID, current); @@ -1114,11 +1134,12 @@ } -void ledtoggle(void){ +void ledtoggle(void) +{ pwm.setPWM(9, 0, 4094); wait(2); pwm.setPWM(9, 0, 0); - } +}