Diff: emg.cpp

Revision:

24:b7b3e87e0687

Parent:

23:8647e71ca713

Child:

25:6d1b035f4838

Child:

26:bd50afa17436

--- a/emg.cpp	Tue Oct 20 14:30:18 2015 +0000
+++ b/emg.cpp	Thu Oct 22 14:43:33 2015 +0000
@@ -1,14 +1,21 @@
 #include "mbed.h"
 #include "HIDScope.h"
-#include "emg.h"
+
+#define MOV_AVG_NUM     100
 
 //Define objects
-AnalogIn    emgIn0(A0); //Analog input
-AnalogIn    emgIn1(A1); //Analog input
-AnalogIn    emgIn2(A2); //Analog input
+AnalogIn    b_emg(A0); //Analog input bicep
+AnalogIn    t_emg(A1); //Analog input tricep
+AnalogIn    p_emg(A2); //Analog input bicep for push
 
-double highV[4];
-double lowV[4];
+double b_highV[4];
+double b_lowV[4];
+double t_highV[4];
+double t_lowV[4];
+double p_highV[4];
+double p_lowV[4];
+
+//double lastOutputs[MOV_AVG_NUM-1];
 
 double filter(double input, double coeff_input[], double coeff_output[], double prev_outputs[])
 {
@@ -26,40 +33,108 @@
     prev_outputs[0] = new_input;
     return new_output;
 }
-
 double fh_b[]= {0.7602, -3.0406, 4.5609, -3.0406, 0.7602};
 double fh_a[]= {1, -3.4532, 4.5041, -2.6273, 0.5778};
-double highpass_filter(double u)
+double b_highpass_filter(double u)
+{
+    return filter(u, fh_a, fh_b, t_highV);
+    }
+double t_highpass_filter(double u)
 {
-    return filter(u, fh_a, fh_b, highV);
+    return filter(u, fh_a, fh_b, b_highV);
+}
+double p_highpass_filter(double u)
+{
+    return filter(u, fh_a, fh_b, p_highV);
 }
 
 double fl_b[]= {0.00001329,    0.00005317,    0.00007976,   0.00005317,    0.00001329};
 double fl_a[]= {1.0000,   -3.6717,    5.0680,   -3.1160,    0.7199};
-double lowpass_filter(double u)
+double b_lowpass_filter(double u)
+{
+        return filter(u, fl_a, fl_b, t_lowV);
+}
+double t_lowpass_filter(double u)
 {
-    return filter(u, fl_a, fl_b, lowV);
+
+    return filter(u, fl_a, fl_b, b_lowV);
+}
+double p_lowpass_filter(double u)
+{
+
+    return filter(u, fl_a, fl_b, p_lowV);
 }
 
+double last_biceps = 0;
+double last_triceps = 0;
+double last_push = 0;
 /** Sample function
  * this function samples the emg and sends it to HIDScope
  **/
-double sample(int emgNum)
+void sample(double& bicout, double& tricout, double& pushout)
 {
-    double input = 0.0;
-    if(emgNum == 1) input = emgIn1.read();
-    if(emgNum == 2) input = emgIn2.read();
-    else input = emgIn0.read();
-    double output1 = highpass_filter(input);
+    double b_input = b_emg.read();
+    double output1 = b_highpass_filter(b_input);
     double output2 = fabs(output1);
-    double output3 = lowpass_filter(output2);
+    double output3 = b_lowpass_filter(output2);
+      
+    double t_input = t_emg.read();
+    double output4 = t_highpass_filter(t_input);
+    double output5 = fabs(output4);
+    double output6 = t_lowpass_filter(output5);
+    
+    double p_input = p_emg.read();
+    double output7 = p_highpass_filter(p_input);
+    double output8 = fabs(output7);
+    double output9 = p_lowpass_filter(output8);
+    
+    /*double tot = output3;
+    for(int i=0; i<MOV_AVG_NUM-1; i++) {
+        tot += lastOutputs[i];
+        if(i != 0) lastOutputs[i] = lastOutputs[i-1];
+    }
+    lastOutputs[0] = output3;
+    output3 = tot/MOV_AVG_NUM;*/
     
-    if(output3 >= 0 && output3 < 0.0507)
-        output3 = 0.0;
-    else if(output3 >= 0.0507 && output3 < 0.0966)
-        output3 = 0.5;
-    else if(output3 >= 0.0966 && output3 < 1)
-        output3 = 1.0;
+    /* Second, set the sampled emg value in channel zero (the first channel) in the 'HIDScope' variable named 'scope' */
+
+    if (last_biceps == 0 and output3 > 0.06) {
+        bicout = 1;
+    } else if (last_biceps == 1 and output3 < 0.045) {
+        bicout = 0;
+    } else {
+        bicout = last_biceps;   
+    }
+    last_biceps = bicout;
+    
+    /*if (output3>0.06) {//This is the output for the right bicep
+        bicout=1;
+    } else if(output3>0.04) {
+        bicout=0.5;
+    } else {
+        bicout=0;
+    }*/
     
-    return output3;
+    /*if (output6>0.0561) {//this is the output for the right tricep
+        tricout=1;
+    } else if(output6>0.0380) {
+        tricout=0.5;
+    } else {
+        tricout=0;
+    }*/
+    
+    if (last_triceps == 0 and output6 > 0.045) {
+        tricout = 1;
+    } else if(last_triceps == 1 and output6 < 0.04) {
+        tricout = 0;    
+    } else {
+        tricout = last_triceps;
+    }
+    last_triceps = tricout;
+    
+    if (output9>0.05) {//this is the output for the left bicep (the push motion)
+        pushout=1;
+    } else {
+        pushout=0;
+    }
 }
\ No newline at end of file