Diff: main.cpp

Revision:

0:8ccd4c66e07f

Child:

1:b9471e519760

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Oct 07 20:52:18 2015 +0000
@@ -0,0 +1,93 @@
+#include "mbed.h"
+#include "HIDScope.h"
+#include "QEI.h"
+AnalogIn    emg(A0)
+HIDScope scope(1);
+Ticker get;
+AnalogIn pot(A1);
+Serial pc(USBTX, USBRX); // tx, rx
+
+volatile bool fn_go = false;
+double y_notch_1;
+double y_notch_2;
+double y_high;
+double y_low;
+
+const double Fs = 500;
+const double Ts = 0.002;
+
+const double gain_notch_1 = 0.912483;
+const double b0_notch_1 = 1.0*gain_notch_1;
+const double b1_notch_1 = -1.62829098849*gain_notch_1;
+const double b2_notch_1 = 1.0*gain_notch_1;
+const double a1_notch_1 = -1.49965530713;
+const double a2_notch_1 =  0.90720693582;
+double v1_notch_1 = 0, v2_notch_1 = 0;
+
+
+const double gain_notch_2 = 1.000000;
+const double b0_notch_2 = 1.0*gain_notch_2;
+const double b1_notch_2 = -1.63220522905*gain_notch_2;
+const double b2_notch_2 = 1.0*gain_notch_2;
+const double a1_notch_2 = -1.61200955424;
+const double a2_notch_2 =  0.91813588764;
+double v1_notch_2 = 0, v2_notch_2 = 0;
+
+const double gain_high = 0.956602;
+const double b0_high = 1.0*gain_high;
+const double b1_high = -1.99999962667*gain_high;
+const double b2_high = 1.0*gain_high;
+const double a1_high = -1.91152224950;
+const double a2_high =  0.91532727281;
+double v1_high = 0, v2_high = 0;
+
+const double gain_low = 0.003428;
+const double b0_low = 1.0*gain_low;
+const double b1_low = 2.00000000000*gain_low;
+const double b2_low = 1.0*gain_low;
+const double a1_low = -1.86528269522;
+const double a2_low =  0.87380140464;
+double v1_low = 0, v2_low = 0;
+
+
+double 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;
+    v2 = v1;
+    v1 = v;
+    return y;
+}
+
+void fn_activate()
+{
+
+    fn_go = true;
+}
+
+void scopeSend()
+{
+  y_high = biquad(emg.read(), v1_high, v1_high, a1_high, a2_high, b0_high, b1_high, b2_high); 
+  y_notch_1 = biquad(y_high, v1_notch_1, v1_notch_1, a1_notch_1,a2_notch_1,b0_notch_1, b1_notch_1, b2_notch_1);
+           y_notch_2 = biquad(y_notch_1, v1_notch_2, v1_notch_2, a1_notch_2, a2_notch_2, b0_notch_2, b1_notch_2, b2_notch_2);  
+               
+            y_low = biquad(y_high,  v1_low, v1_low, a1_low, a2_low, b0_low, b1_low, b2_low);
+  scope.set(0,y_low);
+    scope.send();
+}
+
+int main()
+{
+get.attach(&fn_activate,Ts);
+
+    while (true) {
+        
+        if(fn_go == true) {
+            
+            scopeSend();
+            
+           
+            fn_go = false;
+        }
+    }
+}
\ No newline at end of file