Arthur de Lange
/
Filter_check
12-okt
Fork of Filter_check by
main.cpp
- Committer:
- JornJan
- Date:
- 2017-10-12
- Revision:
- 0:5c4ee2c81f02
- Child:
- 2:7e0279519cbf
File content as of revision 0:5c4ee2c81f02:
#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 }