Texas State IEEE / Mbed 2 deprecated R5 2016 Robotics Team 1

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:
19:4abc62759078
Parent:
15:d8940756d5d5
--- a/r5driver.cpp	Wed Feb 17 16:51:52 2016 +0000
+++ b/r5driver.cpp	Tue Feb 23 23:30:55 2016 +0000
@@ -36,7 +36,7 @@
     r5nav.addGraphNode(0, 3, 9.875, 270);
 
     /*
-    // loading map
+    // loading r5 map
     r5nav.addGraphNode(0, 1, 18, 0);
     r5nav.addGraphNode(1, 0, 18, 180);
     r5nav.addGraphNode(1, 2, 72, 0);
@@ -63,12 +63,18 @@
     */
     StepperDrive drive(pc, PTE19, PTE18, 0, PTE3, PTE2, 1, 9.875, 6.4375, 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
+    Gp2x longRangeL(PTB2);
+    Gp2x longRangeR(PTB3);
+    
+    Scanner (pc, PTB0, PTB1, shortRangeL, shortRangeR, longRangeL, longRangeR, 0.1);
+        // (Serial &, PinName servoL, PinName servoR, shortRangeL, shortRangeR, longRangeL, longRangeR, period)
+    
     pc.printf("Size of graph is %i\n", r5nav.graphSize() );
     wait(0.1);
     
-
-
     pc.printf("\nWaiting for START BUTTON\n");
     while(!active) // wait for start_button
     {