Dan August
/
EMGnieuw
met ins en outs
Fork of new_program by
Revision 2:b0b86581ba50, committed 2013-11-01
- Comitter:
- DanAuhust
- Date:
- Fri Nov 01 11:20:55 2013 +0000
- Parent:
- 1:000418a0aedf
- Commit message:
- meting biceps en triceps met 4 filters, gain 100, threshold optioneel
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Thu Oct 31 12:15:47 2013 +0000 +++ b/main.cpp Fri Nov 01 11:20:55 2013 +0000 @@ -48,125 +48,232 @@ #define DEN3_3 -2.4093 #define DEN4_3 0.5175 -//variabelen definieren -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; - -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; +// highpass filter .5 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. -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; -/** Looper function -* functions used for Ticker and Timeout should be of type void <name>(void) -* i.e. no input arguments, no output arguments. -* if you want to change a variable that you use in other places (for example in main) -* you will have to make that variable global in order to be able to reach it both from -* the function called at interrupt time, and in the main function. -* To make a variable global, define it under the includes. -* variables that are changed in the interrupt routine (written to) should be made -* 'volatile' to let the compiler know that those values may change outside the current context. -* i.e.: "volatile float emg_value;" instead of "float emg_value" -* in the example below, the variable is not re-used in the main function, and is thus declared -* local in the looper function only. -**/ - -float filter(float value, int sig_number){ +#define DEN0_4 1 // constante +#define DEN1_4 -1.9556 +#define DEN2_4 0.9565 + +float filter(int sig_number){ + float sig_out; // eerst variabelen definieren - float emg_value; //output filters + //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: + 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(); + 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); + 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 + 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; 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() -{ - - - /*put raw emg value both in red and in emg_value*/ - - /*send value to PC. use 6 digits after decimal sign*/ - - // 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(); - 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; +{ float emg_value_biceps; + float emg_value_triceps; + float emg_value_flexoren; + float emg_value_extensoren; + float dy; + emg_value_biceps = 100*filter(1); + emg_value_triceps = 100*filter(2); + //emg_value_flexoren = 100*filter(3); + //emg_value_extensoren = 100*filter(4); - //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); - 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; - emg_value = out0_3_biceps; + /*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); + pc.printf("%.6f, %.6f\n",emg_value_biceps,emg_value_triceps); /**When not using the LED, the above could also have been done this way: * pc.printf("%.6\n", emg0.read()); */