Diff: main.cpp

Revision:

0:83108f9ff38b

Child:

1:1fd8a80ad5df

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Oct 29 17:28:17 2018 +0000
@@ -0,0 +1,150 @@
+#include "mbed.h"
+#include "HIDScope.h"
+#include "biquadFilter.h" 
+
+//Input system
+AnalogIn emg1(A0); //right biceps
+AnalogIn emg2(A1); //right triceps
+AnalogIn emg3(A2); //left biceps
+AnalogIn emg4(A3); //left triceps
+
+DigitalOut ledje(LED_GREEN);
+
+//EMG tickers, these tickers are called in the mainscript with fsample 500Hz, also sends to HIDscope with same fsample
+Ticker sample_ticker; 
+Ticker main_ticker;
+
+
+//HIDscope to check whether filter is functional or not
+HIDScope scope(2);  // aantal kanalen voor je HIDScope
+
+//4th order highpass filter with fc = 20 Hz so that means there are 2 biquad chains
+
+const double highp_1_a1 = -1.99997409063390;
+const double highp_1_a2= 0.999975877656141;
+const double highp_1_b0 = 0.0920510808414248;
+const double highp_1_b1= -0.184501278881386;
+const double highp_1_b2 = 0.0924506332000235;
+
+const double highp_2_a1 = -1.99998249672404;
+const double highp_2_a2 = 0.999982653918482;
+const double highp_2_b0 = 1;
+const double highp_2_b1 = -2.00367218495316;
+const double highp_2_b2 = 1.00367558370516;
+
+//notchfilter 50Hz 
+const double notch_1_a1 = 1.00000018882502;
+const double notch_1_a2= -1.97024348390877;
+const double notch_1_b0= 0.995504923090779;
+const double notch_1_b1= 1;
+const double notch_1_b2= -1.97538661427765;
+
+const double notch_2_a1 = 1.00000018882502;
+const double notch_2_a2 = -1.97175302143845;
+const double notch_2_b0 = 0.995629024908953;
+const double notch_2_b1 = 1;
+const double notch_2_b2 = -1.97538624436946;
+
+//Enveloping, 4th order low pass filter is used with 3Hz cutoff
+const double lowp_1_a1 = -2.00357645725849;
+const double lowp_1_a2= 1.00357981243408;
+const double lowp_1_b0 = 0.000148081325059440;
+const double lowp_1_b1 = -0.000295530099020586;
+const double lowp_1_b2 = 0.000147449452281968;
+
+const double lowp_2_a1 = -2.00319253084618;
+const double lowp_2_a2 = 1.00319510326279;
+const double lowp_2_b0 = 1;
+const double lowp_2_b1 = -1.99627963867269;
+const double lowp_2_b2 = 0.996283117943831;
+
+//For every EMG signal a set of biquad cascades
+//EMG1
+biquadFilter     highpass1_1( highp_1_a1 , highp_1_a2 , highp_1_b0 , highp_1_b1 , highp_1_b2);  
+biquadFilter     highpass1_2( highp_2_a1 , highp_2_a2 , highp_2_b0 , highp_2_b1 , highp_2_b2);                 // removes artifacts
+biquadFilter     notch1_1( notch_1_a1  , notch_1_a2 , notch_1_b0 , notch_1_b1 , notch_1_b2 );  // removes 49-51 Hz power interference
+biquadFilter     notch1_2( notch_2_a1  , notch_2_a2 , notch_2_b0 , notch_2_b1 , notch_2_b2 );  //
+biquadFilter     lowpass1_1( lowp_1_a1 , lowp_1_a2 , lowp_1_b0 , lowp_1_b1 , lowp_1_b2 );                     // EMG envelope 
+biquadFilter     lowpass1_2( lowp_2_a1 , lowp_2_a2 , lowp_2_b0 , lowp_2_b1 , lowp_2_b2 ); 
+//EMG2
+biquadFilter     highpass2_1( highp_1_a1 , highp_1_a2 , highp_1_b0 , highp_1_b1 , highp_1_b2);  
+biquadFilter     highpass2_2( highp_2_a1 , highp_2_a2 , highp_2_b0 , highp_2_b1 , highp_2_b2);   
+biquadFilter     notch2_1( notch_1_a1  , notch_1_a2 , notch_1_b0 , notch_1_b1 , notch_1_b2 );  // removes 49-51 Hz power interference
+biquadFilter     notch2_2( notch_2_a1  , notch_2_a2 , notch_2_b0 , notch_2_b1 , notch_2_b2 );   //
+biquadFilter     lowpass2_1( lowp_1_a1 , lowp_1_a2 , lowp_1_b0 , lowp_1_b1 , lowp_1_b2 );                     // EMG envelope 
+biquadFilter     lowpass2_2( lowp_2_a1 , lowp_2_a2 , lowp_2_b0 , lowp_2_b1 , lowp_2_b2 );   
+//EMG3
+biquadFilter     highpass3_1( highp_1_a1 , highp_1_a2 , highp_1_b0 , highp_1_b1 , highp_1_b2);  
+biquadFilter     highpass3_2( highp_2_a1 , highp_2_a2 , highp_2_b0 , highp_2_b1 , highp_2_b2);   
+biquadFilter     notch3_1( notch_1_a1  , notch_1_a2 , notch_1_b0 , notch_1_b1 , notch_1_b2 );  // removes 49-51 Hz power interference
+biquadFilter     notch3_2( notch_2_a1  , notch_2_a2 , notch_2_b0 , notch_2_b1 , notch_2_b2 );   //
+biquadFilter     lowpass3_1( lowp_1_a1 , lowp_1_a2 , lowp_1_b0 , lowp_1_b1 , lowp_1_b2 );                     // EMG envelope 
+biquadFilter     lowpass3_2( lowp_2_a1 , lowp_2_a2 , lowp_2_b0 , lowp_2_b1 , lowp_2_b2 ); 
+//EMG4
+biquadFilter     highpass4_1( highp_1_a1 , highp_1_a2 , highp_1_b0 , highp_1_b1 , highp_1_b2);  
+biquadFilter     highpass4_2( highp_2_a1 , highp_2_a2 , highp_2_b0 , highp_2_b1 , highp_2_b2);   
+biquadFilter     notch4_1( notch_1_a1  , notch_1_a2 , notch_1_b0 , notch_1_b1 , notch_1_b2 );  // removes 49-51 Hz power interference
+biquadFilter     notch4_2( notch_2_a1  , notch_2_a2 , notch_2_b0 , notch_2_b1 , notch_2_b2 );   //
+biquadFilter     lowpass4_1( lowp_1_a1 , lowp_1_a2 , lowp_1_b0 , lowp_1_b1 , lowp_1_b2 );                     // EMG envelope 
+biquadFilter     lowpass4_2( lowp_2_a1 , lowp_2_a2 , lowp_2_b0 , lowp_2_b1 , lowp_2_b2 ); 
+
+//Ticker executes this bit
+double emg1_filtered, emg2_filtered, emg3_filtered, emg4_filtered;
+
+void emg_samples()
+{
+//EMG1
+double y1_1 = highpass1_1.step ( emg1.read());
+double y1_2 = highpass1_2.step ( y1_1);
+double y1_3 = notch1_1.step (y1_2);
+double y1_4 = notch1_2.step (y1_3);
+double y1_5 = abs(y1_4);
+double y1_6 = lowpass1_1.step (y1_5);
+emg1_filtered = lowpass1_2.step (y1_6);
+
+//EMG2
+//double y2_1 = highpass2_1.step ( emg2.read());
+//double y2_2 = highpass2_2.step ( y2_1);
+//double y2_3 = notch2_1.step (y2_2);
+//double y2_4 = notch2_2.step (y2_3);
+//double y2_5 = abs(y2_4);
+//double y2_6 = lowpass2_1.step (y2_5);
+//emg2_filtered = lowpass2_2.step (y2_6);
+//EMG3
+//double y3_1 = highpass3_1.step ( emg3.read());
+//double y3_2 = highpass3_2.step ( y3_1);
+//double y3_3 = notch3_1.step (y3_2);
+//double y3_4 = notch3_2.step (y3_3);
+//double y3_5 = abs(y3_4);
+//double y3_6 = lowpass3_1.step (y3_5);
+//emg3_filtered = lowpass3_2.step (y3_6);
+//EMG4     
+//double y4_1 = highpass4_1.step ( emg4.read());
+//double y4_2 = highpass4_2.step ( y4_1);
+//double y4_3 = notch4_1.step (y4_2);
+//double y4_4 = notch4_2.step (y4_3);
+//double y4_5 = abs(y4_4);
+//double y4_6 = lowpass4_1.step (y4_5);
+//emg4_filtered = lowpass4_1.step (y4_6);
+
+//Send scope value to HIDScope        
+
+//scope.set(2, emg2_filtered);
+//scope.set(3, emg3_filtered);
+
+
+ledje = !ledje;      
+}  
+
+void setscope(){
+//scope.set(0,emg1.read());  
+scope.set(1,emg1_filtered); 
+scope.send();
+} 
+
+int main()
+{
+    sample_ticker.attach(&emg_samples,0.002); //fsample = 500Hz for every ticker
+    main_ticker.attach(&setscope,0.0);
+   while (true){}  
+}  
\ No newline at end of file