![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
..
Fork of statemachineopzet by
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?
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 |
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 | } |