Jorick Leferink
/
programma_filter
programma voor filter
Fork of BMT-K9-Groep7 by
main.cpp
- Committer:
- jorick92
- Date:
- 2013-10-18
- Revision:
- 5:12b9a5cfcf73
- Parent:
- 4:d0b4c806f4ea
File content as of revision 5:12b9a5cfcf73:
#include "mbed.h" #include "MODSERIAL.h" //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); Ticker timer; MODSERIAL pc(USBTX,USBRX,64,1024); #define GAIN_BICEPS 1 #define MAXCOUNT 50 #define NUM0 0.8841 // constante #define NUM1 -3.53647 // z^-1 #define NUM2 5.3046 // z^-2etc. #define NUM3 -3.5364 #define NUM4 0.8841 #define DEN0 1 // constante #define DEN1 -3.7538 #define DEN2 5.2912 #define DEN3 -3.3189 #define DEN4 0.7816 /* hou in de gaten welke waarden globaal gedefinieerd moeten worden*/ //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() { static float biceps, triceps, extensoren, flexoren, emg_filter_test; 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() { /*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); // periode pwm = 2*Fs , blijkbaar. // blue.period_ms(2); /**Here you attach the 'void looper(void)' function to the Ticker object0 * 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.001); while(1) // Loop { // blue = sig_out_biceps; } }