Revision 5:6fd237516c27, committed 2013-10-30

Comitter:

DanAuhust

Date:

Wed Oct 30 15:05:06 2013 +0000

Parent:

4:41222da4a577

Commit message:

asddf

Changed in this revision

--- a/main.cpp	Mon Oct 28 12:52:33 2013 +0000
+++ b/main.cpp	Wed Oct 30 15:05:06 2013 +0000
@@ -17,13 +17,15 @@
 #define inertia 4
 
 #define gain_biceps 1
-#define threshold_biceps 0.04
+#define threshold_biceps 0.06
 #define border_biceps 0.1125 //25% van .57 - .12 
+#define cap_biceps 2
 
 #define gain_triceps 1
-#define threshold_triceps 0.04
+#define threshold_triceps 0.06
 #define border_triceps 0.1237 //25% van .605 - .11 = .12375
-
+#define cap_triceps 2
+// constanten butterworth in aantekeningen EMG op dropbox
 #define NUM0 0.8841 // constante
 #define NUM1 -3.53647 // z^-1
 #define NUM2 5.3046 // z^-2etc.
@@ -42,7 +44,8 @@
 //signal_number=3 --> flexoren filteren
 //signal_number=4 --> extensoren filteren
 
-float filter(int signal_number){ 
+float filter(int signal_number)
+{ 
     //static variables keep their values between function calls
     //the assignents are only executed the first iteration.
            
@@ -69,15 +72,16 @@
     static float square_biceps = 0, square_triceps = 0, square_flexoren = 0, square_extensoren = 0;
     static float sum_biceps = 0, sum_triceps = 0, sum_flexoren = 0, sum_extensoren = 0; 
     static float mean_biceps = 0.1, mean_triceps = 0.1, mean_flexoren = 0.1, mean_extensoren = 0.1;
-//    static float emg_biceps, emg_triceps, emg_flexoren, emg_extensoren; // output ruwe EMG
+    //static float emg_biceps, emg_triceps, emg_flexoren, emg_extensoren; // output ruwe EMG
     static float sig_in_biceps, sig_in_triceps, sig_in_extensoren, sig_in_flexoren; // naam gewijzigd, output StDev
     static float sig_out_biceps, sig_out_triceps, sig_out_extensoren, sig_out_flexoren;
-    float emg_abs, sig_out, deltaV;
+    float sig_out, deltaV;
     static float sig_prev_biceps = 0, sig_prev_triceps = 0, sig_prev_extensoren = 0, sig_prev_flexoren = 0;
     static int stat0_biceps, stat0_triceps, stat0_extensoren, stat0_flexoren;
     static int stat1_biceps = 0, stat1_triceps = 0, stat1_extensoren = 0, stat1_flexoren = 0;
  
-    switch (signal_number){
+    switch (signal_number)
+    {
         case 1:
             //biceps filteren
             in4_biceps = in3_biceps; in3_biceps = in2_biceps; in2_biceps = in1_biceps; in1_biceps = in0_biceps;
@@ -86,16 +90,15 @@
             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;                      
             
             //std deviatie bepalen, om de N metingen
-            emg_abs = fabs(out0_biceps);
             sum_biceps += out0_biceps;
-            square_biceps += (emg_abs - mean_biceps)*(emg_abs - mean_biceps); //neem absolute waarde, kwadrateer, voeg toe aan vorige square
+            square_biceps += (out0_biceps - mean_biceps)*(out0_biceps - mean_biceps); //neem absolute waarde, kwadrateer, voeg toe aan vorige square
             // voeg rest EMG's toe, variabelen alleen _spier geven als het nodig is.
             count_biceps += 1; // hou bij hoeveel squares er zijn opgeteld
             if (count_biceps >= MAXCOUNT)
                 {   sig_in_biceps = sqrt(square_biceps/count_biceps);
                     mean_biceps = sum_biceps/count_biceps;
                     count_biceps = 0; square_biceps = 0; sum_biceps = 0; // en neem de RMS als er genoeg zijn geteld, stuur die door, en reset sqaure en count
-                    // nieuw:
+
                     deltaV = sig_in_biceps - sig_prev_biceps;
                     if (sig_in_biceps <= threshold_biceps) // threshold
                     {   stat0_biceps = 0;
@@ -103,6 +106,10 @@
                         sig_out_biceps = sig_prev_biceps + deltaV / inertia;
                         else sig_out_biceps = 0;
                     }
+                    else if (sig_in_biceps >= cap_biceps) // cap
+                    { stat0_biceps = 0;
+                    sig_out_biceps = cap_biceps;
+                    }
                     else if ( deltaV >= border_biceps ) // stijging
                     {   stat0_biceps = 1;
                         if (stat1_biceps == -1)
@@ -118,10 +125,9 @@
                     else { stat0_biceps = 0;
                          sig_out_biceps = sig_in_biceps;
                          }
-            sig_prev_biceps = sig_in_biceps;
+            sig_prev_biceps = sig_out_biceps;
             stat1_biceps = stat0_biceps;
             sig_out = sig_out_biceps;
-            //red = sig_out_biceps;
             }
             else sig_out = -1;     
             break;
@@ -130,30 +136,33 @@
             in4_triceps = in3_triceps; in3_triceps = in2_triceps; in2_triceps = in1_triceps; in1_triceps = in0_triceps;
             in0_triceps = emg_triceps.read();
             out4_triceps = out3_triceps; out3_triceps = out2_triceps; out2_triceps = out1_triceps; out1_triceps = out0_triceps;
-            out0_biceps = (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; 
-            
+            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; 
             //std deviatie bepalen om de N metingen
-            emg_abs = fabs(out0_triceps);
             sum_triceps += out0_triceps;
-            square_triceps += (emg_abs - mean_triceps)*(emg_abs - mean_triceps); //neem absolute waarde, kwadrateer, voeg toe aan vorige square
+            square_triceps += (out0_triceps - mean_triceps)*(out0_triceps - mean_triceps); //neem absolute waarde, kwadrateer, voeg toe aan vorige square
             // voeg rest EMG's toe, variabelen alleen _spier geven als het nodig is.
             count_triceps += 1; // hou bij hoeveel squares er zijn opgeteld
-            
             if (count_triceps >= MAXCOUNT)
                 {   sig_in_triceps = sqrt(square_triceps/count_triceps);
                     mean_triceps = sum_triceps/count_triceps;
                     count_triceps = 0; square_triceps = 0; sum_triceps = 0;// en neem de RMS als er genoeg zijn geteld, stuur die door, en reset sqaure en count
                     deltaV = sig_in_triceps - sig_prev_triceps;
+                    if(pc.txBufferGetSize(0)-pc.txBufferGetCount() > 30) // in sommige versies staat rx, dit klopt niet
+                        pc.printf("%.6f\n",sig_in_triceps);
                     if (sig_in_triceps <= threshold_triceps) // threshold
                     {   stat0_triceps = 0;
                         if (stat1_triceps == 1)
                         sig_out_triceps = sig_prev_triceps + deltaV / inertia;
                         else sig_out_triceps = 0;
                     }
+                    else if (sig_in_triceps >= cap_triceps) // cap
+                    {   stat0_triceps = 0;
+                        sig_out_triceps = cap_triceps;
+                    }
                     else if ( deltaV >= border_triceps ) // stijging
                     {   stat0_triceps = 1;
                         if (stat1_triceps == -1)
-                        sig_out_triceps = sig_prev_triceps + deltaV / inertia;
+                            sig_out_triceps = sig_prev_triceps + deltaV / inertia;
                         else sig_out_triceps = sig_in_triceps;
                     }
                     else if ( deltaV <= -border_triceps ) // daling
@@ -165,10 +174,9 @@
                     else { stat0_triceps = 0;
                          sig_out_triceps = sig_in_triceps;
                          }
-            sig_prev_triceps = sig_in_triceps;
+            sig_prev_triceps = sig_out_triceps;
             stat1_triceps = stat0_triceps;
             sig_out = sig_out_triceps;
-            //blue = sig_out_triceps;
                  }
             else sig_out = -1;
             break;
@@ -180,9 +188,8 @@
             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; 
                         
             //std deviatie bepalen om de N metingen
-            emg_abs = fabs(out0_flexoren);
             sum_flexoren += out0_flexoren;
-            square_flexoren += (emg_abs - mean_flexoren)*(emg_abs - mean_flexoren); //neem absolute waarde, kwadrateer, voeg toe aan vorige square
+            square_flexoren += (out0_flexoren - mean_flexoren)*(out0_flexoren - mean_flexoren); //neem absolute waarde, kwadrateer, voeg toe aan vorige square
             // voeg rest EMG's toe, variabelen alleen _spier geven als het nodig is.
             count_flexoren += 1; // hou bij hoeveel squares er zijn opgeteld
             if (count_flexoren >= MAXCOUNT)
@@ -202,9 +209,8 @@
             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;
                        
             //std deviatie bepalen om de 50 metingen
-            emg_abs = fabs(out0_extensoren);
             sum_extensoren += out0_extensoren;
-            square_extensoren += (emg_abs - mean_extensoren)*(emg_abs - mean_extensoren); //neem absolute waarde, kwadrateer, voeg toe aan vorige square
+            square_extensoren += (out0_extensoren - mean_extensoren)*(out0_extensoren - mean_extensoren); //neem absolute waarde, kwadrateer, voeg toe aan vorige square
             // voeg rest EMG's toe, variabelen alleen _spier geven als het nodig is.
             count_extensoren += 1; // hou bij hoeveel squares er zijn opgeteld
             if (count_extensoren >= MAXCOUNT)
@@ -227,18 +233,30 @@
 static float biceps, triceps, extensoren, flexoren, emg_filter_test, dy, dx;
 
     emg_filter_test = filter(1);
-    if (emg_filter_test != -1) {
-        biceps = emg_filter_test;
-        red = biceps;
-    }
+    if (emg_filter_test != -1)
+        {biceps = emg_filter_test;
+        }
     emg_filter_test = filter(2);
-    if (emg_filter_test != -1){
-        triceps = emg_filter_test;
-        blue = triceps;
-    }
-    dx=biceps-triceps;
-    if(pc.rxBufferGetSize(0)-pc.rxBufferGetCount() > 30)
-    pc.printf("%.6f\n",biceps);
+    if (emg_filter_test != -1)
+        {triceps = emg_filter_test;
+        }
+        
+        dx = biceps - triceps;
+        
+    if (biceps > triceps)
+        {//dx = biceps;
+        red = biceps; blue = 0;
+        }
+    else if (biceps < triceps)
+        {//dx = -triceps;
+        blue = gain_triceps*triceps; red = 0;
+        }
+    else {
+         dx = 0; red = 0; blue = 0;
+         } 
+    /*if(pc.txBufferGetSize(0)-pc.txBufferGetCount() > 30) // in sommige versies staat rx, dit klopt niet
+    pc.printf("%.6f\n",biceps); */
+    
     /*emg_filter_test = filter(3);
     if (emg_filter_test != -1) flexoren = emg_filter_test;
     emg_filter_test = filter(4);
@@ -251,18 +269,18 @@
     /*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(MAXCOUNT); // periode pwm = 2*Fs , blijkbaar.
-    blue.period_ms(MAXCOUNT);
+    red.period_ms(MAXCOUNT*2); // periode pwm = 2*Fs , blijkbaar.
+    blue.period_ms(MAXCOUNT*2);
 
 
     /**Here you attach the 'void looper(void)' function to the Ticker object0
     * 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.002);
+    timer.attach(looper, 0.001); // programma zou sneller lopen als niet alle 4 de vensters tegelijk aflopen. Hiervoor moeten meer timers gebruikt worden.
     while(1) // Loop
     {   
             // zie 'encoder' , timer bepaalt wanneer een if in de while-loop berekeningen doet voor de motor.
-        
+            // loop is leeg voor enkel meten EMG
     }
-}
+}
\ No newline at end of file