Rianne Bulthuis
/
EMG_test
incl. calibration
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?
User | Revision | Line number | New 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 |