Feike van Veen / Mbed 2 deprecated EMG

Alle drie de signalen gefilterd en binair gemaakt

Dependencies:   mbed HIDScope biquadFilter

main.cpp@25:cfd422c3cf4d, 2019-10-31 (annotated)

Committer:
Feike
Date:
Thu Oct 31 18:25:53 2019 +0000
Revision:
25:cfd422c3cf4d
Parent:
23:a6f18aee31cd
Child:
26:1eb3c3b04b80
final version :):)

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