Revision 41:245f33fb2b8b, committed 2014-10-20

Comitter:

Hooglugt

Date:

Mon Oct 20 13:00:49 2014 +0000

Parent:

40:7e93c2f1c1e9

Child:

42:f45e4dfbc26d

Commit message:

EMG signaal verwerken deel verandert: laagdoorlaat, notch, abs, 10e orde moving average (filters bevatten hoge decimalen)

Changed in this revision

--- a/Project_main.cpp	Fri Oct 10 09:34:43 2014 +0000
+++ b/Project_main.cpp	Mon Oct 20 13:00:49 2014 +0000
@@ -18,21 +18,37 @@
 MODSERIAL pc(USBTX,USBRX);
 HIDScope scope(2);
 
-arm_biquad_casd_df1_inst_f32 bilowpass;
-float bilowpass_const[] = {0.02008337 , 0.04016673 , 0.02008337 , 1.56101808 , -0.64135154}; //biceps lowpass 5 Hz, Fsample = 500Hz
-float bilowpass_states[4];
+arm_biquad_casd_df1_inst_f32 highpass;
+float highpass_const[] = {0.8751821104711265,  -1.750364220942253,  0.8751821104711265, 1.7347238224240125 , -0.7660046194604936}; //highpass, Fc: 15 Hz, Fsample: 500Hz, Q = 0.7071
+float highpass_states[4];
 
-arm_biquad_casd_df1_inst_f32 bihighpass;
-float bihighpass_const[] = {0.97803048, -1.95606096,  0.97803048, 1.95557824 , -0.95654368}; //biceps highpass 0.5 Hz, Fsample = 500Hz
-float bihighpass_states[4];
+arm_biquad_casd_df1_inst_f32 notch;
+float notch_const[] = {0.9714498065192796,  -1.5718388053127037,  0.9714498065192796, 1.5718388053127037 , -0.9428996130385592}; //notch, Fc: 50 Hz, Fsample: 500Hz, Q = 10
+float notch_states[4];
 
-arm_biquad_casd_df1_inst_f32 trilowpass;
-float trilowpass_const[] = {0.02008337 , 0.04016673 , 0.02008337 , 1.56101808 , -0.64135154}; //biceps lowpass 5 Hz, Fsample = 500Hz
-float trilowpass_states[4];
+// variables for biceps MAF
+float y0 = 0;
+float y1 = 0;
+float y2 = 0;
+float y3 = 0;
+float y4 = 0;
+float y5 = 0;
+float y6 = 0;
+float y7 = 0;
+float y8 = 0;
+float y9 = 0;
 
-arm_biquad_casd_df1_inst_f32 trihighpass;
-float trihighpass_const[] = {0.97803048, -1.95606096,  0.97803048, 1.95557824 , -0.95654368}; //biceps highpass 0.5 Hz, Fsample = 500Hz
-float trihighpass_states[4];
+// variables for triceps MAF
+float x0 = 0;
+float x1 = 0;
+float x2 = 0;
+float x3 = 0;
+float x4 = 0;
+float x5 = 0;
+float x6 = 0;
+float x7 = 0;
+float x8 = 0;
+float x9 = 0;
 
 PwmOut      red(LED_RED);
 PwmOut      green(LED_GREEN);
@@ -41,58 +57,72 @@
 uint8_t         direction = 0;
 uint8_t         force = 0;
 
