Rianne Bulthuis / Mbed 2 deprecated EMG_test

incl. calibration

Dependencies:   HIDScope mbed

main.cpp@0:8c2484672018, 2015-10-29 (annotated)

Committer:
riannebulthuis
Date:
Thu Oct 29 12:24:33 2015 +0000
Revision:
0:8c2484672018
incl. calibration

Who changed what in which revision?

UserRevisionLine numberNew contents of line
riannebulthuis 0:8c2484672018 1 #include "mbed.h"
riannebulthuis 0:8c2484672018 2 #include "HIDScope.h"
riannebulthuis 0:8c2484672018 3
riannebulthuis 0:8c2484672018 4 AnalogIn EMG_bicepsright(A0);
riannebulthuis 0:8c2484672018 5 AnalogIn EMG_bicepsleft(A1);
riannebulthuis 0:8c2484672018 6 HIDScope scope(4);
riannebulthuis 0:8c2484672018 7 Ticker EMG;
riannebulthuis 0:8c2484672018 8 Ticker Scope;
riannebulthuis 0:8c2484672018 9
riannebulthuis 0:8c2484672018 10 //EMG calibration
riannebulthuis 0:8c2484672018 11 Timer biceps_calibration;
riannebulthuis 0:8c2484672018 12 Ticker EMGCALIBRATION;
riannebulthuis 0:8c2484672018 13 volatile bool regulation_EMG_flag;
riannebulthuis 0:8c2484672018 14 void setregulation_EMG_flag()
riannebulthuis 0:8c2484672018 15 {
riannebulthuis 0:8c2484672018 16 regulation_EMG_flag = true;
riannebulthuis 0:8c2484672018 17 }
riannebulthuis 0:8c2484672018 18
riannebulthuis 0:8c2484672018 19 // Filter1 = High pass filter tot 20 Hz
riannebulthuis 0:8c2484672018 20 double fh1_v1=0, fh1_v2=0, fh2_v1=0, fh2_v2=0;
riannebulthuis 0:8c2484672018 21 const double fh1_a1=-0.84909054461, fh1_a2=0.00000000000, fh1_b0= 1, fh1_b1=-1, fh1_b2=0;
riannebulthuis 0:8c2484672018 22 const double fh2_a1=-1.82553264091, fh2_a2=0.85001416809, fh2_b0= 1, fh2_b1=-2, fh2_b2=1;
riannebulthuis 0:8c2484672018 23 // Filter2 = Low pass filter na 60 Hz
riannebulthuis 0:8c2484672018 24 double fl1_v1=0, fl1_v2=0, fl2_v1=0, fl2_v2=0;
riannebulthuis 0:8c2484672018 25 const double fl1_a1=-0.66979455390, fl1_a2=0.00000000000, fl1_b0= 1, fl1_b1=1, fl1_b2=0;
riannebulthuis 0:8c2484672018 26 const double fl2_a1=-1.55376616139, fl2_a2=0.68023470431, fl2_b0= 1, fl2_b1=2, fl2_b2=1;
riannebulthuis 0:8c2484672018 27 // Filter3 = Notch filter at 50 Hz
riannebulthuis 0:8c2484672018 28 double fno1_v1=0, fno1_v2=0, fno2_v1=0, fno2_v2=0, fno3_v1=0, fno3_v2=0;
riannebulthuis 0:8c2484672018 29 const double fno1_a1 = -1.87934916386, fno1_a2= 0.97731851355, fno1_b0= 1, fno1_b1= -1.90090686046, fno1_b2= 1;
riannebulthuis 0:8c2484672018 30 const double fno2_a1 = -1.88341028603, fno2_a2= 0.98825147717, fno2_b0= 1, fno2_b1= -1.90090686046, fno2_b2= 1;
riannebulthuis 0:8c2484672018 31 const double fno3_a1 = -1.89635403726, fno3_a2= 0.98894004849, fno3_b0= 1, fno3_b1= -1.90090686046, fno3_b2= 1;
riannebulthuis 0:8c2484672018 32 // Filter4 = Lowpass filter at 5 Hz
riannebulthuis 0:8c2484672018 33 double flp1_v1=0, flp1_v2=0, flp2_v1=0, flp2_v2=0;
riannebulthuis 0:8c2484672018 34 const double flp1_a1=-0.97922725527, flp1_a2=0.00000000000, flp1_b0= 1, flp1_b1=1, flp1_b2=0;
riannebulthuis 0:8c2484672018 35 const double flp2_a1=-1.97879353121, flp2_a2=0.97922951943, flp2_b0= 1, flp2_b1=2, flp2_b2=1;
riannebulthuis 0:8c2484672018 36 double y1, y2, y3, y4, y5, y6, y7, y8, y9,
riannebulthuis 0:8c2484672018 37 y10, y11, y12, y13, y14, y15, y16, y17, y18;
riannebulthuis 0:8c2484672018 38 double u1, u2, u3, u4, u5, u6, u7, u8, u9,
riannebulthuis 0:8c2484672018 39 u10, u11, u12, u13, u14, u15, u16, u17, u18;
riannebulthuis 0:8c2484672018 40 double final_filter1, final_filter2;
riannebulthuis 0:8c2484672018 41
riannebulthuis 0:8c2484672018 42 // Standaard formule voor het biquad filter
riannebulthuis 0:8c2484672018 43 double biquad(double u, double &v1, double &v2, const double a1, const double a2, const double b0, const double b1, const double b2)
riannebulthuis 0:8c2484672018 44
riannebulthuis 0:8c2484672018 45 {
riannebulthuis 0:8c2484672018 46 double v = u - a1*v1 - a2*v2;
riannebulthuis 0:8c2484672018 47 double y = b0*v + b1*v1 + b2*v2;
riannebulthuis 0:8c2484672018 48 v2=v1;
riannebulthuis 0:8c2484672018 49 v1=v;
riannebulthuis 0:8c2484672018 50 return y;
riannebulthuis 0:8c2484672018 51 }
riannebulthuis 0:8c2484672018 52
riannebulthuis 0:8c2484672018 53
riannebulthuis 0:8c2484672018 54 void Signaal_EMG()
riannebulthuis 0:8c2484672018 55 {
riannebulthuis 0:8c2484672018 56 // left
riannebulthuis 0:8c2484672018 57 u1 = EMG_bicepsright.read();
riannebulthuis 0:8c2484672018 58 //Highpass
riannebulthuis 0:8c2484672018 59 y1 = biquad (u1, fh1_v1, fh1_v2, fh1_a1, fh1_a2, fh1_b0*0.924547, fh1_b1*0.924547, fh1_b2*0.924547);
riannebulthuis 0:8c2484672018 60 y2 = biquad (y1, fh2_v1, fh2_v2, fh2_a1, fh2_a2, fh2_b0*0.918885, fh2_b1*0.918885, fh2_b2*0.918885);
riannebulthuis 0:8c2484672018 61 //Lowpass
riannebulthuis 0:8c2484672018 62 y3 = biquad (y2, fl1_v1, fl1_v2, fl1_a1, fl1_a2, fl1_b0*0.165103, fl1_b1*0.165103, fl1_b2*0.165103);
riannebulthuis 0:8c2484672018 63 y4 = biquad (y3, fl2_v1, fl2_v2, fl2_a1, fl2_a2, fl2_b0*0.031617, fl2_b1*0.031617, fl2_b2*0.031617);
riannebulthuis 0:8c2484672018 64 // Notch
riannebulthuis 0:8c2484672018 65 y5 = biquad (y4, fno1_v1, fno1_v2, fno1_a1, fno1_a2, fno1_b0*1.004206, fno1_b1*1.004206, fno1_b2*1.004206);
riannebulthuis 0:8c2484672018 66 y6 = biquad (y5, fno2_v1, fno2_v2, fno2_a1, fno2_a2, fno2_b0, fno2_b1, fno2_b2);
riannebulthuis 0:8c2484672018 67 y7 = biquad (y6, fno3_v1, fno3_v2, fno3_a1, fno3_a2, fno3_b0*0.973227, fno3_b1*0.973227, fno3_b2*0.973227);
riannebulthuis 0:8c2484672018 68 // Rectify sample
riannebulthuis 0:8c2484672018 69 y8 = fabs(y7);
riannebulthuis 0:8c2484672018 70 // Make it smooth
riannebulthuis 0:8c2484672018 71 y9 = biquad (y8, flp1_v1, flp1_v2, flp1_a1, flp1_a2, flp1_b0* 0.010386, flp1_b1* 0.010386, flp1_b2* 0.010386);
riannebulthuis 0:8c2484672018 72 final_filter1 = biquad(y9, flp2_v1, flp2_v2, flp2_a1, flp2_a2, flp2_b0*0.000109, flp2_b1*0.000109, flp2_b2*0.000109);
riannebulthuis 0:8c2484672018 73
riannebulthuis 0:8c2484672018 74 //Biceps right
riannebulthuis 0:8c2484672018 75 u10 = EMG_bicepsleft.read();
riannebulthuis 0:8c2484672018 76 //Highpass
riannebulthuis 0:8c2484672018 77 y10 = biquad (u10, fh1_v1, fh1_v2, fh1_a1, fh1_a2, fh1_b0*0.924547, fh1_b1*0.924547, fh1_b2*0.924547);
riannebulthuis 0:8c2484672018 78 y11 = biquad (y10, fh2_v1, fh2_v2, fh2_a1, fh2_a2, fh2_b0*0.918885, fh2_b1*0.918885, fh2_b2*0.918885);
riannebulthuis 0:8c2484672018 79 //Lowpass
riannebulthuis 0:8c2484672018 80 y12 = biquad (y11, fl1_v1, fl1_v2, fl1_a1, fl1_a2, fl1_b0*0.165103, fl1_b1*0.165103, fl1_b2*0.165103);
riannebulthuis 0:8c2484672018 81 y13 = biquad (y12, fl2_v1, fl2_v2, fl2_a1, fl2_a2, fl2_b0*0.031617, fl2_b1*0.031617, fl2_b2*0.031617);
riannebulthuis 0:8c2484672018 82 // Notch
riannebulthuis 0:8c2484672018 83 y14 = biquad (y13, fno1_v1, fno1_v2, fno1_a1, fno1_a2, fno1_b0*1.004206, fno1_b1*1.004206, fno1_b2*1.004206);
riannebulthuis 0:8c2484672018 84 y15 = biquad (y14, fno2_v1, fno2_v2, fno2_a1, fno2_a2, fno2_b0, fno2_b1, fno2_b2);
riannebulthuis 0:8c2484672018 85 y16 = biquad (y15, fno3_v1, fno3_v2, fno3_a1, fno3_a2, fno3_b0*0.973227, fno3_b1*0.973227, fno3_b2*0.973227);
riannebulthuis 0:8c2484672018 86 // Rectify sample
riannebulthuis 0:8c2484672018 87 y17 = fabs(y16);
riannebulthuis 0:8c2484672018 88 // Make it smooth
riannebulthuis 0:8c2484672018 89 y18 = biquad (y17, flp1_v1, flp1_v2, flp1_a1, flp1_a2, flp1_b0* 0.010386, flp1_b1* 0.010386, flp1_b2* 0.010386);
riannebulthuis 0:8c2484672018 90 final_filter2 = biquad(y18, flp2_v1, flp2_v2, flp2_a1, flp2_a2, flp2_b0*0.000109, flp2_b1*0.000109, flp2_b2*0.000109);
riannebulthuis 0:8c2484672018 91 }
riannebulthuis 0:8c2484672018 92
riannebulthuis 0:8c2484672018 93 void HIDScope(){
riannebulthuis 0:8c2484672018 94 scope.set(0, u1);
riannebulthuis 0:8c2484672018 95 scope.set(1, u10);
riannebulthuis 0:8c2484672018 96 scope.set(2, final_filter1);
riannebulthuis 0:8c2484672018 97 scope.set(3, final_filter2);
riannebulthuis 0:8c2484672018 98 scope.send();
riannebulthuis 0:8c2484672018 99 }
riannebulthuis 0:8c2484672018 100
riannebulthuis 0:8c2484672018 101 int main ()
riannebulthuis 0:8c2484672018 102 {
riannebulthuis 0:8c2484672018 103 EMG.attach_us(Signaal_EMG,1e3);
riannebulthuis 0:8c2484672018 104 Scope.attach_us(HIDScope,1e3);
riannebulthuis 0:8c2484672018 105
riannebulthuis 0:8c2484672018 106 pc.printf("EMG_Kalibratie_biceps/n/r");
riannebulthuis 0:8c2484672018 107 lcd.locate(0,0);
riannebulthuis 0:8c2484672018 108 lcd.printf("start in 5 sec")
riannebulthuis 0:8c2484672018 109 lcd.locate(0,1);
riannebulthuis 0:8c2484672018 110 wait(4);
riannebulthuis 0:8c2484672018 111 lcd.printf("contract both biceps");
riannebulthuis 0:8c2484672018 112 lcd.locate(0,2);
riannebulthuis 0:8c2484672018 113 wait(1);
riannebulthuis 0:8c2484672018 114 lcd.printf("start");
riannebulthuis 0:8c2484672018 115 lcd.cls();
riannebulthuis 0:8c2484672018 116
riannebulthuis 0:8c2484672018 117 biceps_calibration.start();
riannebulthuis 0:8c2484672018 118 EMGCALIBRATION.attach_us(setregelaar_EMG_flag, 5e4);
riannebulthuis 0:8c2484672018 119
riannebulthuis 0:8c2484672018 120 while(biceps_calibration.read() =< 5) {
riannebulthuis 0:8c2484672018 121
riannebulthuis 0:8c2484672018 122 while(regulation_EMG_flag != true);
riannebulthuis 0:8c2484672018 123 regulation_EMG_flag = false;
riannebulthuis 0:8c2484672018 124
riannebulthuis 0:8c2484672018 125
riannebulthuis 0:8c2484672018 126
riannebulthuis 0:8c2484672018 127 }
riannebulthuis 0:8c2484672018 128