Laura Veenendaal
filters aangepast en bepaling threshold
Dependencies: HIDScope MODSERIAL biquadFilter mbed
Fork of
EMGV3
by 
Margreeth de Breij
Diff: main.cpp
- Revision:
- 27:aa5ffd7227a5
- Parent:
- 26:4c867b04c0d8
- Child:
- 28:38d21b78c6aa
--- a/main.cpp Mon Oct 12 16:52:13 2015 +0000
+++ b/main.cpp Tue Oct 13 13:30:16 2015 +0000
@@ -6,11 +6,13 @@
//Define objects
AnalogIn EMG_left(A0); //Analog input
AnalogIn EMG_right(A1);
- Ticker SampleEMG;
+ Ticker SampleEMGLeft;
+ Ticker SampleEMGRight;
Ticker ScopeTimer;
Ticker serial;
- Ticker MovingAverage;
- HIDScope scope(6);
+ Ticker MovingAverageLeft;
+ Ticker MovingAverageRight;
+ HIDScope scope(4);
DigitalOut led(LED_RED);
MODSERIAL pc(USBTX, USBRX);
@@ -26,6 +28,7 @@
double EMG_left_f5;
double EMG_left_f6;
double EMG_left_abs;
+
double EMG_right_value;
double EMG_right_f1;
double EMG_right_f2;
@@ -33,7 +36,6 @@
double EMG_right_f4;
double EMG_right_f5;
double EMG_right_f6;
- double EMG_right_MAF;
double EMG_right_abs;
double Threshold1 = 0.02;
double Threshold2 = 0.06;
@@ -41,19 +43,21 @@
int N = 50;
double MAF_left[50];
double EMG_left_MAF;
+ double MAF_right[50];
+ double EMG_right_MAF;
// coëfficiënten
const double BiGainEMG_H1 = 0.795375, BiGainEMG_H2 = 0.895763;
- const double EMGH1_a1 = -1.56308931068*BiGainEMG_H1, EMGH1_a2 = 0.61765749583*BiGainEMG_H1, EMGH1_b0 = 1.0*BiGainEMG_H1, EMGH1_b1 = -1.99909075151*BiGainEMG_H1, EMGH1_b2 = 1.0*BiGainEMG_H1; //coefficients for high-pass filter
- const double EMGH2_a1 = -1.75651417587*BiGainEMG_H2, EMGH2_a2 = 0.82183182692*BiGainEMG_H2, EMGH2_b0 = 1.0*BiGainEMG_H2, EMGH2_b1 = -1.99470632157*BiGainEMG_H2, EMGH2_b2 = 1.0*BiGainEMG_H2; //coefficients for high-pass filter
+ const double EMGH1_a1 = -1.56308931068, EMGH1_a2 = 0.61765749583, EMGH1_b0 = 1.0*BiGainEMG_H1, EMGH1_b1 = -1.99909075151*BiGainEMG_H1, EMGH1_b2 = 1.0*BiGainEMG_H1; //coefficients for high-pass filter
+ const double EMGH2_a1 = -1.75651417587, EMGH2_a2 = 0.82183182692, EMGH2_b0 = 1.0*BiGainEMG_H2, EMGH2_b1 = -1.99470632157*BiGainEMG_H2, EMGH2_b2 = 1.0*BiGainEMG_H2; //coefficients for high-pass filter
const double BiGainEMG_L1=0.959332, BiGainEMG_L2 = 0.223396;
- const double EMGL1_a1 = -1.55576653052*BiGainEMG_L1, EMGL1_a2 = 0.61374320375*BiGainEMG_L1, EMGL1_b0 = 1.0*BiGainEMG_L1, EMGL1_b1 = -0.90928276835*BiGainEMG_L1, EMGL1_b2 = 1.0*BiGainEMG_L1; // coefficients for low-pass filter
- const double EMGL2_a1 = -1.79696141922*BiGainEMG_L2, EMGL2_a2 = 0.85096669383*BiGainEMG_L2, EMGL2_b0 = 1.0*BiGainEMG_L2, EMGL2_b1 = -1.75825311060*BiGainEMG_L2, EMGL2_b2 = 1.0*BiGainEMG_L2; // coefficients for low-pass filter
+ const double EMGL1_a1 = -1.55576653052, EMGL1_a2 = 0.61374320375, EMGL1_b0 = 1.0*BiGainEMG_L1, EMGL1_b1 = -0.90928276835*BiGainEMG_L1, EMGL1_b2 = 1.0*BiGainEMG_L1; // coefficients for low-pass filter
+ const double EMGL2_a1 = -1.79696141922, EMGL2_a2 = 0.85096669383, EMGL2_b0 = 1.0*BiGainEMG_L2, EMGL2_b1 = -1.75825311060*BiGainEMG_L2, EMGL2_b2 = 1.0*BiGainEMG_L2; // coefficients for low-pass filter
const double BiGainEMG_N1 = 1.0, BiGainEMG_N2 = 0.965081;
- const double EMGN1_a1 = -1.56858163035*BiGainEMG_N1, EMGN1_a2 = 0.96424138362*BiGainEMG_N1, EMGN1_b0 = 1.0*BiGainEMG_N1, EMGN1_b1 = -1.61854514265*BiGainEMG_N1, EMGN1_b2 = 1.0*BiGainEMG_N1; //coefficients for high-pass filter
- const double EMGN2_a1 = -1.61100357722*BiGainEMG_N2, EMGN2_a2 = 0.96592170538*BiGainEMG_N2, EMGN2_b0 = 1.0*BiGainEMG_N2, EMGN2_b1 = -1.61854514265*BiGainEMG_N2, EMGN2_b2 = 1.0*BiGainEMG_N2; //coefficients for high-pass filter
+ const double EMGN1_a1 = -1.56858163035, EMGN1_a2 = 0.96424138362, EMGN1_b0 = 1.0*BiGainEMG_N1, EMGN1_b1 = -1.61854514265*BiGainEMG_N1, EMGN1_b2 = 1.0*BiGainEMG_N1; //coefficients for notch filter
+ const double EMGN2_a1 = -1.61100357722, EMGN2_a2 = 0.96592170538, EMGN2_b0 = 1.0*BiGainEMG_N2, EMGN2_b1 = -1.61854514265*BiGainEMG_N2, EMGN2_b2 = 1.0*BiGainEMG_N2; //coefficients for notch filter
// Filter creation
biquadFilter EMG_highpass1 (EMGH1_a1, EMGH1_a2, EMGH1_b0, EMGH1_b1, EMGH1_b2); // creates the high pass filter
@@ -62,9 +66,16 @@
biquadFilter EMG_lowpass2 (EMGL2_a1, EMGL2_a2, EMGL2_b0, EMGL2_b1, EMGL2_b2);
biquadFilter EMG_notch1 (EMGN1_a1, EMGN1_a2, EMGN1_b0, EMGN1_b1, EMGN1_b2); // creates the notch filter
biquadFilter EMG_notch2 (EMGN2_a1, EMGN2_a2, EMGN2_b0, EMGN2_b1, EMGN2_b2);
-
+
+ biquadFilter EMG_highpass1R (EMGH1_a1, EMGH1_a2, EMGH1_b0, EMGH1_b1, EMGH1_b2); // creates the high pass filter
+ biquadFilter EMG_highpass2R (EMGH2_a1, EMGH2_a2, EMGH2_b0, EMGH2_b1, EMGH2_b2);
+ biquadFilter EMG_lowpass1R (EMGL1_a1, EMGL1_a2, EMGL1_b0, EMGL1_b1, EMGL1_b2); // creates the low pass filter
+ biquadFilter EMG_lowpass2R (EMGL2_a1, EMGL2_a2, EMGL2_b0, EMGL2_b1, EMGL2_b2);
+ biquadFilter EMG_notch1R (EMGN1_a1, EMGN1_a2, EMGN1_b0, EMGN1_b1, EMGN1_b2); // creates the notch filter
+ biquadFilter EMG_notch2R (EMGN2_a1, EMGN2_a2, EMGN2_b0, EMGN2_b1, EMGN2_b2);
+
// EMG filtering function
-void EMGfilter() // Both EMG signals are filtered in one function and with the same filters
+void EMGfilterLeft()
{
EMG_left_value = EMG_left.read();
EMG_left_f1 = EMG_highpass1.step(EMG_left_value);
@@ -74,27 +85,26 @@
EMG_left_f4 = EMG_lowpass2.step(EMG_left_f3);
EMG_left_f5 = EMG_notch1.step(EMG_left_f4);
EMG_left_f6 = EMG_notch1.step(EMG_left_f5);
-
+
+}
+
+void EMGfilterRight()
+{
EMG_right_value = EMG_right.read();
- EMG_right_f1 = EMG_highpass1.step(EMG_right_value);
- EMG_right_f2 = EMG_highpass2.step(EMG_right_f1);
+ EMG_right_f1 = EMG_highpass1R.step(EMG_right_value);
+ EMG_right_f2 = EMG_highpass2R.step(EMG_right_f1);
EMG_right_abs = fabs(EMG_right_f2);
- EMG_right_f3 = EMG_lowpass1.step(EMG_right_abs);
- EMG_right_f4 = EMG_lowpass2.step(EMG_right_f3);
- EMG_right_f5 = EMG_notch1.step(EMG_right_f4);
- EMG_right_f6 = EMG_notch1.step(EMG_right_f5);
+ EMG_right_f3 = EMG_lowpass1R.step(EMG_right_abs);
+ EMG_right_f4 = EMG_lowpass2R.step(EMG_right_f3);
+ EMG_right_f5 = EMG_notch1R.step(EMG_right_f4);
+ EMG_right_f6 = EMG_notch1R.step(EMG_right_f5);
}
// Movingaverage Filter
- void MovingAverageFilter()
+ void MovingAverageFilterLeft()
{
EMG_left_MAF = (MAF_left[0]+MAF_left[1]+MAF_left[2]+MAF_left[3]+MAF_left[4]+MAF_left[5]+MAF_left[6]+MAF_left[7]+MAF_left[8]+MAF_left[9]+MAF_left[10]+MAF_left[11]+MAF_left[12]+MAF_left[13]+MAF_left[14]+MAF_left[15]+MAF_left[16]+MAF_left[17]+MAF_left[18]+MAF_left[19]+MAF_left[20]+MAF_left[21]+MAF_left[22]+MAF_left[23]+MAF_left[24]+MAF_left[25]+MAF_left[26]+MAF_left[27]+MAF_left[28]+MAF_left[29]+MAF_left[30]+MAF_left[31]+MAF_left[32]+MAF_left[33]+MAF_left[34]+MAF_left[35]+MAF_left[36]+MAF_left[37]+MAF_left[38]+MAF_left[39]+MAF_left[40]+MAF_left[41]+MAF_left[42]+MAF_left[43]+MAF_left[44]+MAF_left[45]+MAF_left[46]+MAF_left[47]+MAF_left[48]+MAF_left[49])/N;
- MAF_left[49] = MAF_left[48];
- MAF_left[48] = MAF_left[47];
- MAF_left[47] = MAF_left[46];
- MAF_left[46] = MAF_left[45];
- MAF_left[45] = MAF_left[44];
- MAF_left[44] = MAF_left[43];
+ MAF_left[49] = MAF_left[48], MAF_left[48] = MAF_left[47], MAF_left[47] = MAF_left[46], MAF_left[46] = MAF_left[45], MAF_left[45] = MAF_left[44];MAF_left[44] = MAF_left[43];
MAF_left[43] = MAF_left[42];
MAF_left[42] = MAF_left[41];
MAF_left[41] = MAF_left[40];
@@ -141,23 +151,79 @@
MAF_left[0] = EMG_left_f6;
}
+ void MovingAverageFilterRight()
+ {
+ EMG_right_MAF = (MAF_right[0]+MAF_right[1]+MAF_right[2]+MAF_right[3]+MAF_right[4]+MAF_right[5]+MAF_right[6]+MAF_right[7]+MAF_right[8]+MAF_right[9]+MAF_right[10]+MAF_right[11]+MAF_right[12]+MAF_right[13]+MAF_right[14]+MAF_right[15]+MAF_right[16]+MAF_right[17]+MAF_right[18]+MAF_right[19]+MAF_right[20]+MAF_right[21]+MAF_right[22]+MAF_right[23]+MAF_right[24]+MAF_right[25]+MAF_right[26]+MAF_right[27]+MAF_right[28]+MAF_right[29]+MAF_right[30]+MAF_right[31]+MAF_right[32]+MAF_right[33]+MAF_right[34]+MAF_right[35]+MAF_right[36]+MAF_right[37]+MAF_right[38]+MAF_right[39]+MAF_right[40]+MAF_right[41]+MAF_right[42]+MAF_right[43]+MAF_right[44]+MAF_right[45]+MAF_right[46]+MAF_right[47]+MAF_right[48]+MAF_right[49])/N;
+ MAF_right[49] = MAF_right[48];
+ MAF_right[48] = MAF_right[47];
+ MAF_right[47] = MAF_right[46];
+ MAF_right[46] = MAF_right[45];
+ MAF_right[45] = MAF_right[44];
+ MAF_right[44] = MAF_right[43];
+ MAF_right[43] = MAF_right[42];
+ MAF_right[42] = MAF_right[41];
+ MAF_right[41] = MAF_right[40];
+ MAF_right[40] = MAF_right[39];
+ MAF_right[39] = MAF_right[38];
+ MAF_right[38] = MAF_right[37];
+ MAF_right[37] = MAF_right[36];
+ MAF_right[36] = MAF_right[35];
+ MAF_right[35] = MAF_right[34];
+ MAF_right[34] = MAF_right[33];
+ MAF_right[33] = MAF_right[32];
+ MAF_right[32] = MAF_right[31];
+ MAF_right[31] = MAF_right[30];
+ MAF_right[30] = MAF_right[29];
+ MAF_right[29] = MAF_right[28];
+ MAF_right[28] = MAF_right[27];
+ MAF_right[27] = MAF_right[26];
+ MAF_right[26] = MAF_right[25];
+ MAF_right[25] = MAF_right[24];
+ MAF_right[24] = MAF_right[23];
+ MAF_right[23] = MAF_right[22];
+ MAF_right[22] = MAF_right[21];
+ MAF_right[21] = MAF_right[20];
+ MAF_right[20] = MAF_right[19];
+ MAF_right[19] = MAF_right[18];
+ MAF_right[18] = MAF_right[17];
+ MAF_right[17] = MAF_right[16];
+ MAF_right[16] = MAF_right[15];
+ MAF_right[15] = MAF_right[14];
+ MAF_right[14] = MAF_right[13];
+ MAF_right[13] = MAF_right[12];
+ MAF_right[12] = MAF_right[11];
+ MAF_right[11] = MAF_right[10];
+ MAF_right[10] = MAF_right[9];
+ MAF_right[9] = MAF_right[8];
+ MAF_right[8] = MAF_right[7];
+ MAF_right[7] = MAF_right[6];
+ MAF_right[6] = MAF_right[5];
+ MAF_right[5] = MAF_right[4];
+ MAF_right[4] = MAF_right[3];
+ MAF_right[3] = MAF_right[2];
+ MAF_right[2] = MAF_right[1];
+ MAF_right[1] = MAF_right[0];
+ MAF_right[0] = EMG_right_f6;
+ }
+
+
// HIDScope
void ScopeSend()
{
scope.set(0, EMG_left_value);
scope.set(1, EMG_left_MAF);
scope.set(2, EMG_right_value);
- scope.set(3, EMG_right_f2);
- scope.set(4, EMG_right_f4);
- scope.set(5, EMG_right_f6);
+ scope.set(3, EMG_right_MAF);
scope.send();
}
int main()
{
- SampleEMG.attach(&EMGfilter, 0.002);
+ SampleEMGLeft.attach(&EMGfilterLeft, 0.002);
+ SampleEMGRight.attach(&EMGfilterRight, 0.002);
ScopeTimer.attach(&ScopeSend, 0.002);
- MovingAverage.attach(&MovingAverageFilter, 0.005);
+ MovingAverageLeft.attach(&MovingAverageFilterLeft, 0.005);
+ MovingAverageRight.attach(&MovingAverageFilterRight, 0.005);
while(1)
{
if (EMG_left_f6 > Threshold1)