checken van filter met hidscope
Dependencies: HIDScope mbed-dev mbed
main.cpp@0:5c4ee2c81f02, 2017-10-12 (annotated)
- Committer:
- JornJan
- Date:
- Thu Oct 12 10:19:08 2017 +0000
- Revision:
- 0:5c4ee2c81f02
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 |
JornJan | 0:5c4ee2c81f02 | 19 | float ah[3]={1, 0, 0.1716}; |
JornJan | 0:5c4ee2c81f02 | 20 | float bh[3]={0.2929, -0.5858, 0.2929}; |
JornJan | 0:5c4ee2c81f02 | 21 | |
JornJan | 0:5c4ee2c81f02 | 22 | //innitial conditions high pass filter |
JornJan | 0:5c4ee2c81f02 | 23 | float emg_hpf[3]={0, 0, 0}; |
JornJan | 0:5c4ee2c81f02 | 24 | float emg_in[3]={0, 0, 0}; |
JornJan | 0:5c4ee2c81f02 | 25 | |
JornJan | 0:5c4ee2c81f02 | 26 | // coëfficienten high pass filter |
JornJan | 0:5c4ee2c81f02 | 27 | float al[3]={1, -1.7347, 0.7660}; |
JornJan | 0:5c4ee2c81f02 | 28 | float bl[3]={0.0078, 0.0156, 0.0078}; |
JornJan | 0:5c4ee2c81f02 | 29 | |
JornJan | 0:5c4ee2c81f02 | 30 | //innitial conditions low pass filter |
JornJan | 0:5c4ee2c81f02 | 31 | float emg_lpf[3]={0, 0, 0}; |
JornJan | 0:5c4ee2c81f02 | 32 | float emg_abs[3]={0, 0, 0}; |
JornJan | 0:5c4ee2c81f02 | 33 | |
JornJan | 0:5c4ee2c81f02 | 34 | //Aanmaken van de verschillende tickers |
JornJan | 0:5c4ee2c81f02 | 35 | Ticker tick_sample; |
JornJan | 0:5c4ee2c81f02 | 36 | |
JornJan | 0:5c4ee2c81f02 | 37 | void aansturing() |
JornJan | 0:5c4ee2c81f02 | 38 | { |
JornJan | 0:5c4ee2c81f02 | 39 | timer.reset(); |
JornJan | 0:5c4ee2c81f02 | 40 | timer.start(); |
JornJan | 0:5c4ee2c81f02 | 41 | |
JornJan | 0:5c4ee2c81f02 | 42 | emga = emg0.read(); |
JornJan | 0:5c4ee2c81f02 | 43 | emgb = emg1.read(); |
JornJan | 0:5c4ee2c81f02 | 44 | emg_in[0]=emga; |
JornJan | 0:5c4ee2c81f02 | 45 | |
JornJan | 0:5c4ee2c81f02 | 46 | //Filter |
JornJan | 0:5c4ee2c81f02 | 47 | // high pass filter |
JornJan | 0:5c4ee2c81f02 | 48 | 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 | 49 | |
JornJan | 0:5c4ee2c81f02 | 50 | emg_in[2]=emg_in[1]; |
JornJan | 0:5c4ee2c81f02 | 51 | emg_in[1]=emg_in[0]; |
JornJan | 0:5c4ee2c81f02 | 52 | emg_hpf[2]=emg_hpf[1]; |
JornJan | 0:5c4ee2c81f02 | 53 | emg_hpf[2]=emg_hpf[0]; |
JornJan | 0:5c4ee2c81f02 | 54 | |
JornJan | 0:5c4ee2c81f02 | 55 | |
JornJan | 0:5c4ee2c81f02 | 56 | //absolute value |
JornJan | 0:5c4ee2c81f02 | 57 | emg_abs[0]=fabs(emg_hpf[0]); |
JornJan | 0:5c4ee2c81f02 | 58 | |
JornJan | 0:5c4ee2c81f02 | 59 | |
JornJan | 0:5c4ee2c81f02 | 60 | //low pass filter |
JornJan | 0:5c4ee2c81f02 | 61 | 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 | 62 | |
JornJan | 0:5c4ee2c81f02 | 63 | emg_abs[2]=emg_abs[1]; |
JornJan | 0:5c4ee2c81f02 | 64 | emg_abs[1]=emg_abs[0]; |
JornJan | 0:5c4ee2c81f02 | 65 | emg_lpf[2]=emg_lpf[1]; |
JornJan | 0:5c4ee2c81f02 | 66 | emg_lpf[1]=emg_lpf[0]; |
JornJan | 0:5c4ee2c81f02 | 67 | |
JornJan | 0:5c4ee2c81f02 | 68 | if (emg_lpf[1]>0.05) |
JornJan | 0:5c4ee2c81f02 | 69 | { |
JornJan | 0:5c4ee2c81f02 | 70 | motor1MagnitudePin = emg_lpf[1]; |
JornJan | 0:5c4ee2c81f02 | 71 | motor1DirectionPin = 0; |
JornJan | 0:5c4ee2c81f02 | 72 | } |
JornJan | 0:5c4ee2c81f02 | 73 | else |
JornJan | 0:5c4ee2c81f02 | 74 | { |
JornJan | 0:5c4ee2c81f02 | 75 | motor1MagnitudePin = 0; |
JornJan | 0:5c4ee2c81f02 | 76 | motor1DirectionPin = 0; |
JornJan | 0:5c4ee2c81f02 | 77 | } |
JornJan | 0:5c4ee2c81f02 | 78 | scope.set(0, emg0.read()); |
JornJan | 0:5c4ee2c81f02 | 79 | scope.send(); |
JornJan | 0:5c4ee2c81f02 | 80 | scope.set(1, emg_lpf[1]); |
JornJan | 0:5c4ee2c81f02 | 81 | scope.send(); |
JornJan | 0:5c4ee2c81f02 | 82 | |
JornJan | 0:5c4ee2c81f02 | 83 | timer.stop(); |
JornJan | 0:5c4ee2c81f02 | 84 | pc.printf("time taken was %f milliseconds\n\r", timer.read_ms()); |
JornJan | 0:5c4ee2c81f02 | 85 | |
JornJan | 0:5c4ee2c81f02 | 86 | } |
JornJan | 0:5c4ee2c81f02 | 87 | |
JornJan | 0:5c4ee2c81f02 | 88 | |
JornJan | 0:5c4ee2c81f02 | 89 | int main() |
JornJan | 0:5c4ee2c81f02 | 90 | { |
JornJan | 0:5c4ee2c81f02 | 91 | //Deze tickers roepen de verschillende voids aan |
JornJan | 0:5c4ee2c81f02 | 92 | pc.baud(9600); |
JornJan | 0:5c4ee2c81f02 | 93 | tick_sample.attach_us(&aansturing, 5000); //Deze ticker roept de potmeter aan |
JornJan | 0:5c4ee2c81f02 | 94 | } |