adsf
Dependencies: Encoder HIDScope MODSERIAL Matrix MatrixMath biquad-master mbed
Fork of frdm_gpio1 by
main.cpp@3:2ffbee47fb38, 2017-10-31 (annotated)
- Committer:
- RoyvZ
- Date:
- Tue Oct 31 21:50:46 2017 +0000
- Revision:
- 3:2ffbee47fb38
- Parent:
- 2:293665548183
- Child:
- 4:afa85283eb18
Added Matrices
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
RoyvZ | 3:2ffbee47fb38 | 1 | /** |
RoyvZ | 0:b6c8d56842ce | 2 | * Demo program for BiQuad and BiQuadChain classes |
RoyvZ | 0:b6c8d56842ce | 3 | * author: T.J.W. Lankhorst <t.j.w.lankhorst@student.utwente.nl> and Matthijs and Roy and Dion |
RoyvZ | 0:b6c8d56842ce | 4 | */ |
RoyvZ | 0:b6c8d56842ce | 5 | #include "mbed.h" |
RoyvZ | 0:b6c8d56842ce | 6 | #include "HIDScope.h" |
RoyvZ | 0:b6c8d56842ce | 7 | #include <stdlib.h> |
RoyvZ | 0:b6c8d56842ce | 8 | #include <iostream> |
RoyvZ | 0:b6c8d56842ce | 9 | #include <iomanip> |
RoyvZ | 0:b6c8d56842ce | 10 | #include <complex> |
RoyvZ | 3:2ffbee47fb38 | 11 | #include <vector> |
RoyvZ | 0:b6c8d56842ce | 12 | #include "BiQuad.h" |
RoyvZ | 3:2ffbee47fb38 | 13 | #include "Matrix.h" |
RoyvZ | 3:2ffbee47fb38 | 14 | #include "MatrixMath.h" |
RoyvZ | 3:2ffbee47fb38 | 15 | #include "MODSERIAL.h" |
RoyvZ | 3:2ffbee47fb38 | 16 | #include "encoder.h" |
RoyvZ | 2:293665548183 | 17 | |
RoyvZ | 0:b6c8d56842ce | 18 | AnalogIn emg0( A0 ); |
RoyvZ | 0:b6c8d56842ce | 19 | AnalogIn emg1( A1 ); |
RoyvZ | 3:2ffbee47fb38 | 20 | DigitalIn button1(D11); |
RoyvZ | 3:2ffbee47fb38 | 21 | Encoder encoder1(D13,D12); |
RoyvZ | 3:2ffbee47fb38 | 22 | Encoder encoder2(D13,D12); //moet nog even aangepast worden |
RoyvZ | 3:2ffbee47fb38 | 23 | |
RoyvZ | 3:2ffbee47fb38 | 24 | |
RoyvZ | 3:2ffbee47fb38 | 25 | DigitalOut motorDirection(D4); |
RoyvZ | 3:2ffbee47fb38 | 26 | PwmOut motorSpeed(D5); |
RoyvZ | 3:2ffbee47fb38 | 27 | |
RoyvZ | 3:2ffbee47fb38 | 28 | MODSERIAL pc(USBTX, USBRX); |
RoyvZ | 0:b6c8d56842ce | 29 | |
RoyvZ | 0:b6c8d56842ce | 30 | Ticker sample_timer; |
RoyvZ | 2:293665548183 | 31 | Ticker cal_timer; |
RoyvZ | 3:2ffbee47fb38 | 32 | HIDScope scope( 4 ); |
RoyvZ | 0:b6c8d56842ce | 33 | DigitalOut led(LED1); |
RoyvZ | 3:2ffbee47fb38 | 34 | DigitalOut led2(LED_GREEN); |
RoyvZ | 3:2ffbee47fb38 | 35 | const int size = 100; |
RoyvZ | 3:2ffbee47fb38 | 36 | vector<double> S(size,0); |
RoyvZ | 3:2ffbee47fb38 | 37 | double meanSum = 0; |
RoyvZ | 3:2ffbee47fb38 | 38 | |
RoyvZ | 3:2ffbee47fb38 | 39 | double maxsignal = 0; |
RoyvZ | 2:293665548183 | 40 | |
RoyvZ | 3:2ffbee47fb38 | 41 | double L0 = 0.123; |
RoyvZ | 3:2ffbee47fb38 | 42 | double L1 = 0.37; |
RoyvZ | 3:2ffbee47fb38 | 43 | double L2 = 0.41; |
RoyvZ | 3:2ffbee47fb38 | 44 | double q1 = encoder1.getPosition()/131; //Calibreren nog toevoegen |
RoyvZ | 3:2ffbee47fb38 | 45 | double q2 = encoder2.getPosition()/131; //Calibreren mist nog |
RoyvZ | 3:2ffbee47fb38 | 46 | double Periode = 1/1000; //1000 is het aantal Hz |
RoyvZ | 2:293665548183 | 47 | |
RoyvZ | 2:293665548183 | 48 | //Filter toevoegen, bestaande uit notch, high- en lowpass filter |
RoyvZ | 2:293665548183 | 49 | BiQuadChain Notch; |
RoyvZ | 3:2ffbee47fb38 | 50 | BiQuadChain Notch50; |
RoyvZ | 3:2ffbee47fb38 | 51 | BiQuadChain bqcLP; |
RoyvZ | 3:2ffbee47fb38 | 52 | BiQuadChain bqc; |
RoyvZ | 2:293665548183 | 53 | //50 Hz Notch filter, for radiation from surroundings |
RoyvZ | 0:b6c8d56842ce | 54 | |
RoyvZ | 2:293665548183 | 55 | //Notch filter chain for 100 Hz |
RoyvZ | 2:293665548183 | 56 | BiQuad bqNotch1( 9.75180e-01, -1.85510e+00, 9.75218e-01, -1.87865e+00, 9.75178e-01 ); |
RoyvZ | 2:293665548183 | 57 | BiQuad bqNotch2( 1.00000e+00, -1.90228e+00, 1.00004e+00, -1.88308e+00, 9.87097e-01 ); |
RoyvZ | 2:293665548183 | 58 | BiQuad bqNotch3( 1.00000e+00, -1.90219e+00, 9.99925e-01, -1.89724e+00, 9.87929e-01 ); |
RoyvZ | 2:293665548183 | 59 | |
RoyvZ | 3:2ffbee47fb38 | 60 | //BiQuad Notch(0.9179504645111498,-1.4852750515677946, 0.9179504645111498, -1.4852750515677946, 0.8359009290222995); |
RoyvZ | 3:2ffbee47fb38 | 61 | |
RoyvZ | 2:293665548183 | 62 | //Notch filter chain for 50 Hz |
RoyvZ | 3:2ffbee47fb38 | 63 | BiQuad bq1( 9.50972e-01, -1.53921e+00, 9.51003e-01, -1.57886e+00, 9.50957e-01 ); |
RoyvZ | 3:2ffbee47fb38 | 64 | BiQuad bq2( 1.00000e+00, -1.61851e+00, 9.99979e-01, -1.57185e+00, 9.74451e-01 ); |
RoyvZ | 3:2ffbee47fb38 | 65 | BiQuad bq3( 1.00000e+00, -1.61855e+00, 9.99988e-01, -1.62358e+00, 9.75921e-01 ); |
RoyvZ | 2:293665548183 | 66 | |
RoyvZ | 2:293665548183 | 67 | //15 Hz Highpass filter, as recommended by multiple studies regarding EMG signals |
RoyvZ | 2:293665548183 | 68 | //BiQuad Highpass15(9.35527e-01, -1.87105e+00, 9.35527e-01, -1.86689e+00, 8.75215e-01); |
RoyvZ | 2:293665548183 | 69 | |
RoyvZ | 2:293665548183 | 70 | //2Hz Highpass |
RoyvZ | 3:2ffbee47fb38 | 71 | BiQuad Highpass1(9.82385e-01, -1.96477e+00, 9.82385e-01, -1.96446e+00, 9.65081e-01); |
RoyvZ | 0:b6c8d56842ce | 72 | |
RoyvZ | 0:b6c8d56842ce | 73 | |
RoyvZ | 2:293665548183 | 74 | //20 Hz Lowpass |
RoyvZ | 3:2ffbee47fb38 | 75 | //BiQuad Lowpass100( 3.62168e-03, 7.24336e-03, 3.62168e-03, -1.82269e+00, 8.37182e-01 ); |
RoyvZ | 3:2ffbee47fb38 | 76 | |
RoyvZ | 3:2ffbee47fb38 | 77 | //250 Hz Lowpass |
RoyvZ | 3:2ffbee47fb38 | 78 | //BiQuad Lowpass100 (2.92893e-01, 5.85786e-01, 2.92893e-01, -3.60822e-16, 1.71573e-01 ); |
RoyvZ | 3:2ffbee47fb38 | 79 | |
RoyvZ | 3:2ffbee47fb38 | 80 | //80 Hz Lowpass |
RoyvZ | 3:2ffbee47fb38 | 81 | BiQuad bqLP1( 5.78904e-02, 5.78898e-02, 0.00000e+00, -2.90527e-01, 0.00000e+00 ); |
RoyvZ | 3:2ffbee47fb38 | 82 | BiQuad bqLP2( 1.00000e+00, 2.00001e+00, 1.00001e+00, -7.53537e-01, 4.06308e-01 ); |
RoyvZ | 3:2ffbee47fb38 | 83 | |
RoyvZ | 3:2ffbee47fb38 | 84 | |
RoyvZ | 0:b6c8d56842ce | 85 | |
RoyvZ | 2:293665548183 | 86 | //450 Hz Lowpass filter, to remove any noise (sample frequency is only 500 Hz, but still...) |
RoyvZ | 2:293665548183 | 87 | //BiQuad Lowpass450(8.00592e-01, 1.60118e+00, 8.00592e-01, 1.56102e+00, 6.41352e-01); |
RoyvZ | 3:2ffbee47fb38 | 88 | |
RoyvZ | 3:2ffbee47fb38 | 89 | double findRMS(vector<double> array) |
RoyvZ | 3:2ffbee47fb38 | 90 | { |
RoyvZ | 3:2ffbee47fb38 | 91 | int i; |
RoyvZ | 3:2ffbee47fb38 | 92 | double sumsquared = 0.000; |
RoyvZ | 3:2ffbee47fb38 | 93 | double RMS; |
RoyvZ | 3:2ffbee47fb38 | 94 | //sumsquared = 0; |
RoyvZ | 3:2ffbee47fb38 | 95 | for (i = 0; i < size; i++) |
RoyvZ | 3:2ffbee47fb38 | 96 | { |
RoyvZ | 3:2ffbee47fb38 | 97 | sumsquared = sumsquared + array.at(i)*array.at(i); |
RoyvZ | 3:2ffbee47fb38 | 98 | } |
RoyvZ | 3:2ffbee47fb38 | 99 | RMS = sqrt((double(1)/size)*(sumsquared)); |
RoyvZ | 3:2ffbee47fb38 | 100 | return RMS; |
RoyvZ | 3:2ffbee47fb38 | 101 | } |
RoyvZ | 3:2ffbee47fb38 | 102 | |
RoyvZ | 3:2ffbee47fb38 | 103 | |
RoyvZ | 3:2ffbee47fb38 | 104 | |
RoyvZ | 0:b6c8d56842ce | 105 | void sample() |
RoyvZ | 0:b6c8d56842ce | 106 | { |
RoyvZ | 0:b6c8d56842ce | 107 | /* Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope' */ |
RoyvZ | 2:293665548183 | 108 | //scope.set(0, emg0.read() ); |
RoyvZ | 2:293665548183 | 109 | //scope.set(1, fabs(Notch.step(emg0.read())) ); |
RoyvZ | 3:2ffbee47fb38 | 110 | scope.set(0, emg0.read()-0.4542); |
RoyvZ | 3:2ffbee47fb38 | 111 | //scope.set(1, bqcLP.step(Highpass1.step(emg0.read()-0.4542))); |
RoyvZ | 3:2ffbee47fb38 | 112 | //scope.set(2, fabs(Notch50.step(bqcLP.step(Highpass1.step(emg0.read()-0.4542))))); |
RoyvZ | 3:2ffbee47fb38 | 113 | //scope.set(3, fabs(bqc.step(emg0.read()-0.4542))); |
RoyvZ | 3:2ffbee47fb38 | 114 | //scope.set(3, Notch.step(Notch50.step(bqcLP.step(Highpass1.step(fabs(emg0.read()-0.4542)))))); |
RoyvZ | 3:2ffbee47fb38 | 115 | |
RoyvZ | 3:2ffbee47fb38 | 116 | |
RoyvZ | 3:2ffbee47fb38 | 117 | //scope.set(0, fabs(Highpass1.step(Lowpass100.step(Notch.step(emg0.read())))) ); |
RoyvZ | 3:2ffbee47fb38 | 118 | /* |
RoyvZ | 3:2ffbee47fb38 | 119 | for (int i = size-1; i > 1; i--) { |
RoyvZ | 2:293665548183 | 120 | S[i] = S[i-1]; |
RoyvZ | 2:293665548183 | 121 | } |
RoyvZ | 3:2ffbee47fb38 | 122 | */ |
RoyvZ | 3:2ffbee47fb38 | 123 | S.erase(S.begin()); |
RoyvZ | 3:2ffbee47fb38 | 124 | double b = bqc.step(emg0.read()-0.4542); |
RoyvZ | 3:2ffbee47fb38 | 125 | S.push_back(b); |
RoyvZ | 3:2ffbee47fb38 | 126 | //S[0] = bqc.step(emg0.read()-0.4542))); |
RoyvZ | 3:2ffbee47fb38 | 127 | //Notch50.step(bqcLP.step(Highpass1.step(emg0.read()-0.4542))); |
RoyvZ | 3:2ffbee47fb38 | 128 | double a = findRMS(S); |
RoyvZ | 3:2ffbee47fb38 | 129 | scope.set(1, a); |
RoyvZ | 3:2ffbee47fb38 | 130 | scope.set(2, S[0]); |
RoyvZ | 3:2ffbee47fb38 | 131 | //meanSum = 0; */ |
RoyvZ | 0:b6c8d56842ce | 132 | /* Repeat the step above if required for more channels of required (channel 0 up to 5 = 6 channels) |
RoyvZ | 0:b6c8d56842ce | 133 | * Ensure that enough channels are available (HIDScope scope( 2 )) |
RoyvZ | 0:b6c8d56842ce | 134 | * Finally, send all channels to the PC at once */ |
RoyvZ | 0:b6c8d56842ce | 135 | scope.send(); |
RoyvZ | 0:b6c8d56842ce | 136 | /* To indicate that the function is working, the LED is toggled */ |
RoyvZ | 0:b6c8d56842ce | 137 | led = !led; |
RoyvZ | 3:2ffbee47fb38 | 138 | |
RoyvZ | 3:2ffbee47fb38 | 139 | motorSpeed.write(0.38080*(a/maxsignal)); |
RoyvZ | 3:2ffbee47fb38 | 140 | motorDirection.write(1); |
RoyvZ | 0:b6c8d56842ce | 141 | } |
RoyvZ | 0:b6c8d56842ce | 142 | |
RoyvZ | 2:293665548183 | 143 | void calibration() |
RoyvZ | 2:293665548183 | 144 | { |
RoyvZ | 2:293665548183 | 145 | //function to determine maximal EMG input in order to allow motorcontrol to be activated |
RoyvZ | 3:2ffbee47fb38 | 146 | |
RoyvZ | 2:293665548183 | 147 | if (button1.read() == false){ |
RoyvZ | 3:2ffbee47fb38 | 148 | led2 = !led2; |
RoyvZ | 2:293665548183 | 149 | for (int n = 1; n < 5000; n++){ //calibrate for 5000 samples or 10 seconds |
RoyvZ | 3:2ffbee47fb38 | 150 | |
RoyvZ | 3:2ffbee47fb38 | 151 | S.erase(S.begin()); |
RoyvZ | 3:2ffbee47fb38 | 152 | double b = bqc.step(emg0.read()-0.4542); |
RoyvZ | 3:2ffbee47fb38 | 153 | S.push_back(b); |
RoyvZ | 3:2ffbee47fb38 | 154 | //S[0] = bqc.step(emg0.read()-0.4542))); |
RoyvZ | 3:2ffbee47fb38 | 155 | //Notch50.step(bqcLP.step(Highpass1.step(emg0.read()-0.4542))); |
RoyvZ | 3:2ffbee47fb38 | 156 | double signalfinal = findRMS(S); |
RoyvZ | 2:293665548183 | 157 | if (signalfinal >= maxsignal){ |
RoyvZ | 2:293665548183 | 158 | maxsignal = signalfinal; //keep changing the maximal signal |
RoyvZ | 3:2ffbee47fb38 | 159 | pc.baud(9600); |
RoyvZ | 3:2ffbee47fb38 | 160 | pc.printf("%d \n",maxsignal); |
RoyvZ | 2:293665548183 | 161 | } |
RoyvZ | 2:293665548183 | 162 | } |
RoyvZ | 3:2ffbee47fb38 | 163 | led2 = 1; |
RoyvZ | 2:293665548183 | 164 | } |
RoyvZ | 2:293665548183 | 165 | } |
RoyvZ | 2:293665548183 | 166 | |
RoyvZ | 0:b6c8d56842ce | 167 | int main() |
RoyvZ | 0:b6c8d56842ce | 168 | { |
RoyvZ | 2:293665548183 | 169 | //Constructing the notch filter chain |
RoyvZ | 3:2ffbee47fb38 | 170 | Notch.add( &bqNotch1 ).add( &bqNotch2 ).add( &bqNotch3 ); |
RoyvZ | 3:2ffbee47fb38 | 171 | Notch50.add( &bq1 ).add( &bq2 ).add( &bq3 ); |
RoyvZ | 3:2ffbee47fb38 | 172 | bqcLP.add( &bqLP1 ).add( &bqLP2 ); |
RoyvZ | 3:2ffbee47fb38 | 173 | bqc.add( &bqNotch1 ).add( &bqNotch2 ).add( &bqNotch3 ).add( &bq1 ).add( &bq2 ).add( &bq3 ).add( &bqLP1 ).add( &bqLP2 ); |
RoyvZ | 0:b6c8d56842ce | 174 | /**Attach the 'sample' function to the timer 'sample_timer'. |
RoyvZ | 2:293665548183 | 175 | * this ensures that 'sample' is executed every... 0.001 seconds = 1000 Hz |
RoyvZ | 0:b6c8d56842ce | 176 | */ |
RoyvZ | 3:2ffbee47fb38 | 177 | sample_timer.attach(&sample, 0.002); |
RoyvZ | 2:293665548183 | 178 | cal_timer.attach(&calibration, 0.002);//ticker to check if motor is being calibrated |
RoyvZ | 3:2ffbee47fb38 | 179 | led2 = 1; |
RoyvZ | 3:2ffbee47fb38 | 180 | /*empty loop, sample() is executed periodically*/ |
RoyvZ | 3:2ffbee47fb38 | 181 | |
RoyvZ | 3:2ffbee47fb38 | 182 | pc.baud(9600); |
RoyvZ | 3:2ffbee47fb38 | 183 | DigitalOut myled(LED1); |
RoyvZ | 3:2ffbee47fb38 | 184 | |
RoyvZ | 3:2ffbee47fb38 | 185 | //--- |
RoyvZ | 3:2ffbee47fb38 | 186 | Matrix JAPPAPP(2,2); |
RoyvZ | 3:2ffbee47fb38 | 187 | Matrix qdot(2,1); |
RoyvZ | 3:2ffbee47fb38 | 188 | Matrix Vdes(2,1); |
RoyvZ | 3:2ffbee47fb38 | 189 | Matrix qsetpoint(2,1); |
RoyvZ | 0:b6c8d56842ce | 190 | |
RoyvZ | 3:2ffbee47fb38 | 191 | // Fill Matrix with data. |
RoyvZ | 3:2ffbee47fb38 | 192 | JAPPAPP << -(L0 - L0*sin(q1) + L1*sin(q1) - L2*sin(q2))/(L1*L1*cos(q1)*sin(q1) + L0*L1*cos(q1) + L0*L2*cos(q2) - L0*L1*cos(q1)*sin(q1) - L0*L2*cos(q2)*sin(q1) - L1*L2*cos(q1)*sin(q2)) << -(L2*cos(q2))/(L1*L1*cos(q1)*sin(q1) + L0*L1*cos(q1) + L0*L2*cos(q2) - L0*L1*cos(q1)*sin(q1) - L0*L2*cos(q2)*sin(q1) - L1*L2*cos(q1)*sin(q2)) |
RoyvZ | 3:2ffbee47fb38 | 193 | << (L1*sin(q1) - L2*sin(q2))/(L1*L1*cos(q1)*sin(q1) + L0*L1*cos(q1) + L0*L2*cos(q2) - L0*L1*cos(q1)*sin(q1) - L0*L2*cos(q2)*sin(q1) - L1*L2*cos(q1)*sin(q2)) << (L1*cos(q1) + L2*cos(q2))/(L1*L1*cos(q1)*sin(q1) + L0*L1*cos(q1) + L0*L2*cos(q2) - L0*L1*cos(q1)*sin(q1) - L0*L2*cos(q2)*sin(q1) - L1*L2*cos(q1)*sin(q2)); |
RoyvZ | 3:2ffbee47fb38 | 194 | |
RoyvZ | 3:2ffbee47fb38 | 195 | |
RoyvZ | 3:2ffbee47fb38 | 196 | // Fill Matrix with data. |
RoyvZ | 3:2ffbee47fb38 | 197 | Vdes << emg0.read //Goede code nog toevoegen, Vx |
RoyvZ | 3:2ffbee47fb38 | 198 | << emg1.read; //goede code nog toevoegen, Vy |
RoyvZ | 3:2ffbee47fb38 | 199 | |
RoyvZ | 3:2ffbee47fb38 | 200 | |
RoyvZ | 3:2ffbee47fb38 | 201 | qdot = JAPPAPP*Vdes; |
RoyvZ | 3:2ffbee47fb38 | 202 | pc.printf("\r\nJAPPAPPP: "); //Alleen visualisatie |
RoyvZ | 3:2ffbee47fb38 | 203 | qdot.print(); //Alleen visualisatie |
RoyvZ | 3:2ffbee47fb38 | 204 | |
RoyvZ | 3:2ffbee47fb38 | 205 | qsetpoint = q1 + qdot*Periode; |
RoyvZ | 3:2ffbee47fb38 | 206 | |
RoyvZ | 3:2ffbee47fb38 | 207 | |
RoyvZ | 3:2ffbee47fb38 | 208 | |
RoyvZ | 0:b6c8d56842ce | 209 | while(true) { |
RoyvZ | 0:b6c8d56842ce | 210 | |
RoyvZ | 2:293665548183 | 211 | |
RoyvZ | 0:b6c8d56842ce | 212 | } |
RoyvZ | 0:b6c8d56842ce | 213 | |
RoyvZ | 0:b6c8d56842ce | 214 | } |