nav fixed
Dependencies: Adafruit-16-Ch-PWM-Servo-Driver HCSR04 PID PololuQik2 QEI Sharp mbed-rtos
Fork of theRobot by
Diff: main.cpp
- Revision:
- 10:1a1d52207f59
- Parent:
- 9:1b29cd9ed1ba
- Child:
- 11:8d2455e383ce
diff -r 1b29cd9ed1ba -r 1a1d52207f59 main.cpp --- a/main.cpp Fri Mar 28 15:32:16 2014 +0000 +++ b/main.cpp Sat Mar 29 21:22:14 2014 +0000 @@ -180,9 +180,9 @@ //increase in number 5 rotates gripper {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 + {OIL_RIG1, 161, 90, 90, 47, 100, 0}, // point laser at oilrig1 + {OIL_RIG2, 140, 90, 90, 44, 100, 0}, // point laser at oilrig2 + {OIL_RIG3, 130, 90, 90, 57, 100, 0}, // point laser at oilrig2 {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 @@ -210,7 +210,7 @@ /***************** INITIALIZATIONS *******************/ - float location[3], current=0; + float location[3], current=6; int direction[3]; double distance; @@ -223,12 +223,14 @@ //Servo initialization initServoDriver(); servoBegin(); //initiates servos to start position - //ServoOutputDisable = 0; + //servoPosition(0); + ServoOutputDisable = 0; /******************* WHILE LOOP FOR TESTING ********************/ - /*while(1) { + while(1) { + pc.scanf("%d %d", &servoNum, &servoAngle); if(servoAngle > 175) { servoAngle = 175; @@ -244,20 +246,20 @@ //pc.printf("Distance %d", distLaser); //ledtoggle(); - pc.scanf("%d", &posNum); - servoPosition(posNum); - wait(5); + //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) { /************************************************** @@ -268,12 +270,11 @@ **************************************************/ case START : myled1 = 1; - wait(5); + wait(1); myled1 = 0; state = OILRIG1_POS; break; - /************************************************** * STAGE 1 * @@ -281,10 +282,19 @@ * **************************************************/ case OILRIG1_POS: //aims arm at square oil rig - servoPosition(OIL_RIG1); //position arm to point at first oilrig - wait(5); //wait for servos to settle + + setServoPulse(3, Arm_Table[OIL_RIG1].claw_arm); + setServoPulse(2, 50); + setServoPulse(1, Arm_Table[OIL_RIG1].base_arm); + setServoPulse(2, Arm_Table[OIL_RIG1].big_arm); + setServoPulse(0, Arm_Table[OIL_RIG1].base_rotate); + setServoPulse(4, Arm_Table[OIL_RIG1].claw_rotate); + setServoPulse(5, Arm_Table[OIL_RIG1].claw_open); + + wait(3); //wait for servos to settle + fire = fire_checker(OIL_RIG1); //determines if oil rig is on fire - + pc.printf("FIRE: %d", fire); //determines what tool is needed if (fire == 1) { tool_needed = SQUARE; @@ -292,13 +302,14 @@ } else { state = OILRIG2_POS; } - + break; case OILRIG2_POS: servoPosition(OIL_RIG2); //position arm to point at first oilrig wait(1); //wait for servos to settle fire = fire_checker(OIL_RIG2); + pc.printf("FIRE: %d", fire); if (fire == 1) { tool_needed = TRIANGLE; state = GOTO_TOOLS; @@ -316,8 +327,15 @@ * **************************************************/ case GOTO_TOOLS: - servoPosition(DRIVE_POSITION_NOTOOL); //drive position without a tool - wait(2); //wait for servos to settle + + setServoPulse(3, Arm_Table[DRIVE_POSITION_NOTOOL].claw_arm); + setServoPulse(2, Arm_Table[DRIVE_POSITION_NOTOOL].big_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(5); //wait for servos to settle to_tools_section(location, current); @@ -546,15 +564,10 @@ { pc.printf("Setting Initial Position\n\r"); setServoPulseNo_delay(3, 175); - wait(2); setServoPulseNo_delay(2, 0); - wait(2); - setServoPulseNo_delay(1, 10); - wait(2); + setServoPulseNo_delay(1, 2); setServoPulseNo_delay(0, 85); - wait(1); setServoPulseNo_delay(4, 100); - wait(1); setServoPulseNo_delay(5, 0); setGripper(1); } @@ -600,7 +613,7 @@ case 1: for (int i = 0; i<5; i++) { distLaser = getDistance(); - pc.printf("L DISTANCE: %d", distLaser); + pc.printf("L DISTANCE: %d \n\r", distLaser); if ((distLaser < OILRIG1_MAX) && (distLaser > OILRIG1_MIN)) { fire_detected++; @@ -612,7 +625,7 @@ case 2: for (int i = 0; i<5; i++) { distLaser = getDistance(); - pc.printf("L DISTANCE: %d", distLaser); + pc.printf("L DISTANCE: %d \n\r", distLaser); if ((distLaser < OILRIG2_MAX) && (distLaser > OILRIG2_MIN)) { fire_detected++; @@ -632,12 +645,12 @@ } } break; - + default: for (int i = 0; i<5; i++) { distLaser = getDistance(); if ((distLaser < OILRIG1_MAX) - || (distLaser > OILRIG1_MIN)) { + && (distLaser > OILRIG1_MIN)) { fire_detected++; } else { fire_not_detected++; @@ -683,7 +696,7 @@ location=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; - while(location< 68) { + while(location< 70) { location=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; pid1.setInputLimits(0, set); @@ -1005,14 +1018,10 @@ // 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(); 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