Program to read out fiterd emg signals
Dependencies: HIDScope biquad-master mbed Encoder MODSERIAL Matrix MatrixMath
Revision 3:2ffbee47fb38, committed 2017-10-31
- Comitter:
- RoyvZ
- Date:
- Tue Oct 31 21:50:46 2017 +0000
- Parent:
- 2:293665548183
- Commit message:
- Added Matrices
Changed in this revision
diff -r 293665548183 -r 2ffbee47fb38 Encoder.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Encoder.lib Tue Oct 31 21:50:46 2017 +0000 @@ -0,0 +1,1 @@ +https://developer.mbed.org/users/vsluiter/code/Encoder/#18b000b443af
diff -r 293665548183 -r 2ffbee47fb38 MODSERIAL.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MODSERIAL.lib Tue Oct 31 21:50:46 2017 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/Sissors/code/MODSERIAL/#a3b2bc878529
diff -r 293665548183 -r 2ffbee47fb38 Matrix.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Matrix.lib Tue Oct 31 21:50:46 2017 +0000 @@ -0,0 +1,1 @@ +http://os.mbed.com/users/DiondeGreef/code/Matrix/#92115bd8c705
diff -r 293665548183 -r 2ffbee47fb38 MatrixMath.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MatrixMath.lib Tue Oct 31 21:50:46 2017 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/Yo_Robot/code/MatrixMath/#93948a9bbde2
diff -r 293665548183 -r 2ffbee47fb38 main.cpp --- a/main.cpp Tue Oct 24 11:58:29 2017 +0000 +++ b/main.cpp Tue Oct 31 21:50:46 2017 +0000 @@ -1,4 +1,4 @@ -/** + /** * Demo program for BiQuad and BiQuadChain classes * author: T.J.W. Lankhorst <t.j.w.lankhorst@student.utwente.nl> and Matthijs and Roy and Dion */ @@ -8,24 +8,48 @@ #include <iostream> #include <iomanip> #include <complex> +#include <vector> #include "BiQuad.h" - +#include "Matrix.h" +#include "MatrixMath.h" +#include "MODSERIAL.h" +#include "encoder.h" AnalogIn emg0( A0 ); AnalogIn emg1( A1 ); -DigitalIn button1(PTA4); +DigitalIn button1(D11); +Encoder encoder1(D13,D12); +Encoder encoder2(D13,D12); //moet nog even aangepast worden + + +DigitalOut motorDirection(D4); +PwmOut motorSpeed(D5); + +MODSERIAL pc(USBTX, USBRX); Ticker sample_timer; Ticker cal_timer; -HIDScope scope( 2 ); +HIDScope scope( 4 ); DigitalOut led(LED1); -//DigitalOut led2(LED_GREEN); +DigitalOut led2(LED_GREEN); +const int size = 100; +vector<double> S(size,0); +double meanSum = 0; + +double maxsignal = 0; -double S[50]; -double meanSum = 0; +double L0 = 0.123; +double L1 = 0.37; +double L2 = 0.41; +double q1 = encoder1.getPosition()/131; //Calibreren nog toevoegen +double q2 = encoder2.getPosition()/131; //Calibreren mist nog +double Periode = 1/1000; //1000 is het aantal Hz //Filter toevoegen, bestaande uit notch, high- en lowpass filter BiQuadChain Notch; +BiQuadChain Notch50; +BiQuadChain bqcLP; +BiQuadChain bqc; //50 Hz Notch filter, for radiation from surroundings //Notch filter chain for 100 Hz @@ -33,73 +57,155 @@ BiQuad bqNotch2( 1.00000e+00, -1.90228e+00, 1.00004e+00, -1.88308e+00, 9.87097e-01 ); BiQuad bqNotch3( 1.00000e+00, -1.90219e+00, 9.99925e-01, -1.89724e+00, 9.87929e-01 ); +//BiQuad Notch(0.9179504645111498,-1.4852750515677946, 0.9179504645111498, -1.4852750515677946, 0.8359009290222995); + //Notch filter chain for 50 Hz -BiQuad bq1( 9.87512e-01, -1.95079e+00, 9.87563e-01, -1.96308e+00, 9.87512e-01 ); -BiQuad bq2( 1.00000e+00, -1.97533e+00, 9.99919e-01, -1.96726e+00, 9.93522e-01 ); -BiQuad bq3( 1.00000e+00, -1.97546e+00, 1.00003e+00, -1.97108e+00, 9.93951e-01 ); +BiQuad bq1( 9.50972e-01, -1.53921e+00, 9.51003e-01, -1.57886e+00, 9.50957e-01 ); +BiQuad bq2( 1.00000e+00, -1.61851e+00, 9.99979e-01, -1.57185e+00, 9.74451e-01 ); +BiQuad bq3( 1.00000e+00, -1.61855e+00, 9.99988e-01, -1.62358e+00, 9.75921e-01 ); //15 Hz Highpass filter, as recommended by multiple studies regarding EMG signals //BiQuad Highpass15(9.35527e-01, -1.87105e+00, 9.35527e-01, -1.86689e+00, 8.75215e-01); //2Hz Highpass -BiQuad Highpass1(9.91154e-01, -1.98231e+00, 9.91154e-01, -1.98223e+00, 9.82385e-01); +BiQuad Highpass1(9.82385e-01, -1.96477e+00, 9.82385e-01, -1.96446e+00, 9.65081e-01); //20 Hz Lowpass -BiQuad Lowpass100( 3.62168e-03, 7.24336e-03, 3.62168e-03, -1.82269e+00, 8.37182e-01 ); +//BiQuad Lowpass100( 3.62168e-03, 7.24336e-03, 3.62168e-03, -1.82269e+00, 8.37182e-01 ); + +//250 Hz Lowpass +//BiQuad Lowpass100 (2.92893e-01, 5.85786e-01, 2.92893e-01, -3.60822e-16, 1.71573e-01 ); + +//80 Hz Lowpass +BiQuad bqLP1( 5.78904e-02, 5.78898e-02, 0.00000e+00, -2.90527e-01, 0.00000e+00 ); +BiQuad bqLP2( 1.00000e+00, 2.00001e+00, 1.00001e+00, -7.53537e-01, 4.06308e-01 ); + + //450 Hz Lowpass filter, to remove any noise (sample frequency is only 500 Hz, but still...) //BiQuad Lowpass450(8.00592e-01, 1.60118e+00, 8.00592e-01, 1.56102e+00, 6.41352e-01); - + +double findRMS(vector<double> array) +{ + int i; + double sumsquared = 0.000; + double RMS; + //sumsquared = 0; + for (i = 0; i < size; i++) + { + sumsquared = sumsquared + array.at(i)*array.at(i); + } + RMS = sqrt((double(1)/size)*(sumsquared)); + return RMS; +} + + + void sample() { /* Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope' */ //scope.set(0, emg0.read() ); //scope.set(1, fabs(Notch.step(emg0.read())) ); - scope.set(0, fabs(Highpass1.step(Lowpass100.step(Notch.step(emg0.read())))) ); - - for (int i=1;i < 50; i++) { + scope.set(0, emg0.read()-0.4542); + //scope.set(1, bqcLP.step(Highpass1.step(emg0.read()-0.4542))); + //scope.set(2, fabs(Notch50.step(bqcLP.step(Highpass1.step(emg0.read()-0.4542))))); + //scope.set(3, fabs(bqc.step(emg0.read()-0.4542))); + //scope.set(3, Notch.step(Notch50.step(bqcLP.step(Highpass1.step(fabs(emg0.read()-0.4542)))))); + + + //scope.set(0, fabs(Highpass1.step(Lowpass100.step(Notch.step(emg0.read())))) ); + /* + for (int i = size-1; i > 1; i--) { S[i] = S[i-1]; - meanSum = meanSum + S[i]; } - S[0] = fabs(Highpass1.step(Lowpass100.step(Notch.step(emg0.read())))); - double mean = meanSum / 50; - scope.set(1, fabs(4*Highpass1.step(Lowpass100.step(Notch.step(emg0.read()))))); - meanSum = 0; + */ + S.erase(S.begin()); + double b = bqc.step(emg0.read()-0.4542); + S.push_back(b); + //S[0] = bqc.step(emg0.read()-0.4542))); + //Notch50.step(bqcLP.step(Highpass1.step(emg0.read()-0.4542))); + double a = findRMS(S); + scope.set(1, a); + scope.set(2, S[0]); + //meanSum = 0; */ /* Repeat the step above if required for more channels of required (channel 0 up to 5 = 6 channels) * Ensure that enough channels are available (HIDScope scope( 2 )) * Finally, send all channels to the PC at once */ scope.send(); /* To indicate that the function is working, the LED is toggled */ led = !led; + + motorSpeed.write(0.38080*(a/maxsignal)); + motorDirection.write(1); } void calibration() { //function to determine maximal EMG input in order to allow motorcontrol to be activated - double maxsignal = 0; + if (button1.read() == false){ + led2 = !led2; for (int n = 1; n < 5000; n++){ //calibrate for 5000 samples or 10 seconds - double signalfinal = fabs(Highpass1.step(Lowpass100.step(Notch.step(emg0.read())))); + + S.erase(S.begin()); + double b = bqc.step(emg0.read()-0.4542); + S.push_back(b); + //S[0] = bqc.step(emg0.read()-0.4542))); + //Notch50.step(bqcLP.step(Highpass1.step(emg0.read()-0.4542))); + double signalfinal = findRMS(S); if (signalfinal >= maxsignal){ maxsignal = signalfinal; //keep changing the maximal signal + pc.baud(9600); + pc.printf("%d \n",maxsignal); } } + led2 = 1; } } int main() { //Constructing the notch filter chain - Notch.add( &bqNotch1 ).add( &bqNotch2 ).add( &bqNotch3 ).add( &bq1 ).add( &bq2 ).add( &bq3 ); - + Notch.add( &bqNotch1 ).add( &bqNotch2 ).add( &bqNotch3 ); + Notch50.add( &bq1 ).add( &bq2 ).add( &bq3 ); + bqcLP.add( &bqLP1 ).add( &bqLP2 ); + bqc.add( &bqNotch1 ).add( &bqNotch2 ).add( &bqNotch3 ).add( &bq1 ).add( &bq2 ).add( &bq3 ).add( &bqLP1 ).add( &bqLP2 ); /**Attach the 'sample' function to the timer 'sample_timer'. * this ensures that 'sample' is executed every... 0.001 seconds = 1000 Hz */ - sample_timer.attach(&sample, 0.001); + sample_timer.attach(&sample, 0.002); cal_timer.attach(&calibration, 0.002);//ticker to check if motor is being calibrated + led2 = 1; + /*empty loop, sample() is executed periodically*/ + + pc.baud(9600); + DigitalOut myled(LED1); + +//--- + Matrix JAPPAPP(2,2); + Matrix qdot(2,1); + Matrix Vdes(2,1); + Matrix qsetpoint(2,1); - /*empty loop, sample() is executed periodically*/ + // Fill Matrix with data. + 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)) + << (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)); + + + // Fill Matrix with data. + Vdes << emg0.read //Goede code nog toevoegen, Vx + << emg1.read; //goede code nog toevoegen, Vy + + + qdot = JAPPAPP*Vdes; + pc.printf("\r\nJAPPAPPP: "); //Alleen visualisatie + qdot.print(); //Alleen visualisatie + + qsetpoint = q1 + qdot*Periode; + + + while(true) {