Astrid Schut / Mbed 2 deprecated StateMachineMyoArm

Volledig besturingssysteem voor de myoelectrische prothese arm van mijn bachelor opdracht

Dependencies:   mbed QEI MODSERIAL FastPWM biquadFilter

main.cpp@16:d60c845e149e, 2019-04-20 (annotated)

Committer:
aschut
Date:
Sat Apr 20 19:36:40 2019 +0000
Revision:
16:d60c845e149e
Parent:
15:26dd70c4a837 
Volledig besturing systeem voor de myoelectrische prothese arm van mijn Bachelor opdracht

Who changed what in which revision?

UserRevisionLine numberNew contents of line
aschut 14:f8e84d287a22 1 //libraries
aschut 0:90750f158475 2 #include "mbed.h"
aschut 2:7c9974f0947a 3 #include "MODSERIAL.h"
aschut 2:7c9974f0947a 4 #include "QEI.h"
aschut 2:7c9974f0947a 5 #include "BiQuad.h"
aschut 3:2aaf54ce090b 6 #include "FastPWM.h"
aschut 4:babe09a69296 7
aschut 14:f8e84d287a22 8 //~~~~~~~~~~~~ Algemeen ~~~~~~~~~~~~~~
aschut 6:464d2fdfd7de 9 DigitalIn button3(SW3);
aschut 6:464d2fdfd7de 10 DigitalIn button2(SW2);
aschut 16:d60c845e149e 11 DigitalIn But2(D12); //Button 2 van het motorshield
aschut 16:d60c845e149e 12 DigitalIn But1(D13); //Button 1 van het motorshield
aschut 0:90750f158475 13
aschut 0:90750f158475 14 DigitalOut led1(LED_GREEN);
aschut 0:90750f158475 15 DigitalOut led2(LED_RED);
aschut 0:90750f158475 16 DigitalOut led3(LED_BLUE);
aschut 0:90750f158475 17
aschut 2:7c9974f0947a 18 MODSERIAL pc(USBTX, USBRX);
aschut 14:f8e84d287a22 19 Timer t; // State timer, na vijf minuten gaat de state automatisch terug naar een vorige state
aschut 14:f8e84d287a22 20 Timer t2; // Timer voor de grafiek in matlab
aschut 14:f8e84d287a22 21 int count = 0; // Counter voor de grafiek in matlab
aschut 14:f8e84d287a22 22 float counts = 0; // Counter voor de MOVEMENT state, zorgt ervoor dat de tickers niet steeds opnieuw attached worden
aschut 14:f8e84d287a22 23
aschut 6:464d2fdfd7de 24
aschut 14:f8e84d287a22 25 //~~~~~~~~ Motor waardes ~~~~~~~~~~~~~~~
aschut 14:f8e84d287a22 26 DigitalOut direction1(D4); // Motor 1 is de rotatie motor
aschut 6:464d2fdfd7de 27 FastPWM pwmpin1(D5);
aschut 14:f8e84d287a22 28 FastPWM pwmpin2(D6); // Motor 2 is de motor in de elleboog
aschut 2:7c9974f0947a 29 DigitalOut direction2(D7);
aschut 14:f8e84d287a22 30 volatile float pwm1; //Deze 'pwm' wordt gemaakt met de potmeters, wordt gebruikt voor het testen, maar niet voor de uiteindelijke besturing
aschut 2:7c9974f0947a 31 volatile float pwm2;
aschut 2:7c9974f0947a 32
aschut 2:7c9974f0947a 33 //Encoder
aschut 8:7fd9ac522ea9 34 QEI encoder1 (D1, D0, NC, 1200, QEI::X4_ENCODING);
aschut 8:7fd9ac522ea9 35 QEI encoder2 (D3, D2, NC, 4800, QEI::X4_ENCODING);
aschut 14:f8e84d287a22 36 double Pulses1; // Afgelezen Encoder pulses
aschut 14:f8e84d287a22 37 double motor_position1; // De berekende hoek
aschut 2:7c9974f0947a 38 double Pulses2;
aschut 2:7c9974f0947a 39 double motor_position2;
aschut 14:f8e84d287a22 40 double error1; // Berekende error tussen de berekende hoek en de referentie hoek
aschut 14:f8e84d287a22 41 double error2;
aschut 14:f8e84d287a22 42 double u1; // PWM signaal dat naar de motor gaat
aschut 14:f8e84d287a22 43 double u2;
aschut 2:7c9974f0947a 44
aschut 2:7c9974f0947a 45 //Pot meter
aschut 14:f8e84d287a22 46 AnalogIn pot(A5); // De potmeters worden alleen gebruikt bij het testen
aschut 2:7c9974f0947a 47 AnalogIn pot0(A0);
aschut 2:7c9974f0947a 48 float Pot2;
aschut 2:7c9974f0947a 49 float Pot1;
aschut 2:7c9974f0947a 50
aschut 2:7c9974f0947a 51 //Ticker
aschut 14:f8e84d287a22 52 Ticker Pwm; // Ticker om de motoren te laten bewegen
aschut 14:f8e84d287a22 53 Ticker PotRead; // Ticker voor de Continuous read functie
aschut 14:f8e84d287a22 54 Ticker ServoTick; // Ticker om een PWM signaal met een periode van 20 ms bij een DigitalOut pin te creeëren
aschut 2:7c9974f0947a 55
aschut 4:babe09a69296 56 //Servo
aschut 14:f8e84d287a22 57 DigitalOut myservo1(D8); //Duim
aschut 14:f8e84d287a22 58 DigitalOut myservo2(D11); //Pink tot Middel vinger
aschut 14:f8e84d287a22 59 DigitalOut myservo3(D10); //wijsvinger
aschut 6:464d2fdfd7de 60
aschut 14:f8e84d287a22 61 float Periodlength = 0.02; // de MG996R Servo heeft een PWM periode van 20 ms
aschut 14:f8e84d287a22 62 double servo_position1; // in percentage van 0 tot 1, 0 is met de klok mee, 1 is tegen de klok in.
aschut 9:ad4bd1b16eed 63 double servo_position2;
aschut 9:ad4bd1b16eed 64 double servo_position3;
aschut 6:464d2fdfd7de 65
aschut 6:464d2fdfd7de 66 // Vinger posities
aschut 6:464d2fdfd7de 67 float Duim_krom = 0.05;
aschut 6:464d2fdfd7de 68 float Duim_recht = 0.85;
aschut 6:464d2fdfd7de 69 float MWP_krom = 0.06;
aschut 6:464d2fdfd7de 70 float MWP_recht = 0.89;
aschut 6:464d2fdfd7de 71 float Wijsvinger_krom = 0.15;
aschut 6:464d2fdfd7de 72 float Wijsvinger_recht = 0.93;
aschut 6:464d2fdfd7de 73
aschut 4:babe09a69296 74
aschut 14:f8e84d287a22 75
aschut 3:2aaf54ce090b 76
aschut 9:ad4bd1b16eed 77
aschut 14:f8e84d287a22 78 //~~~~~~~~ EMG waardes ~~~~~~~~~~~~~~~~~
aschut 9:ad4bd1b16eed 79 //Input
aschut 9:ad4bd1b16eed 80 AnalogIn emg1( A1 ); //Duim
aschut 9:ad4bd1b16eed 81 AnalogIn emg2( A2 ); //Bicep
aschut 9:ad4bd1b16eed 82 AnalogIn emg3( A3 ); //Dorsaal
aschut 9:ad4bd1b16eed 83 AnalogIn emg4( A4 ); //Palmair
aschut 9:ad4bd1b16eed 84
aschut 14:f8e84d287a22 85 // Tickers
aschut 14:f8e84d287a22 86 Ticker sample_ticker; //ticker voor filteren met 1000Hz
aschut 14:f8e84d287a22 87 Ticker sample_timer; //ticker voor printen van de uiteindelijke EMG waardes met 2 Hz
aschut 14:f8e84d287a22 88 Ticker threshold_check_ticker; //ticker voor het checken van de threshold met 100Hz
aschut 14:f8e84d287a22 89 Timer timer_calibration; //timer voor EMG Kalibratie
aschut 14:f8e84d287a22 90 double ts = 0.001; //tijdsstap
aschut 9:ad4bd1b16eed 91
aschut 14:f8e84d287a22 92 float Input1; //Input voor motoren
aschut 14:f8e84d287a22 93 float Input2;
aschut 9:ad4bd1b16eed 94
aschut 14:f8e84d287a22 95 //Gefilterde EMG signalen
aschut 14:f8e84d287a22 96 volatile double temp_highest_emg1 = 0; //Hoogste waarde gevonden tijdens kalibratie
aschut 9:ad4bd1b16eed 97 volatile double temp_highest_emg2 = 0;
aschut 9:ad4bd1b16eed 98 volatile double temp_highest_emg3 = 0;
aschut 9:ad4bd1b16eed 99 volatile double temp_highest_emg4 = 0;
aschut 9:ad4bd1b16eed 100
aschut 9:ad4bd1b16eed 101 //Percentage van de hoogste waarde waar de bovenste treshold gezet moet worden
aschut 14:f8e84d287a22 102 double Duim_p_t = 0.5;
aschut 14:f8e84d287a22 103 double Bicep_p_t = 0.4;
aschut 14:f8e84d287a22 104 double Dorsaal_p_t = 0.6;
aschut 12:20ae1d20148d 105 double Palmair_p_t = 0.5;
aschut 9:ad4bd1b16eed 106
aschut 9:ad4bd1b16eed 107 //Percentage van de hoogste waarde waar de onderste treshold gezet moet worden
aschut 14:f8e84d287a22 108 double Duim_p_tL = 0.5;
aschut 14:f8e84d287a22 109 double Bicep_p_tL = 0.4;
aschut 14:f8e84d287a22 110 double Dorsaal_p_tL = 0.5;
aschut 9:ad4bd1b16eed 111 double Palmair_p_tL = 0.5;
aschut 9:ad4bd1b16eed 112
aschut 9:ad4bd1b16eed 113 // Waarde bovenste treshold waar het signaal overheen moet om de arm te activeren
aschut 9:ad4bd1b16eed 114 volatile double threshold1;
aschut 9:ad4bd1b16eed 115 volatile double threshold2;
aschut 9:ad4bd1b16eed 116 volatile double threshold3;
aschut 9:ad4bd1b16eed 117 volatile double threshold4;
aschut 9:ad4bd1b16eed 118
aschut 9:ad4bd1b16eed 119 // Waarde onderste treshold waar het signaal onder moet om de arm te deactiveren
aschut 9:ad4bd1b16eed 120 volatile double threshold1L;
aschut 9:ad4bd1b16eed 121 volatile double threshold2L;
aschut 9:ad4bd1b16eed 122 volatile double threshold3L;
aschut 9:ad4bd1b16eed 123 volatile double threshold4L;
aschut 9:ad4bd1b16eed 124
aschut 14:f8e84d287a22 125 bool thresholdreach1 = false; // Of de waarde boven de bovenste threshold is geweest
aschut 14:f8e84d287a22 126 bool thresholdreach2 = false;
aschut 14:f8e84d287a22 127 bool thresholdreach3 = false;
aschut 14:f8e84d287a22 128 bool thresholdreach4 = false;
aschut 14:f8e84d287a22 129
aschut 9:ad4bd1b16eed 130 // thresholdreads bools
aschut 11:08ba9cfc9c8f 131 bool Duim;
aschut 11:08ba9cfc9c8f 132 bool Bicep;
aschut 11:08ba9cfc9c8f 133 bool Dorsaal;
aschut 11:08ba9cfc9c8f 134 bool Palmair;
aschut 9:ad4bd1b16eed 135
aschut 9:ad4bd1b16eed 136 // filters
aschut 14:f8e84d287a22 137 volatile double emg1_filtered, emg2_filtered, emg3_filtered, emg4_filtered;
aschut 14:f8e84d287a22 138
aschut 14:f8e84d287a22 139 //EMG1
aschut 9:ad4bd1b16eed 140 //Highpass vierde orde cutoff 20Hz, Band filter om 49, 50, 51Hz eruit te filteren
aschut 9:ad4bd1b16eed 141 BiQuadChain highp1;
aschut 9:ad4bd1b16eed 142 BiQuad highp1_1( 0.8485, -1.6969, 0.8485, 1.0000, -1.7783, 0.7924 );
aschut 9:ad4bd1b16eed 143 BiQuad highp1_2( 1.0000, -2.0000, 1.0000, 1.0000, -1.8934, 0.9085 );
aschut 9:ad4bd1b16eed 144 BiQuad notch1_1( 0.9907, -1.8843, 0.9907, 1.0000, -1.8843, 0.9813 );
aschut 9:ad4bd1b16eed 145
aschut 9:ad4bd1b16eed 146 //Lowpass first order cutoff 0.4Hz
aschut 9:ad4bd1b16eed 147 BiQuad lowp1( 0.0013, 0.0013, 0, 1.0000, -0.9975, 0 );
aschut 9:ad4bd1b16eed 148
aschut 14:f8e84d287a22 149 //EMG2
aschut 9:ad4bd1b16eed 150 //Highpass vierde orde cutoff 20Hz, Band filter om 49, 50, 51Hz eruit te filteren
aschut 9:ad4bd1b16eed 151 BiQuadChain highp2;
aschut 9:ad4bd1b16eed 152 BiQuad highp2_1( 0.8485, -1.6969, 0.8485, 1.0000, -1.7783, 0.7924 );
aschut 9:ad4bd1b16eed 153 BiQuad highp2_2( 1.0000, -2.0000, 1.0000, 1.0000, -1.8934, 0.9085 );
aschut 9:ad4bd1b16eed 154 BiQuad notch2_1( 0.9907, -1.8843, 0.9907, 1.0000, -1.8843, 0.9813 );
aschut 9:ad4bd1b16eed 155
aschut 9:ad4bd1b16eed 156 //Lowpass first order cutoff 0.4Hz
aschut 9:ad4bd1b16eed 157 BiQuad lowp2( 0.0013, 0.0013, 0, 1.0000, -0.9975, 0 );
aschut 9:ad4bd1b16eed 158
aschut 14:f8e84d287a22 159 //EMG3
aschut 9:ad4bd1b16eed 160 //Highpass vierde orde cutoff 20Hz, Band filter om 49, 50, 51Hz eruit te filteren
aschut 9:ad4bd1b16eed 161 BiQuadChain highp3;
aschut 9:ad4bd1b16eed 162 BiQuad highp3_1( 0.8485, -1.6969, 0.8485, 1.0000, -1.7783, 0.7924 );
aschut 9:ad4bd1b16eed 163 BiQuad highp3_2( 1.0000, -2.0000, 1.0000, 1.0000, -1.8934, 0.9085 );
aschut 9:ad4bd1b16eed 164 BiQuad notch3_1( 0.9907, -1.8843, 0.9907, 1.0000, -1.8843, 0.9813 );
aschut 9:ad4bd1b16eed 165
aschut 9:ad4bd1b16eed 166 //Lowpass first order cutoff 0.4Hz
aschut 9:ad4bd1b16eed 167 BiQuad lowp3( 0.0013, 0.0013, 0, 1.0000, -0.9975, 0 );
aschut 9:ad4bd1b16eed 168
aschut 14:f8e84d287a22 169 //EMG4
aschut 9:ad4bd1b16eed 170 //Highpass vierde orde cutoff 20Hz, Band filter om 49, 50, 51Hz eruit te filteren
aschut 9:ad4bd1b16eed 171 BiQuadChain highp4;
aschut 9:ad4bd1b16eed 172 BiQuad highp4_1( 0.8485, -1.6969, 0.8485, 1.0000, -1.7783, 0.7924 );
aschut 9:ad4bd1b16eed 173 BiQuad highp4_2( 1.0000, -2.0000, 1.0000, 1.0000, -1.8934, 0.9085 );
aschut 9:ad4bd1b16eed 174 BiQuad notch4_1( 0.9907, -1.8843, 0.9907, 1.0000, -1.8843, 0.9813 );
aschut 9:ad4bd1b16eed 175
aschut 9:ad4bd1b16eed 176 //Lowpass first order cutoff 0.4Hz
aschut 9:ad4bd1b16eed 177 BiQuad lowp4( 0.0013, 0.0013, 0, 1.0000, -0.9975, 0 );
aschut 9:ad4bd1b16eed 178
aschut 14:f8e84d287a22 179 //~~~~~~~~ Kinematica ~~~~~~~~~~~~~~~~~~
aschut 14:f8e84d287a22 180 double stap1; //Stap die bij de referentie hoek wordt opgeteld bij een EMG signaal van 1
aschut 2:7c9974f0947a 181 double stap2;
aschut 2:7c9974f0947a 182 double KPot;
aschut 2:7c9974f0947a 183
aschut 14:f8e84d287a22 184 float ElbowReference;
aschut 2:7c9974f0947a 185 float Ellebooghoek1;
aschut 2:7c9974f0947a 186 float Ellebooghoek2;
aschut 2:7c9974f0947a 187 float Ellebooghoek3;
aschut 2:7c9974f0947a 188 float Ellebooghoek4;
aschut 2:7c9974f0947a 189
aschut 2:7c9974f0947a 190 float PolsReference;
aschut 2:7c9974f0947a 191 float Polshoek1;
aschut 2:7c9974f0947a 192 float Polshoek2;
aschut 2:7c9974f0947a 193 float Polshoek3;
aschut 2:7c9974f0947a 194 float Polshoek4;
aschut 2:7c9974f0947a 195
aschut 2:7c9974f0947a 196 float Hoeknieuw1;
aschut 2:7c9974f0947a 197 float Hoeknieuw2;
aschut 2:7c9974f0947a 198
aschut 2:7c9974f0947a 199 //Limiet in graden
aschut 14:f8e84d287a22 200 float lowerlim1 = -900; //Rotatie motor gewricht limiet, gelijk aan 180 graden rotatie beide kanten op
aschut 3:2aaf54ce090b 201 float upperlim1 = 900;
aschut 14:f8e84d287a22 202 float lowerlim2 = 0; // Elleboog limiet, van 0 graden (helemaal gebogen) tot 110 graden (helemaal gestrekt)
aschut 6:464d2fdfd7de 203 float upperlim2 = 1500;
aschut 2:7c9974f0947a 204
aschut 14:f8e84d287a22 205 //~~~~~~~~ PID controler waardes ~~~~~~
aschut 6:464d2fdfd7de 206 double Kp1 = 12.5;
aschut 6:464d2fdfd7de 207 double Ki1 = 0;
aschut 2:7c9974f0947a 208 double Kd1 = 1;
aschut 14:f8e84d287a22 209 double Kp2 = 20;
aschut 6:464d2fdfd7de 210 double Ki2 = 0;
aschut 6:464d2fdfd7de 211 double Kd2 = 1;
aschut 14:f8e84d287a22 212 double Ts = 0.0005; // tijdstap voor pwm ticker
aschut 2:7c9974f0947a 213
aschut 9:ad4bd1b16eed 214
aschut 14:f8e84d287a22 215 // ~~~~~~~~~~~~~~~~~~~ EMG functies ~~~~~~~~~~~~~~~~~~
aschut 9:ad4bd1b16eed 216 void emgsample()
aschut 9:ad4bd1b16eed 217 {
aschut 14:f8e84d287a22 218
aschut 9:ad4bd1b16eed 219 // EMG signaal lezen
aschut 9:ad4bd1b16eed 220 double emgread1 = emg1.read();
aschut 9:ad4bd1b16eed 221 double emgread2 = emg2.read();
aschut 9:ad4bd1b16eed 222 double emgread3 = emg3.read();
aschut 9:ad4bd1b16eed 223 double emgread4 = emg4.read();
aschut 14:f8e84d287a22 224
aschut 9:ad4bd1b16eed 225 // Vierde orde highpass filter + notch filter
aschut 9:ad4bd1b16eed 226 double emg1_highpassed = highp1.step(emgread1);
aschut 9:ad4bd1b16eed 227 double emg2_highpassed = highp2.step(emgread2);
aschut 9:ad4bd1b16eed 228 double emg3_highpassed = highp3.step(emgread3);
aschut 9:ad4bd1b16eed 229 double emg4_highpassed = highp4.step(emgread4);
aschut 9:ad4bd1b16eed 230
aschut 9:ad4bd1b16eed 231 //Rectificatie
aschut 9:ad4bd1b16eed 232 double emg1_abs = abs(emg1_highpassed);
aschut 9:ad4bd1b16eed 233 double emg2_abs = abs(emg2_highpassed);
aschut 9:ad4bd1b16eed 234 double emg3_abs = abs(emg3_highpassed);
aschut 9:ad4bd1b16eed 235 double emg4_abs = abs(emg4_highpassed);
aschut 9:ad4bd1b16eed 236
aschut 9:ad4bd1b16eed 237 //All EMG abs into lowpass
aschut 9:ad4bd1b16eed 238 emg1_filtered = lowp1.step(emg1_abs);
aschut 9:ad4bd1b16eed 239 emg2_filtered = lowp2.step(emg2_abs);
aschut 9:ad4bd1b16eed 240 emg3_filtered = lowp3.step(emg3_abs);
aschut 9:ad4bd1b16eed 241 emg4_filtered = lowp4.step(emg4_abs);
aschut 9:ad4bd1b16eed 242
aschut 9:ad4bd1b16eed 243 }
aschut 9:ad4bd1b16eed 244
aschut 9:ad4bd1b16eed 245 //Check of emg_filtered boven de threshold is
aschut 9:ad4bd1b16eed 246 void threshold_check()
aschut 9:ad4bd1b16eed 247 {
aschut 9:ad4bd1b16eed 248
aschut 14:f8e84d287a22 249 // EMG1 Check
aschut 14:f8e84d287a22 250 if (thresholdreach1 == false) { //Als emg_filtered nog niet boven de bovenste threshold is geweest
aschut 14:f8e84d287a22 251 //bovenste threshold check
aschut 14:f8e84d287a22 252 if(emg1_filtered>threshold1) {
aschut 14:f8e84d287a22 253 Duim = 1;
aschut 14:f8e84d287a22 254 thresholdreach1 = true;
aschut 14:f8e84d287a22 255
aschut 14:f8e84d287a22 256 } else {
aschut 14:f8e84d287a22 257 Duim= 0;
aschut 14:f8e84d287a22 258 }
aschut 14:f8e84d287a22 259 } else { //Als emg_filtered boven de bovenste threshold is geweest
aschut 14:f8e84d287a22 260 //onderste threshold check
aschut 14:f8e84d287a22 261 if(emg1_filtered<threshold1L) {
aschut 14:f8e84d287a22 262 Duim = 0;
aschut 14:f8e84d287a22 263 thresholdreach1 = false;
aschut 14:f8e84d287a22 264
aschut 14:f8e84d287a22 265 } else {
aschut 14:f8e84d287a22 266 Duim= 1;
aschut 14:f8e84d287a22 267 }
aschut 9:ad4bd1b16eed 268 }
aschut 9:ad4bd1b16eed 269
aschut 14:f8e84d287a22 270 // EMG2 Check
aschut 14:f8e84d287a22 271 if (thresholdreach2 == false) { //Als emg_filtered nog niet boven de bovenste threshold is geweest
aschut 14:f8e84d287a22 272 //bovenste threshold check
aschut 14:f8e84d287a22 273 if(emg2_filtered>threshold2) {
aschut 14:f8e84d287a22 274 Bicep = 1;
aschut 14:f8e84d287a22 275 thresholdreach2 = true;
aschut 14:f8e84d287a22 276 } else {
aschut 14:f8e84d287a22 277 Bicep= 0;
aschut 14:f8e84d287a22 278 }
aschut 14:f8e84d287a22 279 } else { //Als emg_filtered boven de bovenste threshold is geweest
aschut 14:f8e84d287a22 280 //onderste threshold check
aschut 14:f8e84d287a22 281 if(emg2_filtered<threshold2L) {
aschut 14:f8e84d287a22 282 Bicep = 0;
aschut 14:f8e84d287a22 283 thresholdreach2 = false;
aschut 14:f8e84d287a22 284
aschut 14:f8e84d287a22 285 } else {
aschut 14:f8e84d287a22 286 Bicep= 1;
aschut 14:f8e84d287a22 287 }
aschut 9:ad4bd1b16eed 288 }
aschut 9:ad4bd1b16eed 289
aschut 14:f8e84d287a22 290 // EMG3 Check
aschut 14:f8e84d287a22 291 if (thresholdreach3 == false) { //Als emg_filtered nog niet boven de bovenste threshold is geweest
aschut 14:f8e84d287a22 292 //bovenste threshold check
aschut 14:f8e84d287a22 293 if(emg3_filtered>threshold3) {
aschut 14:f8e84d287a22 294 Dorsaal = 1;
aschut 14:f8e84d287a22 295 thresholdreach3 = true;
aschut 14:f8e84d287a22 296 } else {
aschut 14:f8e84d287a22 297 Dorsaal= 0;
aschut 14:f8e84d287a22 298 }
aschut 14:f8e84d287a22 299 } else { //Als emg_filtered boven de bovenste threshold is geweest
aschut 14:f8e84d287a22 300 //onderste threshold check
aschut 14:f8e84d287a22 301 if(emg3_filtered<threshold3L) {
aschut 14:f8e84d287a22 302 Dorsaal = 0;
aschut 14:f8e84d287a22 303 thresholdreach3 = false;
aschut 14:f8e84d287a22 304
aschut 14:f8e84d287a22 305 } else {
aschut 14:f8e84d287a22 306 Dorsaal= 1;
aschut 14:f8e84d287a22 307 }
aschut 9:ad4bd1b16eed 308 }
aschut 9:ad4bd1b16eed 309
aschut 14:f8e84d287a22 310 // EMG4 Check
aschut 14:f8e84d287a22 311 if (thresholdreach4 == false) { //Als emg_filtered nog niet boven de bovenste threshold is geweest
aschut 14:f8e84d287a22 312 //bovenste threshold check
aschut 14:f8e84d287a22 313 if(emg4_filtered>threshold4) {
aschut 14:f8e84d287a22 314 Palmair = 1;
aschut 14:f8e84d287a22 315 thresholdreach4 = true;
aschut 14:f8e84d287a22 316 } else {
aschut 14:f8e84d287a22 317 Palmair= 0;
aschut 14:f8e84d287a22 318 }
aschut 14:f8e84d287a22 319 } else { //Als emg_filtered boven de bovenste threshold is geweest
aschut 14:f8e84d287a22 320 //onderste threshold check
aschut 14:f8e84d287a22 321 if(emg4_filtered<threshold4L) {
aschut 14:f8e84d287a22 322 Palmair = 0;
aschut 14:f8e84d287a22 323 thresholdreach4 = false;
aschut 14:f8e84d287a22 324
aschut 14:f8e84d287a22 325 } else {
aschut 14:f8e84d287a22 326 Palmair= 1;
aschut 14:f8e84d287a22 327 }
aschut 9:ad4bd1b16eed 328 }
aschut 9:ad4bd1b16eed 329
aschut 14:f8e84d287a22 330 }
aschut 9:ad4bd1b16eed 331
aschut 14:f8e84d287a22 332 //~~~~~~~~~ Functies Kinematica ~~~~~~~~~~~~
aschut 14:f8e84d287a22 333 float Kinematics1(float Bicep)
aschut 2:7c9974f0947a 334 {
aschut 2:7c9974f0947a 335
aschut 14:f8e84d287a22 336 if (Bicep == 1 && Duim == 0) { //Als alleen Bicep flexie wordt afgelezen gaat de waarde van de referentie hoek omhoog
aschut 12:20ae1d20148d 337 stap1 = 450*Ts;
aschut 2:7c9974f0947a 338 Hoeknieuw1 = PolsReference + stap1;
aschut 2:7c9974f0947a 339 return Hoeknieuw1;
aschut 2:7c9974f0947a 340 }
aschut 2:7c9974f0947a 341
aschut 14:f8e84d287a22 342 else if (Bicep == 1 && Duim == 1) { //Als Bicep en Duim flexie wordt afgelezen gaat de waarde van de referentie hoek omlaag
aschut 12:20ae1d20148d 343 stap1 = 450*Ts;
aschut 12:20ae1d20148d 344 Hoeknieuw1 = PolsReference - stap1;
aschut 2:7c9974f0947a 345 return Hoeknieuw1;
aschut 2:7c9974f0947a 346 }
aschut 2:7c9974f0947a 347
aschut 2:7c9974f0947a 348 else {
aschut 2:7c9974f0947a 349 return PolsReference;
aschut 2:7c9974f0947a 350 }
aschut 2:7c9974f0947a 351 }
aschut 11:08ba9cfc9c8f 352 float Kinematics2(bool Dorsaal)
aschut 2:7c9974f0947a 353 {
aschut 2:7c9974f0947a 354
aschut 14:f8e84d287a22 355 if (Dorsaal == 1 && Duim == 0) { //Als alleen Dorsaal flexie wordt afgelezen gaat de waarde van de referentie hoek omhoog
aschut 11:08ba9cfc9c8f 356 stap2 = 300*Ts;
aschut 2:7c9974f0947a 357 Hoeknieuw2 = ElbowReference + stap2;
aschut 2:7c9974f0947a 358 return Hoeknieuw2;
aschut 2:7c9974f0947a 359 }
aschut 2:7c9974f0947a 360
aschut 14:f8e84d287a22 361 else if (Dorsaal == 1 && Duim == 1) { //Als Dorsaal en Duim flexie wordt afgelezen gaat de waarde van de referentie hoek omhoog
aschut 11:08ba9cfc9c8f 362 stap2 = 300*Ts;
aschut 11:08ba9cfc9c8f 363 Hoeknieuw2 = ElbowReference - stap2;
aschut 2:7c9974f0947a 364 return Hoeknieuw2;
aschut 2:7c9974f0947a 365 }
aschut 2:7c9974f0947a 366
aschut 2:7c9974f0947a 367 else {
aschut 2:7c9974f0947a 368 return ElbowReference;
aschut 2:7c9974f0947a 369 }
aschut 2:7c9974f0947a 370 }
aschut 2:7c9974f0947a 371
aschut 2:7c9974f0947a 372 float Limits1(float Polshoek2)
aschut 2:7c9974f0947a 373 {
aschut 2:7c9974f0947a 374
aschut 14:f8e84d287a22 375 if (Polshoek2 <= upperlim1 && Polshoek2 >= lowerlim1) { //Binnen de limieten
aschut 2:7c9974f0947a 376 Polshoek3 = Polshoek2;
aschut 2:7c9974f0947a 377 }
aschut 2:7c9974f0947a 378
aschut 2:7c9974f0947a 379 else {
aschut 14:f8e84d287a22 380 if (Polshoek2 >= upperlim1) { //Boven de limiet
aschut 2:7c9974f0947a 381 Polshoek3 = upperlim1;
aschut 2:7c9974f0947a 382 } else { //Onder de limiet
aschut 2:7c9974f0947a 383 Polshoek3 = lowerlim1;
aschut 2:7c9974f0947a 384 }
aschut 2:7c9974f0947a 385 }
aschut 2:7c9974f0947a 386
aschut 2:7c9974f0947a 387 return Polshoek3;
aschut 2:7c9974f0947a 388 }
aschut 2:7c9974f0947a 389
aschut 2:7c9974f0947a 390
aschut 2:7c9974f0947a 391 float Limits2(float Ellebooghoek2)
aschut 2:7c9974f0947a 392 {
aschut 0:90750f158475 393
aschut 14:f8e84d287a22 394 if (Ellebooghoek2 <= upperlim2 && Ellebooghoek2 >= lowerlim2) { //Binnen de limieten
aschut 2:7c9974f0947a 395 Ellebooghoek3 = Ellebooghoek2;
aschut 2:7c9974f0947a 396 }
aschut 2:7c9974f0947a 397
aschut 2:7c9974f0947a 398 else {
aschut 14:f8e84d287a22 399 if (Ellebooghoek2 >= upperlim2) { //Boven de limiet
aschut 2:7c9974f0947a 400 Ellebooghoek3 = upperlim2;
aschut 2:7c9974f0947a 401 } else { //Onder de limiet
aschut 2:7c9974f0947a 402 Ellebooghoek3 = lowerlim2;
aschut 2:7c9974f0947a 403 }
aschut 2:7c9974f0947a 404 }
aschut 2:7c9974f0947a 405
aschut 2:7c9974f0947a 406 return Ellebooghoek3;
aschut 2:7c9974f0947a 407 }
aschut 2:7c9974f0947a 408
aschut 2:7c9974f0947a 409
aschut 14:f8e84d287a22 410 //~~~~~~~~ PID Controller~~~~~~~~~~~~~~~
aschut 2:7c9974f0947a 411 double PID_controller1(double error1)
aschut 2:7c9974f0947a 412 {
aschut 2:7c9974f0947a 413 static double error1_integral = 0;
aschut 2:7c9974f0947a 414 static double error1_prev = error1; // initialization with this value only done once!
aschut 14:f8e84d287a22 415 static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
aschut 6:464d2fdfd7de 416
aschut 2:7c9974f0947a 417 // Proportional part:
aschut 2:7c9974f0947a 418 double u_k1 = Kp1 * error1;
aschut 2:7c9974f0947a 419
aschut 2:7c9974f0947a 420 // Integral part
aschut 2:7c9974f0947a 421 error1_integral = error1_integral + error1 * Ts;
aschut 2:7c9974f0947a 422 double u_i1 = Ki1* error1_integral;
aschut 2:7c9974f0947a 423
aschut 2:7c9974f0947a 424 // Derivative part
aschut 2:7c9974f0947a 425 double error1_derivative = (error1 - error1_prev)/Ts;
aschut 2:7c9974f0947a 426 double filtered_error1_derivative = LowPassFilter.step(error1_derivative);
aschut 2:7c9974f0947a 427 double u_d1 = Kd1 * filtered_error1_derivative;
aschut 2:7c9974f0947a 428 error1_prev = error1;
aschut 2:7c9974f0947a 429
aschut 2:7c9974f0947a 430 // Sum all parts and return it
aschut 2:7c9974f0947a 431 return u_k1 + u_i1 + u_d1;
aschut 2:7c9974f0947a 432 }
aschut 2:7c9974f0947a 433 double PID_controller2(double error2)
aschut 2:7c9974f0947a 434 {
aschut 2:7c9974f0947a 435 static double error2_integral = 0;
aschut 2:7c9974f0947a 436 static double error2_prev = error2; // initialization with this value only done once!
aschut 2:7c9974f0947a 437 static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241); //(BIQUAD_FILTER_TYPE type, T dbGain, T freq, T srate, T bandwidth);
aschut 6:464d2fdfd7de 438
aschut 2:7c9974f0947a 439 // Proportional part:
aschut 2:7c9974f0947a 440 double u_k2 = Kp2 * error2;
aschut 2:7c9974f0947a 441
aschut 2:7c9974f0947a 442 // Integral part
aschut 2:7c9974f0947a 443 error2_integral = error2_integral + error2 * Ts;
aschut 2:7c9974f0947a 444 double u_i2 = Ki2 * error2_integral;
aschut 2:7c9974f0947a 445
aschut 2:7c9974f0947a 446 // Derivative part
aschut 2:7c9974f0947a 447 double error2_derivative = (error2 - error2_prev)/Ts;
aschut 2:7c9974f0947a 448 double filtered_error2_derivative = LowPassFilter.step(error2_derivative);
aschut 2:7c9974f0947a 449 double u_d2 = Kd2 * filtered_error2_derivative;
aschut 2:7c9974f0947a 450 error2_prev = error2;
aschut 2:7c9974f0947a 451
aschut 2:7c9974f0947a 452 // Sum all parts and return it
aschut 2:7c9974f0947a 453 return u_k2 + u_i2 + u_d2;
aschut 2:7c9974f0947a 454 }
aschut 2:7c9974f0947a 455
aschut 14:f8e84d287a22 456 //~~~~~~~~~~~~ Functies Motor ~~~~~~
aschut 2:7c9974f0947a 457 void moter1_control(double u1)
aschut 2:7c9974f0947a 458 {
aschut 14:f8e84d287a22 459 direction1= u1 > 0.0f; //positief = CW
aschut 6:464d2fdfd7de 460 if (fabs(u1)> 0.7f) {
aschut 6:464d2fdfd7de 461 u1 = 0.7f;
aschut 2:7c9974f0947a 462 } else {
aschut 2:7c9974f0947a 463 u1= u1;
aschut 2:7c9974f0947a 464 }
aschut 14:f8e84d287a22 465 pwmpin1.write(fabs(u1)) ;
aschut 2:7c9974f0947a 466 }
aschut 2:7c9974f0947a 467
aschut 2:7c9974f0947a 468 void moter2_control(double u2)
aschut 2:7c9974f0947a 469 {
aschut 14:f8e84d287a22 470 direction2= u2 < 0.0f; //positief = CW
aschut 2:7c9974f0947a 471 if (fabs(u2)> 0.99f) {
aschut 2:7c9974f0947a 472 u2 = 0.99f;
aschut 2:7c9974f0947a 473 } else {
aschut 2:7c9974f0947a 474 u2= u2;
aschut 2:7c9974f0947a 475 }
aschut 14:f8e84d287a22 476 pwmpin2.write(fabs(u2)) ;
aschut 2:7c9974f0947a 477 }
aschut 2:7c9974f0947a 478
aschut 2:7c9974f0947a 479 void PwmMotor(void)
aschut 2:7c9974f0947a 480 {
aschut 5:ef77da99d0d1 481 //Continues Read
aschut 14:f8e84d287a22 482 Pot2 = pot.read(); //Potmeters alleen voor testen
aschut 5:ef77da99d0d1 483 Pot1 = pot0.read();
aschut 5:ef77da99d0d1 484 pwm2 =(Pot2*2)-1; //scaling naar -1 tot 1
aschut 5:ef77da99d0d1 485 pwm1 =(Pot1*2)-1;
aschut 14:f8e84d287a22 486 Input1 = Bicep; //Voor testen hier potmeter waarde 'pwm1'invullen
aschut 12:20ae1d20148d 487 Input2 = Palmair;
aschut 14:f8e84d287a22 488
aschut 14:f8e84d287a22 489 // Reference hoek berekenen, in graden
aschut 3:2aaf54ce090b 490 float Polshoek1 = Kinematics1(Input1);
aschut 3:2aaf54ce090b 491 float Polshoek4 = Limits1(Polshoek1);
aschut 3:2aaf54ce090b 492 PolsReference = Polshoek4;
aschut 6:464d2fdfd7de 493
aschut 2:7c9974f0947a 494 // Reference hoek berekenen, in graden
aschut 3:2aaf54ce090b 495 float Ellebooghoek1 = Kinematics2(Input2);
aschut 2:7c9974f0947a 496 float Ellebooghoek4 = Limits2(Ellebooghoek1);
aschut 2:7c9974f0947a 497 ElbowReference = Ellebooghoek4;
aschut 6:464d2fdfd7de 498
aschut 2:7c9974f0947a 499 // Positie motor berekenen, in graden
aschut 2:7c9974f0947a 500 Pulses1 = encoder1.getPulses();
aschut 2:7c9974f0947a 501 motor_position1 = -(Pulses1/1200)*360;
aschut 2:7c9974f0947a 502 Pulses2 = encoder2.getPulses();
aschut 2:7c9974f0947a 503 motor_position2 = -(Pulses2/4800)*360;
aschut 14:f8e84d287a22 504
aschut 14:f8e84d287a22 505 // Error berekenen, naar PID controller en dan naar motor
aschut 2:7c9974f0947a 506 double error1 = PolsReference - motor_position1;
aschut 2:7c9974f0947a 507 double u1 = PID_controller1(error1);
aschut 2:7c9974f0947a 508 moter1_control(u1);
aschut 6:464d2fdfd7de 509
aschut 2:7c9974f0947a 510 double error2 = ElbowReference - motor_position2;
aschut 2:7c9974f0947a 511 double u2 = PID_controller2(error2);
aschut 2:7c9974f0947a 512 moter2_control(u2);
aschut 2:7c9974f0947a 513
aschut 2:7c9974f0947a 514 }
aschut 2:7c9974f0947a 515
aschut 2:7c9974f0947a 516 void MotorOn(void)
aschut 2:7c9974f0947a 517 {
aschut 2:7c9974f0947a 518 pwmpin1 = 0;
aschut 2:7c9974f0947a 519 pwmpin2 = 0;
aschut 2:7c9974f0947a 520 Pwm.attach (PwmMotor, Ts);
aschut 2:7c9974f0947a 521
aschut 2:7c9974f0947a 522 }
aschut 2:7c9974f0947a 523
aschut 14:f8e84d287a22 524 //~~~~~~~~~~ Servo functies ~~~~~~~~~~~~
aschut 14:f8e84d287a22 525 void servowait1(void)~ //creeërt een PWM periode van 20 ms met een pulslengte tussen de 500 en 2500 microseconden
aschut 6:464d2fdfd7de 526 {
aschut 6:464d2fdfd7de 527 double Pulslength1 = 0.0005 + (servo_position1 * 0.002); //in seconden, waarde tussen de 500 en de 2500 microseconden, als de waarde omhoog gaat beweegt de servo tegen de klok in
aschut 6:464d2fdfd7de 528 myservo1 = true;
aschut 6:464d2fdfd7de 529 wait(Pulslength1);
aschut 6:464d2fdfd7de 530 myservo1 = false;
aschut 6:464d2fdfd7de 531 }
aschut 6:464d2fdfd7de 532
aschut 6:464d2fdfd7de 533 void servowait2(void)
aschut 6:464d2fdfd7de 534 {
aschut 6:464d2fdfd7de 535 double Pulslength2 = 0.0005 + (servo_position2 * 0.002); //in seconden
aschut 6:464d2fdfd7de 536 myservo2 = true;
aschut 6:464d2fdfd7de 537 wait(Pulslength2);
aschut 6:464d2fdfd7de 538 myservo2 = false;
aschut 6:464d2fdfd7de 539 }
aschut 6:464d2fdfd7de 540
aschut 6:464d2fdfd7de 541 void servowait3(void)
aschut 6:464d2fdfd7de 542 {
aschut 6:464d2fdfd7de 543 double Pulslength3 = 0.0005 + (servo_position3 * 0.002); //in seconden
aschut 6:464d2fdfd7de 544 myservo3 = true;
aschut 6:464d2fdfd7de 545 wait(Pulslength3);
aschut 6:464d2fdfd7de 546 myservo3 = false;
aschut 6:464d2fdfd7de 547 }
aschut 6:464d2fdfd7de 548
aschut 4:babe09a69296 549 void ServoPeriod()
aschut 4:babe09a69296 550 {
aschut 6:464d2fdfd7de 551 servowait1();
aschut 6:464d2fdfd7de 552 servowait2();
aschut 6:464d2fdfd7de 553 servowait3();
aschut 4:babe09a69296 554 }
aschut 2:7c9974f0947a 555
aschut 6:464d2fdfd7de 556
aschut 6:464d2fdfd7de 557
aschut 14:f8e84d287a22 558 void ContinuousReader(void) //leest potmeters af
aschut 2:7c9974f0947a 559 {
aschut 2:7c9974f0947a 560 Pot2 = pot.read();
aschut 2:7c9974f0947a 561 Pot1 = pot0.read();
aschut 14:f8e84d287a22 562 pwm2 =(Pot2*2)-1; //scaling naar -1 tot 1
aschut 2:7c9974f0947a 563 pwm1 =(Pot1*2)-1;
aschut 2:7c9974f0947a 564 }
aschut 2:7c9974f0947a 565
aschut 14:f8e84d287a22 566 void sample() //Print uiteindelijke EMG output
aschut 14:f8e84d287a22 567 {
aschut 14:f8e84d287a22 568 pc.printf("Duim = %i\r\n", Duim);
aschut 14:f8e84d287a22 569 pc.printf("Bicep = %i\r\n",Bicep);
aschut 14:f8e84d287a22 570 pc.printf("Dorsaal = %i\r\n", Dorsaal);
aschut 14:f8e84d287a22 571 pc.printf("Palmair = %i\r\n", Palmair);
aschut 14:f8e84d287a22 572
aschut 9:ad4bd1b16eed 573 }
aschut 9:ad4bd1b16eed 574
aschut 2:7c9974f0947a 575 // StateMachine
aschut 2:7c9974f0947a 576
aschut 6:464d2fdfd7de 577 enum states {MOTORS_OFF,CALIBRATION,HOMING1,HOMING2,DEMO,MOVEMENT,FREEZE};
aschut 0:90750f158475 578 int f = 1;
aschut 6:464d2fdfd7de 579 states currentState = MOTORS_OFF;
aschut 0:90750f158475 580 bool stateChanged = true; // Make sure the initialization of first state is executed
aschut 0:90750f158475 581
aschut 0:90750f158475 582 void ProcessStateMachine(void)
aschut 0:90750f158475 583 {
aschut 6:464d2fdfd7de 584 switch (currentState) {
aschut 6:464d2fdfd7de 585 case MOTORS_OFF:
aschut 6:464d2fdfd7de 586 // Actions
aschut 6:464d2fdfd7de 587 if (stateChanged) {
aschut 6:464d2fdfd7de 588 // state initialization: rood
aschut 6:464d2fdfd7de 589
aschut 6:464d2fdfd7de 590 led1 = 1;
aschut 6:464d2fdfd7de 591 led2 = 0;
aschut 6:464d2fdfd7de 592 led3 = 1;
aschut 6:464d2fdfd7de 593 wait (1);
aschut 6:464d2fdfd7de 594 stateChanged = false;
aschut 6:464d2fdfd7de 595 }
aschut 2:7c9974f0947a 596
aschut 14:f8e84d287a22 597 // State transition logic: Als button 3 word ingedrukt --> calibratie, anders motor uithouden
aschut 6:464d2fdfd7de 598 if (!button3) {
aschut 6:464d2fdfd7de 599 currentState = CALIBRATION ;
aschut 6:464d2fdfd7de 600 stateChanged = true;
aschut 6:464d2fdfd7de 601 } else {
aschut 6:464d2fdfd7de 602 currentState = MOTORS_OFF;
aschut 6:464d2fdfd7de 603 stateChanged = true;
aschut 6:464d2fdfd7de 604 }
aschut 6:464d2fdfd7de 605
aschut 6:464d2fdfd7de 606 break;
aschut 6:464d2fdfd7de 607
aschut 6:464d2fdfd7de 608 case CALIBRATION:
aschut 6:464d2fdfd7de 609 // Actions
aschut 6:464d2fdfd7de 610 if (stateChanged) {
aschut 6:464d2fdfd7de 611 // state initialization: oranje
aschut 4:babe09a69296 612 led1 = 0;
aschut 4:babe09a69296 613 led2 = 0;
aschut 4:babe09a69296 614 led3 = 1;
aschut 9:ad4bd1b16eed 615 sample_ticker.attach(&emgsample, 0.001); // Leest het ruwe EMG signaal af met een frequentie van 1000Hz
aschut 9:ad4bd1b16eed 616 timer_calibration.reset();
aschut 9:ad4bd1b16eed 617 timer_calibration.start();
aschut 14:f8e84d287a22 618 while(timer_calibration<20) { //Binnen 20 seconden alle spieren maximaal aanspannen, hoogste waarde wordt opgeslagen
aschut 9:ad4bd1b16eed 619 if(timer_calibration>0 && timer_calibration<20) {
aschut 10:193942c3900a 620
aschut 9:ad4bd1b16eed 621 if(emg1_filtered>temp_highest_emg1) {
aschut 9:ad4bd1b16eed 622 temp_highest_emg1= emg1_filtered;
aschut 9:ad4bd1b16eed 623 pc.printf("Highest value Duim= %f \r\n", temp_highest_emg1);
aschut 9:ad4bd1b16eed 624 }
aschut 6:464d2fdfd7de 625
aschut 9:ad4bd1b16eed 626 if(emg2_filtered>temp_highest_emg2) {
aschut 9:ad4bd1b16eed 627 temp_highest_emg2= emg2_filtered;
aschut 9:ad4bd1b16eed 628 pc.printf("Highest value Bicep= %f \r\n", temp_highest_emg2);
aschut 9:ad4bd1b16eed 629 }
aschut 9:ad4bd1b16eed 630
aschut 9:ad4bd1b16eed 631 if(emg3_filtered>temp_highest_emg3) {
aschut 9:ad4bd1b16eed 632 temp_highest_emg3= emg3_filtered;
aschut 9:ad4bd1b16eed 633 pc.printf("Highest value Dorsaal= %f \r\n", temp_highest_emg3);
aschut 9:ad4bd1b16eed 634 }
aschut 9:ad4bd1b16eed 635
aschut 9:ad4bd1b16eed 636 if(emg4_filtered>temp_highest_emg4) {
aschut 9:ad4bd1b16eed 637 temp_highest_emg4= emg4_filtered;
aschut 9:ad4bd1b16eed 638 pc.printf("Highest value Palmair= %f \r\n", temp_highest_emg4);
aschut 9:ad4bd1b16eed 639 }
aschut 9:ad4bd1b16eed 640
aschut 9:ad4bd1b16eed 641 }
aschut 9:ad4bd1b16eed 642
aschut 9:ad4bd1b16eed 643 }
aschut 9:ad4bd1b16eed 644 pc.printf("threshold calculation\r\n");
aschut 14:f8e84d287a22 645 threshold1 = temp_highest_emg1*Duim_p_t; //Percentage van de hoogste waarde wordt gebruikt om de bovenste en onderste treshold aan te geven
aschut 9:ad4bd1b16eed 646 threshold2 = temp_highest_emg2*Bicep_p_t;
aschut 9:ad4bd1b16eed 647 threshold3 = temp_highest_emg3*Dorsaal_p_t;
aschut 9:ad4bd1b16eed 648 threshold4 = temp_highest_emg4*Palmair_p_t;
aschut 9:ad4bd1b16eed 649
aschut 9:ad4bd1b16eed 650 threshold1L = temp_highest_emg1*Duim_p_tL;
aschut 9:ad4bd1b16eed 651 threshold2L = temp_highest_emg2*Bicep_p_tL;
aschut 9:ad4bd1b16eed 652 threshold3L = temp_highest_emg3*Dorsaal_p_tL;
aschut 9:ad4bd1b16eed 653 threshold4L = temp_highest_emg4*Palmair_p_tL;
aschut 4:babe09a69296 654 stateChanged = false;
aschut 6:464d2fdfd7de 655
aschut 6:464d2fdfd7de 656 }
aschut 9:ad4bd1b16eed 657 sample_ticker.detach();
aschut 12:20ae1d20148d 658 timer_calibration.stop();
aschut 14:f8e84d287a22 659 // State transition logic: automatisch naar HOMING 1
aschut 6:464d2fdfd7de 660
aschut 6:464d2fdfd7de 661 currentState = HOMING1;
aschut 6:464d2fdfd7de 662 stateChanged = true;
aschut 6:464d2fdfd7de 663 break;
aschut 6:464d2fdfd7de 664
aschut 14:f8e84d287a22 665 case HOMING1: //Gebruik de knoppen op het motor shield om het rotatie gewricht naar de beginpositie te brengen
aschut 6:464d2fdfd7de 666 // Actions
aschut 6:464d2fdfd7de 667 if (stateChanged) {
aschut 6:464d2fdfd7de 668 // state initialization: green
aschut 6:464d2fdfd7de 669 t.start();
aschut 6:464d2fdfd7de 670 led1 = 0;
aschut 6:464d2fdfd7de 671 led2 = 1;
aschut 6:464d2fdfd7de 672 led3 = 1;
aschut 0:90750f158475 673
aschut 6:464d2fdfd7de 674 if (!But1) {
aschut 6:464d2fdfd7de 675 led1 = 1;
aschut 6:464d2fdfd7de 676 float H1 = 0.99f;
aschut 6:464d2fdfd7de 677 moter1_control(H1);
aschut 6:464d2fdfd7de 678 wait(0.001f);
aschut 6:464d2fdfd7de 679 } else if (!But2) {
aschut 6:464d2fdfd7de 680 led1 = 1;
aschut 6:464d2fdfd7de 681 float H1 = -0.99f;
aschut 6:464d2fdfd7de 682 moter1_control(H1);
aschut 6:464d2fdfd7de 683 wait(0.001f);
aschut 6:464d2fdfd7de 684 }
aschut 6:464d2fdfd7de 685 encoder1.reset();
aschut 6:464d2fdfd7de 686 motor_position1 = 0;
aschut 6:464d2fdfd7de 687 pwmpin1 = 0;
aschut 6:464d2fdfd7de 688 pwmpin2 = 0;
aschut 6:464d2fdfd7de 689 ;
aschut 6:464d2fdfd7de 690
aschut 6:464d2fdfd7de 691 stateChanged = false;
aschut 6:464d2fdfd7de 692 }
aschut 6:464d2fdfd7de 693
aschut 14:f8e84d287a22 694 // State transition logic: naar HOMING 2(button3), na 5 min naar MOTORS_OFF
aschut 6:464d2fdfd7de 695
aschut 6:464d2fdfd7de 696 if (!button3) {
aschut 12:20ae1d20148d 697 t.stop();
aschut 12:20ae1d20148d 698 t.reset();
aschut 6:464d2fdfd7de 699 currentState = HOMING2 ;
aschut 6:464d2fdfd7de 700 stateChanged = true;
aschut 6:464d2fdfd7de 701 wait(1);
aschut 6:464d2fdfd7de 702 } else if (t>300) {
aschut 6:464d2fdfd7de 703 t.stop();
aschut 6:464d2fdfd7de 704 t.reset();
aschut 6:464d2fdfd7de 705 currentState = MOTORS_OFF ;
aschut 6:464d2fdfd7de 706 stateChanged = true;
aschut 6:464d2fdfd7de 707 } else {
aschut 6:464d2fdfd7de 708 currentState = HOMING1 ;
aschut 6:464d2fdfd7de 709 stateChanged = true;
aschut 6:464d2fdfd7de 710 }
aschut 6:464d2fdfd7de 711 break;
aschut 2:7c9974f0947a 712
aschut 14:f8e84d287a22 713 case HOMING2: // gebruik knoppen op het motor shield om het elleboog gewricht naar de begin positie te brengen ( helemaal gebogen)
aschut 6:464d2fdfd7de 714 // Actions
aschut 14:f8e84d287a22 715
aschut 6:464d2fdfd7de 716 if (stateChanged) {
aschut 6:464d2fdfd7de 717 // state initialization: white
aschut 6:464d2fdfd7de 718 t.start();
aschut 6:464d2fdfd7de 719 led1 = 0;
aschut 6:464d2fdfd7de 720 led2 = 0;
aschut 6:464d2fdfd7de 721 led3 = 0;
aschut 6:464d2fdfd7de 722
aschut 6:464d2fdfd7de 723 if (!But1) {
aschut 6:464d2fdfd7de 724 led1 = 1;
aschut 6:464d2fdfd7de 725 float H2 = 0.98f;
aschut 6:464d2fdfd7de 726 moter2_control(H2);
aschut 6:464d2fdfd7de 727 wait(0.001f);
aschut 6:464d2fdfd7de 728 } else if (!But2) {
aschut 6:464d2fdfd7de 729 led1 = 1;
aschut 6:464d2fdfd7de 730 float H2 = -0.98f;
aschut 6:464d2fdfd7de 731 moter2_control(H2);
aschut 6:464d2fdfd7de 732 wait(0.001f);
aschut 6:464d2fdfd7de 733 }
aschut 6:464d2fdfd7de 734 encoder2.reset();
aschut 6:464d2fdfd7de 735 motor_position2 = 0;
aschut 6:464d2fdfd7de 736 pwmpin1 = 0;
aschut 6:464d2fdfd7de 737 pwmpin2 = 0;
aschut 6:464d2fdfd7de 738 ;
aschut 6:464d2fdfd7de 739
aschut 6:464d2fdfd7de 740 stateChanged = false;
aschut 6:464d2fdfd7de 741 }
aschut 6:464d2fdfd7de 742
aschut 14:f8e84d287a22 743 // State transition logic: naar DEMO (button2), naar MOVEMENT(button3), na 5 min. automatisch naar MOTORS OFF
aschut 6:464d2fdfd7de 744 if (!button2) {
aschut 6:464d2fdfd7de 745 currentState = DEMO;
aschut 6:464d2fdfd7de 746 stateChanged = true;
aschut 12:20ae1d20148d 747 t.stop();
aschut 12:20ae1d20148d 748 t.reset();
aschut 6:464d2fdfd7de 749 } else if (!button3) {
aschut 6:464d2fdfd7de 750 currentState = MOVEMENT ;
aschut 6:464d2fdfd7de 751 stateChanged = true;
aschut 12:20ae1d20148d 752 t.stop();
aschut 12:20ae1d20148d 753 t.reset();
aschut 6:464d2fdfd7de 754 } else if (t>300) {
aschut 6:464d2fdfd7de 755 t.stop();
aschut 6:464d2fdfd7de 756 t.reset();
aschut 6:464d2fdfd7de 757 currentState = MOTORS_OFF ;
aschut 6:464d2fdfd7de 758 stateChanged = true;
aschut 6:464d2fdfd7de 759 } else {
aschut 6:464d2fdfd7de 760 currentState = HOMING2 ;
aschut 6:464d2fdfd7de 761 stateChanged = true;
aschut 6:464d2fdfd7de 762 }
aschut 6:464d2fdfd7de 763 break;
aschut 6:464d2fdfd7de 764
aschut 14:f8e84d287a22 765 case DEMO: // Mogelijkheid om hier je demo routine in te zetten
aschut 6:464d2fdfd7de 766 // Actions
aschut 6:464d2fdfd7de 767 if (stateChanged) {
aschut 6:464d2fdfd7de 768 // state initialization: light blue
aschut 6:464d2fdfd7de 769 led1 = 0;
aschut 6:464d2fdfd7de 770 led2 = 1;
aschut 6:464d2fdfd7de 771 led3 = 0;
aschut 6:464d2fdfd7de 772
aschut 6:464d2fdfd7de 773 servo_position1 = 0.5;
aschut 6:464d2fdfd7de 774 servo_position2 = 0.5;
aschut 6:464d2fdfd7de 775 servo_position3 = 0.5;
aschut 6:464d2fdfd7de 776 ServoTick.attach(&ServoPeriod, Periodlength);
aschut 2:7c9974f0947a 777
aschut 6:464d2fdfd7de 778 wait(1.5 );
aschut 6:464d2fdfd7de 779 servo_position1 = 0.9;
aschut 6:464d2fdfd7de 780 servo_position2 = 0.9;
aschut 6:464d2fdfd7de 781 servo_position3 = 0.9;
aschut 6:464d2fdfd7de 782 wait(1.5);
aschut 6:464d2fdfd7de 783 servo_position1 = 0.1;
aschut 6:464d2fdfd7de 784 servo_position2 = 0.1;
aschut 6:464d2fdfd7de 785 servo_position3 = 0.1;
aschut 6:464d2fdfd7de 786 wait(1);
aschut 6:464d2fdfd7de 787
aschut 6:464d2fdfd7de 788 ServoTick.detach();
aschut 6:464d2fdfd7de 789
aschut 6:464d2fdfd7de 790
aschut 6:464d2fdfd7de 791 wait (1);
aschut 6:464d2fdfd7de 792
aschut 6:464d2fdfd7de 793 stateChanged = false;
aschut 6:464d2fdfd7de 794 }
aschut 6:464d2fdfd7de 795
aschut 14:f8e84d287a22 796 // State transition logic: automatisch terug naar HOMING2
aschut 6:464d2fdfd7de 797 currentState = HOMING2;
aschut 6:464d2fdfd7de 798 stateChanged = true;
aschut 6:464d2fdfd7de 799 break;
aschut 6:464d2fdfd7de 800
aschut 14:f8e84d287a22 801 case MOVEMENT: // Hier worden de motoren bestuurd met EMG input
aschut 6:464d2fdfd7de 802 // Actions
aschut 6:464d2fdfd7de 803
aschut 6:464d2fdfd7de 804 if (stateChanged) {
aschut 6:464d2fdfd7de 805 // state initialization: purple
aschut 6:464d2fdfd7de 806 t.start(); // na 5 minuten terug naar Homing
aschut 6:464d2fdfd7de 807 led1 = 1;
aschut 6:464d2fdfd7de 808 led2 = 0;
aschut 6:464d2fdfd7de 809 led3 = 0;
aschut 14:f8e84d287a22 810
aschut 6:464d2fdfd7de 811 // Tickers aan
aschut 6:464d2fdfd7de 812 if (counts == 0) {
aschut 6:464d2fdfd7de 813 pwmpin1 = 0;
aschut 6:464d2fdfd7de 814 pwmpin2 = 0;
aschut 6:464d2fdfd7de 815 Input1 = pwm1;
aschut 6:464d2fdfd7de 816 Input2 = pwm2;
aschut 14:f8e84d287a22 817
aschut 14:f8e84d287a22 818 Pwm.attach (PwmMotor, Ts); // Motoren gaan aan
aschut 10:193942c3900a 819 sample_ticker.attach(&emgsample, 0.001); // Leest het ruwe EMG signaal af met een frequentie van 1000Hz
aschut 14:f8e84d287a22 820 threshold_check_ticker.attach(&threshold_check, 0.01); //Checkt of threshold overgeschreden wordt
aschut 14:f8e84d287a22 821 sample_timer.attach(&sample, 0.5); // print de EMG waardes
aschut 6:464d2fdfd7de 822 servo_position1 = Duim_krom;
aschut 6:464d2fdfd7de 823 servo_position2 = MWP_krom;
aschut 6:464d2fdfd7de 824 servo_position3 = Wijsvinger_krom;
aschut 14:f8e84d287a22 825
aschut 14:f8e84d287a22 826 ServoTick.attach(&ServoPeriod, Periodlength); //Servo's gaan aan
aschut 6:464d2fdfd7de 827 counts++;
aschut 14:f8e84d287a22 828 wait(1); //Zorgt ervoor dat hij niet doorschiet naar de volgende staat als je te lang de knop indrukt
aschut 6:464d2fdfd7de 829 }
aschut 14:f8e84d287a22 830
aschut 6:464d2fdfd7de 831 // Servo positie
aschut 6:464d2fdfd7de 832
aschut 14:f8e84d287a22 833 if (Dorsaal == 1 && Duim == 0) {
aschut 6:464d2fdfd7de 834 servo_position1 = Duim_krom;
aschut 6:464d2fdfd7de 835 servo_position2 = MWP_krom;
aschut 6:464d2fdfd7de 836 servo_position3 = Wijsvinger_krom;
aschut 6:464d2fdfd7de 837 led1 = !led1;
aschut 6:464d2fdfd7de 838 }
aschut 12:20ae1d20148d 839 if (Dorsaal == 1 && Duim == 1) {
aschut 6:464d2fdfd7de 840 servo_position1 = Duim_recht;
aschut 6:464d2fdfd7de 841 servo_position2 = MWP_recht;
aschut 6:464d2fdfd7de 842 servo_position3 = Wijsvinger_recht;
aschut 6:464d2fdfd7de 843 led1 = !led1;
aschut 6:464d2fdfd7de 844 }
aschut 3:2aaf54ce090b 845
aschut 6:464d2fdfd7de 846 stateChanged = false;
aschut 6:464d2fdfd7de 847 }
aschut 6:464d2fdfd7de 848
aschut 15:26dd70c4a837 849 // State transition logic: naar FREEZE (button2), naar MOTORS_OFF(button3), na vijf min automatisch naar HOMING, anders naar MOVEMENT
aschut 6:464d2fdfd7de 850 if (!button2) {
aschut 6:464d2fdfd7de 851 currentState = FREEZE;
aschut 6:464d2fdfd7de 852 stateChanged = true;
aschut 12:20ae1d20148d 853 threshold_check_ticker.detach();
aschut 12:20ae1d20148d 854 t.stop();
aschut 12:20ae1d20148d 855 t.reset();
aschut 6:464d2fdfd7de 856 } else if (!button3) {
aschut 6:464d2fdfd7de 857 Pwm.detach ();
aschut 6:464d2fdfd7de 858 ServoTick.detach();
aschut 10:193942c3900a 859 sample_ticker.detach();
aschut 11:08ba9cfc9c8f 860 threshold_check_ticker.detach();
aschut 11:08ba9cfc9c8f 861 sample_timer.detach();
aschut 12:20ae1d20148d 862 t.stop();
aschut 12:20ae1d20148d 863 t.reset();
aschut 6:464d2fdfd7de 864 pwmpin2 = 0;
aschut 6:464d2fdfd7de 865 pwmpin1 = 0;
aschut 6:464d2fdfd7de 866 counts = 0;
aschut 6:464d2fdfd7de 867 currentState = MOTORS_OFF ;
aschut 6:464d2fdfd7de 868 stateChanged = true;
aschut 6:464d2fdfd7de 869 } else if (t>300) {
aschut 6:464d2fdfd7de 870 t.stop();
aschut 6:464d2fdfd7de 871 t.reset();
aschut 6:464d2fdfd7de 872 Pwm.detach ();
aschut 6:464d2fdfd7de 873 ServoTick.detach();
aschut 10:193942c3900a 874 sample_ticker.detach();
aschut 11:08ba9cfc9c8f 875 sample_timer.detach();
aschut 11:08ba9cfc9c8f 876 threshold_check_ticker.detach();
aschut 6:464d2fdfd7de 877 counts = 0;
aschut 6:464d2fdfd7de 878 currentState = HOMING1 ;
aschut 6:464d2fdfd7de 879 stateChanged = true;
aschut 6:464d2fdfd7de 880 } else {
aschut 6:464d2fdfd7de 881 currentState = MOVEMENT ;
aschut 6:464d2fdfd7de 882 stateChanged = true;
aschut 6:464d2fdfd7de 883 }
aschut 6:464d2fdfd7de 884 break;
aschut 6:464d2fdfd7de 885
aschut 15:26dd70c4a837 886 case FREEZE: //Koppelt motoren los van EMG input, de motoren zullen niet meer bewegen
aschut 6:464d2fdfd7de 887 // Actions
aschut 6:464d2fdfd7de 888 if (stateChanged) {
aschut 6:464d2fdfd7de 889 // state initialization: blue
aschut 12:20ae1d20148d 890 Duim = 0;
aschut 12:20ae1d20148d 891 Bicep = 0;
aschut 12:20ae1d20148d 892 Dorsaal = 0;
aschut 12:20ae1d20148d 893 Palmair = 0;
aschut 6:464d2fdfd7de 894 led1 = 1;
aschut 6:464d2fdfd7de 895 led2 = 1;
aschut 6:464d2fdfd7de 896 led3 = 0;
aschut 12:20ae1d20148d 897 wait(1);
aschut 6:464d2fdfd7de 898 stateChanged = false;
aschut 6:464d2fdfd7de 899 }
aschut 6:464d2fdfd7de 900
aschut 15:26dd70c4a837 901 // State transition logic: button 2 terug naar MOVEMENT.
aschut 12:20ae1d20148d 902 if (!button2) {
aschut 12:20ae1d20148d 903 currentState = MOVEMENT;
aschut 12:20ae1d20148d 904 stateChanged = true;
aschut 12:20ae1d20148d 905 counts = 0;
aschut 14:f8e84d287a22 906 } else {
aschut 12:20ae1d20148d 907 currentState = FREEZE ;
aschut 12:20ae1d20148d 908 stateChanged = false;
aschut 14:f8e84d287a22 909 }
aschut 14:f8e84d287a22 910
aschut 6:464d2fdfd7de 911 break;
aschut 6:464d2fdfd7de 912
aschut 6:464d2fdfd7de 913 }
aschut 0:90750f158475 914 }
aschut 6:464d2fdfd7de 915
aschut 0:90750f158475 916 int main()
aschut 0:90750f158475 917 {
aschut 6:464d2fdfd7de 918
aschut 15:26dd70c4a837 919 t2.start(); //Timer voor printen naar matlab
aschut 15:26dd70c4a837 920 int counter = 0; //Counter voor printen naar matlab
aschut 15:26dd70c4a837 921 pwmpin1.period_us(60); // PWM periode voor motoren is 60 microseconden
aschut 15:26dd70c4a837 922 //PotRead.attach(ContinuousReader,Ts); //Gebruikt in test fase voor aflezen potmeters
aschut 15:26dd70c4a837 923 pc.baud(115200); // om naar de PC te printen
aschut 2:7c9974f0947a 924
aschut 15:26dd70c4a837 925 //BiQuad Chain add (voegt de filters samen tot een reeks)
aschut 10:193942c3900a 926 highp1.add( &highp1_1 ).add( &highp1_2 ).add( &notch1_1 );
aschut 10:193942c3900a 927 highp2.add( &highp2_1 ).add( &highp2_2 ).add( &notch2_1 );
aschut 10:193942c3900a 928 highp3.add( &highp3_1 ).add( &highp3_2 ).add( &notch3_1 );
aschut 10:193942c3900a 929 highp4.add( &highp4_1 ).add( &highp4_2 ).add( &notch4_1 );
aschut 10:193942c3900a 930
aschut 6:464d2fdfd7de 931 while(true) {
aschut 6:464d2fdfd7de 932 led1 = 1;
aschut 6:464d2fdfd7de 933 led2 =1;
aschut 6:464d2fdfd7de 934 led3 =1;
aschut 15:26dd70c4a837 935 /* //Dit is voor het printen naar matlab
aschut 6:464d2fdfd7de 936 if(counter==10)
aschut 6:464d2fdfd7de 937 {
aschut 6:464d2fdfd7de 938 float tmp = t2.read();
aschut 6:464d2fdfd7de 939 printf("%f,%f,%f,%f\n\r",tmp,motor_position2,ElbowReference,Pulses2);
aschut 6:464d2fdfd7de 940 counter = 0;
aschut 6:464d2fdfd7de 941 }
aschut 6:464d2fdfd7de 942 counter++;
aschut 6:464d2fdfd7de 943 */
aschut 15:26dd70c4a837 944 ProcessStateMachine(); //Zet de statemachine aan
aschut 6:464d2fdfd7de 945
aschut 6:464d2fdfd7de 946
aschut 6:464d2fdfd7de 947 wait(0.001);
aschut 0:90750f158475 948 }
aschut 6:464d2fdfd7de 949 }
aschut 0:90750f158475 950
aschut 0:90750f158475 951
aschut 0:90750f158475 952
aschut 0:90750f158475 953
aschut 0:90750f158475 954
aschut 0:90750f158475 955
aschut 2:7c9974f0947a 956
aschut 6:464d2fdfd7de 957
aschut 6:464d2fdfd7de 958
aschut 6:464d2fdfd7de 959
aschut 6:464d2fdfd7de 960
aschut 6:464d2fdfd7de 961
aschut 6:464d2fdfd7de 962