Dan August
/
EMG_finalfilter
Laatste testen filter
Fork of EMG_MetFilter_HP_05HZ by
main.cpp
- Committer:
- DanAuhust
- Date:
- 2013-11-02
- Revision:
- 4:e12537bac403
- Parent:
- 3:a6c75f643f58
File content as of revision 4:e12537bac403:
#include "mbed.h" #include "MODSERIAL.h" //Define objects 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); #define offset_biceps 0.5 // offset ruwe invoer met adapter motoren //high pass filter constantes 15Hz cutoff 4e orde, Fs = 500Hz #define NUM0 0.7870 // constante #define NUM1 -3.1255 // z^-1 #define NUM2 4.6882 // z^-2etc. #define NUM3 -3.1255 #define NUM4 0.7814 #define DEN0 1 // constante #define DEN1 -3.5078 #define DEN2 4.6409 #define DEN3 -2.7427 #define DEN4 0.6105 //lowpass filter constantes 40 Hz 4e orde #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 -0.7821 #define DEN2_2 0.6800 #define DEN3_2 -0.1827 #define DEN4_2 0.0301 //lowpass filter constantes 4Hz 4e orde, 500Hz #define NUM0_3 0.000000374 // constante #define NUM1_3 0.000001496 // z^-1 #define NUM2_3 0.000002244 // z^-2etc. #define NUM3_3 0.000001496 #define NUM4_3 0.000000374 #define DEN0_3 1 // constante #define DEN1_3 -3.8687 #define DEN2_3 5.6145 #define DEN3_3 -3.6228 #define DEN4_3 0.8769 // 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. #define DEN0_4 1 // constante #define DEN1_4 -1.9556 #define DEN2_4 0.9565 float std_dev (float input, 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 keeper_biceps[50]; static float keeper_triceps[50]; static float keeper_flexoren[50]; static float keeper_extensoren[50]; switch(sig_number){ case 1: keeper_biceps[count_biceps]=input; count_biceps++; if(count_triceps>=size) count_biceps=0; size=sizeof(keeper_biceps); // /sizeof(float); // waarom delen door 1? for(int i=0; i < size; i++) {sum+=keeper_biceps[i]; } mean=sum/size; sum=0; for(int i=0; i < size; i++) {sum+=(keeper_biceps[i]-mean)*(keeper_biceps[i]-mean); } sig_out=sqrt(sum/size); sum=0; break; case 2: keeper_triceps[count_triceps]=input; count_triceps++; if(count_triceps==size) count_triceps=0; size=sizeof(keeper_triceps)/sizeof(float); for(int i=0; i < size; i++){ sum+=keeper_triceps[i]; } mean=sum/size; sum=0; for(int i=0; i < size; i++){ sum+=(keeper_triceps[i]-mean)*(keeper_triceps[i]-mean); } sig_out=sqrt(sum/size); sum=0; break; case 3: keeper_flexoren[count_flexoren]=input; count_flexoren++; if(count_flexoren==size) count_flexoren=0; size=sizeof(keeper_flexoren)/sizeof(float); for(int i; i < size; i++){ sum+=keeper_flexoren[i]; } mean=sum/size; sum=0; for(int i; i < size; i++){ sum+=(keeper_flexoren[i]-mean)*(keeper_flexoren[i]-mean); } sig_out=sqrt(sum/size); sum=0; break; case 4: keeper_extensoren[count_extensoren]=input; count_extensoren++; if(count_extensoren==size) count_extensoren=0; size=sizeof(keeper_extensoren)/sizeof(float); for(int i; i < size; i++){ sum+=keeper_extensoren[i]; } mean=sum/size; sum=0; for(int i; i < size; i++){ sum+=(keeper_extensoren[i]-mean)*(keeper_extensoren[i]-mean); } sig_out=sqrt(sum/size); sum=0; break; } return sig_out; } float filter(int sig_number){ float sig_out; // eerst variabelen definieren //biceps //filter 1 float in0_biceps =0; static float in1_biceps =0, in2_biceps = 0, in3_biceps = 0, in4_biceps = 0; static float out0_biceps = 0, out1_biceps = 0 , out2_biceps = 0, out3_biceps = 0, out4_biceps = 0; //filter 2 float in0_2_biceps =0; static float in1_2_biceps =0, in2_2_biceps = 0, in3_2_biceps = 0, in4_2_biceps = 0; static float out0_2_biceps = 0, out1_2_biceps = 0 , out2_2_biceps = 0, out3_2_biceps = 0, out4_2_biceps = 0; //filter 3 float in0_3_biceps =0; static float in1_3_biceps =0, in2_3_biceps = 0, in3_3_biceps = 0, in4_3_biceps = 0; static float out0_3_biceps = 0, out1_3_biceps = 0 , out2_3_biceps = 0, out3_3_biceps = 0, out4_3_biceps = 0; //filter 4 float in0_4_biceps =0; static float in1_4_biceps =0, in2_4_biceps = 0; static float out0_4_biceps = 0, out1_4_biceps = 0 , out2_4_biceps = 0; //triceps //filter 1 float in0_triceps =0; static float in1_triceps =0, in2_triceps = 0, in3_triceps = 0, in4_triceps = 0; static float out0_triceps = 0, out1_triceps = 0 , out2_triceps = 0, out3_triceps = 0, out4_triceps = 0; //filter 2 float in0_2_triceps =0; static float in1_2_triceps =0, in2_2_triceps = 0, in3_2_triceps = 0, in4_2_triceps = 0; static float out0_2_triceps = 0, out1_2_triceps = 0 , out2_2_triceps = 0, out3_2_triceps = 0, out4_2_triceps = 0; //filter 3 float in0_3_triceps =0; static float in1_3_triceps =0, in2_3_triceps = 0, in3_3_triceps = 0, in4_3_triceps = 0; static float out0_3_triceps = 0, out1_3_triceps = 0 , out2_3_triceps = 0, out3_3_triceps = 0, out4_3_triceps = 0; //filter 4 float in0_4_triceps =0; static float in1_4_triceps =0, in2_4_triceps = 0; static float out0_4_triceps = 0, out1_4_triceps = 0 , out2_4_triceps = 0; //flexoren //filter 1 float in0_flexoren =0; static float in1_flexoren =0, in2_flexoren = 0, in3_flexoren = 0, in4_flexoren = 0; static float out0_flexoren = 0, out1_flexoren = 0 , out2_flexoren = 0, out3_flexoren = 0, out4_flexoren = 0; //filter 2 float in0_2_flexoren =0; static float in1_2_flexoren =0, in2_2_flexoren = 0, in3_2_flexoren = 0, in4_2_flexoren = 0; static float out0_2_flexoren = 0, out1_2_flexoren = 0 , out2_2_flexoren = 0, out3_2_flexoren = 0, out4_2_flexoren = 0; //filter 3 float in0_3_flexoren =0; static float in1_3_flexoren =0, in2_3_flexoren = 0, in3_3_flexoren = 0, in4_3_flexoren = 0; static float out0_3_flexoren = 0, out1_3_flexoren = 0 , out2_3_flexoren = 0, out3_3_flexoren = 0, out4_3_flexoren = 0; //filter 4 float in0_4_flexoren =0; static float in1_4_flexoren =0, in2_4_flexoren = 0; static float out0_4_flexoren = 0, out1_4_flexoren = 0 , out2_4_flexoren = 0; //extensoren //filter 1 float in0_extensoren =0; static float in1_extensoren =0, in2_extensoren = 0, in3_extensoren = 0, in4_extensoren = 0; static float out0_extensoren = 0, out1_extensoren = 0 , out2_extensoren = 0, out3_extensoren = 0, out4_extensoren = 0; //filter 2 float in0_2_extensoren =0; static float in1_2_extensoren =0, in2_2_extensoren = 0, in3_2_extensoren = 0, in4_2_extensoren = 0; static float out0_2_extensoren = 0, out1_2_extensoren = 0 , out2_2_extensoren = 0, out3_2_extensoren = 0, out4_2_extensoren = 0; //filter 3 float in0_3_extensoren =0; static float in1_3_extensoren =0, in2_3_extensoren = 0, in3_3_extensoren = 0, in4_3_extensoren = 0; static float out0_3_extensoren = 0, out1_3_extensoren = 0 , out2_3_extensoren = 0, out3_3_extensoren = 0, out4_3_extensoren = 0; //filter 4 float in0_4_extensoren =0; static float in1_4_extensoren =0, in2_4_extensoren = 0; static float out0_4_extensoren = 0, out1_4_extensoren = 0 , out2_4_extensoren = 0; switch(sig_number){ 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() - 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 = 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 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_biceps; break; case 2: // signaal filteren op 15 Hz HIGHPASS 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_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; //signaal filteren op 40 HZ LOWPASS in4_2_triceps = in3_2_triceps; in3_2_triceps = in2_2_triceps; in2_2_triceps = in1_2_triceps; in1_2_triceps = in0_2_triceps; in0_2_triceps = out0_triceps; out4_2_triceps = out3_2_triceps; out3_2_triceps = out2_2_triceps; out2_2_triceps = out1_2_triceps; out1_2_triceps = out0_2_triceps; out0_2_triceps = (NUM0_2*in0_2_triceps + NUM1_2*in1_2_triceps + NUM2_2*in2_2_triceps + NUM3_2*in3_2_triceps + NUM4_2*in4_2_triceps - DEN1_2*out1_2_triceps - DEN2_2*out2_2_triceps - DEN3_2*out3_2_triceps - DEN4_2*out4_2_triceps ) / DEN0_2; //signaal filteren op 5Hz LOWPASS in4_3_triceps = in3_3_triceps; in3_3_triceps = in2_3_triceps; in2_3_triceps = in1_3_triceps; in1_3_triceps = in0_3_triceps; in0_3_triceps = abs(out0_2_triceps); out4_3_triceps = out3_3_triceps; out3_3_triceps = out2_3_triceps; out2_3_triceps = out1_3_triceps; out1_3_triceps = out0_3_triceps; out0_3_triceps = (NUM0_3*in0_3_triceps + NUM1_3*in1_3_triceps + NUM2_3*in2_3_triceps + NUM3_3*in3_3_triceps + NUM4_3*in4_3_triceps - DEN1_3*out1_3_triceps - DEN2_3*out2_3_triceps - DEN3_3*out3_3_triceps - DEN4_3*out4_3_triceps ) / DEN0_3; //signaal filteren op .5 HZ HIGHPASS in2_4_triceps = in1_4_triceps; in1_4_triceps = in0_4_triceps; in0_4_triceps = out0_3_triceps; out2_4_triceps = out1_4_triceps; out1_4_triceps = out0_4_triceps; out0_4_triceps = (NUM0_4*in0_4_triceps + NUM1_4*in1_4_triceps + NUM2_4*in2_4_triceps - DEN1_4*out1_4_triceps - DEN2_4*out2_4_triceps ) / DEN0_4; sig_out = out0_4_triceps; break; case 3: // signaal filteren op 15 Hz HIGHPASS in4_flexoren = in3_flexoren; in3_flexoren = in2_flexoren; in2_flexoren = in1_flexoren; in1_flexoren = in0_flexoren; in0_flexoren = emg_flexoren.read(); out4_flexoren = out3_flexoren; out3_flexoren = out2_flexoren; out2_flexoren = out1_flexoren; out1_flexoren = out0_flexoren; 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; //signaal filteren op 40 HZ LOWPASS in4_2_flexoren = in3_2_flexoren; in3_2_flexoren = in2_2_flexoren; in2_2_flexoren = in1_2_flexoren; in1_2_flexoren = in0_2_flexoren; in0_2_flexoren = out0_flexoren; out4_2_flexoren = out3_2_flexoren; out3_2_flexoren = out2_2_flexoren; out2_2_flexoren = out1_2_flexoren; out1_2_flexoren = out0_2_flexoren; out0_2_flexoren = (NUM0_2*in0_2_flexoren + NUM1_2*in1_2_flexoren + NUM2_2*in2_2_flexoren + NUM3_2*in3_2_flexoren + NUM4_2*in4_2_flexoren - DEN1_2*out1_2_flexoren - DEN2_2*out2_2_flexoren - DEN3_2*out3_2_flexoren - DEN4_2*out4_2_flexoren ) / DEN0_2; //signaal filteren op 5Hz LOWPASS in4_3_flexoren = in3_3_flexoren; in3_3_flexoren = in2_3_flexoren; in2_3_flexoren = in1_3_flexoren; in1_3_flexoren = in0_3_flexoren; in0_3_flexoren = abs(out0_2_flexoren); out4_3_flexoren = out3_3_flexoren; out3_3_flexoren = out2_3_flexoren; out2_3_flexoren = out1_3_flexoren; out1_3_flexoren = out0_3_flexoren; out0_3_flexoren = (NUM0_3*in0_3_flexoren + NUM1_3*in1_3_flexoren + NUM2_3*in2_3_flexoren + NUM3_3*in3_3_flexoren + NUM4_3*in4_3_flexoren - DEN1_3*out1_3_flexoren - DEN2_3*out2_3_flexoren - DEN3_3*out3_3_flexoren - DEN4_3*out4_3_flexoren ) / DEN0_3; //signaal filteren op .5 HZ HIGHPASS in2_4_flexoren = in1_4_flexoren; in1_4_flexoren = in0_4_flexoren; in0_4_flexoren = out0_3_flexoren; out2_4_flexoren = out1_4_flexoren; out1_4_flexoren = out0_4_flexoren; out0_4_flexoren = (NUM0_4*in0_4_flexoren + NUM1_4*in1_4_flexoren + NUM2_4*in2_4_flexoren - DEN1_4*out1_4_flexoren - DEN2_4*out2_4_flexoren ) / DEN0_4; sig_out = out0_4_flexoren; break; case 4: // signaal filteren op 15 Hz HIGHPASS in4_extensoren = in3_extensoren; in3_extensoren = in2_extensoren; in2_extensoren = in1_extensoren; in1_extensoren = in0_extensoren; in0_extensoren = emg_extensoren.read(); out4_extensoren = out3_extensoren; out3_extensoren = out2_extensoren; out2_extensoren = out1_extensoren; out1_extensoren = out0_extensoren; 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; //signaal filteren op 40 HZ LOWPASS in4_2_extensoren = in3_2_extensoren; in3_2_extensoren = in2_2_extensoren; in2_2_extensoren = in1_2_extensoren; in1_2_extensoren = in0_2_extensoren; in0_2_extensoren = out0_extensoren; out4_2_extensoren = out3_2_extensoren; out3_2_extensoren = out2_2_extensoren; out2_2_extensoren = out1_2_extensoren; out1_2_extensoren = out0_2_extensoren; out0_2_extensoren = (NUM0_2*in0_2_extensoren + NUM1_2*in1_2_extensoren + NUM2_2*in2_2_extensoren + NUM3_2*in3_2_extensoren + NUM4_2*in4_2_extensoren - DEN1_2*out1_2_extensoren - DEN2_2*out2_2_extensoren - DEN3_2*out3_2_extensoren - DEN4_2*out4_2_extensoren ) / DEN0_2; //signaal filteren op 5Hz LOWPASS in4_3_extensoren = in3_3_extensoren; in3_3_extensoren = in2_3_extensoren; in2_3_extensoren = in1_3_extensoren; in1_3_extensoren = in0_3_extensoren; in0_3_extensoren = abs(out0_2_extensoren); out4_3_extensoren = out3_3_extensoren; out3_3_extensoren = out2_3_extensoren; out2_3_extensoren = out1_3_extensoren; out1_3_extensoren = out0_3_extensoren; out0_3_extensoren = (NUM0_3*in0_3_extensoren + NUM1_3*in1_3_extensoren + NUM2_3*in2_3_extensoren + NUM3_3*in3_3_extensoren + NUM4_3*in4_3_extensoren - DEN1_3*out1_3_extensoren - DEN2_3*out2_3_extensoren - DEN3_3*out3_3_extensoren - DEN4_3*out4_3_extensoren ) / DEN0_3; //signaal filteren op .5 HZ HIGHPASS in2_4_extensoren = in1_4_extensoren; in1_4_extensoren = in0_4_extensoren; in0_4_extensoren = out0_3_extensoren; out2_4_extensoren = out1_4_extensoren; out1_4_extensoren = out0_4_extensoren; out0_4_extensoren = (NUM0_4*in0_4_extensoren + NUM1_4*in1_4_extensoren + NUM2_4*in2_4_extensoren - DEN1_4*out1_4_extensoren - DEN2_4*out2_4_extensoren ) / DEN0_4; sig_out = out0_4_extensoren; break; } return sig_out; } void looper() { float emg_value_biceps; float emg_value_triceps; float emg_value_flexoren; float emg_value_extensoren; float dy; 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);*/ /*if(emg_value_biceps < 0.10){ emg_value_biceps=0; } else { emg_value_biceps = emg_value_biceps; } if(emg_value_triceps < 0.10){ emg_value_triceps=0; } else { emg_value_triceps=emg_value_triceps; } */ dy = emg_value_biceps-emg_value_triceps; if(pc.rxBufferGetSize(0)-pc.rxBufferGetCount() > 30) 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()); */ } int main() { /*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(2); /**Here you attach the 'void looper(void)' function to the Ticker object * 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); // invullen in seconde while(1) //Loop { /*Empty!*/ /*Everything is handled by the interrupt routine now!*/ } }