Diff: main.cpp

Revision:

4:e12537bac403

Parent:

3:a6c75f643f58

--- a/main.cpp	Fri Nov 01 16:58:52 2013 +0000
+++ b/main.cpp	Sat Nov 02 08:53:01 2013 +0000
@@ -12,18 +12,18 @@
 
 #define offset_biceps 0.5 // offset ruwe invoer met adapter motoren
 
-//high pass filter constantes 15Hz cutoff 4e orde, 515Hz
+//high pass filter constantes 15Hz cutoff 4e orde, Fs = 500Hz
 #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 NUM1 -3.1255 // z^-1
+#define NUM2 4.6882 // z^-2etc.
+#define NUM3 -3.1255
+#define NUM4 0.7814
 
 #define DEN0 1 // constante
-#define DEN1 -3.5221
-#define DEN2 4.6772
-#define DEN3 -2.7736
-#define DEN4 0.6194
+#define DEN1 -3.5078
+#define DEN2 4.6409
+#define DEN3 -2.7427
+#define DEN4 0.6105
 
 //lowpass filter constantes 40 Hz 4e orde
 #define NUM0_2 0.0466 // constante
@@ -39,19 +39,19 @@
 #define DEN3_2 -0.1827
 #define DEN4_2 0.0301
 
-//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
+//lowpass filter constantes 4Hz 4e orde, 500Hz
+#define NUM0_3 0.000000374 // constante
+#define NUM1_3 0.000001496 // z^-1
+#define NUM2_3 0.000002244 // z^-2etc.
+#define NUM3_3 0.000001496
+#define NUM4_3 0.000000374
 
 
 #define DEN0_3 1 // constante
-#define DEN1_3 -3.8725
-#define DEN2_3 5.6255
-#define DEN3_3 -3.6333
-#define DEN4_3 0.8803
+#define DEN1_3 -3.8687
+#define DEN2_3 5.6145
+#define DEN3_3 -3.6228
+#define DEN4_3 0.8769
 
 // highpass filter 1 Hz 2de orde, tegen storing motorshield
 #define NUM0_4 0.9780 // constante
@@ -63,7 +63,7 @@
 #define DEN2_4 0.9565
 
 
-float std_dev (float variable, int sig_number){
+float std_dev (float input, int sig_number){
  //   define variables
     float sum;
     int size;
@@ -73,76 +73,76 @@
     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];
+    static float keeper_biceps[50];
+    static float keeper_triceps[50];
+    static float keeper_flexoren[50];
+    static float keeper_extensoren[50];
     
     switch(sig_number){
         case 1:
-            emg_values_std_dev_biceps[count_biceps]=variable;
+            keeper_biceps[count_biceps]=input;
             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];        
-            }
+            if(count_triceps>=size)
+                count_biceps=0;
+            size=sizeof(keeper_biceps); // /sizeof(float); // waarom delen door 1?
+            for(int i=0; i < size; i++)
+                {sum+=keeper_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);
+            for(int i=0; i < size; i++)
+            {sum+=(keeper_biceps[i]-mean)*(keeper_biceps[i]-mean);
             }
             sig_out=sqrt(sum/size);
             sum=0;
             break;
         case 2:
-            emg_values_std_dev_triceps[count_triceps]=variable;
+            keeper_triceps[count_triceps]=input;
             count_triceps++;
             if(count_triceps==size) count_triceps=0;
     
-            size=sizeof(emg_values_std_dev_triceps)/sizeof(float);
+            size=sizeof(keeper_triceps)/sizeof(float);
             for(int i=0; i < size; i++){
-                sum+=emg_values_std_dev_triceps[i];        
+                sum+=keeper_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);
+                sum+=(keeper_triceps[i]-mean)*(keeper_triceps[i]-mean);
             }
             sig_out=sqrt(sum/size);
             sum=0;
             break;
         case 3:
-            emg_values_std_dev_flexoren[count_flexoren]=variable;
+            keeper_flexoren[count_flexoren]=input;
             count_flexoren++;
             if(count_flexoren==size) count_flexoren=0;
     
-            size=sizeof(emg_values_std_dev_flexoren)/sizeof(float);
+            size=sizeof(keeper_flexoren)/sizeof(float);
             for(int i; i < size; i++){
-                sum+=emg_values_std_dev_flexoren[i];        
+                sum+=keeper_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);
+                sum+=(keeper_flexoren[i]-mean)*(keeper_flexoren[i]-mean);
             }
             sig_out=sqrt(sum/size);
             sum=0;
             break;
         case 4:
-            emg_values_std_dev_extensoren[count_extensoren]=variable;
+            keeper_extensoren[count_extensoren]=input;
             count_extensoren++;
             if(count_extensoren==size) count_extensoren=0;
     
-            size=sizeof(emg_values_std_dev_extensoren)/sizeof(float);
+            size=sizeof(keeper_extensoren)/sizeof(float);
             for(int i; i < size; i++){
-                sum+=emg_values_std_dev_extensoren[i];        
+                sum+=keeper_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);
+                sum+=(keeper_extensoren[i]-mean)*(keeper_extensoren[i]-mean);
             }
             sig_out=sqrt(sum/size);
             sum=0;
@@ -350,11 +350,9 @@
     float emg_value_extensoren;
     float dy;
     emg_value_biceps = std_dev(filter(1), 1);
-    emg_value_triceps = (100*filter(2)-44);
+    /*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);
+    emg_value_extensoren = (100*filter(4)-44);*/
     
     /*if(emg_value_biceps < 0.10){
         emg_value_biceps=0;
@@ -387,7 +385,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,1.942);
+    timer.attach(looper,0.002); // invullen in seconde
     while(1) //Loop
     {
       /*Empty!*/