Arthur de Lange / Mbed 2 deprecated Filter_check

12-okt

Dependencies:   HIDScope mbed

Fork of Filter_check by Jorn-Jan van de Beld

main.cpp@3:3e5d899a3c8a, 2017-10-12 (annotated)

Committer:
arthurdelange
Date:
Thu Oct 12 13:26:02 2017 +0000
Revision:
3:3e5d899a3c8a
Parent:
2:7e0279519cbf
Child:
4:8ed071e5e3c9
Filter_werkzaam_veelruisnog_12_okt

Who changed what in which revision?

UserRevisionLine numberNew contents of line
JornJan 0:5c4ee2c81f02 1 #include "mbed.h"
JornJan 0:5c4ee2c81f02 2 #include "Serial.h"
JornJan 0:5c4ee2c81f02 3 #include "math.h"
JornJan 0:5c4ee2c81f02 4 #include "HIDScope.h"
JornJan 0:5c4ee2c81f02 5
JornJan 0:5c4ee2c81f02 6 Serial pc(USBTX, USBRX); //Serial PC connectie
JornJan 0:5c4ee2c81f02 7 AnalogIn emg0( A0 ); //EMG1 op A0
JornJan 0:5c4ee2c81f02 8 AnalogIn emg1( A1 ); //EMG2 op A1
JornJan 0:5c4ee2c81f02 9 DigitalOut motor1DirectionPin(D4); //Motorrichting op D4 (connected op het bord)
JornJan 0:5c4ee2c81f02 10 PwmOut motor1MagnitudePin(D5); //Motorkracht op D5 (connected op het bord)
JornJan 0:5c4ee2c81f02 11 Timer timer; //timer voor duur script
JornJan 0:5c4ee2c81f02 12 HIDScope scope(2); //Maakt de scopes aan
JornJan 0:5c4ee2c81f02 13
JornJan 0:5c4ee2c81f02 14 //Benoemen van de variabelen die in de VOID's gebruikt gaan worden
JornJan 0:5c4ee2c81f02 15 double emga = emg0.read(); //EMG1
JornJan 0:5c4ee2c81f02 16 double emgb = emg1.read(); //EMG2
JornJan 0:5c4ee2c81f02 17
JornJan 0:5c4ee2c81f02 18 //Aanmaken filter variabelen
arthurdelange 2:7e0279519cbf 19 double ah[3]={1, 0, 0.1716};
arthurdelange 2:7e0279519cbf 20 double bh[3]={0.2929, -0.5858, 0.2929};
JornJan 0:5c4ee2c81f02 21
JornJan 0:5c4ee2c81f02 22 //innitial conditions high pass filter
arthurdelange 2:7e0279519cbf 23 double emg_hpf[3]={0, 0, 0};
arthurdelange 2:7e0279519cbf 24 double emg_in[3]={0, 0, 0};
JornJan 0:5c4ee2c81f02 25
JornJan 0:5c4ee2c81f02 26 // coëfficienten high pass filter
arthurdelange 2:7e0279519cbf 27 double al[3]={1, -1.7347, 0.7660};
arthurdelange 2:7e0279519cbf 28 double bl[3]={0.0078, 0.0156, 0.0078};
JornJan 0:5c4ee2c81f02 29
JornJan 0:5c4ee2c81f02 30 //innitial conditions low pass filter
arthurdelange 2:7e0279519cbf 31 double emg_lpf[3]={0, 0, 0};
arthurdelange 2:7e0279519cbf 32 double emg_abs[3]={0, 0, 0};
arthurdelange 2:7e0279519cbf 33
arthurdelange 2:7e0279519cbf 34 double emg_lpfg;
JornJan 0:5c4ee2c81f02 35
JornJan 0:5c4ee2c81f02 36 //Aanmaken van de verschillende tickers
JornJan 0:5c4ee2c81f02 37 Ticker tick_sample;
JornJan 0:5c4ee2c81f02 38
JornJan 0:5c4ee2c81f02 39 void aansturing()
JornJan 0:5c4ee2c81f02 40 {
JornJan 0:5c4ee2c81f02 41 timer.reset();
JornJan 0:5c4ee2c81f02 42 timer.start();
JornJan 0:5c4ee2c81f02 43
JornJan 0:5c4ee2c81f02 44 emga = emg0.read();
JornJan 0:5c4ee2c81f02 45 emgb = emg1.read();
arthurdelange 2:7e0279519cbf 46 emg_in[0]=emga-emgb;
JornJan 0:5c4ee2c81f02 47
JornJan 0:5c4ee2c81f02 48 //Filter
JornJan 0:5c4ee2c81f02 49 // high pass filter
JornJan 0:5c4ee2c81f02 50 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];
JornJan 0:5c4ee2c81f02 51
JornJan 0:5c4ee2c81f02 52 emg_in[2]=emg_in[1];
JornJan 0:5c4ee2c81f02 53 emg_in[1]=emg_in[0];
JornJan 0:5c4ee2c81f02 54 emg_hpf[2]=emg_hpf[1];
JornJan 0:5c4ee2c81f02 55 emg_hpf[2]=emg_hpf[0];
JornJan 0:5c4ee2c81f02 56
JornJan 0:5c4ee2c81f02 57
JornJan 0:5c4ee2c81f02 58 //absolute value
JornJan 0:5c4ee2c81f02 59 emg_abs[0]=fabs(emg_hpf[0]);
JornJan 0:5c4ee2c81f02 60
JornJan 0:5c4ee2c81f02 61
JornJan 0:5c4ee2c81f02 62 //low pass filter
JornJan 0:5c4ee2c81f02 63 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];
JornJan 0:5c4ee2c81f02 64
JornJan 0:5c4ee2c81f02 65 emg_abs[2]=emg_abs[1];
JornJan 0:5c4ee2c81f02 66 emg_abs[1]=emg_abs[0];
JornJan 0:5c4ee2c81f02 67 emg_lpf[2]=emg_lpf[1];
JornJan 0:5c4ee2c81f02 68 emg_lpf[1]=emg_lpf[0];
arthurdelange 2:7e0279519cbf 69 emg_lpfg = 5* emg_lpf[1];
JornJan 0:5c4ee2c81f02 70
JornJan 0:5c4ee2c81f02 71 if (emg_lpf[1]>0.05)
JornJan 0:5c4ee2c81f02 72 {
arthurdelange 2:7e0279519cbf 73 motor1MagnitudePin = emg_lpfg;
JornJan 0:5c4ee2c81f02 74 motor1DirectionPin = 0;
JornJan 0:5c4ee2c81f02 75 }
JornJan 0:5c4ee2c81f02 76 else
JornJan 0:5c4ee2c81f02 77 {
JornJan 0:5c4ee2c81f02 78 motor1MagnitudePin = 0;
JornJan 0:5c4ee2c81f02 79 motor1DirectionPin = 0;
JornJan 0:5c4ee2c81f02 80 }
arthurdelange 2:7e0279519cbf 81 scope.set(0, emg_in[0]);
arthurdelange 2:7e0279519cbf 82 scope.set(1, emg_lpfg);
JornJan 0:5c4ee2c81f02 83 scope.send();
JornJan 0:5c4ee2c81f02 84
JornJan 0:5c4ee2c81f02 85 timer.stop();
arthurdelange 3:3e5d899a3c8a 86 pc.printf("time taken was %d milliseconds\n\r", timer.read_us());
JornJan 0:5c4ee2c81f02 87
JornJan 0:5c4ee2c81f02 88 }
JornJan 0:5c4ee2c81f02 89
JornJan 0:5c4ee2c81f02 90
JornJan 0:5c4ee2c81f02 91 int main()
JornJan 0:5c4ee2c81f02 92 {
JornJan 0:5c4ee2c81f02 93 //Deze tickers roepen de verschillende voids aan
arthurdelange 2:7e0279519cbf 94 pc.baud(115200);
JornJan 0:5c4ee2c81f02 95 tick_sample.attach_us(&aansturing, 5000); //Deze ticker roept de potmeter aan
JornJan 0:5c4ee2c81f02 96 }