Richell Booyink / Mbed 2 deprecated EMG_Controlling

EMG controlling calibration + filters biorobotics group 13 BMT

Dependencies:   HIDScope biquadFilter mbed

main.cpp@10:e51b4c73131a, 2015-10-20 (annotated)

Committer:
RichellBooyink
Date:
Tue Oct 20 19:41:02 2015 +0000
Revision:
10:e51b4c73131a
Parent:
9:21e427de5ada 
Alles staat nu in 1 code. Het zou moeten werken. Inclusief kalibratie en t waarden (treshold waarden)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
RichellBooyink 0:2ded24549b70 1 #include "mbed.h"
RichellBooyink 0:2ded24549b70 2 #include "HIDScope.h"
RichellBooyink 0:2ded24549b70 3
RichellBooyink 4:8db85182a00d 4 AnalogIn EMG_bicepsleft(A0);
RichellBooyink 4:8db85182a00d 5 AnalogIn EMG_bicepsright (A1);
RichellBooyink 4:8db85182a00d 6 AnalogIn EMG_legleft (A2);
RichellBooyink 6:30698d9be7f8 7 AnalogIn EMG_legright (A3);
RichellBooyink 3:61f0fc41f3bc 8 HIDScope scope(4);
RichellBooyink 8:63a7b61ca2ae 9 Timer Timer_Calibration;
RichellBooyink 8:63a7b61ca2ae 10 Ticker EMG_Control;
RichellBooyink 8:63a7b61ca2ae 11
RichellBooyink 10:e51b4c73131a 12 bool Cali = false;
RichellBooyink 8:63a7b61ca2ae 13 double TimeCali = 5;
RichellBooyink 0:2ded24549b70 14
RichellBooyink 0:2ded24549b70 15 // Filter1 = High pass filter tot 20 Hz
RichellBooyink 0:2ded24549b70 16 double fh1_v1=0, fh1_v2=0, fh2_v1=0, fh2_v2=0;
RichellBooyink 0:2ded24549b70 17 const double fh1_a1=-0.84909054461, fh1_a2=0.00000000000, fh1_b0= 1, fh1_b1=-1, fh1_b2=0;
RichellBooyink 0:2ded24549b70 18 const double fh2_a1=-1.82553264091, fh2_a2=0.85001416809, fh2_b0= 1, fh2_b1=-2, fh2_b2=1;
RichellBooyink 0:2ded24549b70 19 // Filter2 = Low pass filter na 60 Hz
RichellBooyink 0:2ded24549b70 20 double fl1_v1=0, fl1_v2=0, fl2_v1=0, fl2_v2=0;
RichellBooyink 0:2ded24549b70 21 const double fl1_a1=-0.66979455390, fl1_a2=0.00000000000, fl1_b0= 1, fl1_b1=1, fl1_b2=0;
RichellBooyink 0:2ded24549b70 22 const double fl2_a1=-1.55376616139, fl2_a2=0.68023470431, fl2_b0= 1, fl2_b1=2, fl2_b2=1;
RichellBooyink 0:2ded24549b70 23 // Filter3 = Notch filter at 50 Hz
RichellBooyink 0:2ded24549b70 24 double fno1_v1=0, fno1_v2=0, fno2_v1=0, fno2_v2=0, fno3_v1=0, fno3_v2=0;
RichellBooyink 0:2ded24549b70 25 const double fno1_a1 = -1.87934916386, fno1_a2= 0.97731851355, fno1_b0= 1, fno1_b1= -1.90090686046, fno1_b2= 1;
RichellBooyink 0:2ded24549b70 26 const double fno2_a1 = -1.88341028603, fno2_a2= 0.98825147717, fno2_b0= 1, fno2_b1= -1.90090686046, fno2_b2= 1;
RichellBooyink 0:2ded24549b70 27 const double fno3_a1 = -1.89635403726, fno3_a2= 0.98894004849, fno3_b0= 1, fno3_b1= -1.90090686046, fno3_b2= 1;
RichellBooyink 4:8db85182a00d 28 // Filter4 = Lowpass filter at 5 Hz
RichellBooyink 4:8db85182a00d 29 double flp1_v1=0, flp1_v2=0, flp2_v1=0, flp2_v2=0;
RichellBooyink 4:8db85182a00d 30 const double flp1_a1=-0.97922725527, flp1_a2=0.00000000000, flp1_b0= 1, flp1_b1=1, flp1_b2=0;
RichellBooyink 4:8db85182a00d 31 const double flp2_a1=-1.97879353121, flp2_a2=0.97922951943, flp2_b0= 1, flp2_b1=2, flp2_b2=1;
RichellBooyink 9:21e427de5ada 32 double y1, y2, y3, y4, y5, y6, y7, y8, y9, y10, y11, y12, y13, y14, y15, y16, y17, y18, y19, y20, y21, y22, y23, y24, y25, y26, y27, y28, y29, y30, y31, y32, y33, y34, y35, y36;
RichellBooyink 9:21e427de5ada 33 double u1, u2, u3, u4, u5, u6, u7,u8 , u9, u10, u11, u12, u13, u14, u15, u16, u17, u18, u19, u20, u21, u22, u23, u24, u25, u26, u27, u28, u29, u30, u31, u32, u33, u34, u35, u36;
RichellBooyink 6:30698d9be7f8 34 double final_filter1, final_filter2, final_filter3, final_filter4;
RichellBooyink 6:30698d9be7f8 35 double fblmax = 0;
RichellBooyink 6:30698d9be7f8 36 double fbrmax = 0;
RichellBooyink 6:30698d9be7f8 37 double fllmax = 0;
RichellBooyink 6:30698d9be7f8 38 double flrmax = 0;
RichellBooyink 9:21e427de5ada 39 double Tbl, Tbr, Tll, Tlr;
RichellBooyink 3:61f0fc41f3bc 40
RichellBooyink 3:61f0fc41f3bc 41 // Standaard formule voor het biquad filter
RichellBooyink 3:61f0fc41f3bc 42 double biquad(double u, double &v1, double &v2, const double a1, const double a2, const double b0, const double b1, const double b2)
RichellBooyink 3:61f0fc41f3bc 43
RichellBooyink 6:30698d9be7f8 44 {
RichellBooyink 3:61f0fc41f3bc 45 double v = u - a1*v1 - a2*v2;
RichellBooyink 3:61f0fc41f3bc 46 double y = b0*v + b1*v1 + b2*v2;
RichellBooyink 3:61f0fc41f3bc 47 v2=v1;
RichellBooyink 3:61f0fc41f3bc 48 v1=v;
RichellBooyink 3:61f0fc41f3bc 49 return y;
RichellBooyink 3:61f0fc41f3bc 50 }
RichellBooyink 3:61f0fc41f3bc 51
RichellBooyink 10:e51b4c73131a 52 void MyController()
RichellBooyink 0:2ded24549b70 53 {
RichellBooyink 8:63a7b61ca2ae 54 // Biceps left
RichellBooyink 4:8db85182a00d 55 u1 = EMG_bicepsleft.read();
RichellBooyink 2:e3259334299e 56 //Highpass
RichellBooyink 3:61f0fc41f3bc 57 y1 = biquad (u1, fh1_v1, fh1_v2, fh1_a1, fh1_a2, fh1_b0*0.924547, fh1_b1*0.924547, fh1_b2*0.924547);
RichellBooyink 3:61f0fc41f3bc 58 u2 = y1;
RichellBooyink 3:61f0fc41f3bc 59 y2 = biquad (u2, fh2_v1, fh2_v2, fh2_a1, fh2_a2, fh2_b0*0.918885, fh2_b1*0.918885, fh2_b2*0.918885);
RichellBooyink 2:e3259334299e 60 //Lowpass
RichellBooyink 2:e3259334299e 61 u3 = y2;
RichellBooyink 3:61f0fc41f3bc 62 y3 = biquad (u3, fl1_v1, fl1_v2, fl1_a1, fl1_a2, fl1_b0*0.165103, fl1_b1*0.165103, fl1_b2*0.165103);
RichellBooyink 2:e3259334299e 63 u4 = y3;
RichellBooyink 3:61f0fc41f3bc 64 y4 = biquad (u4, fl2_v1, fl2_v2, fl2_a1, fl2_a2, fl2_b0*0.031617, fl2_b1*0.031617, fl2_b2*0.031617);
RichellBooyink 2:e3259334299e 65 // Notch
RichellBooyink 2:e3259334299e 66 u5 = y4;
RichellBooyink 3:61f0fc41f3bc 67 y5 = biquad (u5, fno1_v1, fno1_v2, fno1_a1, fno1_a2, fno1_b0*1.004206, fno1_b1*1.004206, fno1_b2*1.004206);
RichellBooyink 2:e3259334299e 68 u6 = y5;
RichellBooyink 3:61f0fc41f3bc 69 y6 = biquad (u6, fno2_v1, fno2_v2, fno2_a1, fno2_a2, fno2_b0, fno2_b1, fno2_b2);
RichellBooyink 2:e3259334299e 70 u7 = y6;
RichellBooyink 3:61f0fc41f3bc 71 y7 = biquad (u7, fno3_v1, fno3_v2, fno3_a1, fno3_a2, fno3_b0*0.973227, fno3_b1*0.973227, fno3_b2*0.973227);
RichellBooyink 6:30698d9be7f8 72 // Rectify sample
RichellBooyink 4:8db85182a00d 73 y8 = fabs(y7);
RichellBooyink 6:30698d9be7f8 74 // Make it smooth
RichellBooyink 6:30698d9be7f8 75 u8 = y8;
RichellBooyink 4:8db85182a00d 76 y9 = biquad (u8, flp1_v1, flp1_v2, flp1_a1, flp1_a2, flp1_b0* 0.010386, flp1_b1* 0.010386, flp1_b2* 0.010386);
RichellBooyink 4:8db85182a00d 77 u9 = y9;
RichellBooyink 6:30698d9be7f8 78 final_filter1 = biquad(u9, flp2_v1, flp2_v2, flp2_a1, flp2_a2, flp2_b0*0.000109, flp2_b1*0.000109, flp2_b2*0.000109);
RichellBooyink 8:63a7b61ca2ae 79 //Biceps right
RichellBooyink 8:63a7b61ca2ae 80 u10 = EMG_bicepsright.read();
RichellBooyink 8:63a7b61ca2ae 81 //Highpass
RichellBooyink 8:63a7b61ca2ae 82 y10 = biquad (u10, fh1_v1, fh1_v2, fh1_a1, fh1_a2, fh1_b0*0.924547, fh1_b1*0.924547, fh1_b2*0.924547);
RichellBooyink 8:63a7b61ca2ae 83 u11 = y10;
RichellBooyink 8:63a7b61ca2ae 84 y11 = biquad (u11, fh2_v1, fh2_v2, fh2_a1, fh2_a2, fh2_b0*0.918885, fh2_b1*0.918885, fh2_b2*0.918885);
RichellBooyink 8:63a7b61ca2ae 85 //Lowpass
RichellBooyink 8:63a7b61ca2ae 86 u12 = y11;
RichellBooyink 8:63a7b61ca2ae 87 y12 = biquad (u12, fl1_v1, fl1_v2, fl1_a1, fl1_a2, fl1_b0*0.165103, fl1_b1*0.165103, fl1_b2*0.165103);
RichellBooyink 8:63a7b61ca2ae 88 u13 = y12;
RichellBooyink 8:63a7b61ca2ae 89 y13 = biquad (u13, fl2_v1, fl2_v2, fl2_a1, fl2_a2, fl2_b0*0.031617, fl2_b1*0.031617, fl2_b2*0.031617);
RichellBooyink 8:63a7b61ca2ae 90 // Notch
RichellBooyink 8:63a7b61ca2ae 91 u14 = y13;
RichellBooyink 8:63a7b61ca2ae 92 y14 = biquad (u14, fno1_v1, fno1_v2, fno1_a1, fno1_a2, fno1_b0*1.004206, fno1_b1*1.004206, fno1_b2*1.004206);
RichellBooyink 8:63a7b61ca2ae 93 u15 = y14;
RichellBooyink 8:63a7b61ca2ae 94 y15 = biquad (u15, fno2_v1, fno2_v2, fno2_a1, fno2_a2, fno2_b0, fno2_b1, fno2_b2);
RichellBooyink 8:63a7b61ca2ae 95 u16 = y15;
RichellBooyink 8:63a7b61ca2ae 96 y16 = biquad (u16, fno3_v1, fno3_v2, fno3_a1, fno3_a2, fno3_b0*0.973227, fno3_b1*0.973227, fno3_b2*0.973227);
RichellBooyink 8:63a7b61ca2ae 97 // Rectify sample
RichellBooyink 8:63a7b61ca2ae 98 y17 = fabs(y16);
RichellBooyink 8:63a7b61ca2ae 99 // Make it smooth
RichellBooyink 8:63a7b61ca2ae 100 u17 = y17;
RichellBooyink 8:63a7b61ca2ae 101 y18 = biquad (u17, flp1_v1, flp1_v2, flp1_a1, flp1_a2, flp1_b0* 0.010386, flp1_b1* 0.010386, flp1_b2* 0.010386);
RichellBooyink 8:63a7b61ca2ae 102 u18 = y18;
RichellBooyink 8:63a7b61ca2ae 103 final_filter2 = biquad(u18, flp2_v1, flp2_v2, flp2_a1, flp2_a2, flp2_b0*0.000109, flp2_b1*0.000109, flp2_b2*0.000109);
RichellBooyink 8:63a7b61ca2ae 104 /// EMG Filter left leg
RichellBooyink 8:63a7b61ca2ae 105 u19 = EMG_legleft.read();
RichellBooyink 8:63a7b61ca2ae 106 //Highpass
RichellBooyink 8:63a7b61ca2ae 107 y19 = biquad (u19, fh1_v1, fh1_v2, fh1_a1, fh1_a2, fh1_b0*0.924547, fh1_b1*0.924547, fh1_b2*0.924547);
RichellBooyink 8:63a7b61ca2ae 108 u20 = y19;
RichellBooyink 8:63a7b61ca2ae 109 y20 = biquad (u20, fh2_v1, fh2_v2, fh2_a1, fh2_a2, fh2_b0*0.918885, fh2_b1*0.918885, fh2_b2*0.918885);
RichellBooyink 8:63a7b61ca2ae 110 //Lowpass
RichellBooyink 8:63a7b61ca2ae 111 u21 = y20;
RichellBooyink 8:63a7b61ca2ae 112 y21 = biquad (u21, fl1_v1, fl1_v2, fl1_a1, fl1_a2, fl1_b0*0.165103, fl1_b1*0.165103, fl1_b2*0.165103);
RichellBooyink 8:63a7b61ca2ae 113 u22 = y21;
RichellBooyink 8:63a7b61ca2ae 114 y22 = biquad (u22, fl2_v1, fl2_v2, fl2_a1, fl2_a2, fl2_b0*0.031617, fl2_b1*0.031617, fl2_b2*0.031617);
RichellBooyink 8:63a7b61ca2ae 115 // Notch
RichellBooyink 8:63a7b61ca2ae 116 u23 = y22;
RichellBooyink 8:63a7b61ca2ae 117 y23 = biquad (u23, fno1_v1, fno1_v2, fno1_a1, fno1_a2, fno1_b0*1.004206, fno1_b1*1.004206, fno1_b2*1.004206);
RichellBooyink 8:63a7b61ca2ae 118 u24 = y23;
RichellBooyink 8:63a7b61ca2ae 119 y24 = biquad (u24, fno2_v1, fno2_v2, fno2_a1, fno2_a2, fno2_b0, fno2_b1, fno2_b2);
RichellBooyink 8:63a7b61ca2ae 120 u25 = y24;
RichellBooyink 8:63a7b61ca2ae 121 y25 = biquad (u25, fno3_v1, fno3_v2, fno3_a1, fno3_a2, fno3_b0*0.973227, fno3_b1*0.973227, fno3_b2*0.973227);
RichellBooyink 8:63a7b61ca2ae 122 // Rectify sample
RichellBooyink 8:63a7b61ca2ae 123 y26 = fabs(y7);
RichellBooyink 8:63a7b61ca2ae 124 // Make it smooth
RichellBooyink 8:63a7b61ca2ae 125 u26 = y26;
RichellBooyink 8:63a7b61ca2ae 126 y27 = biquad (u26, flp1_v1, flp1_v2, flp1_a1, flp1_a2, flp1_b0* 0.010386, flp1_b1* 0.010386, flp1_b2* 0.010386);
RichellBooyink 8:63a7b61ca2ae 127 u27 = y27;
RichellBooyink 8:63a7b61ca2ae 128 final_filter3 = biquad(u27, flp2_v1, flp2_v2, flp2_a1, flp2_a2, flp2_b0*0.000109, flp2_b1*0.000109, flp2_b2*0.000109);
RichellBooyink 8:63a7b61ca2ae 129 // EMG filter right leg
RichellBooyink 8:63a7b61ca2ae 130 u28 = EMG_legright.read();
RichellBooyink 8:63a7b61ca2ae 131 //Highpass
RichellBooyink 8:63a7b61ca2ae 132 y28 = biquad (u8, fh1_v1, fh1_v2, fh1_a1, fh1_a2, fh1_b0*0.924547, fh1_b1*0.924547, fh1_b2*0.924547);
RichellBooyink 8:63a7b61ca2ae 133 u29 = y28;
RichellBooyink 8:63a7b61ca2ae 134 y29 = biquad (u29, fh2_v1, fh2_v2, fh2_a1, fh2_a2, fh2_b0*0.918885, fh2_b1*0.918885, fh2_b2*0.918885);
RichellBooyink 8:63a7b61ca2ae 135 //Lowpass
RichellBooyink 8:63a7b61ca2ae 136 u30 = y29;
RichellBooyink 8:63a7b61ca2ae 137 y30 = biquad (u30, fl1_v1, fl1_v2, fl1_a1, fl1_a2, fl1_b0*0.165103, fl1_b1*0.165103, fl1_b2*0.165103);
RichellBooyink 8:63a7b61ca2ae 138 u31 = y30;
RichellBooyink 8:63a7b61ca2ae 139 y31 = biquad (u31, fl2_v1, fl2_v2, fl2_a1, fl2_a2, fl2_b0*0.031617, fl2_b1*0.031617, fl2_b2*0.031617);
RichellBooyink 8:63a7b61ca2ae 140 // Notch
RichellBooyink 8:63a7b61ca2ae 141 u32 = y31;
RichellBooyink 8:63a7b61ca2ae 142 y32 = biquad (u32, fno1_v1, fno1_v2, fno1_a1, fno1_a2, fno1_b0*1.004206, fno1_b1*1.004206, fno1_b2*1.004206);
RichellBooyink 8:63a7b61ca2ae 143 u33 = y32;
RichellBooyink 8:63a7b61ca2ae 144 y33 = biquad (u33, fno2_v1, fno2_v2, fno2_a1, fno2_a2, fno2_b0, fno2_b1, fno2_b2);
RichellBooyink 8:63a7b61ca2ae 145 u34 = y33;
RichellBooyink 8:63a7b61ca2ae 146 y34 = biquad (u34, fno3_v1, fno3_v2, fno3_a1, fno3_a2, fno3_b0*0.973227, fno3_b1*0.973227, fno3_b2*0.973227);
RichellBooyink 8:63a7b61ca2ae 147 // Rectify sample
RichellBooyink 8:63a7b61ca2ae 148 y35 = fabs(y34);
RichellBooyink 8:63a7b61ca2ae 149 // Make it smooth
RichellBooyink 8:63a7b61ca2ae 150 u35 = y35;
RichellBooyink 8:63a7b61ca2ae 151 y36 = biquad (u35, flp1_v1, flp1_v2, flp1_a1, flp1_a2, flp1_b0* 0.010386, flp1_b1* 0.010386, flp1_b2* 0.010386);
RichellBooyink 8:63a7b61ca2ae 152 u36 = y36;
RichellBooyink 8:63a7b61ca2ae 153 final_filter4 = biquad(u36, flp2_v1, flp2_v2, flp2_a1, flp2_a2, flp2_b0*0.000109, flp2_b1*0.000109, flp2_b2*0.000109);
RichellBooyink 10:e51b4c73131a 154
RichellBooyink 10:e51b4c73131a 155 scope.set (0,final_filter1);
RichellBooyink 10:e51b4c73131a 156 scope.set (1,final_filter2);
RichellBooyink 10:e51b4c73131a 157 scope.set (2,final_filter3);
RichellBooyink 10:e51b4c73131a 158 scope.set (3,final_filter4);
RichellBooyink 10:e51b4c73131a 159 scope.send ();
RichellBooyink 10:e51b4c73131a 160
RichellBooyink 8:63a7b61ca2ae 161 if (Cali == true) {
RichellBooyink 8:63a7b61ca2ae 162 if (final_filter1 >= fblmax) {
RichellBooyink 8:63a7b61ca2ae 163 fblmax=final_filter1;
RichellBooyink 8:63a7b61ca2ae 164 }
RichellBooyink 8:63a7b61ca2ae 165 if (final_filter2 >= fbrmax) {
RichellBooyink 8:63a7b61ca2ae 166 fbrmax = final_filter2;
RichellBooyink 8:63a7b61ca2ae 167 }
RichellBooyink 8:63a7b61ca2ae 168 if (final_filter3 >= fllmax) {
RichellBooyink 8:63a7b61ca2ae 169 fllmax=final_filter3;
RichellBooyink 8:63a7b61ca2ae 170 }
RichellBooyink 8:63a7b61ca2ae 171 if (final_filter4 >= flrmax) {
RichellBooyink 8:63a7b61ca2ae 172 flrmax=final_filter4;
RichellBooyink 8:63a7b61ca2ae 173 }
RichellBooyink 8:63a7b61ca2ae 174 Tbl = 0.8*fblmax;
RichellBooyink 8:63a7b61ca2ae 175 Tbr = 0.8*fbrmax;
RichellBooyink 8:63a7b61ca2ae 176 Tll = 0.8*fllmax;
RichellBooyink 8:63a7b61ca2ae 177 Tlr = 0.8*flrmax;
RichellBooyink 9:21e427de5ada 178 wait (3);
RichellBooyink 8:63a7b61ca2ae 179 }
RichellBooyink 6:30698d9be7f8 180 }
RichellBooyink 6:30698d9be7f8 181
RichellBooyink 0:2ded24549b70 182 int main ()
RichellBooyink 6:30698d9be7f8 183 {
RichellBooyink 10:e51b4c73131a 184 EMG_Control.attach_us(MyController,1e3);
RichellBooyink 8:63a7b61ca2ae 185 Timer_Calibration.start();
RichellBooyink 9:21e427de5ada 186 if (Timer_Calibration < TimeCali) {
RichellBooyink 8:63a7b61ca2ae 187 Cali = true;
RichellBooyink 10:e51b4c73131a 188 } else {
RichellBooyink 8:63a7b61ca2ae 189 Cali = false;
RichellBooyink 8:63a7b61ca2ae 190 }
RichellBooyink 8:63a7b61ca2ae 191 Timer_Calibration.stop();
RichellBooyink 8:63a7b61ca2ae 192 Timer_Calibration.reset();
RichellBooyink 8:63a7b61ca2ae 193 }
RichellBooyink 8:63a7b61ca2ae 194