Gaston Gabriël / Mbed 2 deprecated Robot_Project_Jerry_6

FINAL VERSION

Dependencies:   biquadFilter MODSERIAL QEI Servo mbed

Fork of Robot_Battle_met_ARVID by Gaston Gabriël

main.cpp@5:19f59a855475, 2018-11-01 (annotated)

Committer:
gastongab
Date:
Thu Nov 01 15:29:32 2018 +0000
Revision:
5:19f59a855475
Parent:
4:c7be673eb4a1
Child:
6:f55ab7e38a7f
EMG HELEMAAL KLAAR 16:29

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