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@8:ec3ea0623620, 2018-11-01 (annotated)

Committer:
cmaas
Date:
Thu Nov 01 16:40:11 2018 +0000
Revision:
8:ec3ea0623620
Parent:
7:88fa84742b3c
Child:
9:40c9a18c3430
THE OG

Who changed what in which revision?

UserRevisionLine numberNew contents of line
cmaas 8:ec3ea0623620 1 // EMG, RKI, PID, MOTOR
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
gastongab 7:88fa84742b3c 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 4:c7be673eb4a1 21 Ticker threshold_check_ticker;
gastongab 2:0a8622662f6d 22 Timer t; //timer try out for Astrid
gastongab 2:0a8622662f6d 23 Timer timer_calibration; //timer for EMG calibration
gastongab 2:0a8622662f6d 24
gastongab 2:0a8622662f6d 25
gastongab 2:0a8622662f6d 26
gastongab 2:0a8622662f6d 27 //Input system
gastongab 2:0a8622662f6d 28 AnalogIn emg1(A0); //right biceps
gastongab 2:0a8622662f6d 29 AnalogIn emg2(A1); //right triceps
gastongab 2:0a8622662f6d 30 AnalogIn emg3(A2); //left biceps
gastongab 2:0a8622662f6d 31 AnalogIn emg4(A3); //left triceps
gastongab 2:0a8622662f6d 32
gastongab 2:0a8622662f6d 33 //Filtered EMG signals from the end of the chains
gastongab 4:c7be673eb4a1 34 volatile double emg1_filtered, emg2_filtered, emg3_filtered, emg4_filtered;
gastongab 4:c7be673eb4a1 35 int i = 0;
gastongab 2:0a8622662f6d 36
gastongab 7:88fa84742b3c 37 void emgsample()
gastongab 7:88fa84742b3c 38 {
gastongab 2:0a8622662f6d 39 //All EMG signal through Highpass
gastongab 2:0a8622662f6d 40 double emgread1 = emg1.read();
gastongab 2:0a8622662f6d 41 double emgread2 = emg2.read();
gastongab 2:0a8622662f6d 42 double emgread3 = emg3.read();
gastongab 2:0a8622662f6d 43 double emgread4 = emg4.read();
gastongab 7:88fa84742b3c 44
gastongab 2:0a8622662f6d 45 double emg1_highpassed = highp1.step(emgread1);
gastongab 2:0a8622662f6d 46 double emg2_highpassed = highp2.step(emgread2);
gastongab 2:0a8622662f6d 47 double emg3_highpassed = highp3.step(emgread3);
gastongab 2:0a8622662f6d 48 double emg4_highpassed = highp4.step(emgread4);
gastongab 7:88fa84742b3c 49
gastongab 2:0a8622662f6d 50 //All EMG highpassed through Notch
gastongab 2:0a8622662f6d 51 double emg1_notched = notch1.step(emg1_highpassed);
gastongab 2:0a8622662f6d 52 double emg2_notched = notch2.step(emg2_highpassed);
gastongab 2:0a8622662f6d 53 double emg3_notched = notch3.step(emg3_highpassed);
gastongab 2:0a8622662f6d 54 double emg4_notched = notch4.step(emg4_highpassed);
gastongab 7:88fa84742b3c 55
gastongab 2:0a8622662f6d 56 //All EMG notched rectify
gastongab 2:0a8622662f6d 57 double emg1_abs = abs(emg1_notched);
gastongab 2:0a8622662f6d 58 double emg2_abs = abs(emg2_notched);
gastongab 2:0a8622662f6d 59 double emg3_abs = abs(emg3_notched);
gastongab 2:0a8622662f6d 60 double emg4_abs = abs(emg4_notched);
gastongab 7:88fa84742b3c 61
gastongab 2:0a8622662f6d 62 //All EMG abs into lowpass
gastongab 2:0a8622662f6d 63 emg1_filtered = lowp1.step(emg1_abs);
gastongab 2:0a8622662f6d 64 emg2_filtered = lowp2.step(emg2_abs);
gastongab 2:0a8622662f6d 65 emg3_filtered = lowp3.step(emg3_abs);
gastongab 2:0a8622662f6d 66 emg4_filtered = lowp4.step(emg4_abs);
gastongab 7:88fa84742b3c 67
gastongab 7:88fa84742b3c 68
gastongab 7:88fa84742b3c 69 }
gastongab 7:88fa84742b3c 70
gastongab 2:0a8622662f6d 71
gastongab 2:0a8622662f6d 72 //Define doubles for calibration and ticker
gastongab 7:88fa84742b3c 73 double ts = 0.001; //tijdsstap
gastongab 7:88fa84742b3c 74 double calibration_time = 55; //time EMG calibration should take
gastongab 7:88fa84742b3c 75
gastongab 7:88fa84742b3c 76 volatile double temp_highest_emg1 = 0; //highest detected value right biceps
gastongab 7:88fa84742b3c 77 volatile double temp_highest_emg2 = 0;
gastongab 7:88fa84742b3c 78 volatile double temp_highest_emg3 = 0;
gastongab 7:88fa84742b3c 79 volatile double temp_highest_emg4 = 0;
gastongab 7:88fa84742b3c 80
gastongab 7:88fa84742b3c 81 //Doubles for calculation threshold
gastongab 7:88fa84742b3c 82 double biceps_p_t = 0.4; //set threshold at percentage of highest value
gastongab 7:88fa84742b3c 83 double triceps_p_t = 0.5; //set threshold at percentage of highest value
gastongab 7:88fa84742b3c 84 volatile double threshold1;
gastongab 7:88fa84742b3c 85 volatile double threshold2;
gastongab 7:88fa84742b3c 86 volatile double threshold3;
gastongab 7:88fa84742b3c 87 volatile double threshold4;
gastongab 7:88fa84742b3c 88
gastongab 7:88fa84742b3c 89 void CalibrationEMG()
gastongab 7:88fa84742b3c 90 {
gastongab 7:88fa84742b3c 91 //static float samples = calibration_time/ts;
gastongab 7:88fa84742b3c 92 while(timer_calibration<55) {
gastongab 7:88fa84742b3c 93 if(timer_calibration>0 && timer_calibration<10) {
gastongab 7:88fa84742b3c 94 led1=!led1;
gastongab 7:88fa84742b3c 95 if(emg1_filtered>temp_highest_emg1) {
gastongab 7:88fa84742b3c 96 temp_highest_emg1= emg1_filtered;
gastongab 7:88fa84742b3c 97 pc.printf("Temp1 = %f \r\n",temp_highest_emg1);
gastongab 7:88fa84742b3c 98 }
gastongab 7:88fa84742b3c 99 }
gastongab 7:88fa84742b3c 100 if(timer_calibration>10 && timer_calibration<15) {
gastongab 7:88fa84742b3c 101 led1=0;
gastongab 7:88fa84742b3c 102 led2=0;
gastongab 7:88fa84742b3c 103 led3=0;
gastongab 7:88fa84742b3c 104 }
gastongab 7:88fa84742b3c 105 if(timer_calibration>15 && timer_calibration<25) {
gastongab 7:88fa84742b3c 106 led2=!led2;
gastongab 7:88fa84742b3c 107 if(emg2_filtered>temp_highest_emg2) {
gastongab 7:88fa84742b3c 108 temp_highest_emg2= emg2_filtered;
gastongab 7:88fa84742b3c 109 pc.printf("Temp2 = %f \r\n",temp_highest_emg2);
gastongab 7:88fa84742b3c 110 }
gastongab 7:88fa84742b3c 111 }
gastongab 7:88fa84742b3c 112 if(timer_calibration>25 && timer_calibration<30) {
gastongab 7:88fa84742b3c 113 led1=0;
gastongab 7:88fa84742b3c 114 led2=0;
gastongab 7:88fa84742b3c 115 led3=0;
gastongab 7:88fa84742b3c 116 }
gastongab 7:88fa84742b3c 117 if(timer_calibration>30 && timer_calibration<40) {
gastongab 7:88fa84742b3c 118 led3=!led3;
gastongab 7:88fa84742b3c 119 if(emg3_filtered>temp_highest_emg3) {
gastongab 7:88fa84742b3c 120 temp_highest_emg3= emg3_filtered;
gastongab 7:88fa84742b3c 121 pc.printf("Temp3 = %f \r\n",temp_highest_emg3);
gastongab 7:88fa84742b3c 122 }
gastongab 7:88fa84742b3c 123 }
gastongab 7:88fa84742b3c 124 if(timer_calibration>40 && timer_calibration<45) {
gastongab 7:88fa84742b3c 125 led1=0;
gastongab 7:88fa84742b3c 126 led2=0;
gastongab 7:88fa84742b3c 127 led3=0;
gastongab 7:88fa84742b3c 128 }
gastongab 7:88fa84742b3c 129 if(timer_calibration>45 && timer_calibration<55) {
gastongab 7:88fa84742b3c 130 led2=!led2;
gastongab 7:88fa84742b3c 131 led3=!led3;
gastongab 7:88fa84742b3c 132 if(emg4_filtered>temp_highest_emg4) {
gastongab 7:88fa84742b3c 133 temp_highest_emg4= emg4_filtered;
gastongab 7:88fa84742b3c 134 pc.printf("Temp4 = %f \r\n",temp_highest_emg4);
gastongab 7:88fa84742b3c 135 }
gastongab 7:88fa84742b3c 136 }
gastongab 7:88fa84742b3c 137 led1=1;
gastongab 7:88fa84742b3c 138 led2=1;
gastongab 7:88fa84742b3c 139 led3=1;
gastongab 7:88fa84742b3c 140
gastongab 2:0a8622662f6d 141
gastongab 2:0a8622662f6d 142 }
gastongab 7:88fa84742b3c 143
gastongab 2:0a8622662f6d 144 pc.printf("Highest value right biceps= %f \r\n", temp_highest_emg1);
gastongab 2:0a8622662f6d 145 pc.printf("Highest value right triceps= %f \r\n", temp_highest_emg2);
gastongab 2:0a8622662f6d 146 pc.printf("Highest value left biceps= %f \r\n", temp_highest_emg3);
gastongab 2:0a8622662f6d 147 pc.printf("Highest value left triceps= %f \r\n", temp_highest_emg4);
gastongab 7:88fa84742b3c 148
gastongab 7:88fa84742b3c 149
gastongab 6:f55ab7e38a7f 150 threshold1 = temp_highest_emg1*biceps_p_t; //Right Biceps
gastongab 6:f55ab7e38a7f 151 threshold2 = temp_highest_emg2*triceps_p_t; //Right Triceps
gastongab 6:f55ab7e38a7f 152 threshold3 = temp_highest_emg3*biceps_p_t; //Left Biceps
gastongab 7:88fa84742b3c 153 threshold4 = temp_highest_emg4*triceps_p_t; //Left Triceps
gastongab 2:0a8622662f6d 154 }
gastongab 2:0a8622662f6d 155
gastongab 4:c7be673eb4a1 156 //Check if emg_filtered has reached their threshold
gastongab 7:88fa84742b3c 157 int bicepsR;
gastongab 7:88fa84742b3c 158 int tricepsR;
gastongab 7:88fa84742b3c 159 int bicepsL;
gastongab 7:88fa84742b3c 160 int tricepsL;
gastongab 4:c7be673eb4a1 161
gastongab 7:88fa84742b3c 162 void threshold_check()
gastongab 7:88fa84742b3c 163 {
gastongab 7:88fa84742b3c 164
gastongab 2:0a8622662f6d 165 //EMG1 threshold check
gastongab 7:88fa84742b3c 166 if(emg1_filtered>threshold1) {
gastongab 4:c7be673eb4a1 167 bicepsR = 1;
gastongab 7:88fa84742b3c 168 } else {
gastongab 4:c7be673eb4a1 169 bicepsR= 0;
gastongab 7:88fa84742b3c 170 }
gastongab 2:0a8622662f6d 171 //EMG2 threshold check
gastongab 7:88fa84742b3c 172 if(emg2_filtered>threshold2) {
gastongab 4:c7be673eb4a1 173 tricepsR = 1;
gastongab 7:88fa84742b3c 174 } else {
gastongab 4:c7be673eb4a1 175 tricepsR= 0;
gastongab 7:88fa84742b3c 176 }
gastongab 2:0a8622662f6d 177 //EMG3 threshold check
gastongab 7:88fa84742b3c 178 if(emg3_filtered>threshold3) {
gastongab 4:c7be673eb4a1 179 bicepsL = 1;
gastongab 7:88fa84742b3c 180 } else {
gastongab 4:c7be673eb4a1 181 bicepsL= 0;
gastongab 7:88fa84742b3c 182 }
gastongab 2:0a8622662f6d 183 //EMG4 threshold check
gastongab 7:88fa84742b3c 184 if(emg4_filtered>threshold4) {
gastongab 4:c7be673eb4a1 185 tricepsL = 1;
gastongab 7:88fa84742b3c 186 } else {
gastongab 4:c7be673eb4a1 187 tricepsL= 0;
gastongab 7:88fa84742b3c 188 }
gastongab 7:88fa84742b3c 189
gastongab 7:88fa84742b3c 190 /*
gastongab 4:c7be673eb4a1 191 pc.printf("Biceps Right = %i", bicepsR);
gastongab 7:88fa84742b3c 192 pc.printf("Triceps Right = %i",tricepsR);
gastongab 4:c7be673eb4a1 193 pc.printf("Biceps Left = %i", bicepsL);
gastongab 4:c7be673eb4a1 194 pc.printf("Triceps Left = %i", tricepsL);
gastongab 4:c7be673eb4a1 195 */
gastongab 7:88fa84742b3c 196
gastongab 7:88fa84742b3c 197
gastongab 2:0a8622662f6d 198 }
aschut 0:90750f158475 199
gastongab 4:c7be673eb4a1 200
gastongab 4:c7be673eb4a1 201 //Activate ticker for Movement state, filtering and Threshold checking
gastongab 7:88fa84742b3c 202 void movement_ticker_activator()
gastongab 7:88fa84742b3c 203 {
gastongab 4:c7be673eb4a1 204 sample_ticker.attach(&emgsample, ts);
gastongab 4:c7be673eb4a1 205 threshold_check_ticker.attach(&threshold_check, ts);
gastongab 7:88fa84742b3c 206 }
gastongab 7:88fa84742b3c 207 void movement_ticker_deactivator()
gastongab 7:88fa84742b3c 208 {
gastongab 4:c7be673eb4a1 209 sample_ticker.detach();
gastongab 4:c7be673eb4a1 210 threshold_check_ticker.detach();
gastongab 7:88fa84742b3c 211 }
gastongab 7:88fa84742b3c 212
gastongab 7:88fa84742b3c 213 enum states {MOTORS_OFF,CALIBRATION,HOMING,DEMO,MOVEMENT,CLICK};
gastongab 2:0a8622662f6d 214 states currentState = MOTORS_OFF; //Chosen startingposition for states
aschut 0:90750f158475 215 bool stateChanged = true; // Make sure the initialization of first state is executed
aschut 0:90750f158475 216
aschut 0:90750f158475 217 void ProcessStateMachine(void)
aschut 0:90750f158475 218 {
gastongab 7:88fa84742b3c 219 switch (currentState) {
gastongab 7:88fa84742b3c 220 case MOTORS_OFF:
gastongab 7:88fa84742b3c 221 // Actions
gastongab 7:88fa84742b3c 222 if (stateChanged) {
gastongab 7:88fa84742b3c 223 // state initialization: rood
gastongab 7:88fa84742b3c 224 led1 = 1;
gastongab 7:88fa84742b3c 225 led2 = 0;
gastongab 7:88fa84742b3c 226 led3 = 1;
gastongab 7:88fa84742b3c 227 wait (1);
gastongab 7:88fa84742b3c 228 stateChanged = false;
gastongab 7:88fa84742b3c 229 }
gastongab 7:88fa84742b3c 230
gastongab 7:88fa84742b3c 231 // State transition logic: Als button 1 word ingedrukt --> calibratie, anders motor uithouden
gastongab 7:88fa84742b3c 232 if (!button1) {
gastongab 7:88fa84742b3c 233 currentState = CALIBRATION;
gastongab 7:88fa84742b3c 234 stateChanged = true;
gastongab 7:88fa84742b3c 235 } else if (!button2) {
gastongab 7:88fa84742b3c 236 currentState = HOMING ;
gastongab 7:88fa84742b3c 237 stateChanged = true;
gastongab 7:88fa84742b3c 238 } else {
gastongab 7:88fa84742b3c 239 currentState = MOTORS_OFF;
gastongab 7:88fa84742b3c 240 stateChanged = true;
gastongab 7:88fa84742b3c 241 }
gastongab 7:88fa84742b3c 242
gastongab 7:88fa84742b3c 243 break;
gastongab 7:88fa84742b3c 244
gastongab 7:88fa84742b3c 245 case CALIBRATION:
gastongab 7:88fa84742b3c 246 // Actions
gastongab 7:88fa84742b3c 247 if (stateChanged) {
gastongab 7:88fa84742b3c 248 // state initialization: oranje
gastongab 7:88fa84742b3c 249 temp_highest_emg1 = 0; //highest detected value right biceps
gastongab 7:88fa84742b3c 250 temp_highest_emg2 = 0;
gastongab 7:88fa84742b3c 251 temp_highest_emg3 = 0;
gastongab 7:88fa84742b3c 252 temp_highest_emg4 = 0;
gastongab 7:88fa84742b3c 253
gastongab 7:88fa84742b3c 254 timer_calibration.reset();
gastongab 7:88fa84742b3c 255 timer_calibration.start();
gastongab 7:88fa84742b3c 256
gastongab 7:88fa84742b3c 257 sample_ticker.attach(&emgsample, ts);
gastongab 7:88fa84742b3c 258 CalibrationEMG();
gastongab 7:88fa84742b3c 259 sample_ticker.detach();
gastongab 7:88fa84742b3c 260 timer_calibration.stop();
aschut 0:90750f158475 261
gastongab 7:88fa84742b3c 262
gastongab 7:88fa84742b3c 263 stateChanged = false;
gastongab 7:88fa84742b3c 264 }
gastongab 7:88fa84742b3c 265
gastongab 7:88fa84742b3c 266 // State transition logic: automatisch terug naar motors off.
gastongab 7:88fa84742b3c 267
gastongab 7:88fa84742b3c 268 currentState = MOTORS_OFF;
gastongab 7:88fa84742b3c 269 stateChanged = true;
gastongab 7:88fa84742b3c 270 break;
gastongab 7:88fa84742b3c 271
gastongab 7:88fa84742b3c 272 case HOMING:
gastongab 7:88fa84742b3c 273 // Actions
gastongab 7:88fa84742b3c 274 if (stateChanged) {
gastongab 7:88fa84742b3c 275 // state initialization: green
gastongab 7:88fa84742b3c 276 t.reset();
gastongab 7:88fa84742b3c 277 t.start();
gastongab 7:88fa84742b3c 278 led1 = 0;
gastongab 7:88fa84742b3c 279 led2 = 1;
gastongab 7:88fa84742b3c 280 led3 = 1;
gastongab 7:88fa84742b3c 281 wait (1);
gastongab 7:88fa84742b3c 282
gastongab 7:88fa84742b3c 283 stateChanged = false;
gastongab 7:88fa84742b3c 284 }
gastongab 7:88fa84742b3c 285
gastongab 7:88fa84742b3c 286 // State transition logic: naar DEMO (button1), naar MOVEMENT(button2)
gastongab 7:88fa84742b3c 287 if (!button1) {
gastongab 7:88fa84742b3c 288 currentState = DEMO;
gastongab 7:88fa84742b3c 289 stateChanged = true;
gastongab 7:88fa84742b3c 290 } else if (!button2) {
gastongab 7:88fa84742b3c 291 currentState = MOVEMENT ;
gastongab 7:88fa84742b3c 292 stateChanged = true;
gastongab 7:88fa84742b3c 293 } else if (t>300) {
gastongab 7:88fa84742b3c 294 t.stop();
gastongab 7:88fa84742b3c 295 t.reset();
gastongab 7:88fa84742b3c 296 currentState = MOTORS_OFF ;
gastongab 7:88fa84742b3c 297 stateChanged = true;
gastongab 7:88fa84742b3c 298 } else {
gastongab 7:88fa84742b3c 299 currentState = HOMING ;
gastongab 7:88fa84742b3c 300 stateChanged = true;
gastongab 7:88fa84742b3c 301 }
gastongab 7:88fa84742b3c 302 break;
gastongab 7:88fa84742b3c 303
aschut 0:90750f158475 304 case DEMO:
gastongab 7:88fa84742b3c 305 // Actions
gastongab 7:88fa84742b3c 306 if (stateChanged) {
gastongab 7:88fa84742b3c 307 // state initialization: light blue
gastongab 7:88fa84742b3c 308 led1 = 0;
gastongab 7:88fa84742b3c 309 led2 = 1;
gastongab 7:88fa84742b3c 310 led3 = 0;
gastongab 7:88fa84742b3c 311 wait (1);
gastongab 7:88fa84742b3c 312
gastongab 7:88fa84742b3c 313 stateChanged = false;
gastongab 7:88fa84742b3c 314 }
gastongab 7:88fa84742b3c 315
gastongab 7:88fa84742b3c 316 // State transition logic: automatisch terug naar HOMING
gastongab 7:88fa84742b3c 317 currentState = HOMING;
gastongab 7:88fa84742b3c 318 stateChanged = true;
gastongab 7:88fa84742b3c 319 break;
gastongab 7:88fa84742b3c 320
gastongab 7:88fa84742b3c 321 case MOVEMENT:
gastongab 7:88fa84742b3c 322 // Actions
gastongab 7:88fa84742b3c 323 if (stateChanged) {
gastongab 7:88fa84742b3c 324 // state initialization: purple
gastongab 7:88fa84742b3c 325 //t.reset();
gastongab 7:88fa84742b3c 326 //t.start();
gastongab 7:88fa84742b3c 327
gastongab 7:88fa84742b3c 328 led1 = 1;
gastongab 7:88fa84742b3c 329 led2 = 0;
gastongab 7:88fa84742b3c 330 led3 = 0;
gastongab 7:88fa84742b3c 331 wait(2);
gastongab 7:88fa84742b3c 332
gastongab 7:88fa84742b3c 333 movement_ticker_activator();
gastongab 7:88fa84742b3c 334
gastongab 7:88fa84742b3c 335 led1 = 0;
gastongab 7:88fa84742b3c 336 led2 = 0;
gastongab 7:88fa84742b3c 337 led3 = 0;
gastongab 7:88fa84742b3c 338 wait(2);
gastongab 7:88fa84742b3c 339
gastongab 7:88fa84742b3c 340
gastongab 7:88fa84742b3c 341 stateChanged = false;
gastongab 7:88fa84742b3c 342 }
gastongab 7:88fa84742b3c 343
gastongab 7:88fa84742b3c 344 // State transition logic: naar CLICK (button1), naar MOTORS_OFF(button2) anders naar MOVEMENT
gastongab 7:88fa84742b3c 345 if (!button1) {
gastongab 7:88fa84742b3c 346 movement_ticker_deactivator();
gastongab 7:88fa84742b3c 347 currentState = CLICK;
gastongab 7:88fa84742b3c 348 stateChanged = true;
gastongab 7:88fa84742b3c 349 } else if (!button2) {
gastongab 7:88fa84742b3c 350 movement_ticker_deactivator();
gastongab 7:88fa84742b3c 351 currentState = MOTORS_OFF ;
gastongab 7:88fa84742b3c 352 stateChanged = true;
gastongab 7:88fa84742b3c 353 } else if (bicepsR==0 && tricepsR==0 && bicepsL==0 && tricepsL==0) { //this check if person is idle for more than 300seconds
gastongab 7:88fa84742b3c 354 t.start();
gastongab 7:88fa84742b3c 355 } else if (bicepsR==1 || tricepsR==1 || bicepsL==1 || tricepsL==1) {
gastongab 7:88fa84742b3c 356 t.stop();
gastongab 7:88fa84742b3c 357 t.reset();
gastongab 7:88fa84742b3c 358 }
gastongab 7:88fa84742b3c 359
gastongab 7:88fa84742b3c 360 if(t>20) {
gastongab 7:88fa84742b3c 361 movement_ticker_deactivator();
gastongab 7:88fa84742b3c 362 t.stop();
gastongab 7:88fa84742b3c 363 t.reset();
gastongab 7:88fa84742b3c 364 currentState = HOMING ;
gastongab 7:88fa84742b3c 365 stateChanged = true;
gastongab 7:88fa84742b3c 366 }
gastongab 7:88fa84742b3c 367 // here ends the idle checking mode
gastongab 7:88fa84742b3c 368 else {
gastongab 7:88fa84742b3c 369 //For every muscle a different colour if threshold is passed
gastongab 7:88fa84742b3c 370 if(bicepsR==1) {
gastongab 7:88fa84742b3c 371 led1 = 0;
gastongab 7:88fa84742b3c 372 led2 = 1;
gastongab 7:88fa84742b3c 373 led3 = 1;
gastongab 7:88fa84742b3c 374 } else if (bicepsR==0 && tricepsR==0 && bicepsL==0 && tricepsL==0 ) {
gastongab 7:88fa84742b3c 375 led1 = 1;
gastongab 7:88fa84742b3c 376 led2 = 1;
gastongab 7:88fa84742b3c 377 led3 = 1;
gastongab 7:88fa84742b3c 378 }
gastongab 7:88fa84742b3c 379 if(tricepsR==1) {
gastongab 7:88fa84742b3c 380 led1 = 1;
gastongab 7:88fa84742b3c 381 led2 = 0;
gastongab 7:88fa84742b3c 382 led3 = 1;
gastongab 7:88fa84742b3c 383 } else if (bicepsR==0 && tricepsR==0 && bicepsL==0 && tricepsL==0 ) {
gastongab 7:88fa84742b3c 384 led1 = 1;
gastongab 7:88fa84742b3c 385 led2 = 1;
gastongab 7:88fa84742b3c 386 led3 = 1;
gastongab 7:88fa84742b3c 387 }
gastongab 7:88fa84742b3c 388 if(bicepsL==1) {
gastongab 7:88fa84742b3c 389 led1 = 1;
gastongab 7:88fa84742b3c 390 led2 = 1;
gastongab 7:88fa84742b3c 391 led3 = 0;
gastongab 7:88fa84742b3c 392 } else if (bicepsR==0 && tricepsR==0 && bicepsL==0 && tricepsL==0 ) {
gastongab 7:88fa84742b3c 393 led1 = 1;
gastongab 7:88fa84742b3c 394 led2 = 1;
gastongab 7:88fa84742b3c 395 led3 = 1;
gastongab 7:88fa84742b3c 396 }
gastongab 7:88fa84742b3c 397 if(tricepsL==1) {
gastongab 7:88fa84742b3c 398 led1 = 1;
gastongab 7:88fa84742b3c 399 led2 = 0;
gastongab 7:88fa84742b3c 400 led3 = 0;
gastongab 7:88fa84742b3c 401 } else if (bicepsR==0 && tricepsR==0 && bicepsL==0 && tricepsL==0 ) {
gastongab 7:88fa84742b3c 402 led1 = 1;
gastongab 7:88fa84742b3c 403 led2 = 1;
gastongab 7:88fa84742b3c 404 led3 = 1;
gastongab 7:88fa84742b3c 405 }
gastongab 7:88fa84742b3c 406 currentState = MOVEMENT ;
gastongab 7:88fa84742b3c 407 stateChanged = false;
gastongab 7:88fa84742b3c 408 }
gastongab 7:88fa84742b3c 409
gastongab 7:88fa84742b3c 410 break;
gastongab 7:88fa84742b3c 411
aschut 0:90750f158475 412 case CLICK:
gastongab 7:88fa84742b3c 413 // Actions
gastongab 7:88fa84742b3c 414 if (stateChanged) {
gastongab 7:88fa84742b3c 415 // state initialization: blue
gastongab 7:88fa84742b3c 416 led1 = 1;
gastongab 7:88fa84742b3c 417 led2 = 1;
gastongab 7:88fa84742b3c 418 led3 = 0;
gastongab 7:88fa84742b3c 419 wait (1);
aschut 0:90750f158475 420
gastongab 7:88fa84742b3c 421 stateChanged = false;
gastongab 7:88fa84742b3c 422 }
gastongab 7:88fa84742b3c 423
gastongab 7:88fa84742b3c 424 // State transition logic: automatisch terug naar MOVEMENT.
gastongab 7:88fa84742b3c 425
gastongab 7:88fa84742b3c 426 currentState = MOVEMENT;
gastongab 7:88fa84742b3c 427 stateChanged = true;
gastongab 7:88fa84742b3c 428 break;
gastongab 7:88fa84742b3c 429
gastongab 4:c7be673eb4a1 430 }
aschut 0:90750f158475 431 }
gastongab 7:88fa84742b3c 432
aschut 0:90750f158475 433 int main()
aschut 0:90750f158475 434 {
gastongab 4:c7be673eb4a1 435 //BiQuad Chain add
gastongab 4:c7be673eb4a1 436 highp1.add( &highp1_1 ).add( &highp1_2 );
gastongab 4:c7be673eb4a1 437 notch1.add( &notch1_1 ).add( &notch1_2 );
gastongab 4:c7be673eb4a1 438 lowp1.add( &lowp1_1 ).add(&lowp1_2);
gastongab 7:88fa84742b3c 439
gastongab 4:c7be673eb4a1 440 highp2.add( &highp2_1 ).add( &highp2_2 );
gastongab 4:c7be673eb4a1 441 notch2.add( &notch2_1 ).add( &notch2_2 );
gastongab 7:88fa84742b3c 442 lowp2.add( &lowp2_1 ).add(&lowp2_2);
gastongab 7:88fa84742b3c 443
gastongab 4:c7be673eb4a1 444 highp3.add( &highp3_1 ).add( &highp3_2 );
gastongab 4:c7be673eb4a1 445 notch3.add( &notch3_1 ).add( &notch3_2 );
gastongab 4:c7be673eb4a1 446 lowp3.add( &lowp3_1 ).add(&lowp3_2);
gastongab 7:88fa84742b3c 447
gastongab 4:c7be673eb4a1 448 highp4.add( &highp4_1 ).add( &highp4_2 );
gastongab 4:c7be673eb4a1 449 notch4.add( &notch4_1 ).add( &notch4_2 );
gastongab 4:c7be673eb4a1 450 lowp4.add( &lowp4_1 ).add(&lowp4_2);
gastongab 7:88fa84742b3c 451
gastongab 2:0a8622662f6d 452 pc.baud(115200);
aschut 0:90750f158475 453 led1 = 1;
aschut 0:90750f158475 454 led2 = 1;
aschut 0:90750f158475 455 led3 = 1;
gastongab 7:88fa84742b3c 456
gastongab 7:88fa84742b3c 457 while (true) {
gastongab 7:88fa84742b3c 458 ProcessStateMachine();
gastongab 7:88fa84742b3c 459
aschut 0:90750f158475 460 }
gastongab 7:88fa84742b3c 461
aschut 0:90750f158475 462 }
aschut 0:90750f158475 463
aschut 0:90750f158475 464
aschut 0:90750f158475 465
aschut 0:90750f158475 466
aschut 0:90750f158475 467
aschut 0:90750f158475 468