Feike van Veen / Mbed 2 deprecated EMG

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?

UserRevisionLine numberNew 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 }