Diff: Controller.cpp

Revision:

8:c6345e8d607c

Parent:

6:75263c93daf7

--- a/Controller.cpp	Sun Jun 07 21:59:02 2015 +0000
+++ b/Controller.cpp	Thu Jun 11 20:59:22 2015 +0000
@@ -1,80 +1,135 @@
 #include "Controller.h"
+#include "InterruptManager.h"
 
+InterruptIn   imu(p16);
 InterruptIn   button(p23);
-InterruptIn   imu(p16);
-Serial pc(USBTX, USBRX);
+
+
+//Ticker ticker;
+//Ticker tickerData;
 
 Controller::Controller()
 {
-    this->sensors = new Sensors();
-    this->storage = new Storage();
-    
-    this->modeList.push_back(new TrainingCommand(sensors));
-    this->modeList.push_back(new OfflineCommand(sensors, storage));
-    this->modeList.push_back(new ReadCommand(storage));
-    this->modeList.push_back(new IdleCommand(sensors));
-    this->modeList.push_back(new SleepCommand(sensors));
+    buttonIntFlag = false;
+    imuIntFlag = false;
+    tickerIntFlag = false;
+    currentModeSelector = 3;
+    tickerIntDataFlag = false;
+
+    angle = 0;
+    peak = false;
+
+    sensors = new Sensors();
+    storage = new Storage();
+    gonioService = new GonioService();
+
+    //Set up IMU with sample frequency
+    sensors->setupIMU(0x4f);
+
+    modeList.push_back(new TrainingCommand(sensors, gonioService));
+    modeList.push_back(new OfflineCommand(sensors, storage));
+    modeList.push_back(new ReadCommand(storage, gonioService));
+    modeList.push_back(new IdleCommand(sensors, gonioService));
+    modeList.push_back(new SleepCommand(sensors));
+
+    wait(0.01);
+
+    //sensors->disableIMU();
 
     button.fall(this, &Controller::buttonInt);
     imu.fall(this, &Controller::imuInt);
 
     //Set idle command at startup
-    this->currentMode = 3;
-    
-    sensors->setupIMU();
-    sensors->disableIMU();
-    wait(0.1);
-    this->modeList[currentMode]->initialize();
-    
+    currentMode()->initialize();
 }
 
 void Controller::setCommand(int n)
 {
-    this->modeList[currentMode]->finish();
-    this->currentMode = n;
-    this->modeList[currentMode]->initialize();
+    currentMode()->finish();
+    currentModeSelector = n;
+    currentMode()->initialize();
+}
+
+Command* Controller::currentMode()
+{
+    return modeList[currentModeSelector];
 }
 
 void Controller::imuInt()
 {
-    this->modeList[currentMode]->execute();
+     
+     imuIntFlag = true;
 }
 
 void Controller::buttonInt()
 {
-    wait(0.25);
-    this->modeList[currentMode]->button();
+    wait(0.25); //wait for debounce
+    buttonIntFlag = true;
+}
+
+void Controller::tickerInt()
+{
+    tickerIntFlag = true;
+}
+
+void Controller::tickerDataInt()
+{
+    tickerIntDataFlag = true;
 }
 
 void Controller::run()
 {
-    pc.baud(19200);
+    /*
+    ticker.attach(this, &Controller::tickerInt, 0.1);
+    wait(0.05);
+    tickerData.attach(this, &Controller::tickerDataInt, 0.1);
+    */
+
 
     while(1) {
-
-        if(pc.readable()) {
-
-            char command = pc.getc();
-
+        
+        if(gonioService->isConnected() & gonioService->newValue()){
+            
+            int8_t command = gonioService->getWriteValue() - 1;
+            printf("%d", command);
             switch(command) {
-                case '1':
+                case 0 :
+                    printf("een\n");
                     setCommand(0);
                     break;
-                case '2':
+                case 1 :
+                    printf("twee\n");
+                    gonioService->disconnect();
                     setCommand(1);
                     break;
-                case '3':
+                case 2 :
+                    printf("drie\n");
                     setCommand(2);
                     break;
-                case '4':
+                case 3 :
+                    printf("vier\n");
                     setCommand(3);
                     break;
-                case '5':
-                    setCommand(4);
-                    break;
+                }
             }
+        
+        
+        if(buttonIntFlag) {
+            buttonIntFlag = false;
+            currentMode()->button();
+        }
+
+        if(tickerIntDataFlag) {
+            tickerIntDataFlag = false;
+        }
+
+        if(tickerIntFlag) {
+            tickerIntFlag = false;
+        }
+
+        if(imuIntFlag) {
+            imuIntFlag = false;
+            currentMode()->execute();
         }
     }
-
-
 }
\ No newline at end of file