nav fixed
Dependencies: Adafruit-16-Ch-PWM-Servo-Driver HCSR04 PID PololuQik2 QEI Sharp mbed-rtos
Fork of theRobot by
Diff: main.cpp
- Revision:
- 16:8bb212df81b7
- Parent:
- 14:784acd735b8c
- Child:
- 17:a5bb85ee205d
--- a/main.cpp Wed Apr 02 04:04:28 2014 +0000 +++ b/main.cpp Wed Apr 02 23:54:05 2014 +0000 @@ -82,11 +82,11 @@ //******* TODO ********// //*********************// //Oil Rig distance thresholds -#define OILRIG1_MAX 3000 +#define OILRIG1_MAX 2200 #define OILRIG1_MIN 1000 -#define OILRIG2_MAX 5000 +#define OILRIG2_MAX 2200 #define OILRIG2_MIN 1000 -#define OILRIG3_MAX 5000 +#define OILRIG3_MAX 2200 #define OILRIG3_MIN 1000 //for servo normalization @@ -99,6 +99,7 @@ DigitalOut myled2(LED2); DigitalOut myled3(LED3); DigitalOut myled4(LED4); +InterruptIn startBtn(p7); void errFunction(void); bool cRc; @@ -130,6 +131,7 @@ void setServoPulseNo_delay(uint8_t n, int angle); void servoPosition(int set); int fire_checker(int rig); +int button_start = 0; //Navigation Functions @@ -154,6 +156,7 @@ void alignWithWall(int section); void UntilWall(int dir); +extern "C" void mbed_reset(); /************ Main Variables @@ -192,7 +195,7 @@ //increase in number 5 rotates gripper - {STORE_POSITION, 85, 5, 0, 165, 175, 0}, // storing position + {STORE_POSITION, 85, 10, 0, 165, 90, 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_RIG3, 130, 90, 90, 52, 175, 0}, // point laser at oilrig2 @@ -202,9 +205,9 @@ {TOOL_3, 57, 50, 80, 109, 175, 0}, // Look over third tool {DRIVE_POSITION_TOOL, 55, 70, 102, 74, 0, 0}, // Drive with tool loaded {ORIENT_TOOL, 135, 60, 75, 60, 90, 90}, // position tool to be inserted - {PU_TOOL_1, 98, 50, 82, 118, 90, 0}, // STATIC Pickup tool POSITION - {PU_TOOL_2, 78, 50, 82, 108, 90, 0}, // STATIC Pickup tool POSITION - {PU_TOOL_3, 53, 50, 82, 118, 90, 0}, // STATIC Pickup tool POSITION + {PU_TOOL_1, 96, 46, 78, 104, 90, 0}, // STATIC Pickup tool POSITION + {PU_TOOL_2, 74, 47, 80, 105, 90, 0}, // STATIC Pickup tool POSITION + {PU_TOOL_3, 57, 44, 76, 104, 90, 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 @@ -217,6 +220,7 @@ int area; int shape; int shape_alignX_done = 0; +int shape_alignY_done = 0; /* Variables for distance sensor*/ @@ -224,6 +228,18 @@ int fire_detected = 0; int fire_not_detected = 0; +void button_int(void) +{ + if(!button_start) { + button_start = 1; + wait(1.0); + } else { + button_start = 0; + mbed_reset(); + } + return; +} + int main() { @@ -240,6 +256,7 @@ //Laser Range Finder Initialization lrf_baudCalibration(); motors.begin(); + startBtn.rise(&button_int); //Servo initialization initServoDriver(); @@ -247,414 +264,417 @@ //ServoOutputDisable = 0; - /******************* - WHILE LOOP FOR TESTING - ********************/ - //while(1) { - /* - pc.scanf("%d %d", &servoNum, &servoAngle); - if(servoAngle > 175) { - servoAngle = 175; - } - if(servoNum > 5 ) { - servoNum = 0; - servoAngle = 90; - } - setServoPulse(servoNum, servoAngle); - distLaser = getDistance(); - pc.printf("Distance %d", distLaser); - */ - - /* - int shape_alignX_done = 0; - int shape_alignY_done = 0; - - - //pc.scanf("%d", &posNum); - - while(pc.getc() != 'g'); - servoPosition(TOOL_1); - while(pc.getc() != 'g'); - //shape_detected = shapeDetection(); - - ImageToArray(GREYSCALE); - //clearBounds(); - printImageToFile(GREYSCALE); - while(1); - - while(shape_alignY_done == 0) { - shape_detected = shapeDetection(); - pc.printf("\nY movement\n"); - if(get_com_y() >= 80) { - setServoPulse(1, Arm_Table[TOOL_1].base_arm-=2); - setServoPulse(2, Arm_Table[TOOL_1].big_arm-=1); - //setServoPulse(3, Arm_Table[TOOL_1].claw_arm+=2); - } else if(get_com_y() > 70 && get_com_y() < 90) { - setServoPulse(1, Arm_Table[TOOL_1].base_arm-=1); - setServoPulse(2, Arm_Table[TOOL_1].big_arm-=1); - //setServoPulse(3, Arm_Table[TOOL_1].claw_arm+=1); - } else if(get_com_y() <= 35 ) { - setServoPulse(1, Arm_Table[TOOL_1].base_arm+=2); - setServoPulse(2, Arm_Table[TOOL_1].big_arm+=1); - //setServoPulse(3, Arm_Table[TOOL_1].claw_arm-=2); - } else if(get_com_y() < 50 && get_com_y() > 20) { - setServoPulse(1, Arm_Table[TOOL_1].base_arm+=1); - setServoPulse(2, Arm_Table[TOOL_1].big_arm+=1); - //setServoPulse(3, Arm_Table[TOOL_1].claw_arm-=1); - } else { - shape_alignY_done = 1; - } - } - - while( shape_alignX_done == 0){ - shape_detected = shapeDetection(); - pc.printf("\nX movement\n"); - if(get_com_x() > 95) { - Arm_Table[TOOL_1].base_rotate+=1; - setServoPulse(0, Arm_Table[TOOL_1].base_rotate); - setServoPulse(0, Arm_Table[TOOL_1].base_rotate-1); - setServoPulse(0, Arm_Table[TOOL_1].base_rotate+1); - - - } else if(get_com_x() < 65) { - Arm_Table[TOOL_1].base_rotate-=1; - setServoPulse(0, Arm_Table[TOOL_1].base_rotate); - setServoPulse(0, Arm_Table[TOOL_1].base_rotate-1); - setServoPulse(0, Arm_Table[TOOL_1].base_rotate+1); - - } else { - shape_alignX_done = 1; - } - - - } - printImageToFile(BINARY);*/ - - // } - - - - /******************************** MAIN WHILE LOOP FOR COMPETITION *********************************/ while(1) { + if(button_start == 1) { - switch (state) { + + switch (state) { - /************************************************** - * STAGE 0 - * - * - START OF THE COMETITION - * - **************************************************/ - case START : - myled1 = 1; - while(pc.getc() != 'g'); //waits for the letter g before starting program - myled1 = 0; - state = OILRIG1_POS; - break; + /************************************************** + * STAGE 0 + * + * - START OF THE COMETITION + * + **************************************************/ + case START : + myled1 = 1; + state = OILRIG1_POS; + break; - /************************************************** - * STAGE 1 - * - * - DETERMINE OIL RIG ON FIRE - * - **************************************************/ - case OILRIG1_POS: //aims arm at square oil rig + /************************************************** + * STAGE 1 + * + * - DETERMINE OIL RIG ON FIRE + * + **************************************************/ - servoPosition(OIL_RIG1); - wait(3); //wait for servo to settle before laser distance + case OILRIG1_POS: //aims arm at square oil rig - fire = fire_checker(OIL_RIG1); //determines if oil rig is on fire + servoPosition(OIL_RIG1); + wait(3); //wait for servo to settle before laser distance + + fire = fire_checker(OIL_RIG1); //determines if oil rig is on fire - //determines what tool is needed - if (fire == 1) { - pc.printf("FIRE FOUND!!!!!!!!\n\r"); - tool_needed = SQUARE; - state = GOTO_TOOLS1; - } else { - pc.printf("XXXXXX FIRE NOT FOUND XXXXXX"); - state = OILRIG2_POS; - } - break; + //determines what tool is needed + if (fire == 1) { + pc.printf("FIRE FOUND!!!!!!!!\n\r"); + tool_needed = SQUARE; + state = GOTO_TOOLS1; + } else { + pc.printf("XXXXXX FIRE NOT FOUND XXXXXX"); + state = OILRIG2_POS; + } + break; + + case OILRIG2_POS: - case OILRIG2_POS: - - servoPosition(DRIVE_POSITION_NOTOOL); - wait(3); //wait for servos to settle + 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_section2(location, current); // moves to second rig + to_tools_section2(location, current); // moves to second rig - servoPosition(OIL_RIG2); //position arm to point at first oilrig - wait(3); + servoPosition(OIL_RIG2); //position arm to point at first oilrig + wait(3); - fire = fire_checker(OIL_RIG2); - if (fire == 1) { - pc.printf("FIRE FOUND!!!!!!!!"); - tool_needed = TRIANGLE; - state = GOTO_TOOLS2; - } else { - pc.printf("XXXXXX FIRE NOT FOUND XXXXXX"); - tool_needed = CIRCLE; - state = GOTO_TOOLS2; - } - break; + fire = fire_checker(OIL_RIG2); + if (fire == 1) { + pc.printf("FIRE FOUND!!!!!!!!"); + tool_needed = TRIANGLE; + state = GOTO_TOOLS2; + } else { + pc.printf("XXXXXX FIRE NOT FOUND XXXXXX"); + tool_needed = CIRCLE; + state = GOTO_TOOLS2; + } + break; - /************************************************** - * STAGE 2 - * - * - TRAVEL TO TOOLS - * - **************************************************/ - case GOTO_TOOLS1: + /************************************************** + * STAGE 2 + * + * - TRAVEL TO TOOLS + * + **************************************************/ + case GOTO_TOOLS1: + 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; + break; - servoPosition(DRIVE_POSITION_NOTOOL); - wait(3); //wait for servos to settle - to_tools_section1(location, current); - state = IDENTIFY_TOOLS; - break; - - case GOTO_TOOLS2: + case GOTO_TOOLS2: - servoPosition(DRIVE_POSITION_NOTOOL); - wait(3); //wait for servos to settle + 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 - slightMove(FORWARD,3100); - current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; + slightMove(FORWARD,3100); + current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; - state = IDENTIFY_TOOLS; - break; + state = IDENTIFY_TOOLS; + break; - /************************************************** - * STAGE 3 - * - * - Determine order of tools - * - Aquire appropriate tool - * - **************************************************/ - case IDENTIFY_TOOLS: - - servoPosition(TOOL_2); //arm/camera looks over tool - wait(5); //wait for servos to settle - - //shape_detected = shapeDetection(); //determines the shape - //clearBounds(); - //printImageToFile(BINARY); + /************************************************** + * STAGE 3 + * + * - Determine order of tools + * - Aquire appropriate tool + * + **************************************************/ + case IDENTIFY_TOOLS: - while( shape_alignX_done == 0) { - shape_detected = shapeDetection(); - pc.printf("X - Adjust to center tool\n\r"); - if(get_com_x() > 95) { - Arm_Table[TOOL_1].base_rotate+=1; - - } else if(get_com_x() < 65) { - Arm_Table[TOOL_1].base_rotate-=1; + wait(5); + servoPosition(TOOL_2); //arm/camera looks over tool + wait(5); //wait for servos to settle - } else { - shape_alignX_done = 1; - } - } - + //shape_detected = shapeDetection(); //determines the shape + //clearBounds(); + //printImageToFile(BINARY); + shape_alignX_done = 0; + shape_alignY_done = 0; - //either goes to aquire the tool or look at the next shape - if(shape_detected == tool_needed) { - state = AQUIRE_TOOL2; - } else { - servoPosition(TOOL_1); - wait(3); //wait for servos to settle + while( shape_alignX_done == 0) { + wait(1); + shape_detected = shapeDetection(); - shape_alignX_done = 0; - while( shape_alignX_done == 0) { - - shape_detected = shapeDetection(); pc.printf("X - Adjust to center tool\n\r"); if(get_com_x() > 95) { - Arm_Table[TOOL_1].base_rotate+=1; + setServoPulse(0,Arm_Table[TOOL_2].base_rotate+=1); } else if(get_com_x() < 65) { - Arm_Table[TOOL_1].base_rotate-=1; + setServoPulse(0,Arm_Table[TOOL_2].base_rotate-=1); } else { shape_alignX_done = 1; } } - if (shape_detected == tool_needed) { - state = AQUIRE_TOOL1; + //either goes to aquire the tool or look at the next shape + if(shape_detected == tool_needed) { + state = AQUIRE_TOOL2; + break; } else { - servoPosition(TOOL_3); - wait(3); //wait for servos to settle + slightMove(FORWARD,25); + current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; + + + servoPosition(TOOL_1); + wait(5); //wait for servos to settle + shape_alignX_done = 0; + shape_alignY_done = 0; + + while( shape_alignY_done == 0) { + wait(1); + shape_detected = shapeDetection(); + + pc.printf("Y - Adjust to center tool\n\r"); + + if(get_com_y() < 40) { + wait(1); + slightMove(FORWARD,25); + current-=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; + + } else if(get_com_y() > 80) { + wait(1); + slightMove(BACKWARD,25); + current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; + + } else { + shape_alignY_done = 1; + } + } while( shape_alignX_done == 0) { + wait(1); shape_detected = shapeDetection(); + pc.printf("X - Adjust to center tool\n\r"); - if(get_com_x() > 95) { - Arm_Table[TOOL_1].base_rotate+=1; + if(get_com_x() > 100) { + setServoPulse(0,Arm_Table[TOOL_1].base_rotate+=1); - } else if(get_com_x() < 65) { - Arm_Table[TOOL_1].base_rotate-=1; + } else if(get_com_x() < 60) { + setServoPulse(0,Arm_Table[TOOL_1].base_rotate-=1); } else { shape_alignX_done = 1; } } - state = AQUIRE_TOOL3; + if (shape_detected == tool_needed) { + state = AQUIRE_TOOL1; + break; + } else { + servoPosition(TOOL_3); + wait(3); //wait for servos to settle + state = AQUIRE_TOOL3; + } } - } - while(1); - break; + break; + + case AQUIRE_TOOL1: - case AQUIRE_TOOL1: + servoPosition(PU_TOOL_1); + setServoPulse(4, 130); + wait(5); + setServoPulse(0, Arm_Table[PU_TOOL_1].base_rotate ); + wait(1); + setServoPulse(1, Arm_Table[PU_TOOL_1].base_arm - 5); + wait(1); + setServoPulse(5, 105); + wait(.5); + setServoPulse(5, 00); + setServoPulse(1, Arm_Table[PU_TOOL_1].base_arm - 6); + wait(.5); + setServoPulse(5, 105); + wait(.5); + setServoPulse(5, 00); + setServoPulse(1, Arm_Table[PU_TOOL_1].base_arm - 8); + wait(1); + setServoPulse(5, 105); + wait(1); + setServoPulse(1, Arm_Table[PU_TOOL_1].base_arm); - servoPosition(PU_TOOL_1_STAB); - wait(2); + while(1); - servoPosition(DRIVE_POSITION_TOOL); //arm position for driving with the tool - state = NAVIGATE_WAVES_ROW1; - break; + servoPosition(DRIVE_POSITION_TOOL); //arm position for driving with the tool + state = NAVIGATE_WAVES_ROW1; + break; - case AQUIRE_TOOL2: - servoPosition(PU_TOOL_2_STAB); - wait(2); + case AQUIRE_TOOL2: + servoPosition(PU_TOOL_2); + setServoPulse(4, 170); + wait(5); + setServoPulse(0, Arm_Table[PU_TOOL_2].base_rotate ); + wait(1); + setServoPulse(1, Arm_Table[PU_TOOL_2].base_arm - 5); + wait(1); + setServoPulse(5, 105); + wait(.5); + setServoPulse(5, 00); + setServoPulse(1, Arm_Table[PU_TOOL_2].base_arm - 6); + wait(.5); + setServoPulse(5, 105); + wait(.5); + setServoPulse(5, 00); + setServoPulse(1, Arm_Table[PU_TOOL_2].base_arm - 8); + wait(1); + setServoPulse(5, 105); + wait(1); + setServoPulse(1, Arm_Table[PU_TOOL_2].base_arm); + while(1); - servoPosition(DRIVE_POSITION_TOOL); //arm position for driving with the tool - state = NAVIGATE_WAVES_ROW1; - break; + servoPosition(DRIVE_POSITION_TOOL); //arm position for driving with the tool + state = NAVIGATE_WAVES_ROW1; + break; - case AQUIRE_TOOL3: - servoPosition(PU_TOOL_3_STAB); - wait(2); + case AQUIRE_TOOL3: + servoPosition(PU_TOOL_3); + setServoPulse(4, 130); + wait(5); + setServoPulse(0, Arm_Table[PU_TOOL_3].base_rotate ); + wait(1); + setServoPulse(1, Arm_Table[PU_TOOL_3].base_arm - 5); + wait(1); + setServoPulse(5, 105); + wait(.5); + setServoPulse(5, 00); + setServoPulse(1, Arm_Table[PU_TOOL_3].base_arm - 6); + wait(.5); + setServoPulse(5, 105); + wait(.5); + setServoPulse(5, 00); + setServoPulse(1, Arm_Table[PU_TOOL_3].base_arm - 8); + wait(1); + setServoPulse(5, 105); + wait(1); + setServoPulse(1, Arm_Table[PU_TOOL_3].base_arm); + while(1); - - servoPosition(DRIVE_POSITION_TOOL); //arm position for driving with the tool - state = NAVIGATE_WAVES_ROW1; - break; + servoPosition(DRIVE_POSITION_TOOL); //arm position for driving with the tool + state = NAVIGATE_WAVES_ROW1; + break; - /************************************************** - * STAGE 4 - * - * - Navigate through the ocean - * - **************************************************/ + /************************************************** + * STAGE 4 + * + * - Navigate through the ocean + * + **************************************************/ - case NAVIGATE_WAVES_ROW1: - //*********************// - //******* TODO ********// - //*********************// - // CODE TO NAVIGATE ROW1 - state = NAVIGATE_WAVES_ROW2; - break; + case NAVIGATE_WAVES_ROW1: + //*********************// + //******* TODO ********// + //*********************// + // CODE TO NAVIGATE ROW1 + state = NAVIGATE_WAVES_ROW2; + break; + + case NAVIGATE_WAVES_ROW2: + //*********************// + //******* TODO ********// + //*********************// + // CODE TO NAVIGATE ROW2 + state = NAVIGATE_WAVES_ROW3; + break; + + case NAVIGATE_WAVES_ROW3: + //*********************// + //******* TODO ********// + //*********************// + // CODE TO NAVIGATE ROW3 - case NAVIGATE_WAVES_ROW2: - //*********************// - //******* TODO ********// - //*********************// - // CODE TO NAVIGATE ROW2 - state = NAVIGATE_WAVES_ROW3; - break; + //goes to appropriate rig + if(shape_detected == 1) { + state = NAVIGATE_TO_SQUARE_RIG; + } else if(shape_detected == 2) { + state = NAVIGATE_TO_TRIANGLE_RIG; + } else { + state = NAVIGATE_TO_CIRCLE_RIG; + } + break; - case NAVIGATE_WAVES_ROW3: - //*********************// - //******* TODO ********// - //*********************// - // CODE TO NAVIGATE ROW3 - - //goes to appropriate rig - if(shape_detected == 1) { - state = NAVIGATE_TO_SQUARE_RIG; - } else if(shape_detected == 2) { - state = NAVIGATE_TO_TRIANGLE_RIG; - } else { - state = NAVIGATE_TO_CIRCLE_RIG; - } - break; + /************************************************** + * STAGE 5 + * + * - Travel to appropriate rig + * + **************************************************/ + case NAVIGATE_TO_SQUARE_RIG: + //NAVIGATION CODE HERE + state = RIG_ALIGN; + break; + case NAVIGATE_TO_TRIANGLE_RIG: + //NAVIGATION CODE HERE + state = RIG_ALIGN; + break; + case NAVIGATE_TO_CIRCLE_RIG: + //NAVIGATION CODE HERE + state = RIG_ALIGN; + break; - /************************************************** - * STAGE 5 - * - * - Travel to appropriate rig - * - **************************************************/ - case NAVIGATE_TO_SQUARE_RIG: - //NAVIGATION CODE HERE - state = RIG_ALIGN; - break; - case NAVIGATE_TO_TRIANGLE_RIG: - //NAVIGATION CODE HERE - state = RIG_ALIGN; - break; - case NAVIGATE_TO_CIRCLE_RIG: - //NAVIGATION CODE HERE - state = RIG_ALIGN; - break; + /************************************************** + * STAGE 6 + * + * - Align with appropriate rig + * + **************************************************/ + case RIG_ALIGN: + + //*********************// + //******* TODO ********// + //*********************// + // CODE TO ALIGN ROBOT WITH RIG - /************************************************** - * STAGE 6 - * - * - Align with appropriate rig - * - **************************************************/ - case RIG_ALIGN: + servoPosition(ORIENT_TOOL); + wait(1); //wait for servos to settle + state = INSERT_TOOL; + break; - //*********************// - //******* TODO ********// - //*********************// - // CODE TO ALIGN ROBOT WITH RIG - - servoPosition(ORIENT_TOOL); - wait(1); //wait for servos to settle - state = INSERT_TOOL; - break; + /************************************************** + * STAGE 7 + * + * - Insert Tool + * - Extenguish fire + * - win contest + * + **************************************************/ - /************************************************** - * STAGE 7 - * - * - Insert Tool - * - Extenguish fire - * - win contest - * - **************************************************/ - - case INSERT_TOOL: - //*********************// - //******* TODO ********// - //*********************// - // CODE TO INSERT TOOL - break; + case INSERT_TOOL: + //*********************// + //******* TODO ********// + //*********************// + // CODE TO INSERT TOOL + break; - /************************************************** - * STAGE 8 - * - * - END COMPETITION - * - **************************************************/ - case END: - servoPosition(STORE_POSITION); - myled1 = 1; - wait(.2); - myled2 = 1; - wait(.2); - myled3 = 1; - wait(.2); - myled4 = 1; - wait(.2); - break; - default: + /************************************************** + * STAGE 8 + * + * - END COMPETITION + * + **************************************************/ + case END: + servoPosition(STORE_POSITION); + myled1 = 1; + wait(.2); + myled2 = 1; + wait(.2); + myled3 = 1; + wait(.2); + myled4 = 1; + wait(.2); + break; + default: - break; - } - } + break; + } + } // End while loop + + } // End if for start button -} +} // main loop @@ -701,7 +721,13 @@ void servoBegin(void) { pc.printf("Setting Initial Servo Position\n\r"); - servoPosition(STORE_POSITION); + setServoPulseNo_delay(3, Arm_Table[STORE_POSITION].claw_arm); + setServoPulseNo_delay(2, Arm_Table[STORE_POSITION].big_arm); + wait(2); + setServoPulseNo_delay(1, Arm_Table[STORE_POSITION].base_arm); + setServoPulseNo_delay(0, Arm_Table[STORE_POSITION].base_rotate); + setServoPulseNo_delay(4, Arm_Table[STORE_POSITION].claw_rotate); + setServoPulseNo_delay(5, Arm_Table[STORE_POSITION].claw_open); } void setServoPulseNo_delay(uint8_t n, int angle) @@ -1287,7 +1313,7 @@ void to_tools_section1(float* location, float ¤t) { - slightMove(FORWARD,6500); + slightMove(FORWARD,6600); current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; }