Gaston Gabriël / Mbed 2 deprecated StateMachine_EMg_RKI_PID_MOTOR_metklikenalles

23:00

Dependencies:   biquadFilter MODSERIAL QEI Servo mbed

Fork of StateMachine_EMg_RKI_PID_MOTOR_DEMO_CLICK by Gaston Gabriël

main.cpp@3:3a9fdac2ba69, 2018-11-01 (annotated)

Committer:
gastongab
Date:
Thu Nov 01 08:46:04 2018 +0000
Revision:
3:3a9fdac2ba69
Parent:
2:0a8622662f6d
Child:
4:c7be673eb4a1
fixed states 09.46

Who changed what in which revision?

UserRevisionLine numberNew contents of line
aschut 0:90750f158475 1 //Voor het toevoegen van een button:
aschut 0:90750f158475 2 #include "mbed.h"
aschut 0:90750f158475 3 #include <iostream>
gastongab 2:0a8622662f6d 4 #include "BiQuad.h"
gastongab 2:0a8622662f6d 5 #include "BiQuadchains_zelfbeun.h"
gastongab 2:0a8622662f6d 6 #include "MODSERIAL.h"
gastongab 2:0a8622662f6d 7
gastongab 2:0a8622662f6d 8 MODSERIAL pc(USBTX, USBRX);
gastongab 2:0a8622662f6d 9
aschut 0:90750f158475 10 DigitalOut gpo(D0);
aschut 0:90750f158475 11
aschut 0:90750f158475 12 DigitalIn button2(SW3);
aschut 0:90750f158475 13 DigitalIn button1(SW2); //or SW2
aschut 0:90750f158475 14
aschut 0:90750f158475 15 DigitalOut led1(LED_GREEN);
aschut 0:90750f158475 16 DigitalOut led2(LED_RED);
aschut 0:90750f158475 17 DigitalOut led3(LED_BLUE);
aschut 0:90750f158475 18
gastongab 2:0a8622662f6d 19 //EMG tickers, these tickers are called in the mainscript with fsample 500Hz, also sends to HIDscope with same fsample
gastongab 2:0a8622662f6d 20 Ticker sample_ticker; //ticker for filtering pref. with 1000Hz, define in tick.attach
gastongab 2:0a8622662f6d 21 Timer t; //timer try out for Astrid
gastongab 2:0a8622662f6d 22 Timer timer_calibration; //timer for EMG calibration
gastongab 2:0a8622662f6d 23
gastongab 2:0a8622662f6d 24
gastongab 2:0a8622662f6d 25
gastongab 2:0a8622662f6d 26 //Input system
gastongab 2:0a8622662f6d 27 AnalogIn emg1(A0); //right biceps
gastongab 2:0a8622662f6d 28 AnalogIn emg2(A1); //right triceps
gastongab 2:0a8622662f6d 29 AnalogIn emg3(A2); //left biceps
gastongab 2:0a8622662f6d 30 AnalogIn emg4(A3); //left triceps
gastongab 2:0a8622662f6d 31
gastongab 2:0a8622662f6d 32 //Filtered EMG signals from the end of the chains
gastongab 2:0a8622662f6d 33 double emg1_filtered, emg2_filtered, emg3_filtered, emg4_filtered;
gastongab 2:0a8622662f6d 34 volatile int i = 0;
gastongab 2:0a8622662f6d 35
gastongab 2:0a8622662f6d 36 void emgsample(){
gastongab 2:0a8622662f6d 37 //All EMG signal through Highpass
gastongab 2:0a8622662f6d 38 double emgread1 = emg1.read();
gastongab 2:0a8622662f6d 39 double emgread2 = emg2.read();
gastongab 2:0a8622662f6d 40 double emgread3 = emg3.read();
gastongab 2:0a8622662f6d 41 double emgread4 = emg4.read();
gastongab 2:0a8622662f6d 42
gastongab 2:0a8622662f6d 43 double emg1_highpassed = highp1.step(emgread1);
gastongab 2:0a8622662f6d 44 double emg2_highpassed = highp2.step(emgread2);
gastongab 2:0a8622662f6d 45 double emg3_highpassed = highp3.step(emgread3);
gastongab 2:0a8622662f6d 46 double emg4_highpassed = highp4.step(emgread4);
gastongab 2:0a8622662f6d 47
gastongab 2:0a8622662f6d 48 //All EMG highpassed through Notch
gastongab 2:0a8622662f6d 49 double emg1_notched = notch1.step(emg1_highpassed);
gastongab 2:0a8622662f6d 50 double emg2_notched = notch2.step(emg2_highpassed);
gastongab 2:0a8622662f6d 51 double emg3_notched = notch3.step(emg3_highpassed);
gastongab 2:0a8622662f6d 52 double emg4_notched = notch4.step(emg4_highpassed);
gastongab 2:0a8622662f6d 53
gastongab 2:0a8622662f6d 54 //All EMG notched rectify
gastongab 2:0a8622662f6d 55 double emg1_abs = abs(emg1_notched);
gastongab 2:0a8622662f6d 56 double emg2_abs = abs(emg2_notched);
gastongab 2:0a8622662f6d 57 double emg3_abs = abs(emg3_notched);
gastongab 2:0a8622662f6d 58 double emg4_abs = abs(emg4_notched);
gastongab 2:0a8622662f6d 59
gastongab 2:0a8622662f6d 60 //All EMG abs into lowpass
gastongab 2:0a8622662f6d 61 emg1_filtered = lowp1.step(emg1_abs);
gastongab 2:0a8622662f6d 62 emg2_filtered = lowp2.step(emg2_abs);
gastongab 2:0a8622662f6d 63 emg3_filtered = lowp3.step(emg3_abs);
gastongab 2:0a8622662f6d 64 emg4_filtered = lowp4.step(emg4_abs);
gastongab 2:0a8622662f6d 65
gastongab 2:0a8622662f6d 66
gastongab 2:0a8622662f6d 67 //Send data to HIDScope
gastongab 2:0a8622662f6d 68 //scope.set(0,emg1_filtered ); ONLY FOR VISUALIZATION
gastongab 2:0a8622662f6d 69 //scope.set(1,emg2_filtered);
gastongab 2:0a8622662f6d 70 //scope.set(2,emg3_filtered);
gastongab 2:0a8622662f6d 71 //scope.set(3,emg4_filtered);
gastongab 2:0a8622662f6d 72 //scope.send();
gastongab 2:0a8622662f6d 73
gastongab 2:0a8622662f6d 74 }
gastongab 2:0a8622662f6d 75
gastongab 2:0a8622662f6d 76
gastongab 2:0a8622662f6d 77 //Define doubles for calibration and ticker
gastongab 2:0a8622662f6d 78 double ts = 0.001; //tijdsstap
gastongab 2:0a8622662f6d 79 double calibration_time = 55; //time EMG calibration should take
gastongab 2:0a8622662f6d 80
gastongab 2:0a8622662f6d 81 volatile double temp_highest_emg1 = 0; //highest detected value right biceps
gastongab 2:0a8622662f6d 82 volatile double temp_highest_emg2 = 0;
gastongab 2:0a8622662f6d 83 volatile double temp_highest_emg3 = 0;
gastongab 2:0a8622662f6d 84 volatile double temp_highest_emg4 = 0;
gastongab 2:0a8622662f6d 85
gastongab 2:0a8622662f6d 86 //Doubles for calculation threshold
gastongab 3:3a9fdac2ba69 87 double p_t; //set threshold at percentage of highest value
gastongab 2:0a8622662f6d 88 double threshold1;
gastongab 2:0a8622662f6d 89 double threshold2;
gastongab 2:0a8622662f6d 90 double threshold3;
gastongab 2:0a8622662f6d 91 double threshold4;
gastongab 2:0a8622662f6d 92
gastongab 2:0a8622662f6d 93 void CalibrationEMG()
gastongab 2:0a8622662f6d 94 {
gastongab 2:0a8622662f6d 95 //static float samples = calibration_time/ts;
gastongab 2:0a8622662f6d 96 while(timer_calibration<55){
gastongab 2:0a8622662f6d 97 if(timer_calibration>0 && timer_calibration<10)
gastongab 2:0a8622662f6d 98 {
gastongab 2:0a8622662f6d 99 led1=!led1;
gastongab 2:0a8622662f6d 100 if(emg1_filtered>temp_highest_emg1)
gastongab 2:0a8622662f6d 101 {
gastongab 2:0a8622662f6d 102 temp_highest_emg1= emg1_filtered;
gastongab 2:0a8622662f6d 103 }
gastongab 2:0a8622662f6d 104 }
gastongab 2:0a8622662f6d 105 if(timer_calibration>10 && timer_calibration<15)
gastongab 2:0a8622662f6d 106 {
gastongab 2:0a8622662f6d 107 led1=0;
gastongab 2:0a8622662f6d 108 led2=0;
gastongab 2:0a8622662f6d 109 led3=0;
gastongab 2:0a8622662f6d 110 }
gastongab 2:0a8622662f6d 111 if(timer_calibration>15 && timer_calibration<25)
gastongab 2:0a8622662f6d 112 {
gastongab 2:0a8622662f6d 113 led2=!led2;
gastongab 2:0a8622662f6d 114 if(emg2_filtered>temp_highest_emg2)
gastongab 2:0a8622662f6d 115 {
gastongab 2:0a8622662f6d 116 temp_highest_emg2= emg2_filtered;
gastongab 2:0a8622662f6d 117 }
gastongab 2:0a8622662f6d 118 }
gastongab 2:0a8622662f6d 119 if(timer_calibration>25 && timer_calibration<30)
gastongab 2:0a8622662f6d 120 {
gastongab 2:0a8622662f6d 121 led1=0;
gastongab 2:0a8622662f6d 122 led2=0;
gastongab 2:0a8622662f6d 123 led3=0;
gastongab 2:0a8622662f6d 124 }
gastongab 2:0a8622662f6d 125 if(timer_calibration>30 && timer_calibration<40)
gastongab 2:0a8622662f6d 126 {
gastongab 2:0a8622662f6d 127 led3=!led3;
gastongab 2:0a8622662f6d 128 if(emg3_filtered>temp_highest_emg3)
gastongab 2:0a8622662f6d 129 {
gastongab 2:0a8622662f6d 130 temp_highest_emg3= emg3_filtered;
gastongab 2:0a8622662f6d 131 }
gastongab 2:0a8622662f6d 132 }
gastongab 2:0a8622662f6d 133 if(timer_calibration>40 && timer_calibration<45)
gastongab 2:0a8622662f6d 134 {
gastongab 2:0a8622662f6d 135 led1=0;
gastongab 2:0a8622662f6d 136 led2=0;
gastongab 2:0a8622662f6d 137 led3=0;
gastongab 2:0a8622662f6d 138 }
gastongab 2:0a8622662f6d 139 if(timer_calibration>45 && timer_calibration<55)
gastongab 2:0a8622662f6d 140 {
gastongab 2:0a8622662f6d 141 led2=!led2;
gastongab 2:0a8622662f6d 142 led3=!led3;
gastongab 2:0a8622662f6d 143 if(emg3_filtered>temp_highest_emg3)
gastongab 2:0a8622662f6d 144 {
gastongab 2:0a8622662f6d 145 temp_highest_emg3= emg3_filtered;
gastongab 2:0a8622662f6d 146 }
gastongab 2:0a8622662f6d 147 }
gastongab 2:0a8622662f6d 148 led1=1;
gastongab 2:0a8622662f6d 149 led2=1;
gastongab 2:0a8622662f6d 150 led3=1;
gastongab 2:0a8622662f6d 151
gastongab 2:0a8622662f6d 152
gastongab 2:0a8622662f6d 153 }
gastongab 2:0a8622662f6d 154
gastongab 2:0a8622662f6d 155 pc.printf("Highest value right biceps= %f \r\n", temp_highest_emg1);
gastongab 2:0a8622662f6d 156 pc.printf("Highest value right triceps= %f \r\n", temp_highest_emg2);
gastongab 2:0a8622662f6d 157 pc.printf("Highest value left biceps= %f \r\n", temp_highest_emg3);
gastongab 2:0a8622662f6d 158 pc.printf("Highest value left triceps= %f \r\n", temp_highest_emg4);
gastongab 2:0a8622662f6d 159
gastongab 2:0a8622662f6d 160 p_t = 0.8;
gastongab 2:0a8622662f6d 161 threshold1 = temp_highest_emg1*p_t;
gastongab 2:0a8622662f6d 162 threshold2 = temp_highest_emg2*p_t;
gastongab 2:0a8622662f6d 163 threshold3 = temp_highest_emg3*p_t;
gastongab 2:0a8622662f6d 164 threshold4 = temp_highest_emg4*p_t;
gastongab 2:0a8622662f6d 165 }
gastongab 2:0a8622662f6d 166
gastongab 2:0a8622662f6d 167 void threshold_check(){
gastongab 2:0a8622662f6d 168
gastongab 2:0a8622662f6d 169 //Check if emg_filtered has reached their threshold
gastongab 2:0a8622662f6d 170 bool bicepsR;
gastongab 2:0a8622662f6d 171 bool tricepsR;
gastongab 2:0a8622662f6d 172 bool bicepsL;
gastongab 2:0a8622662f6d 173 bool tricepsL;
gastongab 2:0a8622662f6d 174
gastongab 2:0a8622662f6d 175 //EMG1 threshold check
gastongab 2:0a8622662f6d 176 if(emg1_filtered>threshold1){
gastongab 2:0a8622662f6d 177 bicepsR = true;
gastongab 2:0a8622662f6d 178 }
gastongab 2:0a8622662f6d 179 else{
gastongab 2:0a8622662f6d 180 bicepsR= false;
gastongab 2:0a8622662f6d 181 }
gastongab 2:0a8622662f6d 182 //EMG2 threshold check
gastongab 2:0a8622662f6d 183 if(emg2_filtered>threshold2){
gastongab 2:0a8622662f6d 184 tricepsR = true;
gastongab 2:0a8622662f6d 185 }
gastongab 2:0a8622662f6d 186 else{
gastongab 2:0a8622662f6d 187 tricepsR= false;
gastongab 2:0a8622662f6d 188 }
gastongab 2:0a8622662f6d 189 //EMG3 threshold check
gastongab 2:0a8622662f6d 190 if(emg3_filtered>threshold3){
gastongab 2:0a8622662f6d 191 bicepsL = true;
gastongab 2:0a8622662f6d 192 }
gastongab 2:0a8622662f6d 193 else{
gastongab 2:0a8622662f6d 194 bicepsL= false;
gastongab 2:0a8622662f6d 195 }
gastongab 2:0a8622662f6d 196 //EMG4 threshold check
gastongab 2:0a8622662f6d 197 if(emg4_filtered>threshold4){
gastongab 2:0a8622662f6d 198 tricepsL = true;
gastongab 2:0a8622662f6d 199 }
gastongab 2:0a8622662f6d 200 else{
gastongab 2:0a8622662f6d 201 tricepsL= false;
gastongab 2:0a8622662f6d 202 }
gastongab 2:0a8622662f6d 203
gastongab 3:3a9fdac2ba69 204 pc.printf("Biceps Right = %B", bicepsR);
gastongab 3:3a9fdac2ba69 205 pc.printf("Triceps Right = %B",tricepsR);
gastongab 3:3a9fdac2ba69 206 pc.printf("Biceps Left = %B", bicepsL);
gastongab 3:3a9fdac2ba69 207 pc.printf("Triceps Left = %B", tricepsL);
gastongab 2:0a8622662f6d 208
gastongab 2:0a8622662f6d 209 }
aschut 0:90750f158475 210
aschut 0:90750f158475 211 enum states {MOTORS_OFF,CALIBRATION,HOMING,DEMO,MOVEMENT,CLICK};
gastongab 2:0a8622662f6d 212 states currentState = MOTORS_OFF; //Chosen startingposition for states
aschut 0:90750f158475 213 bool stateChanged = true; // Make sure the initialization of first state is executed
aschut 0:90750f158475 214
aschut 0:90750f158475 215 void ProcessStateMachine(void)
aschut 0:90750f158475 216 {
aschut 0:90750f158475 217 switch (currentState)
aschut 0:90750f158475 218 {
aschut 0:90750f158475 219 case MOTORS_OFF:
aschut 0:90750f158475 220 // Actions
aschut 0:90750f158475 221 if (stateChanged)
aschut 0:90750f158475 222 {
aschut 0:90750f158475 223 // state initialization: rood
aschut 0:90750f158475 224 led1 = 1;
aschut 0:90750f158475 225 led2 = 0;
aschut 0:90750f158475 226 led3 = 1;
aschut 0:90750f158475 227 wait (1);
aschut 0:90750f158475 228 stateChanged = false;
aschut 0:90750f158475 229 }
aschut 0:90750f158475 230
aschut 0:90750f158475 231 // State transition logic: Als button 1 word ingedrukt --> calibratie, anders motor uithouden
aschut 0:90750f158475 232 if (!button1)
aschut 0:90750f158475 233 {
aschut 0:90750f158475 234 currentState = CALIBRATION;
aschut 0:90750f158475 235 stateChanged = true;
aschut 0:90750f158475 236 }
aschut 0:90750f158475 237 else if (!button2)
aschut 0:90750f158475 238 {
aschut 0:90750f158475 239 currentState = HOMING ;
aschut 0:90750f158475 240 stateChanged = true;
aschut 0:90750f158475 241 }
aschut 0:90750f158475 242 else
aschut 0:90750f158475 243 {
aschut 0:90750f158475 244 currentState = MOTORS_OFF;
aschut 0:90750f158475 245 stateChanged = true;
aschut 0:90750f158475 246 }
aschut 0:90750f158475 247
aschut 0:90750f158475 248 break;
aschut 0:90750f158475 249
aschut 0:90750f158475 250 case CALIBRATION:
aschut 0:90750f158475 251 // Actions
aschut 0:90750f158475 252 if (stateChanged)
aschut 0:90750f158475 253 {
aschut 0:90750f158475 254 // state initialization: oranje
gastongab 2:0a8622662f6d 255 temp_highest_emg1 = 0; //highest detected value right biceps
gastongab 2:0a8622662f6d 256 temp_highest_emg2 = 0;
gastongab 2:0a8622662f6d 257 temp_highest_emg3 = 0;
gastongab 2:0a8622662f6d 258 temp_highest_emg4 = 0;
gastongab 2:0a8622662f6d 259
gastongab 2:0a8622662f6d 260 timer_calibration.reset();
gastongab 2:0a8622662f6d 261 timer_calibration.start();
gastongab 2:0a8622662f6d 262
gastongab 2:0a8622662f6d 263
gastongab 2:0a8622662f6d 264 if(timer_calibration<55){
gastongab 2:0a8622662f6d 265 sample_ticker.attach(&emgsample, ts);
gastongab 2:0a8622662f6d 266 CalibrationEMG();
gastongab 2:0a8622662f6d 267 }
gastongab 2:0a8622662f6d 268 else{
gastongab 2:0a8622662f6d 269 sample_ticker.detach();
gastongab 2:0a8622662f6d 270 timer_calibration.stop();
gastongab 2:0a8622662f6d 271 }
aschut 0:90750f158475 272
aschut 0:90750f158475 273 stateChanged = false;
aschut 0:90750f158475 274 }
aschut 0:90750f158475 275
aschut 0:90750f158475 276 // State transition logic: automatisch terug naar motors off.
aschut 0:90750f158475 277
aschut 0:90750f158475 278 currentState = MOTORS_OFF;
aschut 0:90750f158475 279 stateChanged = true;
aschut 0:90750f158475 280 break;
aschut 0:90750f158475 281
aschut 0:90750f158475 282 case HOMING:
aschut 0:90750f158475 283 // Actions
aschut 0:90750f158475 284 if (stateChanged)
aschut 0:90750f158475 285 {
aschut 0:90750f158475 286 // state initialization: green
gastongab 3:3a9fdac2ba69 287 t.reset();
aschut 0:90750f158475 288 t.start();
aschut 0:90750f158475 289 led1 = 0;
aschut 0:90750f158475 290 led2 = 1;
aschut 0:90750f158475 291 led3 = 1;
aschut 0:90750f158475 292 wait (1);
aschut 0:90750f158475 293
aschut 0:90750f158475 294 stateChanged = false;
aschut 0:90750f158475 295 }
aschut 0:90750f158475 296
aschut 0:90750f158475 297 // State transition logic: naar DEMO (button1), naar MOVEMENT(button2)
aschut 0:90750f158475 298 if (!button1)
aschut 0:90750f158475 299 {
aschut 0:90750f158475 300 currentState = DEMO;
aschut 0:90750f158475 301 stateChanged = true;
aschut 0:90750f158475 302 }
aschut 0:90750f158475 303 else if (!button2)
aschut 0:90750f158475 304 {
aschut 0:90750f158475 305 currentState = MOVEMENT ;
aschut 0:90750f158475 306 stateChanged = true;
aschut 0:90750f158475 307 }
aschut 0:90750f158475 308 else if (t>300)
aschut 0:90750f158475 309 {
aschut 0:90750f158475 310 t.stop();
aschut 0:90750f158475 311 t.reset();
aschut 0:90750f158475 312 currentState = MOTORS_OFF ;
aschut 0:90750f158475 313 stateChanged = true;
aschut 0:90750f158475 314 }
aschut 0:90750f158475 315 else
aschut 0:90750f158475 316 {
aschut 0:90750f158475 317 currentState = HOMING ;
aschut 0:90750f158475 318 stateChanged = true;
aschut 0:90750f158475 319 }
aschut 0:90750f158475 320 break;
aschut 0:90750f158475 321
aschut 0:90750f158475 322 case DEMO:
aschut 0:90750f158475 323 // Actions
aschut 0:90750f158475 324 if (stateChanged)
aschut 0:90750f158475 325 {
aschut 0:90750f158475 326 // state initialization: light blue
aschut 0:90750f158475 327 led1 = 0;
aschut 0:90750f158475 328 led2 = 1;
aschut 0:90750f158475 329 led3 = 0;
aschut 0:90750f158475 330 wait (1);
aschut 0:90750f158475 331
aschut 0:90750f158475 332 stateChanged = false;
aschut 0:90750f158475 333 }
aschut 0:90750f158475 334
aschut 0:90750f158475 335 // State transition logic: automatisch terug naar HOMING
aschut 0:90750f158475 336 currentState = HOMING;
aschut 0:90750f158475 337 stateChanged = true;
aschut 0:90750f158475 338 break;
aschut 0:90750f158475 339
aschut 0:90750f158475 340 case MOVEMENT:
aschut 0:90750f158475 341 // Actions
aschut 0:90750f158475 342 if (stateChanged)
aschut 0:90750f158475 343 {
aschut 0:90750f158475 344 // state initialization: purple
aschut 0:90750f158475 345 t.start();
aschut 0:90750f158475 346 led1 = 1;
aschut 0:90750f158475 347 led2 = 0;
aschut 0:90750f158475 348 led3 = 0;
gastongab 3:3a9fdac2ba69 349 wait(1);
gastongab 3:3a9fdac2ba69 350 sample_ticker.attach(&threshold_check, ts);
gastongab 3:3a9fdac2ba69 351 sample_ticker.attach(&emgsample, ts);
gastongab 3:3a9fdac2ba69 352
aschut 0:90750f158475 353 stateChanged = false;
aschut 0:90750f158475 354 }
aschut 0:90750f158475 355
aschut 0:90750f158475 356 // State transition logic: naar CLICK (button1), naar MOTORS_OFF(button2) anders naar MOVEMENT
aschut 0:90750f158475 357 if (!button1)
aschut 0:90750f158475 358 {
aschut 0:90750f158475 359 currentState = CLICK;
aschut 1:070092564648 360 stateChanged = true;
aschut 0:90750f158475 361 }
aschut 0:90750f158475 362 else if (!button2)
aschut 0:90750f158475 363 {
aschut 0:90750f158475 364 currentState = MOTORS_OFF ;
aschut 0:90750f158475 365 stateChanged = true;
aschut 0:90750f158475 366 }
aschut 0:90750f158475 367 else if (t>300)
aschut 0:90750f158475 368 {
aschut 0:90750f158475 369 t.stop();
aschut 0:90750f158475 370 t.reset();
aschut 0:90750f158475 371 currentState = HOMING ;
aschut 0:90750f158475 372 stateChanged = true;
aschut 0:90750f158475 373 }
aschut 0:90750f158475 374 else
aschut 0:90750f158475 375 {
aschut 0:90750f158475 376 currentState = MOVEMENT ;
aschut 0:90750f158475 377 stateChanged = true;
aschut 0:90750f158475 378 }
aschut 0:90750f158475 379 break;
aschut 0:90750f158475 380
aschut 0:90750f158475 381 case CLICK:
aschut 0:90750f158475 382 // Actions
aschut 0:90750f158475 383 if (stateChanged)
aschut 0:90750f158475 384 {
aschut 0:90750f158475 385 // state initialization: blue
aschut 0:90750f158475 386 led1 = 1;
aschut 0:90750f158475 387 led2 = 1;
aschut 0:90750f158475 388 led3 = 0;
aschut 0:90750f158475 389 wait (1);
aschut 0:90750f158475 390
aschut 0:90750f158475 391 stateChanged = false;
aschut 0:90750f158475 392 }
aschut 0:90750f158475 393
aschut 0:90750f158475 394 // State transition logic: automatisch terug naar MOVEMENT.
aschut 0:90750f158475 395
aschut 0:90750f158475 396 currentState = MOVEMENT;
aschut 0:90750f158475 397 stateChanged = true;
aschut 0:90750f158475 398 break;
aschut 0:90750f158475 399
aschut 0:90750f158475 400 }
aschut 0:90750f158475 401 }
aschut 0:90750f158475 402
aschut 0:90750f158475 403 int main()
aschut 0:90750f158475 404 {
gastongab 2:0a8622662f6d 405 pc.baud(115200);
aschut 0:90750f158475 406 while (true)
aschut 0:90750f158475 407 {
aschut 0:90750f158475 408 led1 = 1;
aschut 0:90750f158475 409 led2 = 1;
aschut 0:90750f158475 410 led3 = 1;
aschut 0:90750f158475 411 ProcessStateMachine();
aschut 0:90750f158475 412
aschut 0:90750f158475 413 }
aschut 0:90750f158475 414
aschut 0:90750f158475 415 }
aschut 0:90750f158475 416
aschut 0:90750f158475 417
aschut 0:90750f158475 418
aschut 0:90750f158475 419
aschut 0:90750f158475 420
aschut 0:90750f158475 421