Jorick Leferink
/
EMG_MetFilter_HP_05HZ
werkt niet, kies andere freq
Fork of EMGnieuw by
Revision 3:a6c75f643f58, committed 2013-11-01
- Comitter:
- jorick92
- Date:
- Fri Nov 01 16:58:52 2013 +0000
- Parent:
- 2:b0b86581ba50
- Commit message:
- Werkt niet, periode moet heel aantal ms zijn, corrigeer.
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Fri Nov 01 11:20:55 2013 +0000 +++ b/main.cpp Fri Nov 01 16:58:52 2013 +0000 @@ -2,53 +2,58 @@ #include "MODSERIAL.h" //Define objects -AnalogIn emg_biceps(PTB0); //Analog input +AnalogIn emg_biceps(PTB0); +AnalogIn emg_triceps(PTB1); +AnalogIn emg_flexoren(PTB2); +AnalogIn emg_extensoren(PTB3); //Analog input PwmOut red(LED_RED); //PWM output Ticker timer; MODSERIAL pc(USBTX,USBRX,64,1024); -//high pass filter constantes 15Hz cutoff 4e orde -#define NUM0 0.2754 // constante -#define NUM1 -1.1017 // z^-1 -#define NUM2 1.6525 // z^-2etc. -#define NUM3 -1.1017 -#define NUM4 0.2754 +#define offset_biceps 0.5 // offset ruwe invoer met adapter motoren + +//high pass filter constantes 15Hz cutoff 4e orde, 515Hz +#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 DEN0 1 // constante -#define DEN1 -1.5704 -#define DEN2 1.2756 -#define DEN3 -0.4844 -#define DEN4 0.0762 +#define DEN1 -3.5221 +#define DEN2 4.6772 +#define DEN3 -2.7736 +#define DEN4 0.6194 //lowpass filter constantes 40 Hz 4e orde -#define NUM0_2 0.4328 // constante -#define NUM1_2 1.7314 // z^-1 -#define NUM2_2 2.5971 // z^-2etc. -#define NUM3_2 1.7314 -#define NUM4_2 0.4328 +#define NUM0_2 0.0466 // constante +#define NUM1_2 0.1863 // z^-1 +#define NUM2_2 0.2795 // z^-2etc. +#define NUM3_2 0.1863 +#define NUM4_2 0.0466 #define DEN0_2 1 // constante -#define DEN1_2 2.3695 -#define DEN2_2 2.3140 -#define DEN3_2 1.0547 -#define DEN4_2 0.1874 +#define DEN1_2 -0.7821 +#define DEN2_2 0.6800 +#define DEN3_2 -0.1827 +#define DEN4_2 0.0301 -//lowpass filter constantes 4z 4e orde -#define NUM0_3 0.0002 // constante -#define NUM1_3 0.0007 // z^-1 -#define NUM2_3 0.0011 // z^-2etc. -#define NUM3_3 0.0007 -#define NUM4_3 0.0002 +//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 #define DEN0_3 1 // constante -#define DEN1_3 -3.3441 -#define DEN2_3 4.2389 -#define DEN3_3 -2.4093 -#define DEN4_3 0.5175 +#define DEN1_3 -3.8725 +#define DEN2_3 5.6255 +#define DEN3_3 -3.6333 +#define DEN4_3 0.8803 -// highpass filter .5 Hz 2de orde, tegen storing motorshield +// highpass filter 1 Hz 2de orde, tegen storing motorshield #define NUM0_4 0.9780 // constante #define NUM1_4 -1.9561 // z^-1 #define NUM2_4 0.9780 // z^-2etc. @@ -56,7 +61,98 @@ #define DEN0_4 1 // constante #define DEN1_4 -1.9556 #define DEN2_4 0.9565 + + +float std_dev (float variable, int sig_number){ + // define variables + float sum; + int size; + float sig_out; + float mean; + static int count_biceps=0; + 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]; + switch(sig_number){ + case 1: + emg_values_std_dev_biceps[count_biceps]=variable; + 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]; + } + 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); + } + sig_out=sqrt(sum/size); + sum=0; + break; + case 2: + emg_values_std_dev_triceps[count_triceps]=variable; + count_triceps++; + if(count_triceps==size) count_triceps=0; + + size=sizeof(emg_values_std_dev_triceps)/sizeof(float); + for(int i=0; i < size; i++){ + sum+=emg_values_std_dev_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); + } + sig_out=sqrt(sum/size); + sum=0; + break; + case 3: + emg_values_std_dev_flexoren[count_flexoren]=variable; + count_flexoren++; + if(count_flexoren==size) count_flexoren=0; + + size=sizeof(emg_values_std_dev_flexoren)/sizeof(float); + for(int i; i < size; i++){ + sum+=emg_values_std_dev_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); + } + sig_out=sqrt(sum/size); + sum=0; + break; + case 4: + emg_values_std_dev_extensoren[count_extensoren]=variable; + count_extensoren++; + if(count_extensoren==size) count_extensoren=0; + + size=sizeof(emg_values_std_dev_extensoren)/sizeof(float); + for(int i; i < size; i++){ + sum+=emg_values_std_dev_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); + } + sig_out=sqrt(sum/size); + sum=0; + break; + } + + + return sig_out; +} + float filter(int sig_number){ float sig_out; // eerst variabelen definieren @@ -138,29 +234,29 @@ case 1: // signaal filteren op 15 Hz HIGHPASS in4_biceps = in3_biceps; in3_biceps = in2_biceps; in2_biceps = in1_biceps; in1_biceps = in0_biceps; - in0_biceps = emg_biceps.read(); + in0_biceps = emg_biceps.read() - offset_biceps; out4_biceps = out3_biceps; out3_biceps = out2_biceps; out2_biceps = out1_biceps; out1_biceps = out0_biceps; 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; - + /* //signaal filteren op 40 HZ LOWPASS in4_2_biceps = in3_2_biceps; in3_2_biceps = in2_2_biceps; in2_2_biceps = in1_2_biceps; in1_2_biceps = in0_2_biceps; in0_2_biceps = out0_biceps; out4_2_biceps = out3_2_biceps; out3_2_biceps = out2_2_biceps; out2_2_biceps = out1_2_biceps; out1_2_biceps = out0_2_biceps; out0_2_biceps = (NUM0_2*in0_2_biceps + NUM1_2*in1_2_biceps + NUM2_2*in2_2_biceps + NUM3_2*in3_2_biceps + NUM4_2*in4_2_biceps - DEN1_2*out1_2_biceps - DEN2_2*out2_2_biceps - DEN3_2*out3_2_biceps - DEN4_2*out4_2_biceps ) / DEN0_2; - + */ //signaal filteren op 5Hz LOWPASS in4_3_biceps = in3_3_biceps; in3_3_biceps = in2_3_biceps; in2_3_biceps = in1_3_biceps; in1_3_biceps = in0_3_biceps; - in0_3_biceps = abs(out0_2_biceps); + in0_3_biceps = out0_biceps; // ruw - offset -> filter 1 -> stdev (-> filter 3) out4_3_biceps = out3_3_biceps; out3_3_biceps = out2_3_biceps; out2_3_biceps = out1_3_biceps; out1_3_biceps = out0_3_biceps; out0_3_biceps = (NUM0_3*in0_3_biceps + NUM1_3*in1_3_biceps + NUM2_3*in2_3_biceps + NUM3_3*in3_3_biceps + NUM4_3*in4_3_biceps - DEN1_3*out1_3_biceps - DEN2_3*out2_3_biceps - DEN3_3*out3_3_biceps - DEN4_3*out4_3_biceps ) / DEN0_3; - - //signaal filteren op .5 HZ HIGHPASS + /* + //signaal filteren op 1 HZ HIGHPASS in2_4_biceps = in1_4_biceps; in1_4_biceps = in0_4_biceps; in0_4_biceps = out0_3_biceps; out2_4_biceps = out1_4_biceps; out1_4_biceps = out0_4_biceps; out0_4_biceps = (NUM0_4*in0_4_biceps + NUM1_4*in1_4_biceps + NUM2_4*in2_4_biceps - DEN1_4*out1_4_biceps - DEN2_4*out2_4_biceps ) / DEN0_4; - - sig_out = out0_4_biceps; + */ + sig_out = out0_biceps; break; case 2: // signaal filteren op 15 Hz HIGHPASS @@ -253,8 +349,10 @@ float emg_value_flexoren; float emg_value_extensoren; float dy; - emg_value_biceps = 100*filter(1); - emg_value_triceps = 100*filter(2); + emg_value_biceps = std_dev(filter(1), 1); + 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); @@ -273,7 +371,7 @@ */ dy = emg_value_biceps-emg_value_triceps; if(pc.rxBufferGetSize(0)-pc.rxBufferGetCount() > 30) - pc.printf("%.6f, %.6f\n",emg_value_biceps,emg_value_triceps); + pc.printf("%.6f\n",emg_value_biceps); /**When not using the LED, the above could also have been done this way: * pc.printf("%.6\n", emg0.read()); */ @@ -289,7 +387,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, 0.01); + timer.attach(looper,1.942); while(1) //Loop { /*Empty!*/