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:
- 32:82bfece56f8f
- Parent:
- 30:c07381708ffd
- Child:
- 35:4e09da30bda2
diff -r da9d13afbdf9 -r 82bfece56f8f r5driver.cpp --- a/r5driver.cpp Sat Mar 26 05:11:53 2016 +0000 +++ b/r5driver.cpp Sat Mar 26 05:26:23 2016 +0000 @@ -2,7 +2,7 @@ #include "Gripper.h" #include "scanner.h" #include "StepperDrive.h" -#include "VL6180x.h" +#include "ShortRangeSensor.h" #include "stdint.h" #include "mbed.h" @@ -31,11 +31,11 @@ wait(1e-6); } - StepperDrive drive(pc, PTE19, PTE18, 0, PTE3, PTE2, 1, 10.0625, 8.3750, 500); + StepperDrive drive(pc, PTE19, PTE18, 0, PTE3, PTE2, 1, 10.0625, 8.4800, 500); // 8.375 //(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 + ShortRangeSensor shortRangeL(PTE0, PTE1); // verify i2c pins + ShortRangeSensor shortRangeR(PTC9, PTC8); // verify i2c pins LongRangeSensor longRangeL(pc, PTB2); LongRangeSensor longRangeR(pc, PTB3); @@ -44,7 +44,7 @@ Navigation r5nav(scanner, 15); wait(0.1); - /* +/* r5nav.addGraphNode(0, 1, 10.0625, 0); r5nav.addGraphNode(1, 0, 10.0625, 180); r5nav.addGraphNode(1, 2, 10.0625, 270); @@ -54,43 +54,41 @@ 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(0, 1, 13, 0); + r5nav.addGraphNode(1, 0, 13, 180); + r5nav.addGraphNode(1, 2, 72, 356.5); // original angle = 0 + r5nav.addGraphNode(1, 3, 15, 90); r5nav.addGraphNode(2, 1, 72, 180); - r5nav.addGraphNode(3, 1, 13.5, 270); + r5nav.addGraphNode(3, 1, 15, 270); r5nav.addGraphNode(3, 4, 12, 180); - r5nav.addGraphNode(3, 5, 23, 0); + r5nav.addGraphNode(3, 5, 23, 356.5); // (-5 degrees) original angle = 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(5, 8, 12.5, 90); + r5nav.addGraphNode(6, 5, 37, 176.5); // original angle = 180 r5nav.addGraphNode(6, 7, 12, 0); + r5nav.addGraphNode(6, 10, 12.5, 90); 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, 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(6, 10, 12, 90); - r5nav.addGraphNode(10, 6, 12, 270); r5nav.addGraphNode(8, 11, 12, 90); + r5nav.addGraphNode(9, 8, 35, 0); + r5nav.addGraphNode(10, 6, 12.5, 270); 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) ); @@ -116,7 +114,7 @@ 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");