-void looper() 
+void looper()
 {
     //put raw emg value of biceps and triceps in emg_bifloat and emg_trifloat, respectively
     float       emg_bifloat;        //Float voor EMG-waarde biceps
     float       emg_trifloat;       //Float voor EMG-waarde triceps
     emg_bifloat = emg0.read();         // read float value (0..1 = 0..3.3V) biceps
     emg_trifloat = emg1.read();        // read float value (0..1 = 0..3.3V) triceps
-    
+
     //process emg biceps
-    arm_biquad_cascade_df1_f32(&bihighpass, &emg_bifloat, &filemg_bifloat, 1 );
-    filemg_bifloat = fabs(filemg_bifloat);
-    arm_biquad_cascade_df1_f32(&bilowpass, &filemg_bifloat, &filemg_bifloat, 1 );
-    
+    arm_biquad_cascade_df1_f32(&highpass, &emg_bifloat, &filemg_bifloat, 1 );
+    arm_biquad_cascade_df1_f32(&notch, &emg_bifloat, &filemg_bifloat, 1 );
+    y0 = fabs(filemg_bifloat);
+    float bi_result = y0*0.1 +y1*0.1 + y2*0.1 + y3*0.1 + y4*0.1 + y5*0.1 + y6*0.1 + y7*0.1 + y8*0.1 + y9*0.1;
+    y9=y8;
+    y8=y7;
+    y7=y6;
+    y6=y5;
+    y5=y4;
+    y4=y3;
+    y3=y2;
+    y2=y1;
+    y1=y0;
+
     //process emg triceps
-    arm_biquad_cascade_df1_f32(&trihighpass, &emg_trifloat, &filemg_trifloat, 1 );
-    filemg_trifloat = fabs(filemg_trifloat);
-    arm_biquad_cascade_df1_f32(&trilowpass, &filemg_trifloat, &filemg_trifloat, 1 );
+    arm_biquad_cascade_df1_f32(&highpass, &emg_trifloat, &filemg_trifloat, 1 );
+    arm_biquad_cascade_df1_f32(&notch, &emg_trifloat, &filemg_trifloat, 1 );
+    x0 = fabs(filemg_trifloat);
+    float tri_result = x0*0.1 +x1*0.1 + x2*0.1 + x3*0.1 + x4*0.1 + x5*0.1 + x6*0.1 + x7*0.1 + x8*0.1 + x9*0.1;
+    x9=x8;
+    x8=x7;
+    x7=x6;
+    x6=x5;
+    x5=x4;
+    x4=x3;
+    x3=x2;
+    x2=x1;
+    x1=x0;
 
     /*send value to PC. Line below is used to prevent buffer overrun */
-    if(pc.rxBufferGetSize(0)-pc.rxBufferGetCount() > 30) { //VRAAG: praktisch nut hiervan? print emg value wanneer buffercount groter dan 30 is
-        //pc.printf("%u\n",emg_bivalue);
+    if(pc.rxBufferGetSize(0)-pc.rxBufferGetCount() > 30) { 
+        scope.set(0,bi_result);
+        scope.set(1,tri_result);
+        scope.send();
     }
-
-    /* EMG-singaal van biceps en triceps worden hier gefilterd*/
-
-    scope.set(0,filemg_bifloat);
-    scope.set(1,filemg_trifloat);
-    scope.send();
 }
 
 /*
 void resetlooper() // VRAAG: wat gebeurt er wanneer en resetlooper en looper tegelijkertijd gecalled worden?!
 {
-    if(emg_trifloat.read()>0.8 && direction != 0) { //dit is alleen mogelijk wanneer directionchoice is gemaakt
+    if(filemg_trifloat.read()>0.8 && direction != 0) { //dit is alleen mogelijk wanneer directionchoice is gemaakt
         direction = 0;
         force = 0;              // WEGHALEN, wanneer in uiteindelijke script na force keuzen niet meer gereset kan worden (voor nu wel handig)
         pc.printf("reset ");
     }
 }
-CONSTANTE RESETS DOOR BEWEGINGSARTEFACTEN */
+*/
 
 int main()
 {
     pc.baud(115200); //baudrate instellen
     log_timer.attach(looper, 0.002); // The looper() function will be called every 0.002 seconds (with the ticker object)
-    //set up filters triceps
-    arm_biquad_cascade_df1_init_f32(&bilowpass,1 , bilowpass_const, bilowpass_states);
-    arm_biquad_cascade_df1_init_f32(&bihighpass,1 ,bihighpass_const,bihighpass_states);
-    //set up filters triceps
-    arm_biquad_cascade_df1_init_f32(&trilowpass,1 , trilowpass_const, trilowpass_states);
-    arm_biquad_cascade_df1_init_f32(&trihighpass,1 ,trihighpass_const,trihighpass_states);
-    
+    //set up filters
+    arm_biquad_cascade_df1_init_f32(&notch, 1, notch_const, notch_states);
+    arm_biquad_cascade_df1_init_f32(&highpass, 1, highpass_const, highpass_states);
+
+
 //    reset_timer.attach(resetlooper, 0.1); //
 
     goto directionchoice; // goes to first while(1) for the deciding the direction
@@ -265,5 +295,5 @@
         pc.printf("error ");
         // mogelijkheid om een goto te maken naar directionchoice (opzich wel handig)
     }
-    
+
 }
\ No newline at end of file