Diff: main.cpp

Revision:

0:64c100cd152a

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Oct 31 10:47:46 2018 +0000
@@ -0,0 +1,166 @@
+#include "mbed.h"
+#include "MODSERIAL.h"
+
+DigitalOut ledr(LED1);
+DigitalOut ledg(LED2);
+DigitalOut ledb(LED3);
+DigitalIn button1(SW2);
+DigitalIn button2(SW3);
+MODSERIAL pc(USBTX, USBRX);
+enum states{WAITING, EMG_CAL, BCM, FAIL};
+states CurrentState = WAITING;
+bool StateChanged = true;
+volatile int x = 0;
+bool failure = false;
+
+void ProcessStateMachine(void)
+{
+    switch(CurrentState)
+    {
+        case WAITING: //This state is used to only start calibrating upon user input
+            //State initialization
+            if(StateChanged) 
+            {
+                pc.printf("Entering WAITING\r\n");
+                ledr = 0;
+                ledg = 1;
+                ledb = 1;
+                StateChanged = false;
+            }
+            //State actions
+            pc.printf("Still WAITING\r\n"); 
+            while(button1)
+            {
+            }
+            //State transition  
+            CurrentState = EMG_CAL;
+            StateChanged = true;
+            break; // End of WAITING
+        case EMG_CAL: //State for calibrating EMG channels
+            //State initialization
+            if(StateChanged) 
+            {
+                pc.printf("Entering EMG calibration\r\n");
+                x = 0;
+                ledr = 1;
+                ledg = 0;
+                ledb = 1;
+                StateChanged = false;
+            }
+            //State actions
+            pc.printf("EMG calibration starting\r\n");
+            wait(0.3f); //Dit moet nog veranderd worden naar timer die opneemt hoe lang hij in WAITING zit.
+            // calibratiestappen voor de emg hier invoegen
+            while(x < 4 && !failure)
+            {
+                if(x == 1)
+                {
+                    //Functie die het eerste EMG signaal calibreert
+                }
+                if(x == 2)
+                {
+                    //Functie die het tweede  EMG signaal calibreert
+                }
+                if(x == 3)
+                {
+                    //Functie die het derde EMG signaal calibreert
+                }
+                if(!button2)
+                {
+                    failure = true;
+                }
+                if(!button1)
+                {
+                    x++;
+                    pc.printf("EMG calibration %i is now running\r\n", x);
+                    wait(0.2f);
+                }
+            }
+            //State transition
+            if(failure)
+            {
+                CurrentState = FAIL;
+                StateChanged = true;
+            }
+            else
+            {
+                CurrentState = BCM;
+                StateChanged = true;
+            }
+            break; //End of EMG_CAL state
+        case BCM: //State for moving the robot
+            //State initialization
+            if(StateChanged)
+            {
+                pc.printf("Entering Basic Control Mode\r\n");
+                ledr = 1;
+                ledg = 1;
+                ledb = 0;
+                StateChanged = false;
+            }
+            //State actions
+            pc.printf("Basic Control Mode is active\r\n");
+            while(button1 && !failure)
+            {
+                if(!button2)
+                {
+                    failure = true;
+                }
+            }
+            //State transition
+            if(failure)
+            {
+                pc.printf("Go to fail mode\r\n");
+                CurrentState = FAIL;
+                StateChanged = true;
+            }
+            else
+            {
+                CurrentState = WAITING;
+                StateChanged = true;
+            }
+            break; //End of BCM state
+        case FAIL: //Fail state
+            //State initialization
+            if(StateChanged)
+            {
+                pc.printf("Fail mode is entered\r\n");
+                failure = false;
+                StateChanged = false;
+            }
+            //State actions
+            pc.printf("Fail mode is active. Press button1 to enter WAITING\r\n");
+            while(button1)
+            {
+            }
+            //State transition
+            CurrentState = WAITING;
+            StateChanged = true;
+            break; //End of FAIL state
+        default: //Emergency state for when something unexpected happens
+            //motoren uit enzo
+            pc.printf("SOS SOS SOS\r\n");
+            ledr = 0;
+            ledg = 0;
+            ledb = 0;
+            while(button1)
+            {
+                ledr = !ledr;
+                wait(0.3f);
+            }
+            CurrentState = WAITING;
+            StateChanged = true;
+    }
+}
+
+int main() 
+{
+    pc.baud(115200);
+    ledr = 1;
+    ledg = 1;
+    ledb = 1;
+    while (true) 
+    {
+        ProcessStateMachine(); //Activates state machine
+    }
+}
\ No newline at end of file