Marijke Zondag
/
statemachineopzet
..
Fork of statemachineopzet by
Diff: main.cpp
- Revision:
- 0:64c100cd152a
- Child:
- 1:1541f49d8680
- Child:
- 3:8be18839d069
diff -r 000000000000 -r 64c100cd152a main.cpp --- /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