Arthur de Lange
/
Filter_check
12-okt
Fork of Filter_check by
main.cpp
- Committer:
- arthurdelange
- Date:
- 2017-10-12
- Revision:
- 3:3e5d899a3c8a
- Parent:
- 2:7e0279519cbf
- Child:
- 4:8ed071e5e3c9
File content as of revision 3:3e5d899a3c8a:
#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 double ah[3]={1, 0, 0.1716}; double bh[3]={0.2929, -0.5858, 0.2929}; //innitial conditions high pass filter double emg_hpf[3]={0, 0, 0}; double emg_in[3]={0, 0, 0}; // coëfficienten high pass filter double al[3]={1, -1.7347, 0.7660}; double bl[3]={0.0078, 0.0156, 0.0078}; //innitial conditions low pass filter double emg_lpf[3]={0, 0, 0}; double emg_abs[3]={0, 0, 0}; double emg_lpfg; //Aanmaken van de verschillende tickers Ticker tick_sample; void aansturing() { timer.reset(); timer.start(); emga = emg0.read(); emgb = emg1.read(); emg_in[0]=emga-emgb; //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]; emg_lpfg = 5* emg_lpf[1]; if (emg_lpf[1]>0.05) { motor1MagnitudePin = emg_lpfg; motor1DirectionPin = 0; } else { motor1MagnitudePin = 0; motor1DirectionPin = 0; } scope.set(0, emg_in[0]); scope.set(1, emg_lpfg); scope.send(); timer.stop(); pc.printf("time taken was %d milliseconds\n\r", timer.read_us()); } int main() { //Deze tickers roepen de verschillende voids aan pc.baud(115200); tick_sample.attach_us(&aansturing, 5000); //Deze ticker roept de potmeter aan }