Diff: main.cpp

Revision:

43:8b6b4040e635

Parent:

42:2ee563cd6223

Child:

44:0b92f72641d7

--- a/main.cpp	Fri Mar 17 23:17:42 2017 +0000
+++ b/main.cpp	Sat Mar 18 15:59:37 2017 +0000
@@ -1,14 +1,43 @@
+#include <cmath>
+
 #include "mbed.h"
 #include "rtos.h"
+
 #include "definitions.h"
 #include "motorControl.h"
 #include "parser.h"
-#include <cmath>
+
+Mutex mutex;
 
 volatile float w3 = 0;                  //Angular velocity
 volatile float duty = 0.5;
 volatile int count_i3 = 0;
 
+volatile char userInput[256];
+volatile bool commandReady = false;
+ParseResult parseResult;
+volatile bool readyForCommand = true;
+
+void rotateWith(float r, float v) {}
+void rotate(float r) {}
+void setVelocity(float v) {}
+void playTunes(const vector<float>& tunes) {}
+
+void serialThread() {
+  while(true) {
+    if (readyForCommand) {
+      scanf("%s", userInput);
+      ParseResult curr = parse((char *) userInput);
+      if (curr.success) {
+        mutex.lock();
+        commandReady = true;
+        parseResult = curr;
+        mutex.unlock();
+      }
+    }
+  }
+}
+
 volatile int CHA_state = 0x00;
 volatile int CHB_state = 0x00;
 volatile int CH_state = 0x00;
@@ -122,7 +151,11 @@
   updateDiskPosition();
 }
 
+void runCommand(const ParseResult& command);
+
 int main() {
+    Thread serialInput;
+    serialInput.start(callback(serialThread));
     motorHome();                        //Initialise motor before any interrupt
     
     dt_I3.start();                      //Start the time counters for velocity
@@ -147,7 +180,27 @@
     
     while (count_i3 <= goalRevs+1) {
         pc.printf("Speed: %f, duty cycle: %f, revs done: %d, dError: %f , currentError: %f, prevError: %f, currentRevs: %f \n\r",w3, duty, count_i3, dError, currentError, prevError, currentRevs);
+    // Work with user input here
+      if (commandReady) {
+        printf("Got command: %d\n\r", parseResult.mode);
+        commandReady = false;
+        readyForCommand = false;
+        runCommand(parseResult);
+        readyForCommand = true;
+      }
     }
     stopMotor();                        //Stop the motor if position is reached
     return 0;
-}
\ No newline at end of file
+}
+
+void runCommand(const ParseResult& command) {
+  int mode = command.mode;
+  switch(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;
+  }
+}
+