Feike van Veen / Mbed 2 deprecated EMG

Alle drie de signalen gefilterd en binair gemaakt

Dependencies:   mbed HIDScope biquadFilter

main.cpp@23:a6f18aee31cd, 2019-10-30 (annotated)

Committer:
Feike
Date:
Wed Oct 30 12:11:18 2019 +0000
Revision:
23:a6f18aee31cd
Parent:
22:611667172ac3
Child:
25:cfd422c3cf4d
Drie spieren aansturen met de EMG

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 23:a6f18aee31cd 19 const int leng_filt = 60;
Feike 23:a6f18aee31cd 20 const int box_length = 100;
Feike 23:a6f18aee31cd 21 const int box_lengthC = 200;
Feike 23:a6f18aee31cd 22 const int box_checkC = 150;
Feike 23:a6f18aee31cd 23 const float grenswaardeA = 0.01;
Feike 23:a6f18aee31cd 24 const float grenswaardeB = 0.02;
Feike 23:a6f18aee31cd 25 const float grenswaardeC = 0.28;
Feike 23:a6f18aee31cd 26
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 23:a6f18aee31cd 46 float C_ar3[box_length] = {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 23:a6f18aee31cd 51 const double b0 = 0.206572;
Feike 23:a6f18aee31cd 52 const double b1 = 0.413144;
Feike 23:a6f18aee31cd 53 const double b2 = 0.206572;
Feike 23:a6f18aee31cd 54 const double a0 = 1.000000;
Feike 23:a6f18aee31cd 55 const double a1 = -0.369527;
Feike 23:a6f18aee31cd 56 const double a2 = 0.195816;
Feike 21:f6e70856810c 57
tomlankhorst 14:f83354387756 58 void sample()
Feike 21:f6e70856810c 59 {
Feike 23:a6f18aee31cd 60 BiQuad lowpass(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 23:a6f18aee31cd 76 Ay1 = lowpass.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 23:a6f18aee31cd 87 if(Ay2>grenswaardeA)
Feike 23:a6f18aee31cd 88 { Ay3 = 1; }
Feike 23:a6f18aee31cd 89
Feike 23:a6f18aee31cd 90 if(Ay2<=grenswaardeA)
Feike 23:a6f18aee31cd 91 { Ay3 = 0; }
Feike 23:a6f18aee31cd 92
Feike 23:a6f18aee31cd 93 for (int j=box_length-1; j>=1; j--)
Feike 23:a6f18aee31cd 94 { A_ar3[j] = A_ar3[j-1]; }
Feike 21:f6e70856810c 95
Feike 23:a6f18aee31cd 96 A_ar3[0] = Ay3;
Feike 23:a6f18aee31cd 97 int boxcheckA = 0;
Feike 23:a6f18aee31cd 98
Feike 23:a6f18aee31cd 99 for (int j=0; j<=box_length-1; j++)
Feike 23:a6f18aee31cd 100 { if(A_ar3[j] == 1)
Feike 23:a6f18aee31cd 101 { boxcheckA = 1;} }
Feike 23:a6f18aee31cd 102
Feike 23:a6f18aee31cd 103 // Signaal 2 (B)
Feike 23:a6f18aee31cd 104
Feike 23:a6f18aee31cd 105 float B = emg0.read();
Feike 23:a6f18aee31cd 106 float Bmean = 0;
Feike 23:a6f18aee31cd 107 float By2 = 0;
Feike 23:a6f18aee31cd 108
Feike 23:a6f18aee31cd 109 for (int j=leng_filt-1; j>=1; j--)
Feike 23:a6f18aee31cd 110 { B_ar[j] = B_ar[j-1]; }
Feike 23:a6f18aee31cd 111
Feike 23:a6f18aee31cd 112 B_ar[0] = B;
Feike 23:a6f18aee31cd 113
Feike 23:a6f18aee31cd 114 for(int i=0; i<=leng_filt-1; i++)
Feike 23:a6f18aee31cd 115 { Bmean += B_ar[i]*1/leng_filt; }
Feike 23:a6f18aee31cd 116
Feike 23:a6f18aee31cd 117 By1 = B - Bmean;
Feike 23:a6f18aee31cd 118 By1 = fabs(By1);
Feike 23:a6f18aee31cd 119 By1 = lowpass.step(By1); // First signal, after Butterworth
Feike 23:a6f18aee31cd 120
Feike 23:a6f18aee31cd 121 for (int j=leng_filt-1; j>=1; j--)
Feike 23:a6f18aee31cd 122 { B_ar2[j] = B_ar2[j-1]; }
Feike 23:a6f18aee31cd 123
Feike 23:a6f18aee31cd 124 B_ar2[0] = By1;
Feike 21:f6e70856810c 125
Feike 21:f6e70856810c 126 for(int i=0; i<=leng_filt-1; i++)
Feike 23:a6f18aee31cd 127 { By2 += B_ar2[i]*1/leng_filt; }
Feike 23:a6f18aee31cd 128
Feike 23:a6f18aee31cd 129 float By3;
Feike 23:a6f18aee31cd 130 if(By2>grenswaardeB)
Feike 23:a6f18aee31cd 131 { By3 = 1; }
Feike 21:f6e70856810c 132
Feike 23:a6f18aee31cd 133 if(By2<=grenswaardeB)
Feike 23:a6f18aee31cd 134 { By3 = 0; }
Feike 23:a6f18aee31cd 135
Feike 23:a6f18aee31cd 136 for (int j=box_length-1; j>=1; j--)
Feike 23:a6f18aee31cd 137 { B_ar3[j] = B_ar3[j-1]; }
Feike 21:f6e70856810c 138
Feike 23:a6f18aee31cd 139 B_ar3[0] = By3;
Feike 23:a6f18aee31cd 140 int boxcheckB = 0;
Feike 23:a6f18aee31cd 141
Feike 23:a6f18aee31cd 142 for (int j=0; j<=box_length-1; j++)
Feike 23:a6f18aee31cd 143 { if(B_ar3[j] == 1)
Feike 23:a6f18aee31cd 144 { boxcheckB = 1;} }
Feike 21:f6e70856810c 145
Feike 23:a6f18aee31cd 146 // Signaal 3 (C)
Feike 23:a6f18aee31cd 147
Feike 23:a6f18aee31cd 148 float C = emg0.read();
Feike 23:a6f18aee31cd 149 float Cmean = 0;
Feike 23:a6f18aee31cd 150 float Cy2 = 0;
Feike 23:a6f18aee31cd 151
Feike 23:a6f18aee31cd 152 for (int j=leng_filt-1; j>=1; j--)
Feike 23:a6f18aee31cd 153 { C_ar[j] = C_ar[j-1]; }
Feike 21:f6e70856810c 154
Feike 23:a6f18aee31cd 155 C_ar[0] = C;
Feike 21:f6e70856810c 156
Feike 21:f6e70856810c 157 for(int i=0; i<=leng_filt-1; i++)
Feike 23:a6f18aee31cd 158 { Cmean += C_ar[i]*1/leng_filt; }
Feike 22:611667172ac3 159
Feike 23:a6f18aee31cd 160 Cy1 = C - Cmean;
Feike 23:a6f18aee31cd 161 Cy1 = fabs(Cy1);
Feike 23:a6f18aee31cd 162 Cy1 = lowpass.step(Cy1); // First signal, after Butterworth
Feike 23:a6f18aee31cd 163
Feike 23:a6f18aee31cd 164 for (int j=leng_filt-1; j>=1; j--)
Feike 23:a6f18aee31cd 165 { C_ar2[j] = C_ar2[j-1]; }
Feike 22:611667172ac3 166
Feike 23:a6f18aee31cd 167 C_ar2[0] = Cy1;
Feike 22:611667172ac3 168
Feike 22:611667172ac3 169 for(int i=0; i<=leng_filt-1; i++)
Feike 23:a6f18aee31cd 170 { Cy2 += C_ar2[i]*1/leng_filt; }
Feike 23:a6f18aee31cd 171
Feike 23:a6f18aee31cd 172 float Cy3;
Feike 23:a6f18aee31cd 173 if(Cy2>grenswaardeC)
Feike 23:a6f18aee31cd 174 { Cy3 = 1; }
Feike 23:a6f18aee31cd 175
Feike 23:a6f18aee31cd 176 if(Cy2<=grenswaardeC)
Feike 23:a6f18aee31cd 177 { Cy3 = 0; }
Feike 22:611667172ac3 178
Feike 23:a6f18aee31cd 179 for (int j=box_lengthC-1; j>=1; j--)
Feike 23:a6f18aee31cd 180 { C_ar3[j] = C_ar3[j-1]; }
Feike 21:f6e70856810c 181
Feike 23:a6f18aee31cd 182 C_ar3[0] = Cy3;
Feike 23:a6f18aee31cd 183 int boxcheckC = 0;
Feike 23:a6f18aee31cd 184 int C_sum = 0;
Feike 23:a6f18aee31cd 185 for (int j=0; j<=box_length-1; j++)
Feike 23:a6f18aee31cd 186 { C_sum += C_ar3[j];
Feike 23:a6f18aee31cd 187 if(C_sum >= box_checkC)
Feike 23:a6f18aee31cd 188 { boxcheckC = 1;} }
Feike 23:a6f18aee31cd 189
Feike 23:a6f18aee31cd 190
Feike 21:f6e70856810c 191 scope.set(0, emg0.read());
Feike 23:a6f18aee31cd 192 scope.set(1, Cy1);
Feike 23:a6f18aee31cd 193 scope.set(2, Cy2);
Feike 23:a6f18aee31cd 194 scope.set(3, Cy3);
Feike 23:a6f18aee31cd 195 scope.set(4, boxcheckC);
Feike 21:f6e70856810c 196
vsluiter 11:ce72ec658a95 197 scope.send();
tomlankhorst 18:21d8e7a81cf5 198 led = !led;
vsluiter 2:e314bb3b2d99 199 }
vsluiter 0:32bb76391d89 200
vsluiter 0:32bb76391d89 201 int main()
tomlankhorst 19:2bf824669684 202 {
Feike 23:a6f18aee31cd 203 sample_timer.attach(&sample, 0.001);
tomlankhorst 15:0da764eea774 204 while(1) {}
vsluiter 0:32bb76391d89 205 }