Revision 19:fdebe6892633, committed 2014-10-24

Comitter:

s1340735

Date:

Fri Oct 24 11:53:19 2014 +0000

Parent:

18:1110be4aab6e

Child:

20:5cd52a0daab0

Commit message:

notch filter toegevoegd

Changed in this revision

--- a/main.cpp	Fri Oct 24 11:36:27 2014 +0000
+++ b/main.cpp	Fri Oct 24 11:53:19 2014 +0000
@@ -31,15 +31,21 @@
 
 
 //*** FILTERS ***
+arm_biquad_casd_df1_inst_f32 notchT;
+arm_biquad_casd_df1_inst_f32 notchB;
+//constants for 50Hz 
+float notch_const[]={0.5857841106784856, -1.3007020142696517e-16, 0.5857841106784856, 1.3007020142696517e-16, -0.17156822135697122};//{a0 a1 a2 -b1 -b2}
+float notch_states[4];
+
 arm_biquad_casd_df1_inst_f32 lowpassT;
 arm_biquad_casd_df1_inst_f32 lowpassB;
-//constants for 50Hz lowpass
-float lowpass_const[] = {0.2928920553392428, 0.5857841106784856, 0.17156822135697122, 1.3007020142696517e-16,   -0.17156822135697122};//{a0 a1 a2 -b1 -b2} van online calculator
+//constants for 60Hz lowpass
+float lowpass_const[] = {0.39133426347022965, 0.7826685269404593, 0.39133426347022965, -0.3695259524151476, -0.19581110146577096};//{a0 a1 a2 -b1 -b2} van online calculator
 float lowpass_states[4];
 
 arm_biquad_casd_df1_inst_f32 highpassT;
 arm_biquad_casd_df1_inst_f32 highpassB;
-//constants for 10Hz highpass
+//constants for 20Hz highpass
 float highpass_const[] = {0.6389437261127494, -1.2778874522254988, 0.6389437261127494, 1.1429772843080923, -0.41279762014290533};//{a0 a1 a2 -b1 -b2}
 float highpass_states[4];
 
@@ -52,11 +58,12 @@
     emg_value_f32T = emgT.read();
 
     //Triceps filteren
-    arm_biquad_cascade_df1_f32(&lowpassT, &emg_value_f32T, &filtered_emgT, 1 );
+    arm_biquad_cascade_df1_f32(&notchT, &emg_value_f32T, %filtered_emgT, 1);
+    arm_biquad_cascade_df1_f32(&lowpassT, &filtered_emgT, &filtered_emgT, 1 );
     filtered_emgT = fabs(filtered_emgT);
     arm_biquad_cascade_df1_f32(&highpassT, &filtered_emgT, &filtered_emgT, 1 );
+    //filtered_emgT = fabs(filtered_emgT);
     
-
     //Triceps moving average
     T0=filtered_emgT;
     MOVAVG_T=T0*0.03333+T1*0.03333+T2*0.03333+T3*0.03333+T4*0.03333+T5*0.03333+T6*0.03333+T7*0.03333+T8*0.03333+T9*0.03333+T10*0.03333+T11*0.03333+T12*0.03333+T13*0.03333+T14*0.03333+T15*0.03333+T16*0.03333+T17*0.03333+T18*0.03333+T19*0.03333+T20*0.03333+T21*0.03333+T22*0.03333+T23*0.03333+T24*0.03333+T25*0.03333+T26*0.03333+T27*0.03333+T28*0.03333+T29*0.03333;
@@ -92,7 +99,6 @@
     //sturen naar scherm (Realterm)
     pc.printf("%f\r\n",MOVAVG_T);
 
-
     //sturen naar HID Scope
     scope.set(0,emg_valueT);        //ruwe data
     scope.set(1,filtered_emgT);     //filtered
@@ -106,12 +112,12 @@
     emg_value_f32B = emgB.read();
 
     //Biceps filteren
-    arm_biquad_cascade_df1_f32(&lowpassB, &emg_value_f32B, &filtered_emgB, 1 );
+    arm_biquad_cascade_df1_f32(&notchB, &emg_value_f32B, &filtered_emgB, 1 );
+    arm_biquad_cascade_df1_f32(&lowpassB, &filtered_emgB, &filtered_emgB, 1 );
     filtered_emgB = fabs(filtered_emgB);
     arm_biquad_cascade_df1_f32(&highpassB, &filtered_emgB, &filtered_emgB, 1 );
-    
+    //filtered_emgT = fabs(filtered_emgB);
     
-
     //Biceps moving average
     B0=filtered_emgB;
     MOVAVG_B=B0*0.03333+B1*0.03333+B2*0.03333+B3*0.03333+B4*0.03333+B5*0.03333+B6*0.03333+B7*0.03333+B8*0.03333+B9*0.03333+B10*0.03333+B11*0.03333+B12*0.03333+B13*0.03333+B14*0.03333+B15*0.03333+B16*0.03333+B17*0.03333+B18*0.03333+B19*0.03333+B20*0.03333+B21*0.03333+B22*0.03333+B23*0.03333+B24*0.03333+B25*0.03333+B26*0.03333+B27*0.03333+B28*0.03333+B29*0.03333;
@@ -153,6 +159,7 @@
     scope.send();
 }
 
+
 // *** MAIN ***
 int main()
 {
@@ -160,6 +167,8 @@
 
     //bepaling van positie met triceps 1
     Ticker log_timerT1;
+    
+    arm_biquad_cascade_df1_init_f32(&notchT,1,notch_const,notch_states);
     arm_biquad_cascade_df1_init_f32(&lowpassT,1,lowpass_const,lowpass_states);
     arm_biquad_cascade_df1_init_f32(&highpassT,1,highpass_const,highpass_states);
 
@@ -192,7 +201,6 @@
         yT1=0;
     }
 
-
     //*** INPUT MOTOR 2 ***
     positie=yT1+yT2;
 
@@ -212,6 +220,8 @@
     wait(5);
 
     Ticker log_timerB;
+    
+    arm_biquad_cascade_df1_init_f32(&notchB,1,notch_const,notch_states);
     arm_biquad_cascade_df1_init_f32(&lowpassB,1,lowpass_const,lowpass_states);
     arm_biquad_cascade_df1_init_f32(&highpassB,1,highpass_const,highpass_states);
 
@@ -260,5 +270,4 @@
             }
         }
     }
-
 }
\ No newline at end of file