Diff: Controller.cpp

Revision:

5:46947b447701

Parent:

4:2a5a08b14539

Child:

6:75263c93daf7

--- a/Controller.cpp	Wed Jun 03 15:10:45 2015 +0000
+++ b/Controller.cpp	Sun Jun 07 14:11:26 2015 +0000
@@ -2,37 +2,87 @@
 
 InterruptIn   button(p23);
 InterruptIn   imu(p16);
+Serial pc(USBTX, USBRX);
 
-Controller::Controller(){
+Controller::Controller()
+{
     this->modeList.push_back(new TrainingCommand());
-    this->modeList.push_back(new CompetitionCommand());
+    this->modeList.push_back(new OfflineCommand());
     this->modeList.push_back(new ReadCommand());
     this->modeList.push_back(new IdleCommand());
     this->modeList.push_back(new SleepCommand());
-    
+
     button.fall(this, &Controller::buttonInt);
     imu.fall(this, &Controller::imuInt);
-    
+
+    //Set idle command at startup
     this->currentMode = 3;
-    this->modeList[currentMode]->initialize();
+
+    while(Sensors::getInstance()->getFs() != 100) {
+        Sensors::getInstance()->setupIMU();
+        wait(0.01);
+        Sensors::getInstance()->disableIMU();
     }
 
-//FOR GONIOTRINER CLASS
-Command* Controller::getController(int i){
-    return this->modeList[i];
-    }
-    
-void Controller::setCommand(int n){
+    this->modeList[currentMode]->initialize();
+}
+
+void Controller::setCommand(int n)
+{
     this->modeList[currentMode]->finish();
     this->currentMode = n;
     this->modeList[currentMode]->initialize();
-    }
-    
-void Controller::imuInt(){
+}
+
+void Controller::imuInt()
+{
+    this->modeList[currentMode]->execute();
+}
+
+void Controller::buttonInt()
+{
     wait(0.25);
-    this->modeList[currentMode]->execute();
+    this->modeList[currentMode]->button();
+}
+
+void Controller::run()
+{
+    Sensors::getInstance()->setupIMU();
+    Sensors::getInstance()->disableIMU();
+
+    pc.baud(19200);
+
+    while(1) {
+
+        if(pc.readable()) {
+
+            char command = pc.getc();
+
+            switch(command) {
+                case '1':
+                    pc.printf("een\n");
+                    setCommand(0);
+
+                    break;
+                case '2':
+                    pc.printf("twee\n");
+                    setCommand(1);
+                    break;
+                case '3':
+                    pc.printf("drie\n");
+                    setCommand(2);
+                    break;
+                case '4':
+                    pc.printf("vier\n");
+                    setCommand(3);
+                    break;
+                case '5':
+                    pc.printf("vijf\n");
+                    setCommand(4);
+                    break;
+            }
+        }
     }
 
-void Controller::buttonInt(){
-    this->modeList[currentMode]->button();
-    }
\ No newline at end of file
+
+}
\ No newline at end of file