Diff: robot.cpp

Revision:

18:501f1007a572

Parent:

17:47e107f9587b

Child:

19:05b8123905fb

--- a/robot.cpp	Sun Apr 03 19:00:21 2016 +0000
+++ b/robot.cpp	Thu Apr 07 13:19:29 2016 +0000
@@ -81,6 +81,7 @@
 
 void AutoTrack(void) {
     char key;
+    bool isReady;
     motorPeriodicInt.attach(&MotorISR, MOTOR_PERIOD);
     sensorPeriodicInt.attach(&NavigationISR, NAVIGATION_PERIOD);
     
@@ -89,31 +90,52 @@
         if (pc.readable()) {
             key = pc.getc();
             if (key == 'q') {
-                 ShutDown();
-                 return;
+                ShutDown();
+                return;
+            } else if ((key == '1')||(key == 'm')) {
+                ShutDown();
+                ManualControl();
+            } else if (key == '~') {
+                isReady = true;   
             }
         }
-        pc.printf("X Coordinate: %d, Height: %d \r\n", x, height);
-        pc.printf("Speed Set: %f, Steering Set: %f \r\n", speed, steering);
-        pc.printf("Left Motor Set: %f, Right Motor Set: %f \n\r\r\n\r\n", leftMotor, rightMotor);
-        Thread::wait(500); // Go to sleep for 500 ms
+        
+        if (isReady) {
+            pc.printf("X: %d, Height: %d \r\n", x, height);
+            pc.printf("Speed: %f \r\nSteering: %f \r\n", speed, steering);
+            isReady = false;  
+            pc.putc('~');
+        }
+        Thread::wait(250);
     }
 }
 
 void ManualControl(void) {
+    bool isReady;
+    char key;
     motorPeriodicInt.attach(&MotorISR, MOTOR_PERIOD);
     float speedRate, steeringRate;
     
     while (1) {
         osTimerStart(oneShotId, 2000); // Start or restart the watchdog timer interrupt and set to  2000ms.
         if (pc.readable()) {
+            key = pc.getc();
+            if (key == 'q') {
+                ShutDown();
+                return;
+            } else if ((key == '0')||(key == 'a')) {
+                ShutDown();
+                AutoTrack();
+            } else if (key == '~') {
+                isReady = true;   
+            } 
+        }
+        
+        if (isReady) {
             pc.scanf("%f :: %f", &speedRate, &steeringRate); 
-            //flushBuffer()
-
             speed = SPEED_MAX * speedRate;
-            steering = (SPEED_MAX * steeringRate)/3; 
-            pc.printf("speed: %f, steering: %f \r\n", speedRate, steeringRate);  
-            
+            steering = (SPEED_MAX * steeringRate);    
+            isReady = false;
             pc.putc('~'); 
         }