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

Files at this revision

API Documentation at this revision

Revision 45:9a7ddd922706, committed 2016-04-09

Comitter:
Hypna
Date:
Sat Apr 09 11:01:18 2016 +0000
Parent:
44:d4207182bfc2
Commit message:
Added some more debug

Changed in this revision

ColorSensor.lib Show annotated file Show diff for this revision Revisions of this file
r5driver.cpp Show annotated file Show diff for this revision Revisions of this file 
diff -r d4207182bfc2 -r 9a7ddd922706 ColorSensor.lib
--- a/ColorSensor.lib	Sat Apr 09 03:29:24 2016 +0000
+++ b/ColorSensor.lib	Sat Apr 09 11:01:18 2016 +0000
@@ -1,1 +1,1 @@
-http://mbed.org/teams/Texas-State-IEEE-Robotics/code/ColorSensor/#42c0a779b199
+http://mbed.org/teams/Texas-State-IEEE-Robotics/code/ColorSensor/#b4139bd2715f

diff -r d4207182bfc2 -r 9a7ddd922706 r5driver.cpp
--- a/r5driver.cpp	Sat Apr 09 03:29:24 2016 +0000
+++ b/r5driver.cpp	Sat Apr 09 11:01:18 2016 +0000
@@ -4,7 +4,6 @@
 #include "navigation.h"
 #include "Gripper.h"
 
-
 Serial pc(USBTX, USBRX);
 Serial bluetooth(PTE16,PTE17); // bluetooth
 InterruptIn start(SW1);
@@ -21,6 +20,8 @@
     led_red = 1;
     led_green = 0;
     active = true;
+    pc.printf("Activated\n\r");
+    bluetooth.printf("Activated\n\r");
 }
 
 int main() 
@@ -35,22 +36,22 @@
     
     LongRangeSensor longRangeL(bluetooth, PTB2);
     LongRangeSensor longRangeR(bluetooth, PTB3);
-pc.printf("\nLong Range Sensors created");
-bluetooth.printf("\nLong Range Sensors created");
+    pc.printf("\nLong Range Sensors created");
+    bluetooth.printf("\nLong Range Sensors created");
 
     StepperDrive drive(bluetooth, PTE19, PTE18, 1, PTE3, PTE2, PTE22, 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");
+    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");
+    pc.printf("\nNavigation created");
+    bluetooth.printf("\nNavigation created");
     
     //loading r5 map...
     r5map.addGraphNode(0, 1, 6.75, 0);
@@ -175,8 +176,14 @@
     r5map.addGraphNode(59, 60, 2, 270);
     r5map.addGraphNode(60, 59, 2, 90);
     
-//    gripper.lift();
-//    gripper.release();
+    pc.printf("Map loaded\n\r");
+    bluetooth.printf("Map loaded\n\r");
+    
+    gripper.lift();
+    gripper.release();
+    
+    pc.printf("Gripper positioned\n\r");
+    bluetooth.printf("Gripper positioned\n\r");
     
     const uint8_t V1 = 23;
     const uint8_t V2 = 35;
@@ -189,6 +196,7 @@
               
     pc.printf("\nWaiting for START BUTTON\n");
     bluetooth.printf("\nWaiting for START BUTTON\n");
+    
     while(!active) // wait for start_button
     {
         wait(1e-6);