Arthur de Lange
/
Filter_check
12-okt
Fork of Filter_check by
main.cpp@2:7e0279519cbf, 2017-10-12 (annotated)
- Committer:
- arthurdelange
- Date:
- Thu Oct 12 13:11:30 2017 +0000
- Revision:
- 2:7e0279519cbf
- Parent:
- 0:5c4ee2c81f02
- Child:
- 3:3e5d899a3c8a
Filter_werkzaam_12_okt;
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 | 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 | 2:7e0279519cbf | 86 | pc.printf("time taken was %f 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 | } |