Texas State IEEE
Code to drive Team 1's robot for the 2016 R5 robotics competition.
Dependencies: mbed navigation R5_StepperDrive LongRangeSensor DistanceSensor scanner Gripper ColorSensor
Diff: r5driver.cpp
- Revision:
- 38:ebf73b654553
- Parent:
- 36:5b0bf9153c3d
- Child:
- 39:1e26cc57c8b7
--- a/r5driver.cpp Sat Mar 26 23:28:21 2016 +0000
+++ b/r5driver.cpp Fri Apr 01 15:56:01 2016 +0000
@@ -12,6 +12,7 @@
DigitalOut led_red(LED_RED);
DigitalOut led_green(LED_GREEN);
bool active = false;
+bool v3Flag = 0;
void activate();
@@ -31,7 +32,7 @@
wait(1e-6);
}
- StepperDrive drive(pc, PTE19, PTE18, 0, PTE3, PTE2, 1, 10.0625, 8.4800, 500); // 8.375
+ StepperDrive drive(pc, PTE19, PTE18, 0, PTE3, PTE2, 1, 10.0625, 8.4800, 650); // 8.375
//(serial &, stepPinLeft, dirPinLeft, invertLeft, stepPinRight, dirPinRight, invertRight, wheelCircum, wheelSepar, periodUs)
ShortRangeSensor shortRangeL(PTE0, PTE1); // verify i2c pins
@@ -40,7 +41,7 @@
LongRangeSensor longRangeR(pc, PTB3);
Gripper gripper(PTB2, PTB3); // grip pin, wrist pin
- Scanner scanner(pc, drive, PTB0, PTB1, shortRangeL, shortRangeR, longRangeL, longRangeR, gripper, 0.075);
+ Scanner scanner(pc, drive, PTB0, PTB1, shortRangeL, shortRangeR, longRangeL, longRangeR, gripper, 1.0); // original period = 0.075
// (Serial &, PinName servoL, PinName servoR, shortRangeL, shortRangeR, longRangeL, longRangeR, period)
Navigation r5nav(scanner, 25);
@@ -68,7 +69,7 @@
r5nav.addGraphNode(3, 15, 6, 0);
r5nav.addGraphNode(4, 3, 8, 0);
// r5nav.addGraphNode(5, 3, 23, 180);
- r5nav.addGraphNode(5, 6, 37, 0);
+ r5nav.addGraphNode(5, 6, 37, 358); // (-2 degrees) original angle = 0
r5nav.addGraphNode(5, 8, 12.5, 90);
r5nav.addGraphNode(5, 15, 17, 180);
r5nav.addGraphNode(6, 5, 37, 176.5); // original angle = 180
@@ -433,100 +434,282 @@
/* add hunt method call */
- if (scanner.getObjectFound() == 1)
- {
- r5nav.getShortestPath(19);
- pc.printf("\n\nDistance from V3 to 19: %i\n", r5nav.getMinDist(19) );
- pc.printf("Route:\n");
- scanner.setLocalizeRightFlag(1);
- scanner.setLocalizeLeftFlag(0);
- r5nav.executeRoute(pc, drive);
- wait(0.1);
-
- // at node 5
- r5nav.getShortestPath(5);
- pc.printf("\n\nDistance from 19 to 5: %i\n", r5nav.getMinDist(5) );
- pc.printf("Route:\n");
- scanner.setLocalizeRightFlag(0);
- scanner.setLocalizeLeftFlag(0);
- r5nav.executeRoute(pc, drive);
- wait(0.1);
+ r5nav.getShortestPath(19);
+ pc.printf("\n\nDistance from V3 to 19: %i\n", r5nav.getMinDist(19) );
+ pc.printf("Route:\n");
+ scanner.setLocalizeRightFlag(1);
+ scanner.setLocalizeLeftFlag(0);
+ r5nav.executeRoute(pc, drive);
+ wait(0.1);
+
+ r5nav.getShortestPath(5);
+ pc.printf("\n\nDistance from 19 to 5: %i\n", r5nav.getMinDist(5) );
+ pc.printf("Route:\n");
+ scanner.setLocalizeRightFlag(0);
+ scanner.setLocalizeLeftFlag(0);
+ r5nav.executeRoute(pc, drive);
+ wait(0.1);
+ // at node 5
- r5nav.getShortestPath(15);
- pc.printf("\n\nDistance from 5 to 15: %i\n", r5nav.getMinDist(15) );
- pc.printf("Route:\n");
- scanner.setLocalizeRightFlag(0);
- scanner.setLocalizeLeftFlag(1);
- r5nav.executeRoute(pc, drive);
- wait(0.1);
-
- r5nav.getShortestPath(3);
- pc.printf("\n\nDistance from 15 to 3: %i\n", r5nav.getMinDist(3) );
- pc.printf("Route:\n");
- scanner.setLocalizeRightFlag(0);
- scanner.setLocalizeLeftFlag(1);
- r5nav.executeRoute(pc, drive);
- wait(0.1);
-
- if (scanner.getYellowFlag() == 1)
+ if (scanner.getObjectFound() == 1)
{
- r5nav.getShortestPath(YELLOW_DROP_ZONE);
- pc.printf("\n\nDistance from %i to YELLOW_DROP_ZONE: %i\n", r5nav.getVertex(), r5nav.getMinDist(YELLOW_DROP_ZONE) );
+ v3Flag = 1;
+
+ r5nav.getShortestPath(15);
+ pc.printf("\n\nDistance from 5 to 15: %i\n", r5nav.getMinDist(15) );
pc.printf("Route:\n");
- //r5nav.printRoute(pc);
- scanner.setLocalizeRightFlag(1);
- scanner.setLocalizeLeftFlag(0);
+ scanner.setLocalizeRightFlag(0);
+ scanner.setLocalizeLeftFlag(1);
+ r5nav.executeRoute(pc, drive);
+ wait(0.1);
+
+ r5nav.getShortestPath(3);
+ pc.printf("\n\nDistance from 15 to 3: %i\n", r5nav.getMinDist(3) );
+ pc.printf("Route:\n");
+ scanner.setLocalizeRightFlag(0);
+ scanner.setLocalizeLeftFlag(1);
r5nav.executeRoute(pc, drive);
wait(0.1);
+
+ if (scanner.getYellowFlag() == 1)
+ {
+ r5nav.getShortestPath(YELLOW_DROP_ZONE);
+ pc.printf("\n\nDistance from %i to YELLOW_DROP_ZONE: %i\n", r5nav.getVertex(), r5nav.getMinDist(YELLOW_DROP_ZONE) );
+ pc.printf("Route:\n");
+ //r5nav.printRoute(pc);
+ scanner.setLocalizeRightFlag(1);
+ scanner.setLocalizeLeftFlag(0);
+ r5nav.executeRoute(pc, drive);
+ wait(0.1);
+
+ /* add drop peg code */
+
+ r5nav.getShortestPath(15)
+ pc.printf("\n\nDistance from %i to 15%i\n", r5nav.getVertex(), r5nav.getMinDist(15);
+ pc.printf("Route:\n");
+ //r5nav.printRoute(pc);
+ scanner.setLocalizeLeftFlag(1);
+ scanner.setLocalizeRightFlag(0);
+ r5nav.executeRoute(pc, drive);
+ wait(0.1);
+
+ r5nav.getShortestPath(5);
+ pc.printf("\n\nDistance from %i to 5: %i\n", r5nav.getVertex(), r5nav.getMinDist(5) );
+ pc.printf("Route:\n");
+ //r5nav.printRoute(pc);
+ scanner.setLocalizeRightFlag(1);
+ scanner.setLocalizeLeftFlag(0);
+ r5nav.executeRoute(pc, drive);
+ wait(0.1);
+ }
+
+ else
+ {
+ r5nav.getShortestPath(1);
+ pc.printf("\n\nDistance from 3 to 1: %i\n", r5nav.getMinDist(1) );
+ pc.printf("Route:\n");
+ scanner.setLocalizeRightFlag(0);
+ scanner.setLocalizeLeftFlag(0);
+ r5nav.executeRoute(pc, drive);
+ wait(0.1);
+
+ r5nav.getShortestPath(RED_DROP_ZONE);
+ pc.printf("\n\nDistance from %i to RED_DROP_ZONE: %i\n", r5nav.getVertex(), r5nav.getMinDist(RED_DROP_ZONE) );
+ pc.printf("Route:\n");
+ //r5nav.printRoute(pc);
+ scanner.setLocalizeRightFlag(1);
+ scanner.setLocalizeLeftFlag(0);
+ r5nav.executeRoute(pc, drive);
+ wait(0.1);
+
+ r5nav.getShortestPath(1);
+ pc.printf("\n\nDistance from %i to 1: %i\n", r5nav.getVertex(), r5nav.getMinDist(1) );
+ pc.printf("Route:\n");
+ //r5nav.printRoute(pc);
+ scanner.setLocalizeRightFlag(0);
+ scanner.setLocalizeLeftFlag(1);
+ r5nav.executeRoute(pc, drive);
+ wait(0.1);
+
+ r5nav.getShortestPath(3);
+ pc.printf("\n\nDistance from %i to 3: %i\n", r5nav.getVertex(), r5nav.getMinDist(3) );
+ pc.printf("Route:\n");
+ //r5nav.printRoute(pc);
+ scanner.setLocalizeRightFlag(0);
+ scanner.setLocalizeLeftFlag(0);
+ r5nav.executeRoute(pc, drive);
+ wait(0.1);
+
+ r5nav.getShortestPath(15);
+ pc.printf("\n\nDistance from %i to 15: %i\n", r5nav.getVertex(), r5nav.getMinDist(15) );
+ pc.printf("Route:\n");
+ //r5nav.printRoute(pc);
+ scanner.setLocalizeRightFlag(0);
+ scanner.setLocalizeLeftFlag(1);
+ r5nav.executeRoute(pc, drive);
+ wait(0.1);
+
+ r5nav.getShortestPath(5);
+ pc.printf("\n\nDistance from %i to 5: %i\n", r5nav.getVertex(), r5nav.getMinDist(5) );
+ pc.printf("Route:\n");
+ //r5nav.printRoute(pc);
+ scanner.setLocalizeRightFlag(1);
+ scanner.setLocalizeLeftFlag(0);
+ r5nav.executeRoute(pc, drive);
+ wait(0.1);
+
+ /* 3rd peg retrieval complete
+ Begin 4th peg retrieval */
+ }
}
-
- else
- {
- r5nav.getShortestPath(1);
- pc.printf("\n\nDistance from 3 to 1: %i\n", r5nav.getMinDist(1) );
+
+ // peg V3 not found and at node 5
+
+ r5nav.getShortestPath(6);
+ pc.printf("\n\nDistance from %i to 6: %i\n", r5nav.getVertex(), r5nav.getMinDist(6) );
+ pc.printf("Route:\n");
+ //r5nav.printRoute(pc);
+ scanner.setLocalizeRightFlag(1);
+ scanner.setLocalizeLeftFlag(0);
+ r5nav.executeRoute(pc, drive);
+ wait(0.1);
+
+ r5nav.getShortestPath(14);
+ pc.printf("\n\nDistance from %i to 14: %i\n", r5nav.getVertex(), r5nav.getMinDist(14) );
+ pc.printf("Route:\n");
+ //r5nav.printRoute(pc);
+ scanner.setLocalizeRightFlag(0);
+ scanner.setLocalizeLeftFlag(0);
+ r5nav.executeRoute(pc, drive);
+ wait(0.1);
+
+ r5nav.getShortestPath(20);
+ pc.printf("\n\nDistance from %i to 20: %i\n", r5nav.getVertex(), r5nav.getMinDist(20) );
+ pc.printf("Route:\n");
+ //r5nav.printRoute(pc);
+ scanner.setLocalizeRightFlag(1);
+ scanner.setLocalizeLeftFlag(0);
+ r5nav.executeRoute(pc, drive);
+ wait(0.1);
+
+ /* add hunt code */
+
+ if (scanner.getObjectFound() == 1)
+ {
+ r5nav.getShortestPath(15);
+ pc.printf("\n\nDistance from 5 to 15: %i\n", r5nav.getMinDist(15) );
pc.printf("Route:\n");
scanner.setLocalizeRightFlag(0);
scanner.setLocalizeLeftFlag(0);
r5nav.executeRoute(pc, drive);
wait(0.1);
+ // may need to break up above route
- r5nav.getShortestPath(RED_DROP_ZONE);
- pc.printf("\n\nDistance from %i to RED_DROP_ZONE: %i\n", r5nav.getVertex(), r5nav.getMinDist(RED_DROP_ZONE) );
+ r5nav.getShortestPath(3);
+ pc.printf("\n\nDistance from 15 to 3: %i\n", r5nav.getMinDist(3) );
pc.printf("Route:\n");
- //r5nav.printRoute(pc);
- scanner.setLocalizeRightFlag(1);
- scanner.setLocalizeLeftFlag(0);
+ scanner.setLocalizeRightFlag(0);
+ scanner.setLocalizeLeftFlag(1);
r5nav.executeRoute(pc, drive);
wait(0.1);
+
+ if (scanner.getYellowFlag() == 1)
+ {
+ r5nav.getShortestPath(YELLOW_DROP_ZONE);
+ pc.printf("\n\nDistance from %i to YELLOW_DROP_ZONE: %i\n", r5nav.getVertex(), r5nav.getMinDist(YELLOW_DROP_ZONE) );
+ pc.printf("Route:\n");
+ //r5nav.printRoute(pc);
+ scanner.setLocalizeRightFlag(1);
+ scanner.setLocalizeLeftFlag(0);
+ r5nav.executeRoute(pc, drive);
+ wait(0.1);
+
+ /* add drop peg code */
+
+ r5nav.getShortestPath(15)
+ pc.printf("\n\nDistance from %i to 15%i\n", r5nav.getVertex(), r5nav.getMinDist(15);
+ pc.printf("Route:\n");
+ //r5nav.printRoute(pc);
+ scanner.setLocalizeLeftFlag(1);
+ scanner.setLocalizeRightFlag(0);
+ r5nav.executeRoute(pc, drive);
+ wait(0.1);
+
+ r5nav.getShortestPath(5);
+ pc.printf("\n\nDistance from %i to 5: %i\n", r5nav.getVertex(), r5nav.getMinDist(5) );
+ pc.printf("Route:\n");
+ //r5nav.printRoute(pc);
+ scanner.setLocalizeRightFlag(1);
+ scanner.setLocalizeLeftFlag(0);
+ r5nav.executeRoute(pc, drive);
+ wait(0.1);
+
+ /* may need to get back to 20 */
+ }
+
+ else
+ {
+ r5nav.getShortestPath(1);
+ pc.printf("\n\nDistance from 3 to 1: %i\n", r5nav.getMinDist(1) );
+ pc.printf("Route:\n");
+ scanner.setLocalizeRightFlag(0);
+ scanner.setLocalizeLeftFlag(0);
+ r5nav.executeRoute(pc, drive);
+ wait(0.1);
+
+ r5nav.getShortestPath(RED_DROP_ZONE);
+ pc.printf("\n\nDistance from %i to RED_DROP_ZONE: %i\n", r5nav.getVertex(), r5nav.getMinDist(RED_DROP_ZONE) );
+ pc.printf("Route:\n");
+ //r5nav.printRoute(pc);
+ scanner.setLocalizeRightFlag(1);
+ scanner.setLocalizeLeftFlag(0);
+ r5nav.executeRoute(pc, drive);
+ wait(0.1);
+
+ r5nav.getShortestPath(1);
+ pc.printf("\n\nDistance from %i to 1: %i\n", r5nav.getVertex(), r5nav.getMinDist(1) );
+ pc.printf("Route:\n");
+ //r5nav.printRoute(pc);
+ scanner.setLocalizeRightFlag(0);
+ scanner.setLocalizeLeftFlag(1);
+ r5nav.executeRoute(pc, drive);
+ wait(0.1);
+
+ r5nav.getShortestPath(3);
+ pc.printf("\n\nDistance from %i to 3: %i\n", r5nav.getVertex(), r5nav.getMinDist(3) );
+ pc.printf("Route:\n");
+ //r5nav.printRoute(pc);
+ scanner.setLocalizeRightFlag(0);
+ scanner.setLocalizeLeftFlag(0);
+ r5nav.executeRoute(pc, drive);
+ wait(0.1);
+
+ r5nav.getShortestPath(15);
+ pc.printf("\n\nDistance from %i to 15: %i\n", r5nav.getVertex(), r5nav.getMinDist(15) );
+ pc.printf("Route:\n");
+ //r5nav.printRoute(pc);
+ scanner.setLocalizeRightFlag(0);
+ scanner.setLocalizeLeftFlag(1);
+ r5nav.executeRoute(pc, drive);
+ wait(0.1);
+
+ r5nav.getShortestPath(5);
+ pc.printf("\n\nDistance from %i to 5: %i\n", r5nav.getVertex(), r5nav.getMinDist(5) );
+ pc.printf("Route:\n");
+ //r5nav.printRoute(pc);
+ scanner.setLocalizeRightFlag(1);
+ scanner.setLocalizeLeftFlag(0);
+ r5nav.executeRoute(pc, drive);
+ wait(0.1);
+
+ /* may need to get back to 20 */
+
+ /* V4 peg retrieval complete
+ Begin V5 peg retrieval */
+ }
}
- }
- else // peg V3 not found
- {
- /* need to implement rest of moves
-
- r5nav.getShortestPath(19);
- pc.printf("\n\nDistance from 12 to 19: %i\n", r5nav.getMinDist(19) );
- pc.printf("Route:\n");
- scanner.setLocalizeRightFlag(1);
- scanner.setLocalizeLeftFlag(0);
- r5nav.executeRoute(pc, drive);
- wait(0.1);
-
- // at node 5
- r5nav.getShortestPath(5);
- pc.printf("\n\nDistance from 19 to 5: %i\n", r5nav.getMinDist(5) );
- pc.printf("Route:\n");
- scanner.setLocalizeRightFlag(0);
- scanner.setLocalizeLeftFlag(0);
- r5nav.executeRoute(pc, drive);
- wait(0.1); */
- }
-
- /* 3rd peg retrieval complete
- Begin 4th peg retrieval */
-
-
+ /* add more code for for V5 and V6 */
/*
drive.move(0, ((90.0)*(3.14159 / 180.0))); // return to initial angle
