Diff: main.cpp

Revision:

2:293665548183

Parent:

0:b6c8d56842ce

Child:

3:2ffbee47fb38

--- 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