Feike van Veen / Mbed 2 deprecated EMG

Alle drie de signalen gefilterd en binair gemaakt

Dependencies:   mbed HIDScope biquadFilter

main.cpp@26:1eb3c3b04b80, 2019-11-01 (annotated)

Committer:
Feike
Date:
Fri Nov 01 08:26:26 2019 +0000
Revision:
26:1eb3c3b04b80
Parent:
25:cfd422c3cf4d 
af

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
Feike 25:cfd422c3cf4d 8
vsluiter 4:8b298dfada81 9 //Define objects
Feike 25:cfd422c3cf4d 10 AnalogIn emg0( A0 ); // 1e
Feike 25:cfd422c3cf4d 11 AnalogIn emg2( A2 ); // 2e
Feike 25:cfd422c3cf4d 12 AnalogIn emg4( A4 ); // 3e
Feike 25:cfd422c3cf4d 13
Feike 21:f6e70856810c 14 float A;
Feike 21:f6e70856810c 15 float B;
tomlankhorst 14:f83354387756 16 Ticker sample_timer;
Feike 25:cfd422c3cf4d 17 HIDScope scope( 3 );
tomlankhorst 18:21d8e7a81cf5 18 DigitalOut led(LED1);
vsluiter 2:e314bb3b2d99 19
Feike 23:a6f18aee31cd 20 const int leng_filt = 60;
Feike 25:cfd422c3cf4d 21 const int box_length = 50;
Feike 25:cfd422c3cf4d 22 const int box_lengthC = 150;
Feike 25:cfd422c3cf4d 23 const int box_checkC = 50;
Feike 26:1eb3c3b04b80 24 const float grenswaardeA0 = 0.015;
Feike 26:1eb3c3b04b80 25 const float grenswaardeB0 = 0.014;
Feike 26:1eb3c3b04b80 26 const float grenswaardeC = 0.016;
Feike 23:a6f18aee31cd 27 float Ay1;
Feike 23:a6f18aee31cd 28 float Ay2;
Feike 21:f6e70856810c 29 float A_array[leng_filt] = {0};
Feike 23:a6f18aee31cd 30 float A_ar[leng_filt] = {0};
Feike 23:a6f18aee31cd 31 float A_ar2[leng_filt] = {0};
Feike 23:a6f18aee31cd 32 float A_ar3[box_length] = {0};
Feike 23:a6f18aee31cd 33
Feike 23:a6f18aee31cd 34 float By1;
Feike 23:a6f18aee31cd 35 float By2;
Feike 21:f6e70856810c 36 float B_array[leng_filt] = {0};
Feike 23:a6f18aee31cd 37 float B_ar[leng_filt] = {0};
Feike 23:a6f18aee31cd 38 float B_ar2[leng_filt] = {0};
Feike 23:a6f18aee31cd 39 float B_ar3[box_length] = {0};
Feike 23:a6f18aee31cd 40
Feike 23:a6f18aee31cd 41 float Cy1;
Feike 23:a6f18aee31cd 42 float Cy2;
Feike 23:a6f18aee31cd 43 float C_array[leng_filt] = {0};
Feike 23:a6f18aee31cd 44 float C_ar[leng_filt] = {0};
Feike 23:a6f18aee31cd 45 float C_ar2[leng_filt] = {0};
Feike 25:cfd422c3cf4d 46 float C_ar3[box_lengthC] = {0};
Feike 23:a6f18aee31cd 47
Feike 21:f6e70856810c 48 float result = 0;
Feike 21:f6e70856810c 49 float Asum = 0;
Feike 23:a6f18aee31cd 50 const int Fs = 2000; //Sample Frequency
Feike 25:cfd422c3cf4d 51 const double b0 = 0.292893;
Feike 25:cfd422c3cf4d 52 const double b1 = 0.585786;
Feike 25:cfd422c3cf4d 53 const double b2 = 0.292893;
Feike 23:a6f18aee31cd 54 const double a0 = 1.000000;
Feike 25:cfd422c3cf4d 55 const double a1 = -0.00000;
Feike 25:cfd422c3cf4d 56 const double a2 = 0.171573;
Feike 21:f6e70856810c 57
tomlankhorst 14:f83354387756 58 void sample()
Feike 21:f6e70856810c 59 {
Feike 25:cfd422c3cf4d 60 BiQuad lowpassA(b0,b1, b2, a0, a1, a2);
Feike 23:a6f18aee31cd 61 // Signaal 1 (A)
Feike 21:f6e70856810c 62 float A = emg0.read();
Feike 23:a6f18aee31cd 63 float Amean = 0;
Feike 23:a6f18aee31cd 64 float Ay2 = 0;
Feike 21:f6e70856810c 65
Feike 21:f6e70856810c 66 for (int j=leng_filt-1; j>=1; j--)
Feike 23:a6f18aee31cd 67 { A_ar[j] = A_ar[j-1]; }
Feike 23:a6f18aee31cd 68
Feike 23:a6f18aee31cd 69 A_ar[0] = A;
Feike 23:a6f18aee31cd 70
Feike 23:a6f18aee31cd 71 for(int i=0; i<=leng_filt-1; i++)
Feike 23:a6f18aee31cd 72 { Amean += A_ar[i]*1/leng_filt; }
Feike 23:a6f18aee31cd 73
Feike 23:a6f18aee31cd 74 Ay1 = A - Amean;
Feike 23:a6f18aee31cd 75 Ay1 = fabs(Ay1);
Feike 25:cfd422c3cf4d 76 Ay1 = lowpassA.step(Ay1); // First signal, after Butterworth
Feike 23:a6f18aee31cd 77
Feike 23:a6f18aee31cd 78 for (int j=leng_filt-1; j>=1; j--)
Feike 23:a6f18aee31cd 79 { A_ar2[j] = A_ar2[j-1]; }
Feike 23:a6f18aee31cd 80
Feike 23:a6f18aee31cd 81 A_ar2[0] = Ay1;
Feike 23:a6f18aee31cd 82
Feike 23:a6f18aee31cd 83 for(int i=0; i<=leng_filt-1; i++)
Feike 23:a6f18aee31cd 84 { Ay2 += A_ar2[i]*1/leng_filt; }
Feike 23:a6f18aee31cd 85
Feike 23:a6f18aee31cd 86 float Ay3;
Feike 25:cfd422c3cf4d 87 if(Ay2>grenswaardeA0)
Feike 23:a6f18aee31cd 88 { Ay3 = 1; }
Feike 23:a6f18aee31cd 89
Feike 25:cfd422c3cf4d 90 //if(Ay2<=grenswaardeA0)
Feike 25:cfd422c3cf4d 91 //{ if(Ay2>grenswaardeA1)
Feike 25:cfd422c3cf4d 92 // { Ay3 = 0.5; } }
Feike 25:cfd422c3cf4d 93
Feike 25:cfd422c3cf4d 94 if(Ay2<=grenswaardeA0)
Feike 23:a6f18aee31cd 95 { Ay3 = 0; }
Feike 23:a6f18aee31cd 96
Feike 23:a6f18aee31cd 97 for (int j=box_length-1; j>=1; j--)
Feike 23:a6f18aee31cd 98 { A_ar3[j] = A_ar3[j-1]; }
Feike 21:f6e70856810c 99
Feike 23:a6f18aee31cd 100 A_ar3[0] = Ay3;
Feike 23:a6f18aee31cd 101 int boxcheckA = 0;
Feike 23:a6f18aee31cd 102
Feike 23:a6f18aee31cd 103 for (int j=0; j<=box_length-1; j++)
Feike 23:a6f18aee31cd 104 { if(A_ar3[j] == 1)
Feike 23:a6f18aee31cd 105 { boxcheckA = 1;} }
Feike 23:a6f18aee31cd 106
Feike 25:cfd422c3cf4d 107 // Signaal 2 (B)
Feike 25:cfd422c3cf4d 108 BiQuad lowpassB(b0,b1, b2, a0, a1, a2);
Feike 25:cfd422c3cf4d 109 float B = emg2.read();
Feike 23:a6f18aee31cd 110 float Bmean = 0;
Feike 23:a6f18aee31cd 111 float By2 = 0;
Feike 23:a6f18aee31cd 112
Feike 23:a6f18aee31cd 113 for (int j=leng_filt-1; j>=1; j--)
Feike 23:a6f18aee31cd 114 { B_ar[j] = B_ar[j-1]; }
Feike 23:a6f18aee31cd 115
Feike 23:a6f18aee31cd 116 B_ar[0] = B;
Feike 23:a6f18aee31cd 117
Feike 23:a6f18aee31cd 118 for(int i=0; i<=leng_filt-1; i++)
Feike 23:a6f18aee31cd 119 { Bmean += B_ar[i]*1/leng_filt; }
Feike 23:a6f18aee31cd 120
Feike 23:a6f18aee31cd 121 By1 = B - Bmean;
Feike 23:a6f18aee31cd 122 By1 = fabs(By1);
Feike 25:cfd422c3cf4d 123 By1 = lowpassB.step(By1); // First signal, after Butterworth
Feike 23:a6f18aee31cd 124
Feike 23:a6f18aee31cd 125 for (int j=leng_filt-1; j>=1; j--)
Feike 23:a6f18aee31cd 126 { B_ar2[j] = B_ar2[j-1]; }
Feike 23:a6f18aee31cd 127
Feike 23:a6f18aee31cd 128 B_ar2[0] = By1;
Feike 21:f6e70856810c 129
Feike 21:f6e70856810c 130 for(int i=0; i<=leng_filt-1; i++)
Feike 23:a6f18aee31cd 131 { By2 += B_ar2[i]*1/leng_filt; }
Feike 23:a6f18aee31cd 132
Feike 23:a6f18aee31cd 133 float By3;
Feike 25:cfd422c3cf4d 134 if(By2>grenswaardeB0)
Feike 23:a6f18aee31cd 135 { By3 = 1; }
Feike 21:f6e70856810c 136
Feike 25:cfd422c3cf4d 137 if(By2<=grenswaardeB0)
Feike 23:a6f18aee31cd 138 { By3 = 0; }
Feike 23:a6f18aee31cd 139
Feike 23:a6f18aee31cd 140 for (int j=box_length-1; j>=1; j--)
Feike 23:a6f18aee31cd 141 { B_ar3[j] = B_ar3[j-1]; }
Feike 21:f6e70856810c 142
Feike 23:a6f18aee31cd 143 B_ar3[0] = By3;
Feike 23:a6f18aee31cd 144 int boxcheckB = 0;
Feike 23:a6f18aee31cd 145
Feike 23:a6f18aee31cd 146 for (int j=0; j<=box_length-1; j++)
Feike 23:a6f18aee31cd 147 { if(B_ar3[j] == 1)
Feike 26:1eb3c3b04b80 148 { boxcheckB = 1;} }
Feike 25:cfd422c3cf4d 149
Feike 25:cfd422c3cf4d 150 // Signaal 3 (C)
Feike 25:cfd422c3cf4d 151 BiQuad lowpassC(b0,b1, b2, a0, a1, a2);
Feike 25:cfd422c3cf4d 152 float C = emg4.read();
Feike 23:a6f18aee31cd 153 float Cmean = 0;
Feike 23:a6f18aee31cd 154 float Cy2 = 0;
Feike 23:a6f18aee31cd 155
Feike 23:a6f18aee31cd 156 for (int j=leng_filt-1; j>=1; j--)
Feike 23:a6f18aee31cd 157 { C_ar[j] = C_ar[j-1]; }
Feike 21:f6e70856810c 158
Feike 23:a6f18aee31cd 159 C_ar[0] = C;
Feike 21:f6e70856810c 160
Feike 21:f6e70856810c 161 for(int i=0; i<=leng_filt-1; i++)
Feike 23:a6f18aee31cd 162 { Cmean += C_ar[i]*1/leng_filt; }
Feike 22:611667172ac3 163
Feike 23:a6f18aee31cd 164 Cy1 = C - Cmean;
Feike 23:a6f18aee31cd 165 Cy1 = fabs(Cy1);
Feike 25:cfd422c3cf4d 166 Cy1 = lowpassC.step(Cy1); // First signal, after Butterworth
Feike 25:cfd422c3cf4d 167
Feike 23:a6f18aee31cd 168 for (int j=leng_filt-1; j>=1; j--)
Feike 23:a6f18aee31cd 169 { C_ar2[j] = C_ar2[j-1]; }
Feike 22:611667172ac3 170
Feike 23:a6f18aee31cd 171 C_ar2[0] = Cy1;
Feike 22:611667172ac3 172
Feike 22:611667172ac3 173 for(int i=0; i<=leng_filt-1; i++)
Feike 25:cfd422c3cf4d 174 { Cy2 += C_ar2[i]*1/leng_filt; }
Feike 23:a6f18aee31cd 175
Feike 23:a6f18aee31cd 176 float Cy3;
Feike 23:a6f18aee31cd 177 if(Cy2>grenswaardeC)
Feike 23:a6f18aee31cd 178 { Cy3 = 1; }
Feike 23:a6f18aee31cd 179
Feike 23:a6f18aee31cd 180 if(Cy2<=grenswaardeC)
Feike 23:a6f18aee31cd 181 { Cy3 = 0; }
Feike 22:611667172ac3 182
Feike 23:a6f18aee31cd 183 for (int j=box_lengthC-1; j>=1; j--)
Feike 23:a6f18aee31cd 184 { C_ar3[j] = C_ar3[j-1]; }
Feike 21:f6e70856810c 185
Feike 23:a6f18aee31cd 186 C_ar3[0] = Cy3;
Feike 23:a6f18aee31cd 187 int boxcheckC = 0;
Feike 23:a6f18aee31cd 188 int C_sum = 0;
Feike 23:a6f18aee31cd 189 for (int j=0; j<=box_length-1; j++)
Feike 23:a6f18aee31cd 190 { C_sum += C_ar3[j];
Feike 23:a6f18aee31cd 191 if(C_sum >= box_checkC)
Feike 25:cfd422c3cf4d 192 { boxcheckC = 1;}
Feike 25:cfd422c3cf4d 193 }
Feike 25:cfd422c3cf4d 194
Feike 25:cfd422c3cf4d 195 scope.set(0, boxcheckA);
Feike 25:cfd422c3cf4d 196 scope.set(1, boxcheckB);
Feike 25:cfd422c3cf4d 197 scope.set(2, boxcheckC);
Feike 21:f6e70856810c 198
vsluiter 11:ce72ec658a95 199 scope.send();
tomlankhorst 18:21d8e7a81cf5 200 led = !led;
vsluiter 2:e314bb3b2d99 201 }
vsluiter 0:32bb76391d89 202
vsluiter 0:32bb76391d89 203 int main()
tomlankhorst 19:2bf824669684 204 {
Feike 25:cfd422c3cf4d 205 sample_timer.attach(&sample, 0.005);
tomlankhorst 15:0da764eea774 206 while(1) {}
vsluiter 0:32bb76391d89 207 }