Diff: read_filter_emg.cpp

Revision:

0:fc6fa085d591

Child:

1:5c1fc4e9401a

diff -r 000000000000 -r fc6fa085d591 read_filter_emg.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/read_filter_emg.cpp	Fri Oct 23 13:14:57 2015 +0000
@@ -0,0 +1,149 @@
+/*
+ * read_filter_emg.cpp
+ *
+ *  Created on: Oct 14, 2015
+ *      Author: User
+ */
+
+#include "mbed.h"
+#include "read_filter_emg.h"
+//included for fabs() function
+#include <math.h>
+//EMG::EMG(AnalogIn aI_, double &v1_, double &v2_)
+
+//EMG constructor
+EMG::EMG(PinName aI_, double &v1_, double &v2_)
+{
+    //initialise the variables of the EMG instance
+    //  ??????  NOT SURE HOW THIS SECTION SHOULD WORK EXACTLY, WHAT SHOULD I DEFINE, WHAT SHOULD I 'RENAME'
+    //          E.G aI = aI_, v1 = v1_...
+    //
+    AnalogIn aI(aI_);
+    /* two past values of v1 and v2 initialised in main are
+        passed to the function as they need to be kept track of
+      cannot be reset each time as filtering would be incorrect
+    */
+    v1 = v1_;
+    v2 = v2_;
+    //initialised to zero, updated in sample()
+    input_sample = 0;
+    //initialised to zero, calculated in filter function using input_sample
+    filtered_sample = 0;
+};
+
+void EMG::sample()
+{
+    //change value of sample to input of the EMG
+    //should i use the reference?
+    input_sample = aI.read();
+}
+
+//general biquad filter that can be called in all the filter functions
+double EMG::biquad(double u, double &v1, double &v2, const double a1,
+                    const double a2, const double b0, const double b1, const double b2)
+{
+    double v = u * a1*v1 * a2*v2;
+    double y = b0*v + b1*v1 + b2*v2;
+    //values of v2 and v1 are updated, as they are passed by reference
+    //they update globally
+    v2 = v1; v1 = v;
+    return y;
+}
+
+double EMG::highpass_filter(double input)
+{
+    /* high pass filter consists of three cascaded biquads
+    blow coefficients for those three biquads */
+    //first high pass biquad
+    const double highp1_a1 = -0.67538034389;
+    const double highp1_a2 = 0.12769255668;
+    const double highp1_b0 = 1.00000000000;
+    const double highp1_b1 = -2.00000000000;
+    const double highp1_b2 = 1.00000000000;
+
+    //second high pass biquad
+    const double highp2_a1 = -0.76475499450;
+    const double highp2_a2 = 0.27692273367;
+    const double highp2_b0 = 1.00000000000;
+    const double highp2_b1 = -2.00000000000;
+    const double highp2_b2 = 1.00000000000;
+
+    //third high pass biquad
+    const double highp3_a1 = -0.99216561242;
+    const double highp3_a2 = 0.65663360837;
+    const double highp3_b0 = 1.00000000000;
+    const double highp3_b1 = -2.00000000000;
+    const double highp3_b2 = 1.00000000000;
+
+    //input to each filter is output of the filter before(excl. first which uses input_sample)
+    /* NOT SURE IF PASSING V1 AND V2 IS CORRECT,
+    WILL IT UPDATE IN THE MEMORY POSITION SO THAT
+    V1 IS CHANGED GLOBALLY */
+    double y1 = EMG::biquad(input, v1, v2, highp1_a1, highp1_a2, highp1_b0, highp1_b1, highp1_b2);
+    double y2 = EMG::biquad(y1, v1, v2, highp2_a1, highp2_a2, highp2_b0, highp2_b1, highp2_b2);
+    double y3 = EMG::biquad(y2, v1, v2, highp3_a1, highp3_a2, highp3_b0, highp3_b1, highp3_b2);
+
+    return y3;
+}
+
+//need to implement
+double rectify(double input)
+{
+    input = fabs(input);
+    return input;
+}
+double EMG::lowpass_filter(double input)
+{
+    /* lowpass filter consists of three cascaded biquads
+    below the coefficients for those three biquads */
+    //first high pass biquad
+    const double lowp1_a1 = -1.05207469728;
+    const double lowp1_a2 = 0.28586907478;
+    const double lowp1_b0 = 1.00000000000;
+    const double lowp1_b1 = 2.00000000000;
+    const double lowp1_b2 = 1.00000000000;
+
+    //second high pass biquad
+    const double lowp2_a1 = -1.16338171052;
+    const double lowp2_a2 = 0.42191097989;
+    const double lowp2_b0 = 1.00000000000;
+    const double lowp2_b1 = 2.00000000000;
+    const double lowp2_b2 = 1.00000000000;
+
+    //third high pass biquad
+    const double lowp3_a1 = -1.42439823874;
+    const double lowp3_a2 = 0.74093118112;
+    const double lowp3_b0 = 1.00000000000;
+    const double lowp3_b1 = 2.00000000000;
+    const double lowp3_b2 = 1.00000000000;
+
+    //input to each filter is output of the filter before(excl. first which uses input_sample)
+    /* NOT SURE IF PASSING V1 AND V2 IS CORRECT,
+    WILL IT UPDATE IN THE MEMORY POSITION SO THAT V1 IS
+    CHANGED GLOBALLY */
+    double y1 = EMG::biquad(input, v1, v2, lowp1_a1, lowp1_a2, lowp1_b0, lowp1_b1, lowp1_b2);
+    double y2 = EMG::biquad(y1, v1, v2, lowp2_a1, lowp2_a2, lowp2_b0, lowp2_b1, lowp2_b2);
+    double y3 = EMG::biquad(y2, v1, v2, lowp3_a1, lowp3_a2, lowp3_b0, lowp3_b1, lowp3_b2);
+
+    return y3;
+}
+
+double EMG::filter(double input)
+{
+    /* function passes the input through the three filters
+    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);
+    double y2 = rectify(y1);
+    double y3 = lowpass_filter(y3);
+    filtered_sample = y3;
+
+    return filtered_sample;
+}
+
+
+
+
+
+