Diff: commands.h

Revision:

51:bab6e5eb864a

Parent:

49:731c95cd5852

Child:

56:389436192e63

--- a/commands.h	Fri Mar 24 16:57:35 2017 +0000
+++ b/commands.h	Fri Mar 24 17:35:36 2017 +0000
@@ -2,38 +2,66 @@
 
 #include "mbed.h"
 #include "controlLoops.h"
+#include "parser.h"
 
 Ticker lifeTicker;
 
 volatile bool commandFinished = false;
+
 void stopCommand() {
     commandFinished = true;
     lifeTicker.detach();
     controlTicker.detach();
 }
 
-void rotateWith(float r, float v) {
-
+void rotate(float r) {
+    PRINT("Rotating for %.2f revolutions", goalRevs);
+    goalRevs = r;
 }
 
 void setVelocity(float v) {
-//    targetV = v;
-//    goalRevs = 900.0f;
-//    print("Spinning with V=%.2f\n\r", targetV);
+    targetV = v;
+    PRINT("Spinning with V=%.2f rad/s", targetV);
 
     lifeTicker.attach(&stopCommand, 10);
 }
 
+void rotateWith(float r, float v) {
+    goalRevs = r;
+    targetV = v;
+    PRINT("Rotating for %.2f revolutions with V=%.2f", goalRevs, targetV);
+}
+
 void playTune(float freq) {
     motorPWMPeriod = 1.0f / freq;
-//    motorOut(0, 0.5);
+    motorOut(0, 0.5);
     Thread::wait(1000);
 }
 
 void playTunes(const vector<float>& tunes) {
+    PRINT("Playing music");
     for (int i=0; i<tunes.size(); ++i) {
         playTune(tunes[i]);
     }
 //    motorPWMPeriod = defaultMotorPWMPeriod;
     stopMotor();
+}
+
+
+void runCommand() {
+    char buffer[256];
+    PRINT("Ready for command");
+    scanf("%s", buffer);
+    ParseResult command = parse((char*) buffer);
+    if (command.success) {
+        switch(command.mode) {
+            case 0: rotateWith(command.rotations, command.velocity); break;
+            case 1: rotate(command.rotations); break;
+            case 2: setVelocity(command.velocity); break;
+            case 3: playTunes(command.tunes); break;
+            default: return;
+        }
+    } else {
+        PRINT("Invalid command");
+    }
 }
\ No newline at end of file