Paolina Povolotskaya
ksdflsjdfljas
Dependencies: Adafruit-16-Ch-PWM-Servo-Driver HCSR04 PID PololuQik2 QEI Sharp mbed-rtos
Fork of
theRobot
by 
Thomas Ashworth
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;
}