Mbed bordje 1 -af
Dependencies: Encoder HIDScope MODSERIAL Matrix MatrixMath biquad-master mbed
Fork of dsjklafjaslkjdfalkjfdaslkasdjklajadsflkjdasflkjdasflkadsflkasd by
Diff: main.cpp
- Revision:
- 2:293665548183
- Parent:
- 0:b6c8d56842ce
- Child:
- 3:2ffbee47fb38
diff -r 0a95585c9fc9 -r 293665548183 main.cpp --- a/main.cpp Fri Oct 13 09:00:33 2017 +0000 +++ b/main.cpp Tue Oct 24 11:58:29 2017 +0000 @@ -10,28 +10,62 @@ #include <complex> #include "BiQuad.h" + AnalogIn emg0( A0 ); AnalogIn emg1( A1 ); +DigitalIn button1(PTA4); Ticker sample_timer; +Ticker cal_timer; HIDScope scope( 2 ); DigitalOut led(LED1); -DigitalOut led2(LED_GREEN); +//DigitalOut led2(LED_GREEN); + +double S[50]; +double meanSum = 0; + +//Filter toevoegen, bestaande uit notch, high- en lowpass filter +BiQuadChain Notch; +//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 ); + +//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 ); + +//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); -// Example: 3th order Butterworth LP (w_c = 0.1*f_nyquist) +//20 Hz Lowpass +BiQuad Lowpass100( 3.62168e-03, 7.24336e-03, 3.62168e-03, -1.82269e+00, 8.37182e-01 ); -BiQuadChain bqc; -BiQuad bq1( 0.9645651759199596, -1.5606992390030165, 0.9645651759199596, -1.5606992390030165, 0.9291303518399192 ); -BiQuad bq2( 1, 1, 1, 1, 1 ); -//BiQuad bq3( 0, 0, 0, 0, 0 ); +//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); 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, bq1.step(emg0.read()) ); - scope.set(1, bq1.step(emg1.read()) ); + //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++) { + 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; /* 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 */ @@ -40,39 +74,35 @@ led = !led; } +void calibration() +{ + //function to determine maximal EMG input in order to allow motorcontrol to be activated + double maxsignal = 0; + if (button1.read() == false){ + 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())))); + if (signalfinal >= maxsignal){ + maxsignal = signalfinal; //keep changing the maximal signal + } + } + } +} + int main() { - led2 = 0; - bqc = bq1 * bq2; //* bq3; + //Constructing the notch filter chain + Notch.add( &bqNotch1 ).add( &bqNotch2 ).add( &bqNotch3 ).add( &bq1 ).add( &bq2 ).add( &bq3 ); + /**Attach the 'sample' function to the timer 'sample_timer'. - * this ensures that 'sample' is executed every... 0.002 seconds = 500 Hz + * this ensures that 'sample' is executed every... 0.001 seconds = 1000 Hz */ - sample_timer.attach(&sample, 0.002); + sample_timer.attach(&sample, 0.001); + cal_timer.attach(&calibration, 0.002);//ticker to check if motor is being calibrated /*empty loop, sample() is executed periodically*/ while(true) { - // Find the poles of the filter - std::cout << "Filter poles" << std::endl; - std::vector< std::complex<double> > poles = bqc.poles(); - for( size_t i = 0; i < poles.size(); i++ ) - std::cout << "\t" << poles[i] << std::endl; - - // Find the zeros of the filter - std::cout << "Filter zeros" << std::endl; - std::vector< std::complex<double> > zeros = bqc.zeros(); - for( size_t i = 0; i < poles.size(); i++ ) - std::cout << "\t" << zeros[i] << std::endl; - - // Is the filter stable? - std::cout << "This filter is " << (bqc.stable() ? "stable" : "instable") << std::endl; - - // Output the step-response of 20 samples - std::cout << "Step response 20 samples" << std::endl; - for( int i = 0; i < 20; i++ ) - std::cout << "\t" << bqc.step( 1.0 ) << std::endl; - - // Done0 + } } \ No newline at end of file