adsf
Dependencies: Encoder HIDScope MODSERIAL Matrix MatrixMath biquad-master mbed
Fork of frdm_gpio1 by
main.cpp
- Committer:
- RoyvZ
- Date:
- 2017-10-31
- Revision:
- 3:2ffbee47fb38
- Parent:
- 2:293665548183
- Child:
- 4:afa85283eb18
File content as of revision 3:2ffbee47fb38:
/** * 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 */ #include "mbed.h" #include "HIDScope.h" #include <stdlib.h> #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(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( 4 ); DigitalOut led(LED1); DigitalOut led2(LED_GREEN); const int size = 100; vector<double> S(size,0); double meanSum = 0; double maxsignal = 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 BiQuad bqNotch1( 9.75180e-01, -1.85510e+00, 9.75218e-01, -1.87865e+00, 9.75178e-01 ); 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.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.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 ); //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, 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]; } */ 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 if (button1.read() == false){ led2 = !led2; for (int n = 1; n < 5000; n++){ //calibrate for 5000 samples or 10 seconds 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 ); 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.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); // 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) { } }