![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
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-03-22
- Revision:
- 28:17c17157e728
- Parent:
- 26:d99d078921a0
- Child:
- 29:e77e8891d985
File content as of revision 28:17c17157e728:
#include "navigation.h" #include "scanner.h" #include "StepperDrive.h" #include "VL6180x.h" #include "stdint.h" #include "mbed.h" Serial pc(USBTX, USBRX); // Serial pc(PTE16,PTE17); // bluetooth InterruptIn start(SW1); DigitalOut led_red(LED_RED); DigitalOut led_green(LED_GREEN); bool active = false; 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.3750, 500); //(serial &, stepPinLeft, dirPinLeft, invertLeft, stepPinRight, dirPinRight, invertRight, wheelCircum, wheelSepar, periodUs) VL6180x shortRangeL(PTE0, PTE1, 0x52); // I2C1, 0x29 left-shifted by 1 bit VL6180x shortRangeR(PTC9, PTC8, 0x52); // I2C0, 0x29 left-shifted by 1 bit LongRangeSensor longRangeL(pc, PTB2); LongRangeSensor longRangeR(pc, PTB3); Scanner scanner(pc, PTB0, PTB1, shortRangeL, shortRangeR, longRangeL, longRangeR, 0.075); // (Serial &, PinName servoL, PinName servoR, shortRangeL, shortRangeR, longRangeL, longRangeR, period) Navigation r5nav(scanner, 11); 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, 18, 0); r5nav.addGraphNode(1, 0, 18, 180); r5nav.addGraphNode(1, 2, 72, 0); r5nav.addGraphNode(1, 3, 13.5, 90); r5nav.addGraphNode(2, 1, 72, 180); r5nav.addGraphNode(3, 1, 13.5, 270); r5nav.addGraphNode(3, 4, 12, 180); r5nav.addGraphNode(3, 5, 23, 0); r5nav.addGraphNode(4, 3, 12, 0); r5nav.addGraphNode(5, 3, 23, 180); r5nav.addGraphNode(5, 6, 37, 0); r5nav.addGraphNode(5, 8, 12, 90); r5nav.addGraphNode(6, 5, 37, 180); r5nav.addGraphNode(6, 7, 12, 0); r5nav.addGraphNode(7, 6, 12, 180); r5nav.addGraphNode(8, 5, 12, 270); r5nav.addGraphNode(8, 9, 35, 180); r5nav.addGraphNode(9, 8, 35, 0); r5nav.addGraphNode(8, 10, 37, 0); r5nav.addGraphNode(6, 10, 12, 90); r5nav.addGraphNode(10, 6, 12, 270); r5nav.addGraphNode(8, 11, 12, 90); r5nav.addGraphNode(11, 8, 12, 270); r5nav.addGraphNode(11, 12, 35, 180); r5nav.addGraphNode(12, 11, 35, 0); r5nav.addGraphNode(11, 13, 37, 0); r5nav.addGraphNode(13, 11, 37, 180); r5nav.addGraphNode(13, 14, 12, 0); r5nav.addGraphNode(14, 13, 12, 180); const uint16_t YELLOW_VICTIM = 7; const uint16_t RED_VICTIM = 9; 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); /* r5nav.getShortestPath(YELLOW_VICTIM); pc.printf("\n\nDistance from 0 to YELLOW_VICTIM: %i\n", r5nav.getMinDist(YELLOW_VICTIM) ); pc.printf("Route:\n"); //r5nav.printRoute(pc); //r5nav.setVertex(YELLOW_VICTIM) r5nav.executeRoute(pc, drive); wait(0.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); //r5nav.setVertex(YELLOW_DROP_ZONE); r5nav.executeRoute(pc, drive); wait(0.1); r5nav.getShortestPath(RED_VICTIM); pc.printf("\n\nDistance from %i to RED_VICTIM: %i\n", r5nav.getVertex(), r5nav.getMinDist(RED_VICTIM) ); pc.printf("Route:\n"); //r5nav.printRoute(pc); //r5nav.setVertex(RED_VICTIM); 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); //r5nav.setVertex(RED_DROP_ZONE); r5nav.executeRoute(pc, drive); wait(0.1); */ 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 activate() { led_red = 1; led_green = 0; active = true; }