Arthur de Lange
/
Filter_check
12-okt
Fork of Filter_check by
main.cpp@5:41d4aac93351, 2017-10-13 (annotated)
- Committer:
- JornJan
- Date:
- Fri Oct 13 14:24:12 2017 +0000
- Revision:
- 5:41d4aac93351
- Parent:
- 4:8ed071e5e3c9
13okt stuk (tom);
Who changed what in which revision?
User | Revision | Line number | New 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 | 4:8ed071e5e3c9 | 13 | int n=0; |
JornJan | 0:5c4ee2c81f02 | 14 | |
JornJan | 0:5c4ee2c81f02 | 15 | //Benoemen van de variabelen die in de VOID's gebruikt gaan worden |
JornJan | 5:41d4aac93351 | 16 | double emga = emg0.read(); //EMG1 |
JornJan | 0:5c4ee2c81f02 | 17 | double emgb = emg1.read(); //EMG2 |
JornJan | 0:5c4ee2c81f02 | 18 | |
JornJan | 4:8ed071e5e3c9 | 19 | // coëfficienten notch filter |
JornJan | 5:41d4aac93351 | 20 | double an[3]={1.0000, -0.0000, 0.9490}; |
JornJan | 5:41d4aac93351 | 21 | double bn[3]={0.9745, -0.0000, 0.9745}; |
JornJan | 4:8ed071e5e3c9 | 22 | |
JornJan | 4:8ed071e5e3c9 | 23 | // innitial conditions notch filter |
JornJan | 4:8ed071e5e3c9 | 24 | double emg_nf[3]={0,0,0}; |
JornJan | 4:8ed071e5e3c9 | 25 | double emg_in[3]={0, 0, 0}; |
JornJan | 4:8ed071e5e3c9 | 26 | |
JornJan | 4:8ed071e5e3c9 | 27 | //Aanmaken filter coëfficienten |
JornJan | 5:41d4aac93351 | 28 | double ah[3]={1.0000, -1.9933, 0.9934}; |
JornJan | 5:41d4aac93351 | 29 | double bh[3]={0.9967, -1.9933, 0.9967}; |
JornJan | 0:5c4ee2c81f02 | 30 | |
JornJan | 0:5c4ee2c81f02 | 31 | //innitial conditions high pass filter |
arthurdelange | 2:7e0279519cbf | 32 | double emg_hpf[3]={0, 0, 0}; |
JornJan | 0:5c4ee2c81f02 | 33 | |
JornJan | 4:8ed071e5e3c9 | 34 | // coëfficienten low pass filter |
JornJan | 5:41d4aac93351 | 35 | double al[5]={1.0000, -3.9179, 5.7571, -3.7603, 0.9212}; |
JornJan | 5:41d4aac93351 | 36 | double bl[5]={0.0585e-6, 0.2338e-6, 0.3507e-6, 0.2338e-6, 0.0585e-6}; |
JornJan | 0:5c4ee2c81f02 | 37 | |
JornJan | 0:5c4ee2c81f02 | 38 | //innitial conditions low pass filter |
JornJan | 4:8ed071e5e3c9 | 39 | double emg_lpf[5]={0, 0, 0, 0, 0}; |
JornJan | 4:8ed071e5e3c9 | 40 | double emg_abs[5]={0, 0, 0, 0, 0}; |
arthurdelange | 2:7e0279519cbf | 41 | |
arthurdelange | 2:7e0279519cbf | 42 | double emg_lpfg; |
JornJan | 0:5c4ee2c81f02 | 43 | |
JornJan | 0:5c4ee2c81f02 | 44 | //Aanmaken van de verschillende tickers |
JornJan | 0:5c4ee2c81f02 | 45 | Ticker tick_sample; |
JornJan | 0:5c4ee2c81f02 | 46 | |
JornJan | 4:8ed071e5e3c9 | 47 | |
JornJan | 0:5c4ee2c81f02 | 48 | void aansturing() |
JornJan | 0:5c4ee2c81f02 | 49 | { |
JornJan | 4:8ed071e5e3c9 | 50 | timer.reset(); |
JornJan | 0:5c4ee2c81f02 | 51 | |
JornJan | 5:41d4aac93351 | 52 | emga=emg0.read(); |
JornJan | 5:41d4aac93351 | 53 | emg_in[0]=emga; |
JornJan | 0:5c4ee2c81f02 | 54 | |
JornJan | 4:8ed071e5e3c9 | 55 | //Filter cascade |
JornJan | 4:8ed071e5e3c9 | 56 | // notch filter |
JornJan | 4:8ed071e5e3c9 | 57 | 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]; |
JornJan | 4:8ed071e5e3c9 | 58 | |
JornJan | 0:5c4ee2c81f02 | 59 | // high pass filter |
JornJan | 4:8ed071e5e3c9 | 60 | 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]; |
JornJan | 0:5c4ee2c81f02 | 61 | |
JornJan | 0:5c4ee2c81f02 | 62 | //absolute value |
JornJan | 0:5c4ee2c81f02 | 63 | emg_abs[0]=fabs(emg_hpf[0]); |
JornJan | 4:8ed071e5e3c9 | 64 | |
JornJan | 0:5c4ee2c81f02 | 65 | //low pass filter |
JornJan | 4:8ed071e5e3c9 | 66 | 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]; |
JornJan | 0:5c4ee2c81f02 | 67 | |
JornJan | 4:8ed071e5e3c9 | 68 | // RAM |
JornJan | 4:8ed071e5e3c9 | 69 | emg_in[2]=emg_in[1]; |
JornJan | 4:8ed071e5e3c9 | 70 | emg_in[1]=emg_in[0]; |
JornJan | 4:8ed071e5e3c9 | 71 | |
JornJan | 4:8ed071e5e3c9 | 72 | emg_nf[2]=emg_nf[1]; |
JornJan | 4:8ed071e5e3c9 | 73 | emg_nf[1]=emg_nf[0]; |
JornJan | 4:8ed071e5e3c9 | 74 | |
JornJan | 4:8ed071e5e3c9 | 75 | emg_hpf[2]=emg_hpf[1]; |
JornJan | 4:8ed071e5e3c9 | 76 | emg_hpf[1]=emg_hpf[0]; |
JornJan | 4:8ed071e5e3c9 | 77 | |
JornJan | 4:8ed071e5e3c9 | 78 | emg_abs[4]=emg_abs[3]; |
JornJan | 4:8ed071e5e3c9 | 79 | emg_abs[3]=emg_abs[2]; |
JornJan | 0:5c4ee2c81f02 | 80 | emg_abs[2]=emg_abs[1]; |
JornJan | 0:5c4ee2c81f02 | 81 | emg_abs[1]=emg_abs[0]; |
JornJan | 4:8ed071e5e3c9 | 82 | |
JornJan | 4:8ed071e5e3c9 | 83 | emg_lpf[4]=emg_lpf[3]; |
JornJan | 4:8ed071e5e3c9 | 84 | emg_lpf[3]=emg_lpf[2]; |
JornJan | 0:5c4ee2c81f02 | 85 | emg_lpf[2]=emg_lpf[1]; |
JornJan | 0:5c4ee2c81f02 | 86 | emg_lpf[1]=emg_lpf[0]; |
JornJan | 4:8ed071e5e3c9 | 87 | |
JornJan | 4:8ed071e5e3c9 | 88 | |
JornJan | 4:8ed071e5e3c9 | 89 | |
JornJan | 4:8ed071e5e3c9 | 90 | |
JornJan | 0:5c4ee2c81f02 | 91 | |
JornJan | 4:8ed071e5e3c9 | 92 | if (emg_lpf[0]>0.01) |
JornJan | 0:5c4ee2c81f02 | 93 | { |
JornJan | 4:8ed071e5e3c9 | 94 | motor1MagnitudePin = emg_lpf[0]; |
JornJan | 0:5c4ee2c81f02 | 95 | motor1DirectionPin = 0; |
JornJan | 0:5c4ee2c81f02 | 96 | } |
JornJan | 0:5c4ee2c81f02 | 97 | else |
JornJan | 0:5c4ee2c81f02 | 98 | { |
JornJan | 0:5c4ee2c81f02 | 99 | motor1MagnitudePin = 0; |
JornJan | 0:5c4ee2c81f02 | 100 | motor1DirectionPin = 0; |
JornJan | 0:5c4ee2c81f02 | 101 | } |
JornJan | 5:41d4aac93351 | 102 | scope.set(0, emg_in[1]); |
JornJan | 5:41d4aac93351 | 103 | scope.set(1, emg_lpf[1]); |
JornJan | 0:5c4ee2c81f02 | 104 | scope.send(); |
JornJan | 4:8ed071e5e3c9 | 105 | |
JornJan | 4:8ed071e5e3c9 | 106 | if (n==100) |
JornJan | 4:8ed071e5e3c9 | 107 | { |
JornJan | 4:8ed071e5e3c9 | 108 | pc.printf("time taken was %d microseconds\n\r", timer.read_us()); |
JornJan | 4:8ed071e5e3c9 | 109 | n=0; |
JornJan | 4:8ed071e5e3c9 | 110 | } |
JornJan | 4:8ed071e5e3c9 | 111 | else |
JornJan | 4:8ed071e5e3c9 | 112 | { |
JornJan | 4:8ed071e5e3c9 | 113 | n=n+1; |
JornJan | 4:8ed071e5e3c9 | 114 | } |
JornJan | 0:5c4ee2c81f02 | 115 | |
JornJan | 4:8ed071e5e3c9 | 116 | } |
JornJan | 0:5c4ee2c81f02 | 117 | |
JornJan | 0:5c4ee2c81f02 | 118 | |
JornJan | 0:5c4ee2c81f02 | 119 | int main() |
JornJan | 0:5c4ee2c81f02 | 120 | { |
JornJan | 0:5c4ee2c81f02 | 121 | //Deze tickers roepen de verschillende voids aan |
arthurdelange | 2:7e0279519cbf | 122 | pc.baud(115200); |
JornJan | 4:8ed071e5e3c9 | 123 | timer.start(); |
JornJan | 0:5c4ee2c81f02 | 124 | tick_sample.attach_us(&aansturing, 5000); //Deze ticker roept de potmeter aan |
JornJan | 0:5c4ee2c81f02 | 125 | } |