![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
statemachine opzet
main.cpp@0:64c100cd152a, 2018-10-31 (annotated)
- Committer:
- onnoderkman
- Date:
- Wed Oct 31 10:47:46 2018 +0000
- Revision:
- 0:64c100cd152a
State machine opzet. Overgangen van state naar state zijn nog niet compleet.
Who changed what in which revision?
User | Revision | Line number | New 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 |
onnoderkman | 0:64c100cd152a | 31 | pc.printf("Still WAITING\r\n"); |
onnoderkman | 0:64c100cd152a | 32 | while(button1) |
onnoderkman | 0:64c100cd152a | 33 | { |
onnoderkman | 0:64c100cd152a | 34 | } |
onnoderkman | 0:64c100cd152a | 35 | //State transition |
onnoderkman | 0:64c100cd152a | 36 | CurrentState = EMG_CAL; |
onnoderkman | 0:64c100cd152a | 37 | StateChanged = true; |
onnoderkman | 0:64c100cd152a | 38 | break; // End of WAITING |
onnoderkman | 0:64c100cd152a | 39 | case EMG_CAL: //State for calibrating EMG channels |
onnoderkman | 0:64c100cd152a | 40 | //State initialization |
onnoderkman | 0:64c100cd152a | 41 | if(StateChanged) |
onnoderkman | 0:64c100cd152a | 42 | { |
onnoderkman | 0:64c100cd152a | 43 | pc.printf("Entering EMG calibration\r\n"); |
onnoderkman | 0:64c100cd152a | 44 | x = 0; |
onnoderkman | 0:64c100cd152a | 45 | ledr = 1; |
onnoderkman | 0:64c100cd152a | 46 | ledg = 0; |
onnoderkman | 0:64c100cd152a | 47 | ledb = 1; |
onnoderkman | 0:64c100cd152a | 48 | StateChanged = false; |
onnoderkman | 0:64c100cd152a | 49 | } |
onnoderkman | 0:64c100cd152a | 50 | //State actions |
onnoderkman | 0:64c100cd152a | 51 | pc.printf("EMG calibration starting\r\n"); |
onnoderkman | 0:64c100cd152a | 52 | wait(0.3f); //Dit moet nog veranderd worden naar timer die opneemt hoe lang hij in WAITING zit. |
onnoderkman | 0:64c100cd152a | 53 | // calibratiestappen voor de emg hier invoegen |
onnoderkman | 0:64c100cd152a | 54 | while(x < 4 && !failure) |
onnoderkman | 0:64c100cd152a | 55 | { |
onnoderkman | 0:64c100cd152a | 56 | if(x == 1) |
onnoderkman | 0:64c100cd152a | 57 | { |
onnoderkman | 0:64c100cd152a | 58 | //Functie die het eerste EMG signaal calibreert |
onnoderkman | 0:64c100cd152a | 59 | } |
onnoderkman | 0:64c100cd152a | 60 | if(x == 2) |
onnoderkman | 0:64c100cd152a | 61 | { |
onnoderkman | 0:64c100cd152a | 62 | //Functie die het tweede EMG signaal calibreert |
onnoderkman | 0:64c100cd152a | 63 | } |
onnoderkman | 0:64c100cd152a | 64 | if(x == 3) |
onnoderkman | 0:64c100cd152a | 65 | { |
onnoderkman | 0:64c100cd152a | 66 | //Functie die het derde EMG signaal calibreert |
onnoderkman | 0:64c100cd152a | 67 | } |
onnoderkman | 0:64c100cd152a | 68 | if(!button2) |
onnoderkman | 0:64c100cd152a | 69 | { |
onnoderkman | 0:64c100cd152a | 70 | failure = true; |
onnoderkman | 0:64c100cd152a | 71 | } |
onnoderkman | 0:64c100cd152a | 72 | if(!button1) |
onnoderkman | 0:64c100cd152a | 73 | { |
onnoderkman | 0:64c100cd152a | 74 | x++; |
onnoderkman | 0:64c100cd152a | 75 | pc.printf("EMG calibration %i is now running\r\n", x); |
onnoderkman | 0:64c100cd152a | 76 | wait(0.2f); |
onnoderkman | 0:64c100cd152a | 77 | } |
onnoderkman | 0:64c100cd152a | 78 | } |
onnoderkman | 0:64c100cd152a | 79 | //State transition |
onnoderkman | 0:64c100cd152a | 80 | if(failure) |
onnoderkman | 0:64c100cd152a | 81 | { |
onnoderkman | 0:64c100cd152a | 82 | CurrentState = FAIL; |
onnoderkman | 0:64c100cd152a | 83 | StateChanged = true; |
onnoderkman | 0:64c100cd152a | 84 | } |
onnoderkman | 0:64c100cd152a | 85 | else |
onnoderkman | 0:64c100cd152a | 86 | { |
onnoderkman | 0:64c100cd152a | 87 | CurrentState = BCM; |
onnoderkman | 0:64c100cd152a | 88 | StateChanged = true; |
onnoderkman | 0:64c100cd152a | 89 | } |
onnoderkman | 0:64c100cd152a | 90 | break; //End of EMG_CAL state |
onnoderkman | 0:64c100cd152a | 91 | case BCM: //State for moving the robot |
onnoderkman | 0:64c100cd152a | 92 | //State initialization |
onnoderkman | 0:64c100cd152a | 93 | if(StateChanged) |
onnoderkman | 0:64c100cd152a | 94 | { |
onnoderkman | 0:64c100cd152a | 95 | pc.printf("Entering Basic Control Mode\r\n"); |
onnoderkman | 0:64c100cd152a | 96 | ledr = 1; |
onnoderkman | 0:64c100cd152a | 97 | ledg = 1; |
onnoderkman | 0:64c100cd152a | 98 | ledb = 0; |
onnoderkman | 0:64c100cd152a | 99 | StateChanged = false; |
onnoderkman | 0:64c100cd152a | 100 | } |
onnoderkman | 0:64c100cd152a | 101 | //State actions |
onnoderkman | 0:64c100cd152a | 102 | pc.printf("Basic Control Mode is active\r\n"); |
onnoderkman | 0:64c100cd152a | 103 | while(button1 && !failure) |
onnoderkman | 0:64c100cd152a | 104 | { |
onnoderkman | 0:64c100cd152a | 105 | if(!button2) |
onnoderkman | 0:64c100cd152a | 106 | { |
onnoderkman | 0:64c100cd152a | 107 | failure = true; |
onnoderkman | 0:64c100cd152a | 108 | } |
onnoderkman | 0:64c100cd152a | 109 | } |
onnoderkman | 0:64c100cd152a | 110 | //State transition |
onnoderkman | 0:64c100cd152a | 111 | if(failure) |
onnoderkman | 0:64c100cd152a | 112 | { |
onnoderkman | 0:64c100cd152a | 113 | pc.printf("Go to fail mode\r\n"); |
onnoderkman | 0:64c100cd152a | 114 | CurrentState = FAIL; |
onnoderkman | 0:64c100cd152a | 115 | StateChanged = true; |
onnoderkman | 0:64c100cd152a | 116 | } |
onnoderkman | 0:64c100cd152a | 117 | else |
onnoderkman | 0:64c100cd152a | 118 | { |
onnoderkman | 0:64c100cd152a | 119 | CurrentState = WAITING; |
onnoderkman | 0:64c100cd152a | 120 | StateChanged = true; |
onnoderkman | 0:64c100cd152a | 121 | } |
onnoderkman | 0:64c100cd152a | 122 | break; //End of BCM state |
onnoderkman | 0:64c100cd152a | 123 | case FAIL: //Fail state |
onnoderkman | 0:64c100cd152a | 124 | //State initialization |
onnoderkman | 0:64c100cd152a | 125 | if(StateChanged) |
onnoderkman | 0:64c100cd152a | 126 | { |
onnoderkman | 0:64c100cd152a | 127 | pc.printf("Fail mode is entered\r\n"); |
onnoderkman | 0:64c100cd152a | 128 | failure = false; |
onnoderkman | 0:64c100cd152a | 129 | StateChanged = false; |
onnoderkman | 0:64c100cd152a | 130 | } |
onnoderkman | 0:64c100cd152a | 131 | //State actions |
onnoderkman | 0:64c100cd152a | 132 | pc.printf("Fail mode is active. Press button1 to enter WAITING\r\n"); |
onnoderkman | 0:64c100cd152a | 133 | while(button1) |
onnoderkman | 0:64c100cd152a | 134 | { |
onnoderkman | 0:64c100cd152a | 135 | } |
onnoderkman | 0:64c100cd152a | 136 | //State transition |
onnoderkman | 0:64c100cd152a | 137 | CurrentState = WAITING; |
onnoderkman | 0:64c100cd152a | 138 | StateChanged = true; |
onnoderkman | 0:64c100cd152a | 139 | break; //End of FAIL state |
onnoderkman | 0:64c100cd152a | 140 | default: //Emergency state for when something unexpected happens |
onnoderkman | 0:64c100cd152a | 141 | //motoren uit enzo |
onnoderkman | 0:64c100cd152a | 142 | pc.printf("SOS SOS SOS\r\n"); |
onnoderkman | 0:64c100cd152a | 143 | ledr = 0; |
onnoderkman | 0:64c100cd152a | 144 | ledg = 0; |
onnoderkman | 0:64c100cd152a | 145 | ledb = 0; |
onnoderkman | 0:64c100cd152a | 146 | while(button1) |
onnoderkman | 0:64c100cd152a | 147 | { |
onnoderkman | 0:64c100cd152a | 148 | ledr = !ledr; |
onnoderkman | 0:64c100cd152a | 149 | wait(0.3f); |
onnoderkman | 0:64c100cd152a | 150 | } |
onnoderkman | 0:64c100cd152a | 151 | CurrentState = WAITING; |
onnoderkman | 0:64c100cd152a | 152 | StateChanged = true; |
onnoderkman | 0:64c100cd152a | 153 | } |
onnoderkman | 0:64c100cd152a | 154 | } |
onnoderkman | 0:64c100cd152a | 155 | |
onnoderkman | 0:64c100cd152a | 156 | int main() |
onnoderkman | 0:64c100cd152a | 157 | { |
onnoderkman | 0:64c100cd152a | 158 | pc.baud(115200); |
onnoderkman | 0:64c100cd152a | 159 | ledr = 1; |
onnoderkman | 0:64c100cd152a | 160 | ledg = 1; |
onnoderkman | 0:64c100cd152a | 161 | ledb = 1; |
onnoderkman | 0:64c100cd152a | 162 | while (true) |
onnoderkman | 0:64c100cd152a | 163 | { |
onnoderkman | 0:64c100cd152a | 164 | ProcessStateMachine(); //Activates state machine |
onnoderkman | 0:64c100cd152a | 165 | } |
onnoderkman | 0:64c100cd152a | 166 | } |