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