Diff: emg.cpp

Revision:

33:92b645af1388

Parent:

32:00c6e3502bd9

--- a/emg.cpp	Thu Oct 29 17:47:18 2015 +0000
+++ b/emg.cpp	Tue Nov 03 10:15:19 2015 +0000
@@ -1,24 +1,21 @@
 #include "mbed.h"
-#include "HIDScope.h"
-#define MOV_AVG_NUM     100
 #include "math.h"
 
 //Define objects
 AnalogIn    b_emg(A0); //Analog input bicep
 AnalogIn    t_emg(A1); //Analog input tricep
 AnalogIn    p_emg(A2); //Analog input bicep for push
-//HIDScope    scope_hid(6);
 
-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 b_min = 10.0f;
-double t_min = 10.0f;
-double p_min = 10.0f;
-
+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      b_min = 10.0f;
+double      t_min = 10.0f;
+double      p_min = 10.0f;
+double      rust;
 
 double filter(double input, double coeff_input[], double coeff_output[], double prev_outputs[])
 {
@@ -36,16 +33,20 @@
     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 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, b_highV);
 }
+
 double p_highpass_filter(double u)
 {
     return filter(u, fh_a, fh_b, p_highV);
@@ -53,22 +54,24 @@
 
 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 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, b_lowV);
 }
+
 double p_lowpass_filter(double u)
 {
 
     return filter(u, fl_a, fl_b, p_lowV);
 }
 
-double rust;
 void emg_cal(int emg)
 {
     double mem[300];
@@ -93,7 +96,6 @@
         }
         variance /=300;
         rust =  mean-variance;
-        printf("Rust %f\r\n",rust); 
     }
     
     if(emg == 1) {
@@ -115,11 +117,8 @@
         }
         variance /=300;
         b_min =  mean-variance;
-        printf("b %f\r\n",b_min);
-        printf("r %f\r\n",rust);
         b_min += rust;
         b_min /= 2;        
-        printf("b_min %f\r\n",b_min);
     }
     else if(emg == 2) {
         for(int i=0; i<900; i++) {
@@ -140,11 +139,8 @@
         }
         variance /=300;
         t_min =  mean-variance;
-        printf("b %f\r\n",t_min);
-        printf("r %f\r\n",rust);
         t_min += rust;
         t_min /= 2;
-        printf("t_min %f\r\n",t_min);
     }
     
     else if(emg == 3) {
@@ -166,17 +162,13 @@
         }
         variance /=300;
         p_min =  mean-variance;
-        printf("b %f\r\n",p_min);
-        printf("r %f\r\n",rust);
         p_min += rust;
         p_min /= 2;
-        printf("p_min %f\r\n",p_min);
     }
     
     
 }
 
-
 /** Sample function
  * this function samples the emg and sends it to HIDScope
  **/
@@ -201,14 +193,6 @@
     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;*/
-
     /* Second, set the sampled emg value in channel zero (the first channel) in the 'HIDScope' variable named 'scope' */
 
     if (last_biceps == 0 and output3 > b_min+b_min*0.1) {
@@ -218,25 +202,8 @@
     } else {
         bicout = last_biceps;
     }
-    //printf("minbic %f\t",bicout);
     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;
-    }*/
-
-    /*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 > t_min+0.1*t_min) {
         tricout = 1;
     } else if(last_triceps == 1 and output6 < t_min-0.1*t_min) {
@@ -244,26 +211,15 @@
     } else {
         tricout = last_triceps;
     }
-    //printf("mintric %f\t",tricout);
     last_triceps = tricout;
 
-    if (last_push ==0 and output9>p_min+0.1*p_min) {//this is the output for the left bicep (the push motion)
+    if (last_push ==0 and output9>p_min+0.1*p_min) {
         pushout=1;
     } else if(last_push == 1 and output9<p_min-0.1*p_min) {
         pushout=0;
     } else {
         pushout = last_push;
     }
-    //printf("minpush %f\r\n",pushout);
 
     last_push = pushout;
-
-    /*scope_hid.set(0,output3);
-    scope_hid.set(1,output6);
-    scope_hid.set(2,output9);
-    scope_hid.set(3,bicout);
-    scope_hid.set(4,tricout);
-    scope_hid.set(5,pushout);
-
-    scope_hid.send();*/
 }
\ No newline at end of file