Feike van Veen / Mbed 2 deprecated EMG

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?

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