Diff: main.cpp

Revision:

3:7cb317c00afb

Parent:

2:1d29b91bc46a

Child:

4:d32946ab9c54

--- a/main.cpp	Mon Oct 26 15:00:52 2015 +0000
+++ b/main.cpp	Mon Oct 26 15:49:35 2015 +0000
@@ -5,17 +5,18 @@
 #include "HIDScope.h"
 
 Ticker sample_timer;
-HIDScope scope(1);
+HIDScope scope(2);
 
 AnalogIn analog_emg_left(A0);
-AnalogIn analog_emg_right(A1);
-double input_left = 0;
-double input_right = 0;
+//AnalogIn analog_emg_right(A1);
+double input = 0;
+double filter_signal;
+//double input_right = 0;
 
-double v1_left=0;
-double v2_left=0;
-double v1_right=0;
-double v2_right=0;
+double v1=0;
+double v2=0;
+//double v1_right=0;
+//double v2_right=0;
 
 double filter_left;
 double filter_right;
@@ -35,6 +36,28 @@
 
 //Specifying filter coefficients highpass
 
+/* notch filter with 3 cascaded biquads*/
+//first notch biquad
+const double notch1_a1 = -1.55951422433;
+const double notch1_a2 = 0.92705680308;
+const double notch1_b0 = 1.00000000000;
+const double notch1_b1 = -1.61854515325;
+const double notch1_b2 = 1.00000000000;
+
+//second notch biquad
+const double notch2_a1 = -1.54767435801;
+const double notch2_a2 = 0.96124842048;
+const double notch2_b0 = 1.00000000000;
+const double notch2_b1 = -1.61854515325;
+const double notch2_b2 = 1.00000000000;
+
+//third notch biquad
+const double notch3_a1 = -1.62600366964;
+const double notch3_a2 = 0.96453460373;
+const double notch3_b0 = 1.00000000000;
+const double notch3_b1 = -1.61854515325;
+const double notch3_b2 = 1.00000000000;
+
 /* high pass filter consists of three cascaded biquads
  blow coefficients for those three biquads */
 //first high pass biquad
@@ -88,32 +111,44 @@
 WILL IT UPDATE IN THE MEMORY POSITION SO THAT
 V1 IS CHANGED GLOBALLY */
 
-//highpass
+//notch
 
-double highpass_filter(double input, double v1, double v2)
+double notch_filter(double input, double v1, double v2)
 {
-    double y1 = biquad(input, v1, v2, highp1_a1, highp1_a2, highp1_b0, highp1_b1, highp1_b2);
-    double y2 = biquad(y1, v1, v2, highp2_a1, highp2_a2, highp2_b0, highp2_b1, highp2_b2);
-    double y3 = biquad(y2, v1, v2, highp3_a1, highp3_a2, highp3_b0, highp3_b1, highp3_b2);
+    double y1 = biquad(input, v1, v2, notch1_a1, notch1_a2, notch1_b0, notch1_b1, notch1_b2);
+    double y2 = biquad(y1, v1, v2, notch2_a1, notch2_a2, notch2_b0, notch2_b1, notch2_b2);
+    double y3 = biquad(y2, v1, v2, notch3_a1, notch3_a2, notch3_b0, notch3_b1, notch3_b2);
 
     return y3;
 }
 
 
-//rectifier
-double rectify(double y3)
+//highpass
+
+double highpass_filter(double y3, double v1, double v2)
 {
-    y3 = fabs(y3);
-    return y3;
+    double y4 = biquad(y3, v1, v2, highp1_a1, highp1_a2, highp1_b0, highp1_b1, highp1_b2);
+    double y5 = biquad(y4, v1, v2, highp2_a1, highp2_a2, highp2_b0, highp2_b1, highp2_b2);
+    double y6 = biquad(y5, v1, v2, highp3_a1, highp3_a2, highp3_b0, highp3_b1, highp3_b2);
+
+    return y6;
+}
+
+
+//rectifier
+double rectify(double y6)
+{
+    y6 = fabs(y6);
+    return y6;
 }
 
 //lowpass
 
-double lowpass_filter(double y3, double v1, double v2)
+double lowpass_filter(double y6, double v1, double v2)
 {
-    double y4 = biquad(y3, v1, v2, lowp1_a1, lowp1_a2, lowp1_b0, lowp1_b1, lowp1_b2);
-    double y5 = biquad(y4, v1, v2, lowp2_a1, lowp2_a2, lowp2_b0, lowp2_b1, lowp2_b2);
-    double filtered_signal = biquad(y5, v1, v2, lowp3_a1, lowp3_a2, lowp3_b0, lowp3_b1, lowp3_b2);
+    double y7 = biquad(y6, v1, v2, lowp1_a1, lowp1_a2, lowp1_b0, lowp1_b1, lowp1_b2);
+    double y8 = biquad(y7, v1, v2, lowp2_a1, lowp2_a2, lowp2_b0, lowp2_b1, lowp2_b2);
+    double filtered_signal = biquad(y8, v1, v2, lowp3_a1, lowp3_a2, lowp3_b0, lowp3_b1, lowp3_b2);
 
     return filtered_signal;
 }
@@ -124,7 +159,8 @@
     returns the final output value as filtered sample
     this is used in check_state() function to determine state of system
     */
-    double y1 = highpass_filter(input, v1, v2);
+    double y0 = notch_filter(input, v1, v2);
+    double y1 = highpass_filter(y0, v1, v2);
     double y2 = rectify(y1);
     double filtered_signal = lowpass_filter(y2, v1, v2);
 
@@ -135,13 +171,13 @@
 
 void sample()
 {
-    input_left = analog_emg_left.read();
-    input_right = analog_emg_right.read();
-    filter_left = filter(input_left, v1_left, v2_left);
-    filter_right = filter(input_right, v1_right, v2_right);
+    input = analog_emg_left.read();
+    //input_right = analog_emg_right.read();
+    filter_signal = filter(input, v1, v2);
+    //filter_right = filter(input_right, v1_right, v2_right);
     
-    scope.set(1, input_left);
-    scope.set(0, filter_left);
+    scope.set(0, input);
+    scope.set(1, filter_signal);
     scope.send();
 }