Revision 3:a6c75f643f58, committed 2013-11-01

Comitter:

jorick92

Date:

Fri Nov 01 16:58:52 2013 +0000

Parent:

2:b0b86581ba50

Child:

4:e12537bac403

Commit message:

Werkt niet, periode moet heel aantal ms zijn, corrigeer.

Changed in this revision

--- a/main.cpp	Fri Nov 01 11:20:55 2013 +0000
+++ b/main.cpp	Fri Nov 01 16:58:52 2013 +0000
@@ -2,53 +2,58 @@
 #include "MODSERIAL.h"
 
 //Define objects
-AnalogIn    emg_biceps(PTB0); //Analog input
+AnalogIn    emg_biceps(PTB0);
+AnalogIn    emg_triceps(PTB1);
+AnalogIn    emg_flexoren(PTB2);
+AnalogIn    emg_extensoren(PTB3); //Analog input
 PwmOut      red(LED_RED); //PWM output
 Ticker timer;
 MODSERIAL pc(USBTX,USBRX,64,1024);
 
-//high pass filter constantes 15Hz cutoff 4e orde
-#define NUM0 0.2754 // constante
-#define NUM1 -1.1017 // z^-1
-#define NUM2 1.6525 // z^-2etc.
-#define NUM3 -1.1017
-#define NUM4 0.2754
+#define offset_biceps 0.5 // offset ruwe invoer met adapter motoren
+
+//high pass filter constantes 15Hz cutoff 4e orde, 515Hz
+#define NUM0 0.7870 // constante
+#define NUM1 -3.1481 // z^-1
+#define NUM2 4.7221 // z^-2etc.
+#define NUM3 -3.1481
+#define NUM4 0.7870
 
 #define DEN0 1 // constante
-#define DEN1 -1.5704
-#define DEN2 1.2756
-#define DEN3 -0.4844
-#define DEN4 0.0762
+#define DEN1 -3.5221
+#define DEN2 4.6772
+#define DEN3 -2.7736
+#define DEN4 0.6194
 
 //lowpass filter constantes 40 Hz 4e orde
-#define NUM0_2 0.4328 // constante
-#define NUM1_2 1.7314 // z^-1
-#define NUM2_2 2.5971 // z^-2etc.
-#define NUM3_2 1.7314
-#define NUM4_2 0.4328
+#define NUM0_2 0.0466 // constante
+#define NUM1_2 0.1863 // z^-1
+#define NUM2_2 0.2795 // z^-2etc.
+#define NUM3_2 0.1863
+#define NUM4_2 0.0466
 
 
 #define DEN0_2 1 // constante
-#define DEN1_2 2.3695
-#define DEN2_2 2.3140
-#define DEN3_2 1.0547
-#define DEN4_2 0.1874
+#define DEN1_2 -0.7821
+#define DEN2_2 0.6800
+#define DEN3_2 -0.1827
+#define DEN4_2 0.0301
 
-//lowpass filter constantes 4z 4e orde
-#define NUM0_3 0.0002 // constante
-#define NUM1_3 0.0007 // z^-1
-#define NUM2_3 0.0011 // z^-2etc.
-#define NUM3_3 0.0007
-#define NUM4_3 0.0002
+//lowpass filter constantes 4Hz 4e orde
+#define NUM0_3 0.000000333 // constante
+#define NUM1_3 0.000001331 // z^-1
+#define NUM2_3 0.000001997 // z^-2etc.
+#define NUM3_3 0.1331
+#define NUM4_3 0.0333
 
 
 #define DEN0_3 1 // constante
-#define DEN1_3 -3.3441
-#define DEN2_3 4.2389
-#define DEN3_3 -2.4093
-#define DEN4_3 0.5175
+#define DEN1_3 -3.8725
+#define DEN2_3 5.6255
+#define DEN3_3 -3.6333
+#define DEN4_3 0.8803
 
-// highpass filter .5 Hz 2de orde, tegen storing motorshield
+// highpass filter 1 Hz 2de orde, tegen storing motorshield
 #define NUM0_4 0.9780 // constante
 #define NUM1_4 -1.9561 // z^-1
 #define NUM2_4 0.9780 // z^-2etc.
@@ -56,7 +61,98 @@
 #define DEN0_4 1 // constante
 #define DEN1_4 -1.9556
 #define DEN2_4 0.9565
