Marijke Zondag / Mbed OS statemachineopzet

..

Dependencies:   MODSERIAL

Fork of statemachineopzet by Onno Derkman

main.cpp@3:8be18839d069, 2018-11-06 (annotated)

Committer:
MarijkeZondag
Date:
Tue Nov 06 14:40:53 2018 +0000
Revision:
3:8be18839d069
Parent:
0:64c100cd152a 
final statemachine opzet;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
onnoderkman 0:64c100cd152a 1 #include "mbed.h"
onnoderkman 0:64c100cd152a 2 #include "MODSERIAL.h"
onnoderkman 0:64c100cd152a 3
onnoderkman 0:64c100cd152a 4 DigitalOut ledr(LED1);
onnoderkman 0:64c100cd152a 5 DigitalOut ledg(LED2);
onnoderkman 0:64c100cd152a 6 DigitalOut ledb(LED3);
onnoderkman 0:64c100cd152a 7 DigitalIn button1(SW2);
onnoderkman 0:64c100cd152a 8 DigitalIn button2(SW3);
onnoderkman 0:64c100cd152a 9 MODSERIAL pc(USBTX, USBRX);
onnoderkman 0:64c100cd152a 10 enum states{WAITING, EMG_CAL, BCM, FAIL};
onnoderkman 0:64c100cd152a 11 states CurrentState = WAITING;
onnoderkman 0:64c100cd152a 12 bool StateChanged = true;
onnoderkman 0:64c100cd152a 13 volatile int x = 0;
onnoderkman 0:64c100cd152a 14 bool failure = false;
onnoderkman 0:64c100cd152a 15
onnoderkman 0:64c100cd152a 16 void ProcessStateMachine(void)
onnoderkman 0:64c100cd152a 17 {
onnoderkman 0:64c100cd152a 18 switch(CurrentState)
onnoderkman 0:64c100cd152a 19 {
onnoderkman 0:64c100cd152a 20 case WAITING: //This state is used to only start calibrating upon user input
onnoderkman 0:64c100cd152a 21 //State initialization
onnoderkman 0:64c100cd152a 22 if(StateChanged)
onnoderkman 0:64c100cd152a 23 {
onnoderkman 0:64c100cd152a 24 pc.printf("Entering WAITING\r\n");
onnoderkman 0:64c100cd152a 25 ledr = 0;
onnoderkman 0:64c100cd152a 26 ledg = 1;
onnoderkman 0:64c100cd152a 27 ledb = 1;
onnoderkman 0:64c100cd152a 28 StateChanged = false;
onnoderkman 0:64c100cd152a 29 }
onnoderkman 0:64c100cd152a 30 //State actions
MarijkeZondag 3:8be18839d069 31 pc.printf("Still WAITING\r\n");
MarijkeZondag 3:8be18839d069 32 wait(1.0f);
onnoderkman 0:64c100cd152a 33 while(button1)
onnoderkman 0:64c100cd152a 34 {
onnoderkman 0:64c100cd152a 35 }
onnoderkman 0:64c100cd152a 36 //State transition
onnoderkman 0:64c100cd152a 37 CurrentState = EMG_CAL;
onnoderkman 0:64c100cd152a 38 StateChanged = true;
onnoderkman 0:64c100cd152a 39 break; // End of WAITING
onnoderkman 0:64c100cd152a 40 case EMG_CAL: //State for calibrating EMG channels
onnoderkman 0:64c100cd152a 41 //State initialization
onnoderkman 0:64c100cd152a 42 if(StateChanged)
onnoderkman 0:64c100cd152a 43 {
onnoderkman 0:64c100cd152a 44 pc.printf("Entering EMG calibration\r\n");
onnoderkman 0:64c100cd152a 45 x = 0;
onnoderkman 0:64c100cd152a 46 ledr = 1;
onnoderkman 0:64c100cd152a 47 ledg = 0;
onnoderkman 0:64c100cd152a 48 ledb = 1;
onnoderkman 0:64c100cd152a 49 StateChanged = false;
onnoderkman 0:64c100cd152a 50 }
onnoderkman 0:64c100cd152a 51 //State actions
onnoderkman 0:64c100cd152a 52 pc.printf("EMG calibration starting\r\n");
onnoderkman 0:64c100cd152a 53 wait(0.3f); //Dit moet nog veranderd worden naar timer die opneemt hoe lang hij in WAITING zit.
onnoderkman 0:64c100cd152a 54 // calibratiestappen voor de emg hier invoegen
onnoderkman 0:64c100cd152a 55 while(x < 4 && !failure)
onnoderkman 0:64c100cd152a 56 {
onnoderkman 0:64c100cd152a 57 if(x == 1)
onnoderkman 0:64c100cd152a 58 {
onnoderkman 0:64c100cd152a 59 //Functie die het eerste EMG signaal calibreert
onnoderkman 0:64c100cd152a 60 }
onnoderkman 0:64c100cd152a 61 if(x == 2)
onnoderkman 0:64c100cd152a 62 {
onnoderkman 0:64c100cd152a 63 //Functie die het tweede EMG signaal calibreert
onnoderkman 0:64c100cd152a 64 }
onnoderkman 0:64c100cd152a 65 if(x == 3)
onnoderkman 0:64c100cd152a 66 {
onnoderkman 0:64c100cd152a 67 //Functie die het derde EMG signaal calibreert
onnoderkman 0:64c100cd152a 68 }
onnoderkman 0:64c100cd152a 69 if(!button2)
onnoderkman 0:64c100cd152a 70 {
onnoderkman 0:64c100cd152a 71 failure = true;
onnoderkman 0:64c100cd152a 72 }
onnoderkman 0:64c100cd152a 73 if(!button1)
onnoderkman 0:64c100cd152a 74 {
onnoderkman 0:64c100cd152a 75 x++;
onnoderkman 0:64c100cd152a 76 pc.printf("EMG calibration %i is now running\r\n", x);
onnoderkman 0:64c100cd152a 77 wait(0.2f);
onnoderkman 0:64c100cd152a 78 }
onnoderkman 0:64c100cd152a 79 }
onnoderkman 0:64c100cd152a 80 //State transition
onnoderkman 0:64c100cd152a 81 if(failure)
onnoderkman 0:64c100cd152a 82 {
onnoderkman 0:64c100cd152a 83 CurrentState = FAIL;
onnoderkman 0:64c100cd152a 84 StateChanged = true;
onnoderkman 0:64c100cd152a 85 }
onnoderkman 0:64c100cd152a 86 else
onnoderkman 0:64c100cd152a 87 {
onnoderkman 0:64c100cd152a 88 CurrentState = BCM;
onnoderkman 0:64c100cd152a 89 StateChanged = true;
onnoderkman 0:64c100cd152a 90 }
onnoderkman 0:64c100cd152a 91 break; //End of EMG_CAL state
onnoderkman 0:64c100cd152a 92 case BCM: //State for moving the robot
onnoderkman 0:64c100cd152a 93 //State initialization
onnoderkman 0:64c100cd152a 94 if(StateChanged)
onnoderkman 0:64c100cd152a 95 {
onnoderkman 0:64c100cd152a 96 pc.printf("Entering Basic Control Mode\r\n");
onnoderkman 0:64c100cd152a 97 ledr = 1;
onnoderkman 0:64c100cd152a 98 ledg = 1;
onnoderkman 0:64c100cd152a 99 ledb = 0;
onnoderkman 0:64c100cd152a 100 StateChanged = false;
onnoderkman 0:64c100cd152a 101 }
onnoderkman 0:64c100cd152a 102 //State actions
onnoderkman 0:64c100cd152a 103 pc.printf("Basic Control Mode is active\r\n");
onnoderkman 0:64c100cd152a 104 while(button1 && !failure)
onnoderkman 0:64c100cd152a 105 {
onnoderkman 0:64c100cd152a 106 if(!button2)
onnoderkman 0:64c100cd152a 107 {
onnoderkman 0:64c100cd152a 108 failure = true;
onnoderkman 0:64c100cd152a 109 }
onnoderkman 0:64c100cd152a 110 }
onnoderkman 0:64c100cd152a 111 //State transition
onnoderkman 0:64c100cd152a 112 if(failure)
onnoderkman 0:64c100cd152a 113 {
onnoderkman 0:64c100cd152a 114 pc.printf("Go to fail mode\r\n");
onnoderkman 0:64c100cd152a 115 CurrentState = FAIL;
onnoderkman 0:64c100cd152a 116 StateChanged = true;
onnoderkman 0:64c100cd152a 117 }
onnoderkman 0:64c100cd152a 118 else
onnoderkman 0:64c100cd152a 119 {
onnoderkman 0:64c100cd152a 120 CurrentState = WAITING;
onnoderkman 0:64c100cd152a 121 StateChanged = true;
onnoderkman 0:64c100cd152a 122 }
onnoderkman 0:64c100cd152a 123 break; //End of BCM state
onnoderkman 0:64c100cd152a 124 case FAIL: //Fail state
onnoderkman 0:64c100cd152a 125 //State initialization
onnoderkman 0:64c100cd152a 126 if(StateChanged)
onnoderkman 0:64c100cd152a 127 {
onnoderkman 0:64c100cd152a 128 pc.printf("Fail mode is entered\r\n");
onnoderkman 0:64c100cd152a 129 failure = false;
onnoderkman 0:64c100cd152a 130 StateChanged = false;
onnoderkman 0:64c100cd152a 131 }
onnoderkman 0:64c100cd152a 132 //State actions
onnoderkman 0:64c100cd152a 133 pc.printf("Fail mode is active. Press button1 to enter WAITING\r\n");
onnoderkman 0:64c100cd152a 134 while(button1)
onnoderkman 0:64c100cd152a 135 {
onnoderkman 0:64c100cd152a 136 }
onnoderkman 0:64c100cd152a 137 //State transition
onnoderkman 0:64c100cd152a 138 CurrentState = WAITING;
onnoderkman 0:64c100cd152a 139 StateChanged = true;
onnoderkman 0:64c100cd152a 140 break; //End of FAIL state
onnoderkman 0:64c100cd152a 141 default: //Emergency state for when something unexpected happens
onnoderkman 0:64c100cd152a 142 //motoren uit enzo
onnoderkman 0:64c100cd152a 143 pc.printf("SOS SOS SOS\r\n");
onnoderkman 0:64c100cd152a 144 ledr = 0;
onnoderkman 0:64c100cd152a 145 ledg = 0;
onnoderkman 0:64c100cd152a 146 ledb = 0;
onnoderkman 0:64c100cd152a 147 while(button1)
onnoderkman 0:64c100cd152a 148 {
onnoderkman 0:64c100cd152a 149 ledr = !ledr;
onnoderkman 0:64c100cd152a 150 wait(0.3f);
onnoderkman 0:64c100cd152a 151 }
onnoderkman 0:64c100cd152a 152 CurrentState = WAITING;
onnoderkman 0:64c100cd152a 153 StateChanged = true;
onnoderkman 0:64c100cd152a 154 }
onnoderkman 0:64c100cd152a 155 }
onnoderkman 0:64c100cd152a 156
onnoderkman 0:64c100cd152a 157 int main()
onnoderkman 0:64c100cd152a 158 {
onnoderkman 0:64c100cd152a 159 pc.baud(115200);
onnoderkman 0:64c100cd152a 160 ledr = 1;
onnoderkman 0:64c100cd152a 161 ledg = 1;
onnoderkman 0:64c100cd152a 162 ledb = 1;
onnoderkman 0:64c100cd152a 163 while (true)
onnoderkman 0:64c100cd152a 164 {
onnoderkman 0:64c100cd152a 165 ProcessStateMachine(); //Activates state machine
onnoderkman 0:64c100cd152a 166 }
onnoderkman 0:64c100cd152a 167 }