![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
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?
User | Revision | Line number | New 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( ¬ch1_1 ); |
aschut | 10:193942c3900a | 927 | highp2.add( &highp2_1 ).add( &highp2_2 ).add( ¬ch2_1 ); |
aschut | 10:193942c3900a | 928 | highp3.add( &highp3_1 ).add( &highp3_2 ).add( ¬ch3_1 ); |
aschut | 10:193942c3900a | 929 | highp4.add( &highp4_1 ).add( &highp4_2 ).add( ¬ch4_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 |