checken van filter met hidscope
Dependencies: HIDScope mbed-dev mbed
Diff: main.cpp
- Revision:
- 0:5c4ee2c81f02
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Thu Oct 12 10:19:08 2017 +0000 @@ -0,0 +1,94 @@ +#include "mbed.h" +#include "Serial.h" +#include "math.h" +#include "HIDScope.h" + +Serial pc(USBTX, USBRX); //Serial PC connectie +AnalogIn emg0( A0 ); //EMG1 op A0 +AnalogIn emg1( A1 ); //EMG2 op A1 +DigitalOut motor1DirectionPin(D4); //Motorrichting op D4 (connected op het bord) +PwmOut motor1MagnitudePin(D5); //Motorkracht op D5 (connected op het bord) +Timer timer; //timer voor duur script +HIDScope scope(2); //Maakt de scopes aan + +//Benoemen van de variabelen die in de VOID's gebruikt gaan worden +double emga = emg0.read(); //EMG1 +double emgb = emg1.read(); //EMG2 + +//Aanmaken filter variabelen +float ah[3]={1, 0, 0.1716}; +float bh[3]={0.2929, -0.5858, 0.2929}; + +//innitial conditions high pass filter +float emg_hpf[3]={0, 0, 0}; +float emg_in[3]={0, 0, 0}; + +// coëfficienten high pass filter +float al[3]={1, -1.7347, 0.7660}; +float bl[3]={0.0078, 0.0156, 0.0078}; + +//innitial conditions low pass filter +float emg_lpf[3]={0, 0, 0}; +float emg_abs[3]={0, 0, 0}; + +//Aanmaken van de verschillende tickers +Ticker tick_sample; + +void aansturing() + { + timer.reset(); + timer.start(); + + emga = emg0.read(); + emgb = emg1.read(); + emg_in[0]=emga; + + //Filter + // high pass filter + emg_hpf[0]=bh[0]*emg_in[0] +bh[1]*emg_in[1] +bh[2]*emg_in[2] -ah[1]*emg_hpf[1] -ah[2]*emg_hpf[2]; + + emg_in[2]=emg_in[1]; + emg_in[1]=emg_in[0]; + emg_hpf[2]=emg_hpf[1]; + emg_hpf[2]=emg_hpf[0]; + + + //absolute value + emg_abs[0]=fabs(emg_hpf[0]); + + + //low pass filter + emg_lpf[0]=bl[0]*emg_abs[0] +bl[1]*emg_abs[1] +bl[2]*emg_abs[2] -al[1]*emg_lpf[1] -al[2]*emg_lpf[2]; + + emg_abs[2]=emg_abs[1]; + emg_abs[1]=emg_abs[0]; + emg_lpf[2]=emg_lpf[1]; + emg_lpf[1]=emg_lpf[0]; + + if (emg_lpf[1]>0.05) + { + motor1MagnitudePin = emg_lpf[1]; + motor1DirectionPin = 0; + } + else + { + motor1MagnitudePin = 0; + motor1DirectionPin = 0; + } + scope.set(0, emg0.read()); + scope.send(); + scope.set(1, emg_lpf[1]); + scope.send(); + + timer.stop(); + pc.printf("time taken was %f milliseconds\n\r", timer.read_ms()); + + } + + +int main() +{ + //Deze tickers roepen de verschillende voids aan + pc.baud(9600); + tick_sample.attach_us(&aansturing, 5000); //Deze ticker roept de potmeter aan +}