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:
- 39:1e26cc57c8b7
- Parent:
- 38:ebf73b654553
- Child:
- 40:2d33bb4d6d6f
--- a/r5driver.cpp Fri Apr 01 15:56:01 2016 +0000
+++ b/r5driver.cpp Wed Apr 06 22:07:38 2016 +0000
@@ -1,727 +1,19 @@
+#include "mbed.h"
+#include "StepperDrive.h"
+#include "LongRangeSensor.h"
#include "navigation.h"
#include "Gripper.h"
-#include "scanner.h"
-#include "StepperDrive.h"
-#include "ShortRangeSensor.h"
-#include "stdint.h"
-#include "mbed.h"
+
Serial pc(USBTX, USBRX);
-// Serial pc(PTE16,PTE17); // bluetooth
+Serial bluetooth(PTE16,PTE17); // bluetooth
InterruptIn start(SW1);
DigitalOut led_red(LED_RED);
DigitalOut led_green(LED_GREEN);
bool active = false;
-bool v3Flag = 0;
-
-void activate();
-
-int main()
-{
- pc.baud(115200);
- // pc.baud(9600); /* interface via Bluetooth at 9600 */
- start.mode(PullUp); /* Start/Stop button is active low needing PullUp */
- start.fall(&activate);
-
- led_red = 0;
- led_green = 1;
-
- pc.printf("\nWaiting for START BUTTON\n");
- while(!active) // wait for start_button
- {
- wait(1e-6);
- }
-
- 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
- ShortRangeSensor shortRangeR(PTC9, PTC8); // verify i2c pins
- LongRangeSensor longRangeL(pc, PTB2);
- LongRangeSensor longRangeR(pc, PTB3);
- Gripper gripper(PTB2, PTB3); // grip pin, wrist pin
-
- 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);
- wait(0.1);
-/*
- r5nav.addGraphNode(0, 1, 10.0625, 0);
- r5nav.addGraphNode(1, 0, 10.0625, 180);
- r5nav.addGraphNode(1, 2, 10.0625, 270);
- r5nav.addGraphNode(2, 1, 10.0625, 90);
- r5nav.addGraphNode(2, 3, 10.0625, 180);
- r5nav.addGraphNode(3, 2, 10.0625, 0);
- r5nav.addGraphNode(3, 0, 10.0625, 90);
- r5nav.addGraphNode(0, 3, 10.0625, 270);
-*/
-
- // loading r5 map
- r5nav.addGraphNode(0, 1, 13, 0);
- r5nav.addGraphNode(1, 0, 13, 180);
- r5nav.addGraphNode(1, 2, 68, 356.5); // original angle = 0
- r5nav.addGraphNode(1, 3, 15, 90);
- r5nav.addGraphNode(2, 1, 68, 180);
- r5nav.addGraphNode(3, 1, 15, 270);
- r5nav.addGraphNode(3, 4, 8, 180);
-// r5nav.addGraphNode(3, 5, 23, 356.5); // (-5 degrees) original angle = 0
- r5nav.addGraphNode(3, 15, 6, 0);
- r5nav.addGraphNode(4, 3, 8, 0);
-// r5nav.addGraphNode(5, 3, 23, 180);
- 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
- r5nav.addGraphNode(6, 7, 6, 0);
- r5nav.addGraphNode(6, 10, 12.5, 90);
- r5nav.addGraphNode(7, 6, 6, 180);
- r5nav.addGraphNode(8, 5, 12.75, 270); // original distance = 12.5
-// r5nav.addGraphNode(8, 9, 35, 174.5); // original angle = 180
-// r5nav.addGraphNode(8, 10, 37, 0);
- r5nav.addGraphNode(8, 11, 12, 90);
- r5nav.addGraphNode(8, 16, 6, 180);
- r5nav.addGraphNode(8, 17, 6, 0);
-// r5nav.addGraphNode(9, 8, 35, 0);
- r5nav.addGraphNode(9, 16, 23, 0);
- r5nav.addGraphNode(10, 6, 12.5, 270);
- r5nav.addGraphNode(10, 13, 12, 90);
- r5nav.addGraphNode(10, 18, 6, 180);
- r5nav.addGraphNode(11, 8, 12, 270);
- r5nav.addGraphNode(11, 12, 35, 180);
- r5nav.addGraphNode(11, 19, 6, 180);
- r5nav.addGraphNode(12, 11, 35, 0);
- r5nav.addGraphNode(12, 19, 23, 0);
- r5nav.addGraphNode(13, 10, 12, 270);
- r5nav.addGraphNode(13, 14, 12, 0);
- r5nav.addGraphNode(14, 13, 12, 180);
- r5nav.addGraphNode(14, 20, 21, 90);
- r5nav.addGraphNode(15, 3, 6, 180);
- r5nav.addGraphNode(15, 5, 17, 0);
- r5nav.addGraphNode(16, 8, 6, 0);
- r5nav.addGraphNode(16, 9, 23, 180);
- r5nav.addGraphNode(17, 8, 6, 180);
- r5nav.addGraphNode(17, 18, 25, 0);
- r5nav.addGraphNode(18, 10, 6, 0);
- r5nav.addGraphNode(18, 17, 25, 180);
- r5nav.addGraphNode(19, 11, 6, 0);
- r5nav.addGraphNode(19, 12, 23, 180);
- r5nav.addGraphNode(20, 14, 21, 270);
- r5nav.addGraphNode(20, 21, 24, 90);
- r5nav.addGraphNode(21, 20, 24, 270);
- r5nav.addGraphNode(21, 22, 4, 180);
- r5nav.addGraphNode(22, 21, 4, 0);
- r5nav.addGraphNode(22, 23, 80, 180);
- r5nav.addGraphNode(23, 22, 80, 0);
- r5nav.addGraphNode(23, 24, 2, 270);
- r5nav.addGraphNode(24, 23, 2, 90);
-
- const uint16_t V1 = 7;
- const uint16_t V2 = 9;
- const uint16_t V3 = 12;
- const uint16_t V4 = 20;
- const uint16_t V5 = 22;
- const uint16_t V6 = 24;
- const uint16_t YELLOW_DROP_ZONE = 4;
- const uint16_t RED_DROP_ZONE = 2;
-
-/*
- r5nav.getShortestPath(1);
- pc.printf("\n\nDistance from 0 to 1: %i\n", r5nav.getMinDist(1) );
- pc.printf("Route:\n");
- r5nav.executeRoute(pc, drive);
- wait(0.1);
-
- r5nav.getShortestPath(2);
- pc.printf("\n\nDistance from 1 to 2: %i\n", r5nav.getMinDist(2) );
- pc.printf("Route:\n");
- r5nav.executeRoute(pc, drive);
- wait(0.1);
-
- r5nav.getShortestPath(3);
- pc.printf("\n\nDistance from 2 to 3: %i\n", r5nav.getMinDist(3) );
- pc.printf("Route:\n");
- r5nav.executeRoute(pc, drive);
- wait(0.1);
-
- r5nav.getShortestPath(0);
- pc.printf("\n\nDistance from 3 to 0: %i\n", r5nav.getMinDist(0) );
- pc.printf("Route:\n");
- r5nav.executeRoute(pc, drive);
- wait(0.1);
-*/
-
- /* Begin 1st peg retrieval */
-
- r5nav.getShortestPath(1);
- pc.printf("\n\nDistance from 0 to 1: %i\n", r5nav.getMinDist(1) );
- pc.printf("Route:\n");
- scanner.setLocalizeRightFlag(1);
- scanner.setLocalizeLeftFlag(0);
- r5nav.executeRoute(pc, drive);
- wait(0.1);
-
- r5nav.getShortestPath(3);
- pc.printf("\n\nDistance from 1 to 3: %i\n", r5nav.getMinDist(3) );
- pc.printf("Route:\n");
- scanner.setLocalizeRightFlag(0);
- scanner.setLocalizeLeftFlag(0);
- r5nav.executeRoute(pc, drive);
- wait(0.1);
-
- r5nav.getShortestPath(15);
- pc.printf("\n\nDistance from 3 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(V1);
- pc.printf("\n\nDistance from 15 to V1: %i\n", r5nav.getMinDist(V1) );
- pc.printf("Route:\n");
- scanner.setLocalizeRightFlag(1);
- scanner.setLocalizeLeftFlag(0);
- r5nav.executeRoute(pc, drive);
- wait(0.1);
-
- /* add hunt method call */
-
- r5nav.getShortestPath(15);
- pc.printf("\n\nDistance from V1 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(1);
- scanner.setLocalizeLeftFlag(0);
- 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);
- }
-
- 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);
- }
-
- /* 1st peg retrieval complete
- Begin 2nd peg retrieval */
-
- if (r5nav.getVertex() == YELLOW_DROP_ZONE)
- {
- r5nav.getShortestPath(3);
- pc.printf("\n\nDistance from YELLOW_DROP_ZONE to 3: %i\n", r5nav.getMinDist(3) );
- pc.printf("Route:\n");
- scanner.setLocalizeRightFlag(0);
- scanner.setLocalizeLeftFlag(1);
- r5nav.executeRoute(pc, drive);
- wait(0.1);
- }
+float distLocalL = 0.0;
- else
- {
- r5nav.getShortestPath(1);
- pc.printf("\n\nDistance from RED_DROP_ZONE to 1: %i\n", r5nav.getMinDist(1) );
- 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 1 to 3: %i\n", r5nav.getMinDist(3) );
- pc.printf("Route:\n");
- scanner.setLocalizeRightFlag(0);
- scanner.setLocalizeLeftFlag(0);
- r5nav.executeRoute(pc, drive);
- wait(0.1);
- }
-
- r5nav.getShortestPath(15);
- pc.printf("\n\nDistance from 3 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(5);
- pc.printf("\n\nDistance from 15 to 5: %i\n", r5nav.getMinDist(5) );
- pc.printf("Route:\n");
- scanner.setLocalizeRightFlag(1);
- scanner.setLocalizeLeftFlag(0);
- r5nav.executeRoute(pc, drive);
- wait(0.1);
-
- r5nav.getShortestPath(16);
- pc.printf("\n\nDistance from 5 to 16: %i\n", r5nav.getMinDist(16) );
- pc.printf("Route:\n");
- scanner.setLocalizeRightFlag(0);
- scanner.setLocalizeLeftFlag(0);
- r5nav.executeRoute(pc, drive);
- wait(0.1);
-
- r5nav.getShortestPath(V2);
- pc.printf("\n\nDistance from 16 to V2: %i\n", r5nav.getMinDist(V2) );
- pc.printf("Route:\n");
- scanner.setLocalizeRightFlag(1);
- scanner.setLocalizeLeftFlag(0);
- r5nav.executeRoute(pc, drive);
- wait(0.1);
-
- /* add hunt method call */
-
- r5nav.getShortestPath(16);
- pc.printf("\n\nDistance from V2 to 16: %i\n", r5nav.getMinDist(16) );
- 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 16 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(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)
- {
- 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);
- }
-
- 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);
- }
-
- /* 2nd peg retrieval complete
- Begin 3nd peg retrieval */
-
-
- if (r5nav.getVertex() == YELLOW_DROP_ZONE)
- {
- r5nav.getShortestPath(3);
- pc.printf("\n\nDistance from YELLOW_DROP_ZONE to 3: %i\n", r5nav.getMinDist(3) );
- pc.printf("Route:\n");
- scanner.setLocalizeRightFlag(0);
- scanner.setLocalizeLeftFlag(1);
- r5nav.executeRoute(pc, drive);
- wait(0.1);
- }
-
- else
- {
- r5nav.getShortestPath(1);
- pc.printf("\n\nDistance from RED_DROP_ZONE to 1: %i\n", r5nav.getMinDist(1) );
- 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 1 to 3: %i\n", r5nav.getMinDist(3) );
- pc.printf("Route:\n");
- scanner.setLocalizeRightFlag(0);
- scanner.setLocalizeLeftFlag(0);
- r5nav.executeRoute(pc, drive);
- wait(0.1);
- }
-
- r5nav.getShortestPath(15);
- pc.printf("\n\nDistance from 3 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(5);
- pc.printf("\n\nDistance from 15 to 5: %i\n", r5nav.getMinDist(5) );
- pc.printf("Route:\n");
- scanner.setLocalizeRightFlag(1);
- scanner.setLocalizeLeftFlag(0);
- r5nav.executeRoute(pc, drive);
- wait(0.1);
-
- r5nav.getShortestPath(19);
- pc.printf("\n\nDistance from 5 to 19: %i\n", r5nav.getMinDist(19) );
- pc.printf("Route:\n");
- scanner.setLocalizeRightFlag(0);
- scanner.setLocalizeLeftFlag(0);
- r5nav.executeRoute(pc, drive);
- wait(0.1);
-
- r5nav.getShortestPath(V3);
- pc.printf("\n\nDistance from 19 to V3: %i\n", r5nav.getMinDist(V3) );
- pc.printf("Route:\n");
- scanner.setLocalizeRightFlag(0);
- scanner.setLocalizeLeftFlag(1);
- r5nav.executeRoute(pc, drive);
- wait(0.1);
-
- /* add hunt method call */
-
- 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
-
- if (scanner.getObjectFound() == 1)
- {
- v3Flag = 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(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 */
- }
- }
-
- // 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(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);
-
- /* 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 */
- }
- }
-
- /* add more code for for V5 and V6 */
-
-/*
- drive.move(0, ((90.0)*(3.14159 / 180.0))); // return to initial angle
- // wait for move to complete
- while(!drive.isMoveDone())
- {
- wait(1e-6);
- }
-*/
- pc.printf("\n\nExercise Complete");
- return 0;
-}
+void reverseMove(StepperDrive &drive, float dist, float angle);
void activate()
{
@@ -729,3 +21,409 @@
led_green = 0;
active = true;
}
+
+int main()
+{
+ pc.baud(115200);
+ bluetooth.baud(9600); /* interface via Bluetooth at 9600 */
+ start.mode(PullUp); /* Button is active low needing PullUp */
+ start.fall(&activate);
+
+ led_red = 0;
+ led_green = 1;
+
+ LongRangeSensor longRangeL(bluetooth, PTB2);
+ LongRangeSensor longRangeR(bluetooth, PTB3);
+pc.printf("\nLong Range Sensors created");
+bluetooth.printf("\nLong Range Sensors created");
+
+ StepperDrive drive(bluetooth, PTE19, PTE18, 1, PTE3, PTE2, 0, 10.0625,
+ 8.4800, 650); // 8.375
+ /* (serial &, stepPinLeft, dirPinLeft, invertLeft,
+ stepPinRight, dirPinRight, invertRight, wheelCircum,
+ wheelSepar, periodUs) */
+pc.printf("\nStepperDrive created");
+bluetooth.printf("\nStepperDrive created");
+
+// Gripper gripper(PTE20, PTE21); // grip pin, wrist pin
+
+ Navigation r5map(bluetooth, drive, longRangeL, longRangeR, led_red, led_green, 61);
+pc.printf("\nNavigation created");
+bluetooth.printf("\nNavigation created");
+
+ //loading r5 map...
+ r5map.addGraphNode(0, 1, 6, 0);
+ r5map.addGraphNode(1, 0, 6, 180);
+ r5map.addGraphNode(1, 2, 7, 0);
+ r5map.addGraphNode(2, 1, 7, 180);
+ r5map.addGraphNode(2, 3, 8, 0);
+ r5map.addGraphNode(2, 12, 15, 90);
+ r5map.addGraphNode(3, 2, 8, 180);
+ r5map.addGraphNode(3, 4, 8, 0);
+ r5map.addGraphNode(4, 3, 8, 180);
+ r5map.addGraphNode(4, 5, 8, 0);
+ r5map.addGraphNode(5, 4, 8, 180);
+ r5map.addGraphNode(5, 6, 8, 0);
+ r5map.addGraphNode(6, 5, 8, 180);
+ r5map.addGraphNode(6, 7, 8, 0);
+ r5map.addGraphNode(7, 6, 8, 180);
+ r5map.addGraphNode(7, 8, 8, 0);
+ r5map.addGraphNode(8, 7, 8, 180);
+ r5map.addGraphNode(8, 9, 8, 0);
+ r5map.addGraphNode(9, 8, 8, 180);
+ r5map.addGraphNode(9, 10, 8, 0);
+ r5map.addGraphNode(10, 9, 8, 180);
+ r5map.addGraphNode(10, 11, 4, 0);
+ r5map.addGraphNode(11, 10, 4, 180);
+ r5map.addGraphNode(12, 2, 15, 270);
+ r5map.addGraphNode(12, 13, 8, 180);
+ r5map.addGraphNode(12, 14, 6, 0);
+ r5map.addGraphNode(13, 12, 8, 0);
+ r5map.addGraphNode(14, 12, 6, 180);
+ r5map.addGraphNode(14, 15, 7, 0);
+ r5map.addGraphNode(15, 14, 7, 180);
+ r5map.addGraphNode(15, 16, 7, 0);
+ r5map.addGraphNode(16, 15, 7, 180);
+ r5map.addGraphNode(16, 17, 3, 0);
+ r5map.addGraphNode(17, 16, 3, 180);
+ r5map.addGraphNode(17, 18, 8, 0);
+ r5map.addGraphNode(17, 30, 12, 90);
+ r5map.addGraphNode(18, 17, 8, 180);
+ r5map.addGraphNode(18, 19, 8, 0);
+ r5map.addGraphNode(19, 18, 8, 180);
+ r5map.addGraphNode(19, 20, 7, 0);
+ r5map.addGraphNode(20, 19, 7, 180);
+ r5map.addGraphNode(20, 21, 7, 0);
+ r5map.addGraphNode(21, 20, 7, 180);
+ r5map.addGraphNode(21, 22, 7, 0);
+ r5map.addGraphNode(22, 21, 7, 180);
+ r5map.addGraphNode(22, 23, 6, 0);
+ r5map.addGraphNode(23, 22, 6, 180);
+ r5map.addGraphNode(24, 22, 12, 270);
+ r5map.addGraphNode(24, 25, 6, 180);
+ r5map.addGraphNode(24, 42, 12, 90);
+ r5map.addGraphNode(25, 24, 6, 0);
+ r5map.addGraphNode(25, 26, 8, 180);
+ r5map.addGraphNode(26, 25, 7, 0);
+ r5map.addGraphNode(26, 27, 7, 180);
+ r5map.addGraphNode(27, 26, 7, 0);
+ r5map.addGraphNode(27, 28, 7, 180);
+ r5map.addGraphNode(28, 27, 7, 0);
+ r5map.addGraphNode(28, 29, 4, 180);
+ r5map.addGraphNode(29, 28, 4, 0);
+ r5map.addGraphNode(29, 30, 7, 180);
+ r5map.addGraphNode(30, 18, 12, 270);
+ r5map.addGraphNode(30, 29, 6, 0);
+ r5map.addGraphNode(30, 31, 6, 180);
+ r5map.addGraphNode(30, 36, 12, 90);
+ r5map.addGraphNode(31, 30, 6, 0);
+ r5map.addGraphNode(31, 32, 7, 180);
+ r5map.addGraphNode(32, 31, 7, 0);
+ r5map.addGraphNode(32, 33, 7, 180);
+ r5map.addGraphNode(33, 32, 7, 0);
+ r5map.addGraphNode(33, 34, 7, 180);
+ r5map.addGraphNode(34, 33, 7, 0);
+ r5map.addGraphNode(34, 35, 2, 180);
+ r5map.addGraphNode(35, 34, 2, 0);
+ r5map.addGraphNode(36, 30, 12, 270);
+ r5map.addGraphNode(36, 37, 6, 180);
+ r5map.addGraphNode(37, 36, 7, 0);
+ r5map.addGraphNode(37, 38, 7, 180);
+ r5map.addGraphNode(38, 37, 7, 0);
+ r5map.addGraphNode(38, 39, 7, 180);
+ r5map.addGraphNode(39, 38, 7, 0);
+ r5map.addGraphNode(39, 40, 7, 180);
+ r5map.addGraphNode(40, 39, 7, 0);
+ r5map.addGraphNode(40, 41, 2, 180);
+ r5map.addGraphNode(41, 40, 2, 0);
+ r5map.addGraphNode(42, 24, 12, 270);
+ r5map.addGraphNode(42, 43, 6, 0);
+ r5map.addGraphNode(43, 42, 6, 180);
+ r5map.addGraphNode(43, 44, 6, 0);
+ r5map.addGraphNode(44, 43, 6, 180);
+ r5map.addGraphNode(44, 45, 7, 90);
+ r5map.addGraphNode(45, 44, 7, 270);
+ r5map.addGraphNode(45, 46, 7, 90);
+ r5map.addGraphNode(46, 45, 7, 270);
+ r5map.addGraphNode(46, 47, 7, 90);
+ r5map.addGraphNode(47, 46, 7, 270);
+ r5map.addGraphNode(47, 48, 8, 90);
+ r5map.addGraphNode(48, 47, 8, 270);
+ r5map.addGraphNode(48, 49, 8, 90);
+ r5map.addGraphNode(49, 48, 8, 270);
+ r5map.addGraphNode(49, 50, 8, 90);
+ r5map.addGraphNode(50, 49, 8, 270);
+ r5map.addGraphNode(50, 51, 4, 180);
+ r5map.addGraphNode(51, 50, 4, 0);
+ r5map.addGraphNode(51, 52, 10, 180);
+ r5map.addGraphNode(52, 51, 10, 0);
+ r5map.addGraphNode(52, 53, 10, 180);
+ r5map.addGraphNode(53, 52, 10, 0);
+ r5map.addGraphNode(53, 54, 10, 180);
+ r5map.addGraphNode(54, 53, 10, 0);
+ r5map.addGraphNode(54, 55, 10, 180);
+ r5map.addGraphNode(55, 54, 10, 0);
+ r5map.addGraphNode(55, 56, 10, 180);
+ r5map.addGraphNode(56, 55, 10, 0);
+ r5map.addGraphNode(56, 57, 10, 180);
+ r5map.addGraphNode(57, 56, 10, 0);
+ r5map.addGraphNode(57, 55, 10, 180);
+ r5map.addGraphNode(58, 57, 10, 0);
+ r5map.addGraphNode(58, 59, 10, 180);
+ r5map.addGraphNode(59, 58, 10, 0);
+ r5map.addGraphNode(59, 60, 2, 270);
+ r5map.addGraphNode(60, 59, 2, 90);
+
+// gripper.lift();
+// gripper.release();
+
+ const uint8_t V1 = 23;
+ const uint8_t V2 = 35;
+ const uint8_t V3 = 41;
+ const uint8_t V4 = 47;
+ const uint8_t V5 = 51;
+ const uint8_t V6 = 60;
+ const uint8_t YELLOW_DROP_ZONE = 13;
+ const uint8_t RED_DROP_ZONE = 11;
+
+ pc.printf("\nWaiting for START BUTTON\n");
+ bluetooth.printf("\nWaiting for START BUTTON\n");
+ while(!active) // wait for start_button
+ {
+ wait(1e-6);
+ }
+
+ int target = V1;
+ r5map.getShortestPath(target);
+ pc.printf("\n\nDistance from %i to %i: %f\n", r5map.getVertex(), target, r5map.getMinDist(V1) );
+ bluetooth.printf("\n\nDistance from %i to %i: %f\n", r5map.getVertex(), target, r5map.getMinDist(V1) );
+ r5map.executeRoute();
+ wait(0.1);
+
+// gripper.lower();
+// gripper.grip();
+// gripper.lift();
+
+ /* back up to safe turn-around location */
+ pc.printf("\nBack up to safe turn around location");
+ bluetooth.printf("\nBack up to safe turn around location");
+ reverseMove(drive, 6.0, 0);
+ while(!drive.isMoveDone())
+ {
+ wait(1e-6);
+ }
+ r5map.setVertex(22);
+
+ target = YELLOW_DROP_ZONE;
+ r5map.getShortestPath(target);
+ pc.printf("\n\nDistance from %i to %i: %f\n", r5map.getVertex(), target, r5map.getMinDist(YELLOW_DROP_ZONE) );
+ bluetooth.printf("\n\nDistance from %i to %i: %f\n", r5map.getVertex(), target, r5map.getMinDist(YELLOW_DROP_ZONE) );
+ r5map.executeRoute();
+ wait(0.1);
+
+ /* back up to safe turn-around location */
+ pc.printf("\nBack up to a safe turn around location");
+ bluetooth.printf("\nBack up to a safe turn around location");
+ r5map.localizeLeftReverse();
+ reverseMove(drive, 6.0, 0);
+ // wait for move to complete
+ while(!drive.isMoveDone())
+ {
+ wait(1e-6);
+ }
+ r5map.setVertex(12);
+ r5map.localizeLeftReverse();
+
+ target = V2;
+ r5map.getShortestPath(target);
+ pc.printf("\n\nDistance from %i to %i: %f\n", r5map.getVertex(), target, r5map.getMinDist(V2) );
+ bluetooth.printf("\n\nDistance from %i to %i: %f\n", r5map.getVertex(), target, r5map.getMinDist(V2) );
+ r5map.executeRoute();
+ wait(0.1);
+
+ /* back up to safe turn-around location */
+ pc.printf("\nBack up to a safe turn around location");
+ bluetooth.printf("\nBack up to a safe turn around location");
+ r5map.localizeLeftReverse();
+ reverseMove(drive, 2.0, 0);
+ // wait for move to complete
+ while(!drive.isMoveDone())
+ {
+ wait(1e-6);
+ }
+ r5map.setVertex(34);
+ r5map.localizeLeftReverse();
+ reverseMove(drive, 7.0, 0);
+ // wait for move to complete
+ while(!drive.isMoveDone())
+ {
+ wait(1e-6);
+ }
+ r5map.setVertex(33);
+ r5map.localizeLeftReverse();
+ reverseMove(drive, 7.0, 0);
+ // wait for move to complete
+ while(!drive.isMoveDone())
+ {
+ wait(1e-6);
+ }
+ r5map.setVertex(32);
+ r5map.localizeLeftReverse();
+ reverseMove(drive, 7.0, 0);
+ // wait for move to complete
+ while(!drive.isMoveDone())
+ {
+ wait(1e-6);
+ }
+ r5map.setVertex(31);
+ r5map.localizeLeftReverse();
+ reverseMove(drive, 6.0, 0);
+ // wait for move to complete
+ while(!drive.isMoveDone())
+ {
+ wait(1e-6);
+ }
+ r5map.setVertex(30);
+ r5map.localizeLeftReverse();
+
+
+ target = RED_DROP_ZONE;
+ r5map.getShortestPath(target);
+ pc.printf("\n\nDistance from %i to %i: %f\n", r5map.getVertex(), target, r5map.getMinDist(RED_DROP_ZONE) );
+ bluetooth.printf("\n\nDistance from %i to %i: %f\n", r5map.getVertex(), target, r5map.getMinDist(RED_DROP_ZONE) );
+ r5map.executeRoute();
+ wait(0.1);
+
+ /* back up to safe turn around location
+ pc.printf("\nBack up to a safe turn around location");
+ bluetooth.printf("\nBack up to a safe turn around location");
+ r5map.localizeLeftReverse();
+ reverseMove(drive, 4.0, 0);
+ while(!drive.isMoveDone())
+ {
+ wait(1e-6);
+ }
+ r5map.setVertex(10);
+ r5map.localizeLeftReverse();
+ reverseMove(drive, 8.0, 0);
+ while(!drive.isMoveDone())
+ {
+ wait(1e-6);
+ }
+ r5map.setVertex(9);
+ r5map.localizeLeftReverse();
+ reverseMove(drive, 8.0, 0);
+ while(!drive.isMoveDone())
+ {
+ wait(1e-6);
+ }
+ r5map.setVertex(8);
+ r5map.localizeLeftReverse();
+ reverseMove(drive, 8.0, 0);
+ while(!drive.isMoveDone())
+ {
+ wait(1e-6);
+ }
+ r5map.setVertex(7);
+ r5map.localizeLeftReverse();
+ reverseMove(drive, 8.0, 0);
+ while(!drive.isMoveDone())
+ {
+ wait(1e-6);
+ }
+ r5map.setVertex(6);
+ r5map.localizeLeftReverse();
+ reverseMove(drive, 8.0, 0);
+ while(!drive.isMoveDone())
+ {
+ wait(1e-6);
+ }
+ r5map.setVertex(5);
+ r5map.localizeLeftReverse();
+ reverseMove(drive, 8.0, 0);
+ while(!drive.isMoveDone())
+ {
+ wait(1e-6);
+ }
+ r5map.setVertex(4);
+ r5map.localizeLeftReverse();
+ reverseMove(drive, 8.0, 0);
+ while(!drive.isMoveDone())
+ {
+ wait(1e-6);
+ }
+ r5map.setVertex(3);
+ r5map.localizeLeftReverse();
+ reverseMove(drive, 8.0, 0);
+ while(!drive.isMoveDone())
+ {
+ wait(1e-6);
+ }
+ r5map.setVertex(2);
+ r5map.localizeLeftReverse();*/
+
+ target = V3;
+ r5map.getShortestPath(target);
+ pc.printf("\n\nDistance from %i to %i: %f\n", r5map.getVertex(), target, r5map.getMinDist(V3) );
+ bluetooth.printf("\n\nDistance from %i to %i: %f\n", r5map.getVertex(), target, r5map.getMinDist(V3) );
+ r5map.executeRoute();
+ wait(0.1);
+
+ target = RED_DROP_ZONE;
+ r5map.getShortestPath(target);
+ pc.printf("\n\nDistance from %i to %i: %f\n", r5map.getVertex(), target, r5map.getMinDist(RED_DROP_ZONE) );
+ bluetooth.printf("\n\nDistance from %i to %i: %f\n", r5map.getVertex(), target, r5map.getMinDist(RED_DROP_ZONE) );
+ r5map.executeRoute();
+ wait(0.1);
+
+ target = V4;
+ r5map.getShortestPath(target);
+ pc.printf("\n\nDistance from %i to %i: %f\n", r5map.getVertex(), target, r5map.getMinDist(V4) );
+ bluetooth.printf("\n\nDistance from %i to %i: %f\n", r5map.getVertex(), target, r5map.getMinDist(V4) );
+ r5map.executeRoute();
+ wait(0.1);
+
+ target = YELLOW_DROP_ZONE;
+ r5map.getShortestPath(target);
+ pc.printf("\n\nDistance from %i to %i: %f\n", r5map.getVertex(), target, r5map.getMinDist(YELLOW_DROP_ZONE) );
+ bluetooth.printf("\n\nDistance from %i to %i: %f\n", r5map.getVertex(), target, r5map.getMinDist(YELLOW_DROP_ZONE) );
+ r5map.executeRoute();
+ wait(0.1);
+
+ target = 0;
+ r5map.getShortestPath(target);
+ pc.printf("\n\nDistance from %i to %i: %f\n", r5map.getVertex(), target, r5map.getMinDist(0) );
+ bluetooth.printf("\n\nDistance from %i to %i: %f\n", r5map.getVertex(), target, r5map.getMinDist(0) );
+ r5map.executeRoute();
+ wait(0.1);
+
+ pc.printf("\nExercise Complete");
+ bluetooth.printf("\nExercise Complete");
+} // end of main
+
+// FUNCTION:
+// void reverseMove(float dist, float angle)
+// IN-PARAMETERS:
+// dist (float), angle(float)
+// OUT-PARAMETERS:
+// None
+// DESCRIPTION:
+// Inverts values of invertLeft and invertRight and sends move
+// command to move robot in reverse direction.
+void reverseMove(StepperDrive &drive, float dist, float angle)
+{
+ // swap values for invertLeft and invertRight
+ drive.setInvertLeft(!drive.getInvertLeft());
+ drive.setInvertRight(!drive.getInvertRight());
+
+ drive.move(dist, angle);
+ // wait for move to complete
+ while(!drive.isMoveDone())
+ {
+ wait(1e-6);
+ }
+
+ // restore original values for invertLeft and invertRight
+ drive.setInvertLeft(!drive.getInvertLeft());
+ drive.setInvertRight(!drive.getInvertRight());
+}
\ No newline at end of file
