Marjolein Scheffers / Mbed 2 deprecated gr14_motor_HidScope

emg trial

Dependencies:   FastPWM HIDScope MODSERIAL mbed biquadFilter

main.cpp@6:51c87d04371d, 2018-10-26 (annotated)

Committer:
s1680897
Date:
Fri Oct 26 13:54:12 2018 +0000
Revision:
6:51c87d04371d
Parent:
5:dd64e0cf20fe
Child:
7:d72cbd7055af
Werkend EMG script met 4 signalen, nog niet genormaliseerd.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
s1680897 6:51c87d04371d 1 #include "mbed.h"
s1680897 1:e81ca29ae626 2 #include "HIDScope.h"
s1680897 0:94b5c70f818b 3 #include "MODSERIAL.h"
s1680897 2:0189c5170834 4 #include "BiQuad.h"
s1680897 0:94b5c70f818b 5
s1680897 6:51c87d04371d 6
s1680897 1:e81ca29ae626 7 // Two analog inputs to read from
s1680897 1:e81ca29ae626 8 AnalogIn a0(A0);
s1680897 1:e81ca29ae626 9 AnalogIn a1(A1);
s1680897 6:51c87d04371d 10 AnalogIn a2(A2);
s1680897 6:51c87d04371d 11 AnalogIn a3(A3);
s1680897 6:51c87d04371d 12 DigitalOut ledr(LED_RED);
s1680897 0:94b5c70f818b 13 MODSERIAL pc(USBTX, USBRX);
s1680897 0:94b5c70f818b 14
s1680897 0:94b5c70f818b 15
s1680897 1:e81ca29ae626 16 // Define the HIDScope and Ticker objects
s1680897 6:51c87d04371d 17 HIDScope scope(4);
s1680897 1:e81ca29ae626 18 Ticker scopeTimer;
s1680897 2:0189c5170834 19 Ticker EMGTicker;
s1680897 2:0189c5170834 20
s1680897 6:51c87d04371d 21 // BiQuad filters
s1680897 6:51c87d04371d 22 //BiQuad Chains
s1680897 6:51c87d04371d 23 BiQuadChain bqc1;
s1680897 6:51c87d04371d 24 BiQuadChain bqc2;
s1680897 6:51c87d04371d 25 BiQuadChain bqc3;
s1680897 6:51c87d04371d 26 BiQuadChain bqc4;
s1680897 6:51c87d04371d 27
s1680897 6:51c87d04371d 28 //High pass filters 20 Hz
s1680897 6:51c87d04371d 29 BiQuad HP_emg1(1,-2,1,-1.142980502539901,0.412801598096189);
s1680897 6:51c87d04371d 30 BiQuad HP_emg2(1,-2,1,-1.142980502539901,0.412801598096189);
s1680897 6:51c87d04371d 31 BiQuad HP_emg3(1,-2,1,-1.142980502539901,0.412801598096189);
s1680897 6:51c87d04371d 32 BiQuad HP_emg4(1,-2,1,-1.142980502539901,0.412801598096189);
s1680897 6:51c87d04371d 33
s1680897 6:51c87d04371d 34 //Notch filters 50 Hz
s1680897 6:51c87d04371d 35 BiQuad Notch_emg1(1,-1.225251386743515e-16,1,-1.187919512117619e-16,0.939062505817492);
s1680897 6:51c87d04371d 36 BiQuad Notch_emg2(1,-1.225251386743515e-16,1,-1.187919512117619e-16,0.939062505817492);
s1680897 6:51c87d04371d 37 BiQuad Notch_emg3(1,-1.225251386743515e-16,1,-1.187919512117619e-16,0.939062505817492);
s1680897 6:51c87d04371d 38 BiQuad Notch_emg4(1,-1.225251386743515e-16,1,-1.187919512117619e-16,0.939062505817492);
s1680897 6:51c87d04371d 39
s1680897 6:51c87d04371d 40 //Low pass filters 25 Hz
s1680897 6:51c87d04371d 41 BiQuad LP_emg1(1,2,1,-0.942809041582063,0.333333333333333);
s1680897 6:51c87d04371d 42 BiQuad LP_emg2(1,2,1,-0.942809041582063,0.333333333333333);
s1680897 6:51c87d04371d 43 BiQuad LP_emg3(1,2,1,-0.942809041582063,0.333333333333333);
s1680897 6:51c87d04371d 44 BiQuad LP_emg4(1,2,1,-0.942809041582063,0.333333333333333);
s1680897 6:51c87d04371d 45
s1680897 2:0189c5170834 46
s1680897 4:bd4dd411dc7b 47 // Global variables
s1680897 6:51c87d04371d 48 float EMGdata1;
s1680897 6:51c87d04371d 49 float EMGdata2;
s1680897 6:51c87d04371d 50 float EMGdata3;
s1680897 6:51c87d04371d 51 float EMGdata4;
s1680897 4:bd4dd411dc7b 52 int count;
s1680897 2:0189c5170834 53
s1680897 2:0189c5170834 54 void ReadEMG()
s1680897 6:51c87d04371d 55 {
s1680897 4:bd4dd411dc7b 56 EMGdata1 = a0.read(); // store values in the scope
s1680897 4:bd4dd411dc7b 57 EMGdata2 = a1.read();
s1680897 6:51c87d04371d 58 EMGdata3 = a2.read();
s1680897 6:51c87d04371d 59 EMGdata4 = a3.read();
s1680897 6:51c87d04371d 60 }
s1680897 6:51c87d04371d 61
s1680897 6:51c87d04371d 62 // EMG High pass filters
s1680897 6:51c87d04371d 63 float EMG_HP1(float EMGdata) //data 1
s1680897 6:51c87d04371d 64 {
s1680897 6:51c87d04371d 65 float HP_abs_EMGdata = bqc1.step(EMGdata);
s1680897 6:51c87d04371d 66
s1680897 6:51c87d04371d 67 return fabs(HP_abs_EMGdata);
s1680897 2:0189c5170834 68 }
s1680897 2:0189c5170834 69
s1680897 6:51c87d04371d 70 float EMG_HP2(float EMGdata) //data 2
s1680897 6:51c87d04371d 71 {
s1680897 6:51c87d04371d 72 float HP_abs_EMGdata = bqc2.step(EMGdata);
s1680897 6:51c87d04371d 73
s1680897 6:51c87d04371d 74 return fabs(HP_abs_EMGdata);
s1680897 6:51c87d04371d 75 }
s1680897 6:51c87d04371d 76
s1680897 6:51c87d04371d 77 float EMG_HP3(float EMGdata) //data 3
s1680897 1:e81ca29ae626 78 {
s1680897 6:51c87d04371d 79 float HP_abs_EMGdata = bqc3.step(EMGdata);
s1680897 6:51c87d04371d 80
s1680897 6:51c87d04371d 81 return fabs(HP_abs_EMGdata);
s1680897 6:51c87d04371d 82 }
s1680897 6:51c87d04371d 83
s1680897 6:51c87d04371d 84 float EMG_HP4(float EMGdata) // data 4
s1680897 6:51c87d04371d 85 {
s1680897 6:51c87d04371d 86 float HP_abs_EMGdata = bqc4.step(EMGdata);
s1680897 6:51c87d04371d 87
s1680897 4:bd4dd411dc7b 88 return fabs(HP_abs_EMGdata);
s1680897 4:bd4dd411dc7b 89 }
s1680897 4:bd4dd411dc7b 90
s1680897 6:51c87d04371d 91
s1680897 6:51c87d04371d 92 /*
s1680897 6:51c87d04371d 93 // Lowpass filters
s1680897 6:51c87d04371d 94
s1680897 6:51c87d04371d 95 // LP 1
s1680897 6:51c87d04371d 96 float LP_EMG1(float HP_abs_EMGdata)
s1680897 6:51c87d04371d 97 {
s1680897 6:51c87d04371d 98 float EMG_filt = LP_emg1.step(HP_abs_EMGdata);
s1680897 6:51c87d04371d 99
s1680897 6:51c87d04371d 100 return EMG_filt;
s1680897 6:51c87d04371d 101 }
s1680897 6:51c87d04371d 102
s1680897 6:51c87d04371d 103 // LP 2
s1680897 6:51c87d04371d 104 float LP_EMG2(float HP_abs_EMGdata)
s1680897 6:51c87d04371d 105 {
s1680897 6:51c87d04371d 106 float EMG_filt = LP_emg2.step(HP_abs_EMGdata);
s1680897 6:51c87d04371d 107
s1680897 6:51c87d04371d 108 return EMG_filt;
s1680897 6:51c87d04371d 109 }
s1680897 6:51c87d04371d 110
s1680897 6:51c87d04371d 111 // LP 3
s1680897 6:51c87d04371d 112 float LP_EMG3(float HP_abs_EMGdata)
s1680897 4:bd4dd411dc7b 113 {
s1680897 6:51c87d04371d 114 float EMG_filt = LP_emg3.step(HP_abs_EMGdata);
s1680897 6:51c87d04371d 115
s1680897 6:51c87d04371d 116 return EMG_filt;
s1680897 6:51c87d04371d 117 }
s1680897 6:51c87d04371d 118
s1680897 6:51c87d04371d 119 // LP 4
s1680897 6:51c87d04371d 120 float LP_EMG4(float HP_abs_EMGdata)
s1680897 6:51c87d04371d 121 {
s1680897 6:51c87d04371d 122 float EMG_filt = LP_emg4.step(HP_abs_EMGdata);
s1680897 2:0189c5170834 123
s1680897 6:51c87d04371d 124 return EMG_filt;
s1680897 6:51c87d04371d 125 }
s1680897 6:51c87d04371d 126 */
s1680897 6:51c87d04371d 127
s1680897 6:51c87d04371d 128 float EMG_mean (float EMGarray[200])
s1680897 6:51c87d04371d 129 {
s1680897 6:51c87d04371d 130 float sum = 0.0;
s1680897 6:51c87d04371d 131
s1680897 6:51c87d04371d 132 for(int j=0; j<200; j++)
s1680897 6:51c87d04371d 133 {
s1680897 6:51c87d04371d 134 sum += EMGarray[j];
s1680897 6:51c87d04371d 135 }
s1680897 6:51c87d04371d 136
s1680897 6:51c87d04371d 137 float EMG_filt = sum / 200.0;
s1680897 6:51c87d04371d 138
s1680897 6:51c87d04371d 139 return EMG_filt;
s1680897 1:e81ca29ae626 140 }
s1680897 0:94b5c70f818b 141
s1680897 4:bd4dd411dc7b 142 // Moving mean of EMG data 1
s1680897 6:51c87d04371d 143 /* float Moving_mean1(float HP_abs_EMGdata1)
s1680897 1:e81ca29ae626 144 {
s1680897 6:51c87d04371d 145 int P1 = 200;
s1680897 6:51c87d04371d 146 static float EMGarray1[200];
s1680897 6:51c87d04371d 147
s1680897 6:51c87d04371d 148 for(int i1 = P1-1; i1 >= 1; i1--)
s1680897 6:51c87d04371d 149 {
s1680897 6:51c87d04371d 150 EMGarray1[i1] = EMGarray1[i1-1];
s1680897 6:51c87d04371d 151 }
s1680897 4:bd4dd411dc7b 152 EMGarray1[0] = HP_abs_EMGdata1;
s1680897 6:51c87d04371d 153
s1680897 6:51c87d04371d 154 float sum1 = 0.0;
s1680897 6:51c87d04371d 155
s1680897 6:51c87d04371d 156 for(int j1=0; j1<200; j1++)
s1680897 6:51c87d04371d 157 {
s1680897 6:51c87d04371d 158 sum1 += EMGarray1[j1];
s1680897 6:51c87d04371d 159 }
s1680897 6:51c87d04371d 160
s1680897 6:51c87d04371d 161 float EMG_filt1 = sum1 / (float) P1;
s1680897 6:51c87d04371d 162
s1680897 6:51c87d04371d 163 return EMG_filt1;
s1680897 4:bd4dd411dc7b 164 }
s1680897 4:bd4dd411dc7b 165
s1680897 4:bd4dd411dc7b 166 // Moving mean of EMG data 2
s1680897 6:51c87d04371d 167 float Moving_mean2(float HP_abs_EMGdata2)
s1680897 4:bd4dd411dc7b 168 {
s1680897 6:51c87d04371d 169 int P2 = 200;
s1680897 6:51c87d04371d 170 static float EMGarray2[200];
s1680897 6:51c87d04371d 171
s1680897 6:51c87d04371d 172 for(int i2 = P2-1; i2 >= 1; i2--)
s1680897 6:51c87d04371d 173 {
s1680897 6:51c87d04371d 174 EMGarray2[i2] = EMGarray2[i2-1];
s1680897 6:51c87d04371d 175 }
s1680897 4:bd4dd411dc7b 176 EMGarray2[0] = HP_abs_EMGdata2;
s1680897 6:51c87d04371d 177
s1680897 6:51c87d04371d 178 float sum2;
s1680897 6:51c87d04371d 179
s1680897 6:51c87d04371d 180 for(int j2=0; j2<200; j2++)
s1680897 6:51c87d04371d 181 {
s1680897 6:51c87d04371d 182 sum2 += EMGarray2[j2];
s1680897 6:51c87d04371d 183 }
s1680897 6:51c87d04371d 184
s1680897 6:51c87d04371d 185 float EMG_filt2 = sum2 / (float) P2;
s1680897 6:51c87d04371d 186
s1680897 6:51c87d04371d 187 return EMG_filt2;
s1680897 1:e81ca29ae626 188 }
s1680897 0:94b5c70f818b 189
s1680897 6:51c87d04371d 190 // Moving mean of EMG data 3
s1680897 6:51c87d04371d 191 float Moving_mean3(float HP_abs_EMGdata3)
s1680897 6:51c87d04371d 192 {
s1680897 6:51c87d04371d 193 int P3 = 200;
s1680897 6:51c87d04371d 194 static float EMGarray3[200];
s1680897 6:51c87d04371d 195
s1680897 6:51c87d04371d 196 for(int i3 = P3-1; i3 >= 1; i3--)
s1680897 6:51c87d04371d 197 {
s1680897 6:51c87d04371d 198 EMGarray3[i3] = EMGarray3[i3-1];
s1680897 6:51c87d04371d 199 }
s1680897 6:51c87d04371d 200 EMGarray3[0] = HP_abs_EMGdata3;
s1680897 6:51c87d04371d 201
s1680897 6:51c87d04371d 202 float sum3;
s1680897 6:51c87d04371d 203
s1680897 6:51c87d04371d 204 for(int j3=0; j3<200; j3++)
s1680897 6:51c87d04371d 205 {
s1680897 6:51c87d04371d 206 sum3 += EMGarray3[j3];
s1680897 6:51c87d04371d 207 }
s1680897 6:51c87d04371d 208
s1680897 6:51c87d04371d 209 float EMG_filt3 = sum3 / (float) P3;
s1680897 6:51c87d04371d 210
s1680897 6:51c87d04371d 211 return EMG_filt3;
s1680897 6:51c87d04371d 212 }
s1680897 6:51c87d04371d 213
s1680897 6:51c87d04371d 214 // Moving mean of EMG data 4
s1680897 6:51c87d04371d 215 float Moving_mean4(float HP_abs_EMGdata)
s1680897 5:dd64e0cf20fe 216 {
s1680897 6:51c87d04371d 217 int P = 200;
s1680897 6:51c87d04371d 218 static float EMGarray[200];
s1680897 6:51c87d04371d 219
s1680897 6:51c87d04371d 220 for(int i = P-1; i >= 1; i--)
s1680897 6:51c87d04371d 221 {
s1680897 6:51c87d04371d 222 EMGarray[i] = EMGarray[i-1];
s1680897 6:51c87d04371d 223 }
s1680897 6:51c87d04371d 224 EMGarray[0] = HP_abs_EMGdata;
s1680897 6:51c87d04371d 225
s1680897 6:51c87d04371d 226 float sum;
s1680897 6:51c87d04371d 227
s1680897 6:51c87d04371d 228 for(int j=0; j<200; j++)
s1680897 6:51c87d04371d 229 {
s1680897 6:51c87d04371d 230 sum += EMGarray[j];
s1680897 6:51c87d04371d 231 }
s1680897 6:51c87d04371d 232
s1680897 6:51c87d04371d 233 float EMG_filt = sum / (float) P;
s1680897 6:51c87d04371d 234
s1680897 6:51c87d04371d 235 return EMG_filt;
s1680897 6:51c87d04371d 236 }
s1680897 6:51c87d04371d 237 */
s1680897 6:51c87d04371d 238
s1680897 6:51c87d04371d 239 // Filtered signal to output between -1 and 1
s1680897 6:51c87d04371d 240 float filt2kin (float EMG_filt1, float EMG_filt2, float max1, float max2)
s1680897 6:51c87d04371d 241 {
s1680897 6:51c87d04371d 242 float EMG_scaled1 = EMG_filt1 / max1;
s1680897 6:51c87d04371d 243 float EMG_scaled2 = EMG_filt2 / max2;
s1680897 6:51c87d04371d 244
s1680897 6:51c87d04371d 245 float kin_input = EMG_scaled2 - EMG_scaled1;
s1680897 6:51c87d04371d 246
s1680897 6:51c87d04371d 247 if (kin_input > 1.0) {
s1680897 5:dd64e0cf20fe 248 kin_input = 1.0;
s1680897 5:dd64e0cf20fe 249 }
s1680897 6:51c87d04371d 250 if (kin_input < -1.0) {
s1680897 5:dd64e0cf20fe 251 kin_input = -1.0;
s1680897 5:dd64e0cf20fe 252 }
s1680897 6:51c87d04371d 253
s1680897 5:dd64e0cf20fe 254 return kin_input;
s1680897 5:dd64e0cf20fe 255 }
s1680897 5:dd64e0cf20fe 256
s1680897 2:0189c5170834 257 void EMG_filtering ()
s1680897 6:51c87d04371d 258 {
s1680897 2:0189c5170834 259 ReadEMG();
s1680897 6:51c87d04371d 260
s1680897 6:51c87d04371d 261 float HP_abs_EMGdata1 = EMG_HP1(EMGdata1);
s1680897 6:51c87d04371d 262 float HP_abs_EMGdata2 = EMG_HP2(EMGdata2);
s1680897 6:51c87d04371d 263 float HP_abs_EMGdata3 = EMG_HP3(EMGdata3);
s1680897 6:51c87d04371d 264 float HP_abs_EMGdata4 = EMG_HP4(EMGdata4);
s1680897 6:51c87d04371d 265
s1680897 6:51c87d04371d 266 static float EMG_array1[100];
s1680897 6:51c87d04371d 267 static float EMG_array2[100];
s1680897 6:51c87d04371d 268 static float EMG_array3[100];
s1680897 6:51c87d04371d 269 static float EMG_array4[100];
s1680897 4:bd4dd411dc7b 270
s1680897 6:51c87d04371d 271 // Fill array 1
s1680897 6:51c87d04371d 272 for(int i = 100-1; i >= 1; i--)
s1680897 6:51c87d04371d 273 {
s1680897 6:51c87d04371d 274 EMG_array1[i] = EMG_array1[i-1];
s1680897 6:51c87d04371d 275 }
s1680897 6:51c87d04371d 276 EMG_array1[0] = HP_abs_EMGdata1;
s1680897 4:bd4dd411dc7b 277
s1680897 6:51c87d04371d 278 // Fill array 2
s1680897 6:51c87d04371d 279 for(int i = 100-1; i >= 1; i--)
s1680897 6:51c87d04371d 280 {
s1680897 6:51c87d04371d 281 EMG_array2[i] = EMG_array2[i-1];
s1680897 6:51c87d04371d 282 }
s1680897 6:51c87d04371d 283 EMG_array2[0] = HP_abs_EMGdata2;
s1680897 6:51c87d04371d 284
s1680897 6:51c87d04371d 285 // Fill array 3
s1680897 6:51c87d04371d 286 for(int i = 100-1; i >= 1; i--)
s1680897 6:51c87d04371d 287 {
s1680897 6:51c87d04371d 288 EMG_array3[i] = EMG_array3[i-1];
s1680897 6:51c87d04371d 289 }
s1680897 6:51c87d04371d 290 EMG_array3[0] = HP_abs_EMGdata3;
s1680897 2:0189c5170834 291
s1680897 6:51c87d04371d 292 // Fill array 4
s1680897 6:51c87d04371d 293 for(int i = 100-1; i >= 1; i--)
s1680897 6:51c87d04371d 294 {
s1680897 6:51c87d04371d 295 EMG_array4[i] = EMG_array4[i-1];
s1680897 6:51c87d04371d 296 }
s1680897 6:51c87d04371d 297 EMG_array4[0] = HP_abs_EMGdata4;
s1680897 5:dd64e0cf20fe 298
s1680897 6:51c87d04371d 299
s1680897 6:51c87d04371d 300 /* float EMG_filt1 = Moving_mean1(HP_abs_EMGdata1);
s1680897 6:51c87d04371d 301 float EMG_filt2 = Moving_mean2(HP_abs_EMGdata2);
s1680897 6:51c87d04371d 302 float EMG_filt3 = Moving_mean3(HP_abs_EMGdata3);
s1680897 6:51c87d04371d 303 float EMG_filt4 = Moving_mean4(HP_abs_EMGdata4); */
s1680897 5:dd64e0cf20fe 304
s1680897 6:51c87d04371d 305 float EMG_filt1 = EMG_mean(EMG_array1);
s1680897 6:51c87d04371d 306 float EMG_filt2 = EMG_mean(EMG_array2);
s1680897 6:51c87d04371d 307 float EMG_filt3 = EMG_mean(EMG_array3);
s1680897 6:51c87d04371d 308 float EMG_filt4 = EMG_mean(EMG_array4);
s1680897 6:51c87d04371d 309
s1680897 6:51c87d04371d 310 float max1 = 0.45;
s1680897 6:51c87d04371d 311 float max2 = 0.43;
s1680897 6:51c87d04371d 312 float max3 = 0.45;
s1680897 6:51c87d04371d 313 float max4 = 0.43;
s1680897 6:51c87d04371d 314
s1680897 6:51c87d04371d 315 float kin_input_horizontal = filt2kin (EMG_filt1, EMG_filt2, max1, max2);
s1680897 6:51c87d04371d 316 float kin_input_vertical = filt2kin (EMG_filt3, EMG_filt4, max3, max4);
s1680897 6:51c87d04371d 317
s1680897 6:51c87d04371d 318 scope.set(0, EMG_filt1);
s1680897 6:51c87d04371d 319 scope.set(1, EMG_filt2);
s1680897 6:51c87d04371d 320 scope.set(2, EMG_filt3);
s1680897 6:51c87d04371d 321 scope.set(3, EMG_filt4);
s1680897 4:bd4dd411dc7b 322
s1680897 6:51c87d04371d 323
s1680897 4:bd4dd411dc7b 324 count++;
s1680897 6:51c87d04371d 325
s1680897 6:51c87d04371d 326 if (count == 50)
s1680897 4:bd4dd411dc7b 327 {
s1680897 4:bd4dd411dc7b 328 pc.printf("EMG filt 1 = %lf \t EMG filt 2 = %lf \n\r", EMG_filt1, EMG_filt2);
s1680897 4:bd4dd411dc7b 329 count = 0;
s1680897 4:bd4dd411dc7b 330 }
s1680897 2:0189c5170834 331 }
s1680897 6:51c87d04371d 332
s1680897 0:94b5c70f818b 333 int main()
s1680897 0:94b5c70f818b 334 {
s1680897 0:94b5c70f818b 335 pc.baud(115200);
s1680897 6:51c87d04371d 336 //BiQuad chains
s1680897 6:51c87d04371d 337 bqc1.add( &HP_emg1 ).add( &Notch_emg1 );
s1680897 6:51c87d04371d 338 bqc2.add( &HP_emg2 ).add( &Notch_emg2 );
s1680897 6:51c87d04371d 339 bqc3.add( &HP_emg3 ).add( &Notch_emg3 );
s1680897 6:51c87d04371d 340 bqc4.add( &HP_emg4 ).add( &Notch_emg4 );
s1680897 0:94b5c70f818b 341
s1680897 6:51c87d04371d 342 // Attach the HIDScope::send method from the scope object to the timer at 50Hz
s1680897 6:51c87d04371d 343 scopeTimer.attach_us(&scope, &HIDScope::send, 5e3);
s1680897 6:51c87d04371d 344 EMGTicker.attach_us(EMG_filtering, 5e3);
s1680897 6:51c87d04371d 345
s1680897 2:0189c5170834 346 while(1) {}
s1680897 6:51c87d04371d 347
s1680897 0:94b5c70f818b 348 }