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