Diff: emg.cpp

Revision:

30:aa0389e04d47

Parent:

29:373a2facb3ae

Child:

31:6845e4099492

Child:

32:00c6e3502bd9

--- a/emg.cpp	Thu Oct 29 10:53:46 2015 +0000
+++ b/emg.cpp	Thu Oct 29 16:36:36 2015 +0000
@@ -1,11 +1,13 @@
 #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];
@@ -17,38 +19,15 @@
 double t_min = 10.0f;
 double p_min = 10.0f;
 
-void emg_cal(int emg){
-    double cur;
-    if(emg == 1){
-        for(int i=0; i<300; i++){
-            cur = b_emg.read();
-            if(cur < b_min)
-                b_min = cur;
-        }
-    }
-    else if(emg == 2){
-        for(int i=0; i<300; i++){
-            cur = t_emg.read();
-            if(cur < t_min)
-                t_min = cur;
-        }
-    }
-    else if(emg == 3){
-        for(int i=0; i<300; i++){
-            cur = p_emg.read();
-            if(cur < p_min)
-                p_min = cur;
-        }
-    }
-}    
+
 double filter(double input, double coeff_input[], double coeff_output[], double prev_outputs[])
 {
     double new_input = input;
-    for(int i=1; i<5; i++) 
+    for(int i=1; i<5; i++)
         new_input -= coeff_input[i] * prev_outputs[i-1];
 
     double new_output = coeff_output[0] * new_input;
-    for(int i=1; i<5; i++) 
+    for(int i=1; i<5; i++)
         new_output += coeff_output[i] * prev_outputs[i-1];
 
     // Set the new output as the first value of the 'recent outputs'
@@ -62,7 +41,7 @@
 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);
@@ -76,7 +55,7 @@
 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);
+    return filter(u, fl_a, fl_b, t_lowV);
 }
 double t_lowpass_filter(double u)
 {
@@ -89,29 +68,139 @@
     return filter(u, fl_a, fl_b, p_lowV);
 }
 
-double last_biceps = 0;
-double last_triceps = 0;
-double last_push = 0;
+double rust;
+void emg_cal(int emg)
+{
+    double mem[300];
+    double cur;
+    double mean;
+    if(emg == 0) {
+        for(int i=0; i<900; i++) {
+            cur = b_emg.read();
+            double output1 = b_highpass_filter(cur);
+            double output2 = fabs(output1);
+            cur = b_lowpass_filter(output2);
+            if(i >= 300 && i < 600) {
+                mem[i-300] = cur;
+                mean += cur;
+            }
+            wait(0.002);
+        }
+        mean /= 300;
+        double variance = 0;
+        for (int i = 0; i < 300; ++i) {
+            variance += pow((mem[i] - mean),2);
+        }
+        variance /=300;
+        rust =  mean-variance;
+        printf("Rust %f\r\n",rust); 
+    }
+    
+    if(emg == 1) {
+        for(int i=0; i<900; i++) {
+            cur = b_emg.read();
+            double output1 = b_highpass_filter(cur);
+            double output2 = fabs(output1);
+            cur = b_lowpass_filter(output2);
+            if(i >= 300 && i < 600) {
+                mem[i-300] = cur;
+                mean += cur;
+            }
+            wait(0.002);
+        }
+        mean /= 300;
+        double variance = 0;
+        for (int i = 0; i < 300; ++i) {
+            variance += pow((mem[i] - mean),2);
+        }
+        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++) {
+            cur = t_emg.read();
+            double output1 = t_highpass_filter(cur);
+            double output2 = fabs(output1);
+            cur = t_lowpass_filter(output2);
+            if(i >= 300 && i < 600) {
+                mem[i-300] = cur;
+                mean += cur;
+            }
+            wait(0.002);
+        }
+        mean /= 300;
+        double variance = 0;
+        for (int i = 0; i < 300; ++i) {
+            variance += pow((mem[i] - mean),2);
+        }
+        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) {
+        for(int i=0; i<900; i++) {
+            cur = p_emg.read();
+            double output1 = p_highpass_filter(cur);
+            double output2 = fabs(output1);
+            cur = p_lowpass_filter(output2);
+            if(i >= 300 && i < 600) {
+                mem[i-300] = cur;
+                mean += cur;
+            }
+            wait(0.002);
+        }
+        mean /= 300;
+        double variance = 0;
+        for (int i = 0; i < 300; ++i) {
+            variance += pow((mem[i] - mean),2);
+        }
+        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
  **/
 void sample(double& bicout, double& tricout, double& pushout)
 {
+    double last_biceps = 0;
+    double last_triceps = 0;
+    double last_push = 0;
+
     double b_input = b_emg.read();
     double output1 = b_highpass_filter(b_input);
     double output2 = fabs(output1);
     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];
@@ -119,18 +208,19 @@
     }
     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+0.2*b_min) {
+    if (last_biceps == 0 and output3 > b_min+b_min*0.1) {
         bicout = 1;
-    } else if (last_biceps == 1 and output3 < b_min-0.2*b_min) {
+    } else if (last_biceps == 1 and output3 < b_min-b_min*0.1) {
         bicout = 0;
     } else {
-        bicout = last_biceps;   
+        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) {
@@ -138,7 +228,7 @@
     } else {
         bicout=0;
     }*/
-    
+
     /*if (output6>0.0561) {//this is the output for the right tricep
         tricout=1;
     } else if(output6>0.0380) {
@@ -146,22 +236,34 @@
     } 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.01*t_min) {
-        tricout = 0;    
+    } else if(last_triceps == 1 and output6 < t_min-0.1*t_min) {
+        tricout = 0;
     } else {
         tricout = last_triceps;
     }
+    printf("mintric %f\t",tricout);
     last_triceps = tricout;
-    
-    if (last_push ==0 and output9>p_min+0.2*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) {//this is the output for the left bicep (the push motion)
         pushout=1;
-    } else if(last_triceps == 1 and output9<b_min-0.2*b_min) {
+    } 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