+
+
+float std_dev (float variable, int sig_number){
+ //   define variables
+    float sum;
+    int size;
+    float sig_out;
+    float mean;
+    static int count_biceps=0;
+    static int count_triceps=0;
+    static int count_flexoren=0;
+    static int count_extensoren=0;
+    static float emg_values_std_dev_biceps[50];
+    static float emg_values_std_dev_triceps[50];
+    static float emg_values_std_dev_flexoren[50];
+    static float emg_values_std_dev_extensoren[50];
     
+    switch(sig_number){
+        case 1:
+            emg_values_std_dev_biceps[count_biceps]=variable;
+            count_biceps++;
+            if(count_triceps==size) count_biceps=0;
+    
+            size=sizeof(emg_values_std_dev_biceps)/sizeof(float);
+            for(int i=0; i < size; i++){
+                sum+=emg_values_std_dev_biceps[i];        
+            }
+            mean=sum/size;
+            sum=0;
+            for(int i=0; i < size; i++){
+                sum+=(emg_values_std_dev_biceps[i]-mean)*(emg_values_std_dev_biceps[i]-mean);
+            }
+            sig_out=sqrt(sum/size);
+            sum=0;
+            break;
+        case 2:
+            emg_values_std_dev_triceps[count_triceps]=variable;
+            count_triceps++;
+            if(count_triceps==size) count_triceps=0;
+    
+            size=sizeof(emg_values_std_dev_triceps)/sizeof(float);
+            for(int i=0; i < size; i++){
+                sum+=emg_values_std_dev_triceps[i];        
+            }
+            mean=sum/size;
+            sum=0;
+            for(int i=0; i < size; i++){
+                sum+=(emg_values_std_dev_biceps[i]-mean)*(emg_values_std_dev_biceps[i]-mean);
+            }
+            sig_out=sqrt(sum/size);
+            sum=0;
+            break;
+        case 3:
+            emg_values_std_dev_flexoren[count_flexoren]=variable;
+            count_flexoren++;
+            if(count_flexoren==size) count_flexoren=0;
+    
+            size=sizeof(emg_values_std_dev_flexoren)/sizeof(float);
+            for(int i; i < size; i++){
+                sum+=emg_values_std_dev_flexoren[i];        
+            }
+            mean=sum/size;
+            sum=0;
+            for(int i; i < size; i++){
+                sum+=(emg_values_std_dev_biceps[i]-mean)*(emg_values_std_dev_biceps[i]-mean);
+            }
+            sig_out=sqrt(sum/size);
+            sum=0;
+            break;
+        case 4:
+            emg_values_std_dev_extensoren[count_extensoren]=variable;
+            count_extensoren++;
+            if(count_extensoren==size) count_extensoren=0;
+    
+            size=sizeof(emg_values_std_dev_extensoren)/sizeof(float);
+            for(int i; i < size; i++){
+                sum+=emg_values_std_dev_extensoren[i];        
+            }
+            mean=sum/size;
+            sum=0;
+            for(int i; i < size; i++){
+                sum+=(emg_values_std_dev_biceps[i]-mean)*(emg_values_std_dev_biceps[i]-mean);
+            }
+            sig_out=sqrt(sum/size);
+            sum=0;
+            break;
+    }
+    
+
+    return sig_out;
+}
+   
 float filter(int sig_number){
     float sig_out;
     // eerst variabelen definieren
@@ -138,29 +234,29 @@
         case 1:        
             // signaal filteren op 15 Hz HIGHPASS
             in4_biceps = in3_biceps; in3_biceps = in2_biceps; in2_biceps = in1_biceps; in1_biceps = in0_biceps;
-            in0_biceps = emg_biceps.read();
+            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 40 HZ LOWPASS
             in4_2_biceps = in3_2_biceps; in3_2_biceps = in2_2_biceps; in2_2_biceps = in1_2_biceps; in1_2_biceps = in0_2_biceps;
             in0_2_biceps = out0_biceps;
             out4_2_biceps = out3_2_biceps; out3_2_biceps = out2_2_biceps; out2_2_biceps = out1_2_biceps; out1_2_biceps = out0_2_biceps;           
             out0_2_biceps = (NUM0_2*in0_2_biceps + NUM1_2*in1_2_biceps + NUM2_2*in2_2_biceps + NUM3_2*in3_2_biceps + NUM4_2*in4_2_biceps - DEN1_2*out1_2_biceps - DEN2_2*out2_2_biceps - DEN3_2*out3_2_biceps - DEN4_2*out4_2_biceps ) / DEN0_2;
-      
+            */
             //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_2_biceps);
+            in0_3_biceps = 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;    
-            
-            //signaal filteren op .5 HZ HIGHPASS
+            /*
+            //signaal filteren op 1 HZ HIGHPASS
             in2_4_biceps = in1_4_biceps; in1_4_biceps = in0_4_biceps;
             in0_4_biceps = out0_3_biceps;
             out2_4_biceps = out1_4_biceps; out1_4_biceps = out0_4_biceps;           
             out0_4_biceps = (NUM0_4*in0_4_biceps + NUM1_4*in1_4_biceps + NUM2_4*in2_4_biceps - DEN1_4*out1_4_biceps - DEN2_4*out2_4_biceps ) / DEN0_4;
-            
-            sig_out = out0_4_biceps;
+            */
+            sig_out = out0_biceps;
             break;
         case 2:
             // signaal filteren op 15 Hz HIGHPASS
@@ -253,8 +349,10 @@
     float emg_value_flexoren;
     float emg_value_extensoren;
     float dy;
-    emg_value_biceps = 100*filter(1);
-    emg_value_triceps = 100*filter(2);
+    emg_value_biceps = std_dev(filter(1), 1);
+    emg_value_triceps = (100*filter(2)-44);
+    emg_value_flexoren = (100*filter(3)-44);
+    emg_value_extensoren = (100*filter(4)-44);
     //emg_value_flexoren = 100*filter(3);
     //emg_value_extensoren = 100*filter(4);
     
@@ -273,7 +371,7 @@
     */     
     dy = emg_value_biceps-emg_value_triceps;
     if(pc.rxBufferGetSize(0)-pc.rxBufferGetCount() > 30)
-        pc.printf("%.6f, %.6f\n",emg_value_biceps,emg_value_triceps);
+        pc.printf("%.6f\n",emg_value_biceps);
     /**When not using the LED, the above could also have been done this way:
     * pc.printf("%.6\n", emg0.read());
     */
@@ -289,7 +387,7 @@
     * The looper() function will be called every 0.001 seconds.
     * Please mind that the parentheses after looper are omitted when using attach.
     */
-    timer.attach(looper, 0.01);
+    timer.attach(looper,1.942);
     while(1) //Loop
     {
       /*Empty!*/