adsf
Dependencies: Encoder HIDScope MODSERIAL Matrix MatrixMath biquad-master mbed
Fork of frdm_gpio1 by
main.cpp
- Committer:
- DiondeGreef
- Date:
- 2017-11-01
- Revision:
- 4:afa85283eb18
- Parent:
- 3:2ffbee47fb38
File content as of revision 4:afa85283eb18:
/** * 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); DigitalOut motorDirection1(D4); //nog goede channel toevoegen PwmOut motorSpeed1(D5); //nog goede channel toevoegen 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); vector<double> S1(size,0); double meanSum = 0; double maxsignal = 0; double emgX = 0; double emgY = 0; double L0 = 0.123; double L1 = 0.37; double L2 = 0.41; double q1 = encoder1.getPosition()/131.25; //Calibreren nog toevoegen double q2 = encoder2.getPosition()/131.25; //Calibreren mist nog double Periode = 1/500; //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); //Making matrices globaly Matrix JAPPAPP(2,2); Matrix qdot(2,1); Matrix Vdes(2,1); Matrix qsetpoint(2,1); 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()); S1.erase(S1.begin()); double b = bqc.step(emg0.read()-0.4542); double c = bqc.step(emg1.read()-0.4542); S.push_back(b); S1.push_back(c); //S[0] = bqc.step(emg0.read()-0.4542))); //Notch50.step(bqcLP.step(Highpass1.step(emg0.read()-0.4542))); emgX = findRMS(S); emgY = findRMS(S1); scope.set(1, emgX); //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*(emgX/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.printf("%d \n",maxsignal); } } led2 = 1; } } void qSetpointSet(){ // Fill Matrix with data. 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)); 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)); 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)); 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)); // Fill Matrix with data. Vdes(1,1) = emgX; //Goede code nog toevoegen, Vx Vdes(2,1) = emgY; //goede code nog toevoegen, Vy qdot = JAPPAPP*Vdes; qsetpoint(1,1) = q1 + qdot(1,1)*Periode; qsetpoint(2,1) = q2 + qdot(2,1)*Periode; } int main() { pc.baud(115200); //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, Periode); cal_timer.attach(&calibration, 0.002);//ticker to check if motor is being calibrated led2 = 1; /*empty loop, sample() is executed periodically*/ DigitalOut myled(LED1); //--- while(true) { qdot.print(); //Alleen visualisatie wait(0.5); } }