Alle drie de signalen gefilterd en binair gemaakt
Dependencies: mbed HIDScope biquadFilter
main.cpp@22:611667172ac3, 2019-10-30 (annotated)
- Committer:
- Feike
- Date:
- Wed Oct 30 08:28:29 2019 +0000
- Revision:
- 22:611667172ac3
- Parent:
- 21:f6e70856810c
- Child:
- 23:a6f18aee31cd
5 signalen, allemaal werkend
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 | 22:611667172ac3 | 16 | HIDScope scope( 5 ); |
tomlankhorst | 18:21d8e7a81cf5 | 17 | DigitalOut led(LED1); |
vsluiter | 2:e314bb3b2d99 | 18 | |
Feike | 22:611667172ac3 | 19 | const int leng_filt = 20; |
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 | 22:611667172ac3 | 24 | float Asum_ar2[leng_filt] = {0}; |
Feike | 22:611667172ac3 | 25 | float Bsum_ar2[leng_filt] = {0}; |
Feike | 21:f6e70856810c | 26 | float result = 0; |
Feike | 21:f6e70856810c | 27 | float Asum = 0; |
Feike | 21:f6e70856810c | 28 | float Bsum = 0; |
Feike | 21:f6e70856810c | 29 | |
Feike | 21:f6e70856810c | 30 | |
tomlankhorst | 14:f83354387756 | 31 | void sample() |
Feike | 21:f6e70856810c | 32 | { |
Feike | 21:f6e70856810c | 33 | |
Feike | 21:f6e70856810c | 34 | float A = emg0.read(); |
Feike | 21:f6e70856810c | 35 | |
Feike | 21:f6e70856810c | 36 | for (int j=leng_filt-1; j>=1; j--) |
Feike | 21:f6e70856810c | 37 | { |
Feike | 21:f6e70856810c | 38 | A_array[j] = A_array[j-1]; |
Feike | 21:f6e70856810c | 39 | } |
Feike | 21:f6e70856810c | 40 | |
Feike | 21:f6e70856810c | 41 | A_array[0] = A; |
Feike | 21:f6e70856810c | 42 | Asum = 0; |
Feike | 21:f6e70856810c | 43 | |
Feike | 21:f6e70856810c | 44 | for(int i=0; i<=leng_filt-1; i++) |
Feike | 21:f6e70856810c | 45 | { |
Feike | 21:f6e70856810c | 46 | Asum += A_array[i]*1/leng_filt; |
Feike | 21:f6e70856810c | 47 | } |
Feike | 21:f6e70856810c | 48 | |
Feike | 21:f6e70856810c | 49 | // Vanaf hier begint het butterworth laagdoorlaatfilter |
Feike | 21:f6e70856810c | 50 | |
Feike | 21:f6e70856810c | 51 | const int Fs = 2000; //Sample Frequency |
Feike | 22:611667172ac3 | 52 | const double b0 = 0.292893; |
Feike | 22:611667172ac3 | 53 | const double b1 = 0.585786; |
Feike | 22:611667172ac3 | 54 | const double b2 = 0.292893; |
Feike | 21:f6e70856810c | 55 | const double a0 = 1.000000; |
Feike | 22:611667172ac3 | 56 | const double a1 = -0; |
Feike | 22:611667172ac3 | 57 | const double a2 = 0.171573; |
Feike | 21:f6e70856810c | 58 | |
Feike | 21:f6e70856810c | 59 | BiQuad lowpass(b0,b1, b2, a0, a1, a2); |
Feike | 21:f6e70856810c | 60 | |
Feike | 21:f6e70856810c | 61 | // voor bepaald ingangssignaal u1 en output y1 |
Feike | 22:611667172ac3 | 62 | // Eerst mean dan Buttterworth |
Feike | 22:611667172ac3 | 63 | |
Feike | 21:f6e70856810c | 64 | double u1 = Asum; |
Feike | 21:f6e70856810c | 65 | double y1; |
Feike | 21:f6e70856810c | 66 | |
Feike | 21:f6e70856810c | 67 | float Amean = 0; |
Feike | 22:611667172ac3 | 68 | float Amean2 = 0; |
Feike | 22:611667172ac3 | 69 | |
Feike | 21:f6e70856810c | 70 | for (int j=leng_filt-1; j>=1; j--) |
Feike | 21:f6e70856810c | 71 | { |
Feike | 21:f6e70856810c | 72 | Asum_ar[j] = Asum_ar[j-1]; |
Feike | 21:f6e70856810c | 73 | } |
Feike | 22:611667172ac3 | 74 | |
Feike | 21:f6e70856810c | 75 | Asum_ar[0] = Asum; |
Feike | 21:f6e70856810c | 76 | |
Feike | 21:f6e70856810c | 77 | for(int i=0; i<=leng_filt-1; i++) |
Feike | 21:f6e70856810c | 78 | { |
Feike | 21:f6e70856810c | 79 | Amean += Asum_ar[i]*1/leng_filt; |
Feike | 21:f6e70856810c | 80 | } |
Feike | 21:f6e70856810c | 81 | |
Feike | 21:f6e70856810c | 82 | y1 = u1 - Amean; //offset? |
Feike | 21:f6e70856810c | 83 | y1 = fabs(y1); |
Feike | 21:f6e70856810c | 84 | y1 = lowpass.step(y1); |
Feike | 22:611667172ac3 | 85 | |
Feike | 22:611667172ac3 | 86 | // Eerst butterworth dan mean |
Feike | 22:611667172ac3 | 87 | float y2; |
Feike | 22:611667172ac3 | 88 | y2 = A - Asum; |
Feike | 22:611667172ac3 | 89 | y2 = fabs(y2); |
Feike | 22:611667172ac3 | 90 | y2 = lowpass.step(y2); |
Feike | 22:611667172ac3 | 91 | |
Feike | 22:611667172ac3 | 92 | for (int j=leng_filt-1; j>=1; j--) |
Feike | 22:611667172ac3 | 93 | { |
Feike | 22:611667172ac3 | 94 | Asum_ar2[j] = Asum_ar2[j-1]; |
Feike | 22:611667172ac3 | 95 | } |
Feike | 22:611667172ac3 | 96 | |
Feike | 22:611667172ac3 | 97 | Asum_ar2[0] = y2; |
Feike | 22:611667172ac3 | 98 | |
Feike | 22:611667172ac3 | 99 | for(int i=0; i<=leng_filt-1; i++) |
Feike | 22:611667172ac3 | 100 | { |
Feike | 22:611667172ac3 | 101 | Amean2 += Asum_ar2[i]*1/leng_filt; |
Feike | 22:611667172ac3 | 102 | } |
Feike | 22:611667172ac3 | 103 | |
Feike | 22:611667172ac3 | 104 | |
tomlankhorst | 19:2bf824669684 | 105 | /* 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 | 106 | |
Feike | 21:f6e70856810c | 107 | scope.set(0, emg0.read()); |
Feike | 21:f6e70856810c | 108 | scope.set(1, Asum); |
Feike | 21:f6e70856810c | 109 | scope.set(2, y1); |
Feike | 22:611667172ac3 | 110 | scope.set(3, y2); |
Feike | 22:611667172ac3 | 111 | scope.set(4, Amean2); |
Feike | 21:f6e70856810c | 112 | |
tomlankhorst | 19:2bf824669684 | 113 | /* Repeat the step above if required for more channels of required (channel 0 up to 5 = 6 channels) |
tomlankhorst | 19:2bf824669684 | 114 | * Ensure that enough channels are available (HIDScope scope( 2 )) |
tomlankhorst | 20:97059009a491 | 115 | * Finally, send all channels to the PC at once */ |
vsluiter | 11:ce72ec658a95 | 116 | scope.send(); |
tomlankhorst | 18:21d8e7a81cf5 | 117 | /* To indicate that the function is working, the LED is toggled */ |
tomlankhorst | 18:21d8e7a81cf5 | 118 | led = !led; |
vsluiter | 2:e314bb3b2d99 | 119 | } |
vsluiter | 0:32bb76391d89 | 120 | |
vsluiter | 0:32bb76391d89 | 121 | int main() |
tomlankhorst | 19:2bf824669684 | 122 | { |
tomlankhorst | 14:f83354387756 | 123 | /**Attach the 'sample' function to the timer 'sample_timer'. |
tomlankhorst | 19:2bf824669684 | 124 | * this ensures that 'sample' is executed every... 0.002 seconds = 500 Hz |
vsluiter | 4:8b298dfada81 | 125 | */ |
tomlankhorst | 19:2bf824669684 | 126 | sample_timer.attach(&sample, 0.002); |
tomlankhorst | 15:0da764eea774 | 127 | |
tomlankhorst | 14:f83354387756 | 128 | /*empty loop, sample() is executed periodically*/ |
tomlankhorst | 15:0da764eea774 | 129 | while(1) {} |
vsluiter | 0:32bb76391d89 | 130 | } |