Jorick Leferink
/
programma_filter
programma voor filter
Fork of BMT-K9-Groep7 by
Diff: main.cpp
- Revision:
- 0:3843c26cd5fd
- Child:
- 1:3687c7cb0648
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Thu Oct 17 08:29:57 2013 +0000 @@ -0,0 +1,89 @@ +#include "mbed.h" +#include "MODSERIAL.h" + +//Define objects +AnalogIn emg_biceps(PTB0); //Analog input +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*/ +float count; float square_biceps; float sum_biceps; float mean_biceps; + +void looper() +{ + /*value0 is huidig, 1 is t-1, 2 is t-2 etc. Gebruik later aanduidingen ABCD. */ + char startup = 0; float mean; + float sig_out_biceps; + float in0; float in1; float in2; float in3; float in4; + float out0; float out1; float out2; float out3; float out4; + + if (startup != 1) + {in0 = 0; in1 = 0; in2 = 0; in3 = 0; in4 = 0; + out0 = 0; out1 = 0; out2 = 0; out3 = 0; out4 = 0; + count = 0; square_biceps = 0; sum_biceps = 0; mean_biceps = 0.2; startup = 1;} + 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()); + */ + float emg_abs; // square, mean en count eerder gedefinieerd + 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.rxBufferGetSize(0)-pc.rxBufferGetCount() > 30) + pc.printf("%.6f\n",sig_out_biceps); + } +} + +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; + + } +}