Jorick Leferink
/
programma_filter
programma voor filter
Fork of BMT-K9-Groep7 by
Diff: main.cpp
- Revision:
- 5:12b9a5cfcf73
- Parent:
- 4:d0b4c806f4ea
--- a/main.cpp Thu Oct 17 09:38:14 2013 +0000 +++ b/main.cpp Fri Oct 18 12:31:17 2013 +0000 @@ -3,6 +3,9 @@ //Define objects AnalogIn emg_biceps(PTB0); //Analog input +AnalogIn emg_triceps(PTB1); +AnalogIn emg_flexoren(PTB2); +AnalogIn emg_extensoren(PTB3); PwmOut red(LED_RED); // EMG meting // PwmOut blue(LED_BLUE); // uitgangssignaal controle // PwmOut green(LED_GREEN); @@ -27,50 +30,148 @@ /* hou in de gaten welke waarden globaal gedefinieerd moeten worden*/ -float count =0; -float square_biceps = 0; -float sum_biceps = 0; -float mean_biceps = 0.2; + + + + +//filter functie definieren. filter(signal_number) +//signal_number=1 --> biceps filteren +//signal_number=2 --> triceps filteren +//signal_number=3 --> flexoren filteren +//signal_number=4 --> extensoren filteren + +float filter(int signal_number){ + //static variables keep their values between function calls + //the assignents are only executed the first iteration. + + //variabelen biceps definieren + static float in0_biceps =0 , 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; + + // variabelen triceps definieren + static float in0_triceps = 0, 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; + + //variabelen flexoren definieren + static float in0_flexoren = 0, 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; + + //variablen extensoren definieren + static float in0_extensoren = 0, 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; + + //overige variabelen definieren + static float count_biceps = 0, count_triceps = 0, count_extensoren = 0, count_flexoren = 0; + static float square_biceps = 0, square_triceps = 0, square_flexoren = 0, square_extensoren = 0; + static float sum_biceps = 0, sum_triceps = 0, sum_flexoren = 0, sum_extensoren = 0; + static float mean_biceps = 0.2, mean_triceps = 0.2, mean_flexoren = 0.2, mean_extensoren = 0.2; + static float sig_out_biceps, sig_out_triceps, sig_out_extensoren, sig_out_flexoren; + float emg_abs, sig_out; + + switch (signal_number){ + case 1: + //biceps filteren + 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; + + //std deviatie bepalen, om de 50 metingen + emg_abs = fabs(out0_biceps); + sum_biceps += out0_biceps; + square_biceps += (emg_abs - mean_biceps)*(emg_abs - mean_biceps); //neem absolute waarde, kwadrateer, voeg toe aan vorige square + // voeg rest EMG's toe, variabelen alleen _spier geven als het nodig is. + count_biceps += 1; // hou bij hoeveel squares er zijn opgeteld + if (count_biceps >= MAXCOUNT) + { sig_out_biceps = sqrt(square_biceps/count_biceps); + mean_biceps = sum_biceps/count_biceps; + count_biceps = 0; square_biceps = 0; sum_biceps = 0; // en neem de RMS als er genoeg zijn geteld, stuur die door, en reset sqaure en count + sig_out = sig_out_biceps; + } + else sig_out = -1; + break; + case 2: + //triceps filteren + 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_biceps = (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; + + //std deviatie bepalen om de 50 metingen + emg_abs = fabs(out0_triceps); + sum_triceps += out0_triceps; + square_triceps += (emg_abs - mean_triceps)*(emg_abs - mean_triceps); //neem absolute waarde, kwadrateer, voeg toe aan vorige square + // voeg rest EMG's toe, variabelen alleen _spier geven als het nodig is. + count_triceps += 1; // hou bij hoeveel squares er zijn opgeteld + if (count_triceps >= MAXCOUNT) + { sig_out_triceps = sqrt(square_triceps/count_triceps); + mean_triceps = sum_triceps/count_triceps; + count_triceps = 0; square_triceps = 0; sum_triceps = 0; // en neem de RMS als er genoeg zijn geteld, stuur die door, en reset sqaure en count + sig_out = sig_out_triceps; + } + else sig_out = -1; + break; + case 3: + //flexoren filteren + 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; + + //std deviatie bepalen om de 50 metingen + emg_abs = fabs(out0_flexoren); + sum_flexoren += out0_flexoren; + square_flexoren += (emg_abs - mean_flexoren)*(emg_abs - mean_flexoren); //neem absolute waarde, kwadrateer, voeg toe aan vorige square + // voeg rest EMG's toe, variabelen alleen _spier geven als het nodig is. + count_flexoren += 1; // hou bij hoeveel squares er zijn opgeteld + if (count_flexoren >= MAXCOUNT) + { sig_out_flexoren = sqrt(square_flexoren/count_flexoren); + mean_flexoren = sum_flexoren/count_flexoren; + count_flexoren = 0; square_flexoren = 0; sum_flexoren = 0; // en neem de RMS als er genoeg zijn geteld, stuur die door, en reset sqaure en count + sig_out = sig_out_flexoren; + } + else sig_out = -1; + break; + case 4: + //extensoren filteren + 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; + + //std deviatie bepalen om de 50 metingen + emg_abs = fabs(out0_extensoren); + sum_extensoren += out0_extensoren; + square_extensoren += (emg_abs - mean_extensoren)*(emg_abs - mean_extensoren); //neem absolute waarde, kwadrateer, voeg toe aan vorige square + // voeg rest EMG's toe, variabelen alleen _spier geven als het nodig is. + count_extensoren += 1; // hou bij hoeveel squares er zijn opgeteld + if (count_extensoren >= MAXCOUNT) + { sig_out_extensoren = sqrt(square_extensoren/count_extensoren); + mean_extensoren = sum_extensoren/count_extensoren; + count_extensoren = 0; square_extensoren = 0; sum_extensoren = 0; // en neem de RMS als er genoeg zijn geteld, stuur die door, en reset sqaure en count + sig_out=sig_out_extensoren; + } + else sig_out = -1; + break; + } + + return sig_out; + +} void looper() { - /*value0 is huidig, 1 is t-1, 2 is t-2 etc. Gebruik later aanduidingen ABCD. */ - float mean; - float sig_out_biceps; - //static variables keep their values between function calls - //the assignents are only executed the first iteration. - static float in0 =0,in1 =0 ,in2 = 0,in3 = 0,in4 = 0; - static float out0 = 0,out1 = 0 ,out2 = 0,out3 = 0,out4 = 0; - float emg_abs; // square, mean en count eerder gedefinieerd - // initialisation of globals should be done at initialization of program - // count = 0; square_biceps = 0; sum_biceps = 0; mean_biceps = 0.2; - in4 = in3; in3 = in4; in3 = in2; in2 = in1; in1 = in0; - in0 = emg_biceps.read(); - red = in0; - /* rode led aan voor meting emg*/ - out4 = out3; out3 = out2; out2 = out1; out1 = out0; - out0 = (NUM0*in0 + NUM1*in1 + NUM2*in2 + NUM3*in3 + NUM4*in4 - DEN1*out1 - DEN2*out2 - DEN3*out3 - DEN4*out4 ) / DEN0; - - /*send value to PC. use 6 digits after decimal sign*/ - //if(pc.rxBufferGetSize(0)-pc.rxBufferGetCount() > 30) // ! Testen filter: gebruik de if om de serial optimaal te gebruiken. - //pc.printf("%.6f\n",emg_out_biceps); - /**When not using the LED, the above could also have been done this way: - * pc.printf("%.6\n", emg0.read()); - */ +static float biceps, triceps, extensoren, flexoren, emg_filter_test; - emg_abs = fabs(out0); - mean = mean_biceps; - sum_biceps += out0; - square_biceps += (emg_abs - mean)*(emg_abs - mean); //neem absolute waarde, kwadrateer, voeg toe aan vorige square - // voeg rest EMG's toe, variabelen alleen _spier geven als het nodig is. - count += 1; // hou bij hoeveel squares er zijn opgeteld - if (count >= MAXCOUNT) - { sig_out_biceps = sqrt(square_biceps/count); - mean_biceps = sum_biceps/count; - count= 0; square_biceps = 0; sum_biceps = 0; // en neem de RMS als er genoeg zijn geteld, stuur die door, en reset sqaure en count - if(pc.txBufferGetSize(0)-pc.txBufferGetCount() > 30) - pc.printf("%.6f\n",sig_out_biceps); - } + emg_filter_test = filter(1); + if (emg_filter_test != -1) biceps = emg_filter_test; + emg_filter_test = filter(2); + if (emg_filter_test != -1) triceps = emg_filter_test; + emg_filter_test = filter(3); + if (emg_filter_test != -1) flexoren = emg_filter_test; + emg_filter_test = filter(3); + if (emg_filter_test != -1) extensoren = emg_filter_test; + } int main()