Diff: r5driver.cpp

Revision:

45:9a7ddd922706

Parent:

44:d4207182bfc2

--- 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);