Diff: main.cpp

Revision:

1:b9471e519760

Parent:

0:8ccd4c66e07f

--- a/main.cpp	Wed Oct 07 20:52:18 2015 +0000
+++ b/main.cpp	Sun Oct 18 14:11:58 2015 +0000
@@ -1,93 +1,69 @@
 #include "mbed.h"
+#include "MODSERIAL.h"
+#include "math.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;
+AnalogIn EMG(A1);
+MODSERIAL pc(USBTX, USBRX);
+
+//const int baudrate = 115200;
+//const int ms_wait = 100;
+
+// Define the HIDScope and Ticker objects
+HIDScope    scope(1);
+Ticker scopeTimer;
+
+// Define moving average variables
+const int N = 125;
+double a;  
+
+ // Define the storage variables and filter coe icients for two filters
+ double f1_v1 = 0, f1_v2 = 0, f2_v1 = 0, f2_v2 = 0;
+const double f1_a1 =  -1.822694925196308, f1_a2 = 0.837181651256022, f1_b0 = 0.914969144113083, f1_b1 = -1.829938288226165, f1_b2 = 0.914969144113083;
+const double f2_a1 = -1.142980502539901, f2_a2 = 0.412801598096189, f2_b0 = 0.067455273889072, f2_b1 =  0.134910547778144, f2_b2 = 0.067455273889072;
 
-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;
+ 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;
+ }
 
-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 sliding_average(double u,const int f_N)
+{
+    double f_x[f_N];
+    double f_sum=0;
+    f_x[1]=abs(u);
+    for (int i=f_N; i>=1; i--){
+        f_x[i]=f_x[i-1];
+        }
+        
+      
+    for (int i=f_N; i>=0; i--){
+        f_sum=f_x[i]+f_sum;
+       }
+        a=f_sum/double(f_N);
+        f_sum=0;
+    return a;
+}
 
 
-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();
-}
-
+void scopeSend(){
+ double u1 = EMG.read()  ; //filter 1 input
+ double y1 = biquad( u1, f1_v1, f1_v2, f1_a1, f1_a2, f1_b0, f1_b1, f1_b2 ); //notch
+ double y2 = biquad( y1, f2_v1, f2_v2, f2_a1, f2_a2, f2_b0, f2_b1, f2_b2 ); //lowpass with rectifier
+ double z1 = 3*sliding_average(y2, N); //moving average
+  scope.set(0,z1);
+scope.send();
+ }
+ 
 int main()
 {
-get.attach(&fn_activate,Ts);
-
-    while (true) {
+    scopeTimer.attach(&scopeSend,0.002);
+     while(1){}
         
-        if(fn_go == true) {
-            
-            scopeSend();
-            
-           
-            fn_go = false;
-        }
-    }
 }
\ No newline at end of file