Diff: main.cpp

Revision:

5:7644d6b16ceb

Parent:

4:117c0bb09513

Child:

6:33f0741dbb5b

--- a/main.cpp	Mon Nov 04 20:10:50 2013 +0000
+++ b/main.cpp	Tue Nov 05 08:38:03 2013 +0000
@@ -38,13 +38,13 @@
 #define DEN3_3 -3.4119
 #define DEN4_3 0.8104
 
-float std_dev (float input, int sig_number){
- //   define variables
-    static int startcount=0;
-    float sum;
-    int size = 20;
-    float sig_out;
-    float mean;
+double std_dev(float value, int number) {
+      int n = 20;
+      static int startcount = 0;
+      float sum;
+      float sq_sum;
+      float mean;
+      float variance;
     static int count_biceps=0;
     static int count_triceps=0;
     static int count_flexoren=0;
@@ -53,140 +53,134 @@
     static float keeper_triceps[20];
     static float keeper_flexoren[20];
     static float keeper_extensoren[20];
-    
-    if (startcount >= size)
-        {//ja, dan gewoon std dev nemen, want keeper is al vol
-        switch(sig_number){
+   
+   if (startcount >= n)
+    {switch (number){
         case 1:
-            keeper_biceps[count_biceps]=input;
+            //int n = sizeof(keeper_biceps)/sizeof(float);
+            keeper_biceps[count_biceps]=value;
             count_biceps++;
-            if(count_biceps >= size)
-                count_biceps=0;
-            //sizeof(keeper_biceps)/sizeof(float);
-            for(int i=0; i < size; i++)
-                {sum+=keeper_biceps[i];        
-                }
-            mean=sum/size;
-            sum=0;
-            for(int i=0; i < size; i++)
-            {sum+=(keeper_biceps[i]-mean)*(keeper_biceps[i]-mean);
+            if ( count_biceps >= n)
+                count_biceps = 0;
+            double sum = 0;
+            double sq_sum = 0;
+            for(int i = 0; i < n; ++i) {
+                sum += keeper_biceps[i];
+                sq_sum += keeper_biceps[i] * keeper_biceps[i];
             }
-            sig_out=sqrt(sum/size);
-            sum=0;
+            double mean = sum / n;
+            double variance = sq_sum / n - mean * mean;
+            return sqrt(variance);
             break;
+            
         case 2:
-            keeper_triceps[count_triceps]=input;
+            //int n = sizeof(keeper_triceps)/sizeof(float);
+            keeper_triceps[count_triceps]=value;
             count_triceps++;
-            if(count_triceps==size)
-                count_triceps=0;
-            //sizeof(keeper_triceps) / sizeof(float);
-            for(int i=0; i < size; i++){
-                sum+=keeper_triceps[i];        
+            if ( count_triceps >= n)
+                count_triceps = 0;
+            sum = 0;
+            sq_sum = 0;
+            for(int i = 0; i < n; ++i) {
+                sum += keeper_triceps[i];
+                sq_sum += keeper_triceps[i] * keeper_triceps[i];
             }
-            mean=sum/size;
-            sum=0;
-            for(int i=0; i < size; i++){
-                sum+=(keeper_triceps[i]-mean)*(keeper_triceps[i]-mean);
-            }
-            sig_out=sqrt(sum/size);
-            sum=0;
+            mean = sum / n;
+            variance = sq_sum / n - mean * mean;
+            return sqrt(variance);
             break;
+
         case 3:
-            keeper_flexoren[count_flexoren]=input;
+            //n = sizeof(keeper_flexoren)/sizeof(float);
+            keeper_flexoren[count_flexoren]=value;
             count_flexoren++;
-            if(count_flexoren==size) count_flexoren=0;
-    
-            //size=sizeof(keeper_flexoren)/sizeof(float);
-            for(int i=0; i < size; i++){
-                sum+=keeper_flexoren[i];        
+            if ( count_flexoren >= n)
+                count_flexoren = 0;
+            sum = 0;
+            sq_sum = 0;
+            for(int i = 0; i < n; ++i) {
+                sum += keeper_flexoren[i];
+                sq_sum += keeper_flexoren[i] * keeper_flexoren[i];
             }
-            mean=sum/size;
-            sum=0;
-            for(int i=0; i < size; i++){
-                sum+=(keeper_flexoren[i]-mean)*(keeper_flexoren[i]-mean);
-            }
-            sig_out=sqrt(sum/size);
-            sum=0;
+            mean = sum / n;
+            variance = sq_sum / n - mean * mean;
+            return sqrt(variance);
             break;
+
         case 4:
-            keeper_extensoren[count_extensoren]=input;
+            //n = sizeof(keeper_extensoren)/sizeof(float);
+            keeper_triceps[count_extensoren]=value;
             count_extensoren++;
-            if(count_extensoren==size) count_extensoren=0;
-    
-            //size=sizeof(keeper_extensoren)/sizeof(float);
-            for(int i=0; i < size; i++){
-                sum+=keeper_extensoren[i];        
+            if ( count_extensoren >= n)
+                count_extensoren = 0;
+            sum = 0;
+            sq_sum = 0;
+            for(int i = 0; i < n; ++i) {
+                sum += keeper_extensoren[i];
+                sq_sum += keeper_extensoren[i] * keeper_extensoren[i];
             }
-            mean=sum/size;
-            sum=0;
-            for(int i=0; i < size; i++){
-                sum+=(keeper_extensoren[i]-mean)*(keeper_extensoren[i]-mean);
-            }
-            sig_out=sqrt(sum/size);
-            sum=0;
+            mean = sum / n;
+            variance = sq_sum / n - mean * mean;
+            return sqrt(variance);
             break;
-            } // einde switch
-        return sig_out;
-        } // einde if startcount ...
-        else
+    } // einde switch
+   } // einde if
+   else
             {startcount+=1;
             switch(sig_number){
             case 1:
                 keeper_biceps[count_biceps]=input;
                 count_biceps++;
-                if(count_biceps >= size)
+                if(count_biceps >= n)
                     count_biceps=0;
                 break;
             case 2:
                 keeper_triceps[count_triceps]=input;
                 count_triceps++;
-                if(count_triceps >= size)
+                if(count_triceps >= n)
                     count_triceps=0;
                 break;
             case 3:
                 keeper_flexoren[count_flexoren]=input;
                 count_flexoren++;
-                if(count_flexoren >= size)
+                if(count_flexoren >= n)
                     count_flexoren=0;
                 break;
             case 4:
                 keeper_extensoren[count_extensoren]=input;
                 count_extensoren++;
-                if(count_extensoren >= size)
+                if(count_extensoren >= n)
                     count_extensoren=0;
                 break;    
             } // einde switch
         } // einde else
-} // einde std_dev
-   
+}
+
 float filter(int sig_number){
     float sig_out;
     // eerst variabelen definieren
-    
     //biceps
         //filter 1
     float in0_biceps =0;
     static float in1_biceps =0, in2_biceps = 0, in3_biceps = 0, in4_biceps = 0;
     static float out0_biceps = 0, out1_biceps = 0 , out2_biceps = 0, out3_biceps = 0, out4_biceps = 0;
-        
+
         //filter 3
     float in0_3_biceps =0;
     static float in1_3_biceps =0, in2_3_biceps = 0, in3_3_biceps = 0, in4_3_biceps = 0;
     static float out0_3_biceps = 0, out1_3_biceps = 0 , out2_3_biceps = 0, out3_3_biceps = 0, out4_3_biceps = 0;
-        
-   
+
     //triceps
         //filter 1
     float in0_triceps =0;
     static float in1_triceps =0, in2_triceps = 0, in3_triceps = 0, in4_triceps = 0;
     static float out0_triceps = 0, out1_triceps = 0 , out2_triceps = 0, out3_triceps = 0, out4_triceps = 0;
-        
+
         //filter 3
     float in0_3_triceps =0;
     static float in1_3_triceps =0, in2_3_triceps = 0, in3_3_triceps = 0, in4_3_triceps = 0;
     static float out0_3_triceps = 0, out1_3_triceps = 0 , out2_3_triceps = 0, out3_3_triceps = 0, out4_3_triceps = 0;
-        
-    
+
     //flexoren
         //filter 1
     float in0_flexoren =0;
@@ -197,21 +191,18 @@
     float in0_3_flexoren =0;
     static float in1_3_flexoren =0, in2_3_flexoren = 0, in3_3_flexoren = 0, in4_3_flexoren = 0;
     static float out0_3_flexoren = 0, out1_3_flexoren = 0 , out2_3_flexoren = 0, out3_3_flexoren = 0, out4_3_flexoren = 0;
-        
-    
+
     //extensoren
         //filter 1
     float in0_extensoren =0;
     static float in1_extensoren =0, in2_extensoren = 0, in3_extensoren = 0, in4_extensoren = 0;
     static float out0_extensoren = 0, out1_extensoren = 0 , out2_extensoren = 0, out3_extensoren = 0, out4_extensoren = 0;
-        
+
         //filter 3
     float in0_3_extensoren =0;
     static float in1_3_extensoren =0, in2_3_extensoren = 0, in3_3_extensoren = 0, in4_3_extensoren = 0;
     static float out0_3_extensoren = 0, out1_3_extensoren = 0 , out2_3_extensoren = 0, out3_3_extensoren = 0, out4_3_extensoren = 0;
-        
-    
-    
+
     switch(sig_number){            
         case 1:        
             // signaal filteren op 15 Hz HIGHPASS
@@ -219,68 +210,62 @@
             in0_biceps = emg_biceps.read() - offset_biceps;
             out4_biceps = out3_biceps; out3_biceps = out2_biceps; out2_biceps = out1_biceps; out1_biceps = out0_biceps;           
             out0_biceps = (NUM0*in0_biceps + NUM1*in1_biceps + NUM2*in2_biceps + NUM3*in3_biceps + NUM4*in4_biceps - DEN1*out1_biceps - DEN2*out2_biceps - DEN3*out3_biceps - DEN4*out4_biceps ) / DEN0;                      
-            /*
+            
             //signaal filteren op 5Hz LOWPASS
             in4_3_biceps = in3_3_biceps; in3_3_biceps = in2_3_biceps; in2_3_biceps = in1_3_biceps; in1_3_biceps = in0_3_biceps;
             in0_3_biceps = abs(out0_biceps); // ruw - offset -> filter 1 -> stdev (-> filter 3)
             out4_3_biceps = out3_3_biceps; out3_3_biceps = out2_3_biceps; out2_3_biceps = out1_3_biceps; out1_3_biceps = out0_3_biceps;           
-            out0_3_biceps = (NUM0_3*in0_3_biceps + NUM1_3*in1_3_biceps + NUM2_3*in2_3_biceps + NUM3_3*in3_3_biceps + NUM4_3*in4_3_biceps - DEN1_3*out1_3_biceps - DEN2_3*out2_3_biceps - DEN3_3*out3_3_biceps - DEN4_3*out4_3_biceps ) / DEN0_3;    
-            */
-            
-            sig_out = out0_biceps;
+            out0_3_biceps = (NUM0_3*in0_3_biceps + NUM1_3*in1_3_biceps + NUM2_3*in2_3_biceps + NUM3_3*in3_3_biceps + NUM4_3*in4_3_biceps - DEN1_3*out1_3_biceps - DEN2_3*out2_3_biceps - DEN3_3*out3_3_biceps - DEN4_3*out4_3_biceps ) / DEN0_3;
+
+            sig_out = out0_3_biceps;
             break;
+
         case 2:
             // signaal filteren op 15 Hz HIGHPASS
             in4_triceps = in3_triceps; in3_triceps = in2_triceps; in2_triceps = in1_triceps; in1_triceps = in0_triceps;
             in0_triceps = emg_triceps.read() - offset_biceps;
             out4_triceps = out3_triceps; out3_triceps = out2_triceps; out2_triceps = out1_triceps; out1_triceps = out0_triceps;           
             out0_triceps = (NUM0*in0_triceps + NUM1*in1_triceps + NUM2*in2_triceps + NUM3*in3_triceps + NUM4*in4_triceps - DEN1*out1_triceps - DEN2*out2_triceps - DEN3*out3_triceps - DEN4*out4_triceps ) / DEN0;                      
-            
-       
+
             //signaal filteren op 5Hz LOWPASS
-            /*in4_3_triceps = in3_3_triceps; in3_3_triceps = in2_3_triceps; in2_3_triceps = in1_3_triceps; in1_3_triceps = in0_3_triceps;
+            in4_3_triceps = in3_3_triceps; in3_3_triceps = in2_3_triceps; in2_3_triceps = in1_3_triceps; in1_3_triceps = in0_3_triceps;
             in0_3_triceps = abs(out0_triceps);
             out4_3_triceps = out3_3_triceps; out3_3_triceps = out2_3_triceps; out2_3_triceps = out1_3_triceps; out1_3_triceps = out0_3_triceps;           
             out0_3_triceps = (NUM0_3*in0_3_triceps + NUM1_3*in1_3_triceps + NUM2_3*in2_3_triceps + NUM3_3*in3_3_triceps + NUM4_3*in4_3_triceps - DEN1_3*out1_3_triceps - DEN2_3*out2_3_triceps - DEN3_3*out3_3_triceps - DEN4_3*out4_3_triceps ) / DEN0_3;    
-            */
-            
-            sig_out = out0_triceps;
+
+            sig_out = out0_3_triceps;
             break;
+
         case 3:
             // signaal filteren op 15 Hz HIGHPASS
             in4_flexoren = in3_flexoren; in3_flexoren = in2_flexoren; in2_flexoren = in1_flexoren; in1_flexoren = in0_flexoren;
             in0_flexoren = emg_flexoren.read();
             out4_flexoren = out3_flexoren; out3_flexoren = out2_flexoren; out2_flexoren = out1_flexoren; out1_flexoren = out0_flexoren;           
             out0_flexoren = (NUM0*in0_flexoren + NUM1*in1_flexoren + NUM2*in2_flexoren + NUM3*in3_flexoren + NUM4*in4_flexoren - DEN1*out1_flexoren - DEN2*out2_flexoren - DEN3*out3_flexoren - DEN4*out4_flexoren ) / DEN0;                      
-    /*
-            
+
             //signaal filteren op 5Hz LOWPASS
             in4_3_flexoren = in3_3_flexoren; in3_3_flexoren = in2_3_flexoren; in2_3_flexoren = in1_3_flexoren; in1_3_flexoren = in0_3_flexoren;
-            in0_3_flexoren = abs(out0_2_flexoren);
+            in0_3_flexoren = abs(out0_flexoren);
             out4_3_flexoren = out3_3_flexoren; out3_3_flexoren = out2_3_flexoren; out2_3_flexoren = out1_3_flexoren; out1_3_flexoren = out0_3_flexoren;           
             out0_3_flexoren = (NUM0_3*in0_3_flexoren + NUM1_3*in1_3_flexoren + NUM2_3*in2_3_flexoren + NUM3_3*in3_3_flexoren + NUM4_3*in4_3_flexoren - DEN1_3*out1_3_flexoren - DEN2_3*out2_3_flexoren - DEN3_3*out3_3_flexoren - DEN4_3*out4_3_flexoren ) / DEN0_3;    
-            
-            
-            */
-            sig_out = out0_flexoren;
+
+            sig_out = out0_3_flexoren;
             break;
+
         case 4:
             // signaal filteren op 15 Hz HIGHPASS
             in4_extensoren = in3_extensoren; in3_extensoren = in2_extensoren; in2_extensoren = in1_extensoren; in1_extensoren = in0_extensoren;
             in0_extensoren = emg_extensoren.read();
             out4_extensoren = out3_extensoren; out3_extensoren = out2_extensoren; out2_extensoren = out1_extensoren; out1_extensoren = out0_extensoren;           
             out0_extensoren = (NUM0*in0_extensoren + NUM1*in1_extensoren + NUM2*in2_extensoren + NUM3*in3_extensoren + NUM4*in4_extensoren - DEN1*out1_extensoren - DEN2*out2_extensoren - DEN3*out3_extensoren - DEN4*out4_extensoren ) / DEN0;                      
-    /*
-            
+
             //signaal filteren op 5Hz LOWPASS
             in4_3_extensoren = in3_3_extensoren; in3_3_extensoren = in2_3_extensoren; in2_3_extensoren = in1_3_extensoren; in1_3_extensoren = in0_3_extensoren;
-            in0_3_extensoren = abs(out0_2_extensoren);
+            in0_3_extensoren = abs(out0_extensoren);
             out4_3_extensoren = out3_3_extensoren; out3_3_extensoren = out2_3_extensoren; out2_3_extensoren = out1_3_extensoren; out1_3_extensoren = out0_3_extensoren;           
             out0_3_extensoren = (NUM0_3*in0_3_extensoren + NUM1_3*in1_3_extensoren + NUM2_3*in2_3_extensoren + NUM3_3*in3_3_extensoren + NUM4_3*in4_3_extensoren - DEN1_3*out1_3_extensoren - DEN2_3*out2_3_extensoren - DEN3_3*out3_3_extensoren - DEN4_3*out4_3_extensoren ) / DEN0_3;    
-            
-            
-     */       
-            sig_out = out0_extensoren;
+
+            sig_out = out0_3_extensoren;
             break;
     }
     return sig_out;
@@ -292,11 +277,13 @@
     float emg_value_flexoren;
     float emg_value_extensoren;
     float dy;
+    float dx;
     //static int sig_count = 1;
-emg_value_biceps=std_dev(filter(1),1);
-emg_value_triceps=std_dev(filter(2),2);
-emg_value_flexoren = std_dev(filter(3),3);
-emg_value_extensoren = std_dev(filter(4),4);
+
+    emg_value_biceps = std_dev(filter(1),1);
+    emg_value_triceps = std_dev(filter(2),2); 
+    emg_value_extensoren = std_dev(filter(3),3);
+    emg_value_flexoren = std_dev(filter(4),4);
 
     if(emg_value_biceps < 4.5)
         emg_value_biceps=0;
@@ -323,20 +310,48 @@
  
 dy = emg_value_biceps - emg_value_triceps;
 dx = 2*(emg_value_flexoren - emg_value_extensoren);
+    if(emg_value_biceps < 4.5)
+        emg_value_biceps=0;
+    else if (emg_value_biceps > 13)
+         emg_value_biceps = 13;
+    //emg_value_biceps =  emg_value_biceps;
     
+    if(emg_value_triceps < 2.5)
+        emg_value_biceps=0;
+    else if (emg_value_biceps > 10)
+         emg_value_biceps = 10;
+    //emg_value_triceps =  emg_value_triceps;
+    
+    if(emg_value_flexoren < 2)
+        emg_value_flexoren=0;
+    else if (emg_value_flexoren > 8)
+         emg_value_flexoren = 8;
+    emg_value_flexoren = 2*emg_value_flexoren;
+    
+    if(emg_value_extensoren < 5)
+        emg_value_extensoren=0;
+    else if (emg_value_extensoren > 13)
+         emg_value_extensoren = 13;
+ 
+    dy = emg_value_biceps - emg_value_triceps;
+    dx = 2*(emg_value_flexoren - emg_value_extensoren);
+
+    dy = emg_value_biceps-emg_value_triceps;
+    dx = emg_value_extensoren - emg_value_flexoren;    
     if(pc.rxBufferGetSize(0)-pc.rxBufferGetCount() > 30)
-        pc.printf("%.6f, %.6f, %.6f\n",emg_value_biceps, emg_value_triceps, dy);
+        pc.printf("%.6f\n",dy);
     /**When not using the LED, the above could also have been done this way:
     * pc.printf("%.6\n", emg0.read());
     */
 }
 
 int main()
+
 {
     /*setup baudrate. Choose the same in your program on PC side*/
     pc.baud(115200);
     /*set the period for the PWM to the red LED*/
-    red.period_ms(2);
+    //red.period_ms(2);
     /**Here you attach the 'void looper(void)' function to the Ticker object
     * The looper() function will be called every 1/Fs seconds.
     * Please mind that the parentheses after looper are omitted when using attach.