Diff: main.cpp

Revision:

1:04e5611501f6

Parent:

0:49ab9907d70d

diff -r 49ab9907d70d -r 04e5611501f6 main.cpp
--- a/main.cpp	Fri Oct 18 07:25:14 2013 +0000
+++ b/main.cpp	Fri Oct 25 07:25:14 2013 +0000
@@ -10,8 +10,11 @@
 Ticker timer;
 MODSERIAL pc(USBTX,USBRX,64,1024);
 
-#define gain_biceps 1
-#define maxcount 50
+#define gain_biceps 10 // nog niet gebruikt.
+#define threshold_biceps 0.04
+#define border_biceps 0.1125
+#define maxcount 40 // kies niet te groot, anders werkt rem niet snel genoeg. het kost maxcount*2/1000 seconde om van richting te veranderen.
+#define inertia 4
 
 #define NUM0 0.8841 // constante
 #define NUM1 -3.53647 // z^-1
@@ -26,7 +29,8 @@
 #define DEN4 0.7816
 
 /* hou in de gaten welke waarden globaal gedefinieerd moeten worden*/ 
-float count = 0, square_biceps = 0, sum_biceps = 0, mean_biceps = 0.2;
+float count = 0, square_biceps = 0, sum_biceps = 0, mean_biceps = 0;
+// mean_biceps wel of niet constant nemen?
 
 void looper()
 {
@@ -35,7 +39,7 @@
     static float in0 = 0, in1 = 0, in2 = 0, in3 = 0, in4 = 0;
     static float out0 = 0, out1 = 0, out2 = 0, out3 = 0, out4 = 0;
     
-    in4 = in3; in3 = in4; in3 = in2; in2 = in1; in1 = in0;
+    in4 = in3; in3 = in2; in2 = in1; in1 = in0;
     in0 = emg_biceps.read();
     red = in0;
     /* rode led voor meting emg*/ 
@@ -48,11 +52,10 @@
     /**When not using the LED, the above could also have been done this way:
     * pc.printf("%.6\n", emg0.read());
     */
-    float emg_abs; // square, mean en count eerder gedefinieerd
-    emg_abs = fabs(out0);
+    
     mean = mean_biceps;
     sum_biceps += out0;
-    square_biceps += (emg_abs - mean)*(emg_abs - mean); //neem absolute waarde, kwadrateer, voeg toe aan vorige square
+    square_biceps += (out0 - mean)*(out0 - mean); // WAS EERST (out0 - mean)^2 neem absolute waarde, kwadrateer, voeg toe aan vorige square
     // voeg rest EMG's toe, variabelen alleen _spier geven als het nodig is.
     count += 1; // hou bij hoeveel squares er zijn opgeteld
 }
@@ -68,15 +71,52 @@
     * The looper() function will be called every 0.001 seconds.
     * Please mind that the parentheses after looper are omitted when using attach.
     */ 
-    float sig_out_biceps;
+    float sig_in_biceps; float sig_out_biceps;
+    static float sig_prev_biceps = 0;
+    float dV_biceps;
+    /* rem: eerst threshold na STD, threshold van .03 lijkt prima in testmetingen (voor biceps).
+    Als onder threshold, output is nul, maar waarde wel onthouden, negeer rem verder. -> else if structuur
+    als terug in threshold terwijl stat1 = 1, dan inertia, om trilling bij verlaten threshold tegen te gaan.
+    status is -1, 0 of 1.
+    biceps: threshold 0.12 , max is 0.57 , speling = .57-.12 = 4.5 , 25% van 4,5 = 1.125
+    sigin < threshold -> stat0 = 0 , stat1 == 1 ? JA dan remmen, anders trillend rond threshold, NEE dan sigout = 0
+    dV > 1.125 -> stat0 = 1 , stat1 == -1 ? JA dan remmen, NEE dan sigout=sigin
+    dV < -1.125 -> stat0 = -1 , stat1 == 1 ? Ja dan remen, NEE dan sigout=sigin 
+    binnen grenzen: status0 = 0, voorlopig geen verdere vragen. */
+    int stat0_biceps; // huidige status
+    int stat1_biceps = 0; // vorige status
     timer.attach(looper, 0.001);
     while(1) // Loop
     {   if (count >= maxcount)
-        {   sig_out_biceps = sqrt(square_biceps/count);
-            mean_biceps = sum_biceps/count;
-            count= 0; square_biceps = 0; sum_biceps = 0; // en neem de RMS als er genoeg zijn geteld, stuur die door, en reset sqaure en count
+        {   sig_in_biceps = sqrt(square_biceps/count);
+        dV_biceps = sig_in_biceps - sig_prev_biceps;
+        mean_biceps = sum_biceps/count;
+        count= 0; square_biceps = 0; sum_biceps = 0; // en neem de STD als er genoeg zijn geteld, stuur die door, en reset sqaure en count
+            if (sig_in_biceps <= threshold_biceps) // threshold
+                { stat0_biceps = 0;
+                 if (stat1_biceps == 1)
+                    sig_out_biceps = sig_prev_biceps + dV_biceps / inertia;
+                 else sig_out_biceps = 0;
+                }
+            else if ( dV_biceps >= border_biceps ) // stijging
+                { stat0_biceps = 1;
+                 if (stat1_biceps == -1)
+                 sig_out_biceps = sig_prev_biceps + dV_biceps / inertia;
+                 else sig_out_biceps = sig_in_biceps;
+                }
+            else if ( dV_biceps <= -border_biceps ) // daling
+                { stat0_biceps = -1;
+                 if (stat1_biceps == 1)
+                 sig_out_biceps = sig_prev_biceps + dV_biceps / inertia;
+                 else sig_out_biceps = sig_in_biceps;
+                }
+            else { stat0_biceps = 0;
+                 sig_out_biceps = sig_in_biceps;
+                 }
+            sig_prev_biceps = sig_in_biceps;
+            stat1_biceps = stat0_biceps;
             if(pc.rxBufferGetSize(0)-pc.rxBufferGetCount() > 30)
-                pc.printf("%.6f\n",sig_out_biceps);
+                pc.printf("%.6f\n",sig_in_biceps); // verwissel in en out om verschillende delen te testen.
          }
            
     }