Code to drive Team 1's robot for the 2016 R5 robotics competition.
Dependencies: mbed navigation R5_StepperDrive LongRangeSensor DistanceSensor scanner Gripper ColorSensor
r5driver.cpp
- Committer:
- j_j205
- Date:
- 2016-04-08
- Revision:
- 42:1284491771ff
- Parent:
- 41:029135230ebf
- Child:
- 43:048c307bf8ac
File content as of revision 42:1284491771ff:
#include "mbed.h" #include "StepperDrive.h" #include "LongRangeSensor.h" #include "DistanceSensor.h" #include "navigation.h" #include "Gripper.h" Serial pc(USBTX, USBRX); Serial bluetooth(PTE16,PTE17); // bluetooth InterruptIn start(SW1); DigitalOut led_red(LED_RED); DigitalOut led_green(LED_GREEN); bool active = false; float distLocalL = 0.0; void reverseMove(StepperDrive &drive, float dist, float angle); void activate() { led_red = 1; 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; DistanceSensor shortRangeL(PTC2, 2); DistanceSensor shortRangeR(PTC1, 3); pc.printf("\nShort Range Sensors created"); bluetooth.printf("\nShort Range Sensors created"); 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.1875, 700); // 8.4800 /* (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.75, 0); r5map.addGraphNode(1, 0, 6.75, 180); r5map.addGraphNode(1, 2, 7, 0); r5map.addGraphNode(2, 1, 7, 180); r5map.addGraphNode(2, 3, 8, 0); r5map.addGraphNode(2, 12, 14.75, 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, 14.75, 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, 17, 11, 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, 7, 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; 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); // call to hunt // 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, 5.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, 8.0, 0); // wait for move to complete while(!drive.isMoveDone()) { wait(1e-6); } r5map.setVertex(12); r5map.localizeLeftReverse(); // turn 180 degrees to left pc.printf("\nTurn left 180 degrees"); bluetooth.printf("\nTurn left 180 degrees"); drive.move(0, -185.0*(3.14159/180)); // wait for move to complete while(!drive.isMoveDone()) { wait(1e-6); } r5map.setAngle(0); // move to compensate for coming up short pc.printf("\nCompensation move forward 2.0"); bluetooth.printf("\nCompensation move forward 2.0"); drive.move(2.0, 0); // wait for move to complete while(!drive.isMoveDone()) { wait(1e-6); } 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.newLocalizeLeftReverse(); reverseMove(drive, 2.0, 0); // wait for move to complete while(!drive.isMoveDone()) { wait(1e-6); } r5map.setVertex(34); r5map.newLocalizeLeftReverse(); reverseMove(drive, 7.0, 0); // wait for move to complete while(!drive.isMoveDone()) { wait(1e-6); } r5map.setVertex(33); r5map.newLocalizeLeftReverse(); reverseMove(drive, 7.0, 0); // wait for move to complete while(!drive.isMoveDone()) { wait(1e-6); } r5map.setVertex(32); r5map.newLocalizeLeftReverse(); reverseMove(drive, 7.0, 0); // wait for move to complete while(!drive.isMoveDone()) { wait(1e-6); } r5map.setVertex(31); r5map.newLocalizeLeftReverse(); reverseMove(drive, 6.0, 0); // wait for move to complete while(!drive.isMoveDone()) { wait(1e-6); } r5map.setVertex(30); r5map.newLocalizeLeftReverse(); 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()); }