Alle drie de signalen gefilterd en binair gemaakt
Dependencies: mbed HIDScope biquadFilter
main.cpp@21:f6e70856810c, 2019-10-29 (annotated)
- Committer:
- Feike
- Date:
- Tue Oct 29 14:48:30 2019 +0000
- Revision:
- 21:f6e70856810c
- Parent:
- 20:97059009a491
- Child:
- 22:611667172ac3
Werkend filter met Butterworth en mean averaging
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
vsluiter | 0:32bb76391d89 | 1 | #include "mbed.h" |
vsluiter | 11:ce72ec658a95 | 2 | #include "HIDScope.h" |
Feike | 21:f6e70856810c | 3 | #include "MAF.h" |
Feike | 21:f6e70856810c | 4 | #include "vector" |
Feike | 21:f6e70856810c | 5 | #include "numeric" |
Feike | 21:f6e70856810c | 6 | #include "BiQuad.h" |
vsluiter | 0:32bb76391d89 | 7 | |
vsluiter | 4:8b298dfada81 | 8 | //Define objects |
tomlankhorst | 19:2bf824669684 | 9 | AnalogIn emg0( A0 ); |
tomlankhorst | 19:2bf824669684 | 10 | AnalogIn emg1( A1 ); |
Feike | 21:f6e70856810c | 11 | //AnalogIn emg2( A2 ); |
Feike | 21:f6e70856810c | 12 | //AnalogIn emg3( A3 ); |
Feike | 21:f6e70856810c | 13 | float A; |
Feike | 21:f6e70856810c | 14 | float B; |
tomlankhorst | 14:f83354387756 | 15 | Ticker sample_timer; |
Feike | 21:f6e70856810c | 16 | HIDScope scope( 3 ); |
tomlankhorst | 18:21d8e7a81cf5 | 17 | DigitalOut led(LED1); |
vsluiter | 2:e314bb3b2d99 | 18 | |
Feike | 21:f6e70856810c | 19 | const int leng_filt = 10; |
Feike | 21:f6e70856810c | 20 | float A_array[leng_filt] = {0}; |
Feike | 21:f6e70856810c | 21 | float B_array[leng_filt] = {0}; |
Feike | 21:f6e70856810c | 22 | float Asum_ar[leng_filt] = {0}; |
Feike | 21:f6e70856810c | 23 | float Bsum_ar[leng_filt] = {0}; |
Feike | 21:f6e70856810c | 24 | float result = 0; |
Feike | 21:f6e70856810c | 25 | float Asum = 0; |
Feike | 21:f6e70856810c | 26 | float Bsum = 0; |
Feike | 21:f6e70856810c | 27 | |
Feike | 21:f6e70856810c | 28 | |
tomlankhorst | 14:f83354387756 | 29 | void sample() |
Feike | 21:f6e70856810c | 30 | { |
Feike | 21:f6e70856810c | 31 | |
Feike | 21:f6e70856810c | 32 | float A = emg0.read(); |
Feike | 21:f6e70856810c | 33 | float B = emg1.read(); |
Feike | 21:f6e70856810c | 34 | |
Feike | 21:f6e70856810c | 35 | for (int j=leng_filt-1; j>=1; j--) |
Feike | 21:f6e70856810c | 36 | { |
Feike | 21:f6e70856810c | 37 | A_array[j] = A_array[j-1]; |
Feike | 21:f6e70856810c | 38 | B_array[j] = B_array[j-1]; |
Feike | 21:f6e70856810c | 39 | } |
Feike | 21:f6e70856810c | 40 | |
Feike | 21:f6e70856810c | 41 | A_array[0] = A; |
Feike | 21:f6e70856810c | 42 | B_array[0] = B; |
Feike | 21:f6e70856810c | 43 | |
Feike | 21:f6e70856810c | 44 | Asum = 0; |
Feike | 21:f6e70856810c | 45 | Bsum = 0; |
Feike | 21:f6e70856810c | 46 | |
Feike | 21:f6e70856810c | 47 | for(int i=0; i<=leng_filt-1; i++) |
Feike | 21:f6e70856810c | 48 | { |
Feike | 21:f6e70856810c | 49 | Asum += A_array[i]*1/leng_filt; |
Feike | 21:f6e70856810c | 50 | Bsum += B_array[i]*1/leng_filt; |
Feike | 21:f6e70856810c | 51 | } |
Feike | 21:f6e70856810c | 52 | |
Feike | 21:f6e70856810c | 53 | |
Feike | 21:f6e70856810c | 54 | // Vanaf hier begint het butterworth laagdoorlaatfilter |
Feike | 21:f6e70856810c | 55 | |
Feike | 21:f6e70856810c | 56 | const int Fs = 2000; //Sample Frequency |
Feike | 21:f6e70856810c | 57 | const double b0 = 0.097631; |
Feike | 21:f6e70856810c | 58 | const double b1 = 0.195262; |
Feike | 21:f6e70856810c | 59 | const double b2 = 0.097631; |
Feike | 21:f6e70856810c | 60 | const double a0 = 1.000000; |
Feike | 21:f6e70856810c | 61 | const double a1 = -0.942809; |
Feike | 21:f6e70856810c | 62 | const double a2 = 0.333333; |
Feike | 21:f6e70856810c | 63 | |
Feike | 21:f6e70856810c | 64 | BiQuad lowpass(b0,b1, b2, a0, a1, a2); |
Feike | 21:f6e70856810c | 65 | |
Feike | 21:f6e70856810c | 66 | // voor bepaald ingangssignaal u1 en output y1 |
Feike | 21:f6e70856810c | 67 | double u1 = Asum; |
Feike | 21:f6e70856810c | 68 | double y1; |
Feike | 21:f6e70856810c | 69 | |
Feike | 21:f6e70856810c | 70 | float Amean = 0; |
Feike | 21:f6e70856810c | 71 | float Bmean = 0; |
Feike | 21:f6e70856810c | 72 | |
Feike | 21:f6e70856810c | 73 | for (int j=leng_filt-1; j>=1; j--) |
Feike | 21:f6e70856810c | 74 | { |
Feike | 21:f6e70856810c | 75 | Asum_ar[j] = Asum_ar[j-1]; |
Feike | 21:f6e70856810c | 76 | Bsum_ar[j] = Bsum_ar[j-1]; |
Feike | 21:f6e70856810c | 77 | } |
Feike | 21:f6e70856810c | 78 | |
Feike | 21:f6e70856810c | 79 | Asum_ar[0] = Asum; |
Feike | 21:f6e70856810c | 80 | Bsum_ar[0] = Bsum; |
Feike | 21:f6e70856810c | 81 | |
Feike | 21:f6e70856810c | 82 | for(int i=0; i<=leng_filt-1; i++) |
Feike | 21:f6e70856810c | 83 | { |
Feike | 21:f6e70856810c | 84 | Amean += Asum_ar[i]*1/leng_filt; |
Feike | 21:f6e70856810c | 85 | Bmean += Bsum_ar[i]*1/leng_filt; |
Feike | 21:f6e70856810c | 86 | } |
Feike | 21:f6e70856810c | 87 | |
Feike | 21:f6e70856810c | 88 | y1 = u1 - Amean; //offset? |
Feike | 21:f6e70856810c | 89 | y1 = fabs(y1); |
Feike | 21:f6e70856810c | 90 | y1 = lowpass.step(y1); |
Feike | 21:f6e70856810c | 91 | |
tomlankhorst | 19:2bf824669684 | 92 | /* Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope' */ |
Feike | 21:f6e70856810c | 93 | |
Feike | 21:f6e70856810c | 94 | scope.set(0, emg0.read()); |
Feike | 21:f6e70856810c | 95 | scope.set(1, Asum); |
Feike | 21:f6e70856810c | 96 | scope.set(2, y1); |
Feike | 21:f6e70856810c | 97 | |
tomlankhorst | 19:2bf824669684 | 98 | /* Repeat the step above if required for more channels of required (channel 0 up to 5 = 6 channels) |
tomlankhorst | 19:2bf824669684 | 99 | * Ensure that enough channels are available (HIDScope scope( 2 )) |
tomlankhorst | 20:97059009a491 | 100 | * Finally, send all channels to the PC at once */ |
vsluiter | 11:ce72ec658a95 | 101 | scope.send(); |
tomlankhorst | 18:21d8e7a81cf5 | 102 | /* To indicate that the function is working, the LED is toggled */ |
tomlankhorst | 18:21d8e7a81cf5 | 103 | led = !led; |
vsluiter | 2:e314bb3b2d99 | 104 | } |
vsluiter | 0:32bb76391d89 | 105 | |
vsluiter | 0:32bb76391d89 | 106 | int main() |
tomlankhorst | 19:2bf824669684 | 107 | { |
tomlankhorst | 14:f83354387756 | 108 | /**Attach the 'sample' function to the timer 'sample_timer'. |
tomlankhorst | 19:2bf824669684 | 109 | * this ensures that 'sample' is executed every... 0.002 seconds = 500 Hz |
vsluiter | 4:8b298dfada81 | 110 | */ |
tomlankhorst | 19:2bf824669684 | 111 | sample_timer.attach(&sample, 0.002); |
tomlankhorst | 15:0da764eea774 | 112 | |
tomlankhorst | 14:f83354387756 | 113 | /*empty loop, sample() is executed periodically*/ |
tomlankhorst | 15:0da764eea774 | 114 | while(1) {} |
vsluiter | 0:32bb76391d89 | 115 | } |