Arthur de Lange
/
Filter_check
12-okt
Fork of Filter_check by
main.cpp
- Committer:
- JornJan
- Date:
- 2017-10-13
- Revision:
- 4:8ed071e5e3c9
- Parent:
- 3:3e5d899a3c8a
- Child:
- 5:41d4aac93351
File content as of revision 4:8ed071e5e3c9:
#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 int n=0; //Benoemen van de variabelen die in de VOID's gebruikt gaan worden double emga = emg0.read(); //EMG1 double emgb = emg1.read(); //EMG2 // coëfficienten notch filter double an[3]={1.0000, -0.0000, 0.9561}; double bn[3]={0.9780, -0.0000, 0.9780}; // innitial conditions notch filter double emg_nf[3]={0,0,0}; double emg_in[3]={0, 0, 0}; //Aanmaken filter coëfficienten double ah[3]={1.0000, -1.9778, 0.9780}; double bh[3]={0.9890, -1.9779, 0.9890}; //innitial conditions high pass filter double emg_hpf[3]={0, 0, 0}; // coëfficienten low pass filter double al[5]={1.0000, -3.5897, 4.8513, -2.9241, 0.6630}; double bl[5]={0.0000312, 0.0001250, 0.0001874, 0.0001250, 0.0000312}; //innitial conditions low pass filter double emg_lpf[5]={0, 0, 0, 0, 0}; double emg_abs[5]={0, 0, 0, 0, 0}; double emg_lpfg; //Aanmaken van de verschillende tickers Ticker tick_sample; void aansturing() { timer.reset(); emga = emg0.read(); emgb = emg1.read(); emg_in[0]=emga-emgb; //Filter cascade // notch filter emg_nf[0]=bn[0]*emg_in[0] +bn[1]*emg_in[1] +bn[2]*emg_in[2] -an[1]*emg_nf[1] -an[2]*emg_nf[2]; // high pass filter emg_hpf[0]=bh[0]*emg_nf[0] +bh[1]*emg_nf[1] +bh[2]*emg_nf[2] -ah[1]*emg_hpf[1] -ah[2]*emg_hpf[2]; //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] +bl[3]*emg_abs[3] +bl[4]*emg_abs[4] -al[1]*emg_lpf[1] -al[2]*emg_lpf[2] -al[3]*emg_lpf[3] -al[4]*emg_lpf[4]; // RAM emg_in[2]=emg_in[1]; emg_in[1]=emg_in[0]; emg_nf[2]=emg_nf[1]; emg_nf[1]=emg_nf[0]; emg_hpf[2]=emg_hpf[1]; emg_hpf[1]=emg_hpf[0]; emg_abs[4]=emg_abs[3]; emg_abs[3]=emg_abs[2]; emg_abs[2]=emg_abs[1]; emg_abs[1]=emg_abs[0]; emg_lpf[4]=emg_lpf[3]; emg_lpf[3]=emg_lpf[2]; emg_lpf[2]=emg_lpf[1]; emg_lpf[1]=emg_lpf[0]; if (emg_lpf[0]>0.01) { motor1MagnitudePin = emg_lpf[0]; motor1DirectionPin = 0; } else { motor1MagnitudePin = 0; motor1DirectionPin = 0; } scope.set(0, emg_in[0]); scope.set(1, emg_lpf[0]); scope.send(); if (n==100) { pc.printf("time taken was %d microseconds\n\r", timer.read_us()); n=0; } else { n=n+1; } } int main() { //Deze tickers roepen de verschillende voids aan pc.baud(115200); timer.start(); tick_sample.attach_us(&aansturing, 5000); //Deze ticker roept de potmeter aan }