Richell Booyink / Mbed 2 deprecated EMG_Controlling

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   HIDScope biquadFilter mbed

main.cpp@6:30698d9be7f8, 2015-10-20 (annotated)

Committer:
RichellBooyink
Date:
Tue Oct 20 12:40:29 2015 +0000
Revision:
6:30698d9be7f8
Parent:
5:e78295b978ab
Child:
7:d6416ed1672c
Included the calibration for the EMG signals

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 6:30698d9be7f8 9 Timer cali;
RichellBooyink 0:2ded24549b70 10
RichellBooyink 0:2ded24549b70 11 // Filter1 = High pass filter tot 20 Hz
RichellBooyink 0:2ded24549b70 12 double fh1_v1=0, fh1_v2=0, fh2_v1=0, fh2_v2=0;
RichellBooyink 0:2ded24549b70 13 const double fh1_a1=-0.84909054461, fh1_a2=0.00000000000, fh1_b0= 1, fh1_b1=-1, fh1_b2=0;
RichellBooyink 0:2ded24549b70 14 const double fh2_a1=-1.82553264091, fh2_a2=0.85001416809, fh2_b0= 1, fh2_b1=-2, fh2_b2=1;
RichellBooyink 0:2ded24549b70 15 // Filter2 = Low pass filter na 60 Hz
RichellBooyink 0:2ded24549b70 16 double fl1_v1=0, fl1_v2=0, fl2_v1=0, fl2_v2=0;
RichellBooyink 0:2ded24549b70 17 const double fl1_a1=-0.66979455390, fl1_a2=0.00000000000, fl1_b0= 1, fl1_b1=1, fl1_b2=0;
RichellBooyink 0:2ded24549b70 18 const double fl2_a1=-1.55376616139, fl2_a2=0.68023470431, fl2_b0= 1, fl2_b1=2, fl2_b2=1;
RichellBooyink 0:2ded24549b70 19 // Filter3 = Notch filter at 50 Hz
RichellBooyink 0:2ded24549b70 20 double fno1_v1=0, fno1_v2=0, fno2_v1=0, fno2_v2=0, fno3_v1=0, fno3_v2=0;
RichellBooyink 0:2ded24549b70 21 const double fno1_a1 = -1.87934916386, fno1_a2= 0.97731851355, fno1_b0= 1, fno1_b1= -1.90090686046, fno1_b2= 1;
RichellBooyink 0:2ded24549b70 22 const double fno2_a1 = -1.88341028603, fno2_a2= 0.98825147717, fno2_b0= 1, fno2_b1= -1.90090686046, fno2_b2= 1;
RichellBooyink 0:2ded24549b70 23 const double fno3_a1 = -1.89635403726, fno3_a2= 0.98894004849, fno3_b0= 1, fno3_b1= -1.90090686046, fno3_b2= 1;
RichellBooyink 4:8db85182a00d 24 // Filter4 = Lowpass filter at 5 Hz
RichellBooyink 4:8db85182a00d 25 double flp1_v1=0, flp1_v2=0, flp2_v1=0, flp2_v2=0;
RichellBooyink 4:8db85182a00d 26 const double flp1_a1=-0.97922725527, flp1_a2=0.00000000000, flp1_b0= 1, flp1_b1=1, flp1_b2=0;
RichellBooyink 4:8db85182a00d 27 const double flp2_a1=-1.97879353121, flp2_a2=0.97922951943, flp2_b0= 1, flp2_b1=2, flp2_b2=1;
RichellBooyink 6:30698d9be7f8 28 double y1, y2, y3, y4, y5, y6, y7, y8, y9, u1, u2, u3, u4, u5, u6, u7,u8 , u9 ;
RichellBooyink 6:30698d9be7f8 29 double final_filter1, final_filter2, final_filter3, final_filter4;
RichellBooyink 6:30698d9be7f8 30 double fblmax = 0;
RichellBooyink 6:30698d9be7f8 31 double fbrmax = 0;
RichellBooyink 6:30698d9be7f8 32 double fllmax = 0;
RichellBooyink 6:30698d9be7f8 33 double flrmax = 0;
RichellBooyink 6:30698d9be7f8 34
RichellBooyink 6:30698d9be7f8 35 void Calibrations ()
RichellBooyink 6:30698d9be7f8 36 {
RichellBooyink 6:30698d9be7f8 37 cali.start();
RichellBooyink 6:30698d9be7f8 38 while (cali.read()<= 5) {
RichellBooyink 6:30698d9be7f8 39 if (final_filter1 >= fblmax) {
RichellBooyink 6:30698d9be7f8 40 fblmax=final_filter1;
RichellBooyink 6:30698d9be7f8 41 }
RichellBooyink 6:30698d9be7f8 42 if (final_filter2 >= fbrmax) {
RichellBooyink 6:30698d9be7f8 43 fbrmax = final_filter2;
RichellBooyink 6:30698d9be7f8 44 }
RichellBooyink 6:30698d9be7f8 45 if (final_filter3 >= fllmax) {
RichellBooyink 6:30698d9be7f8 46 fllmax=final_filter3;
RichellBooyink 6:30698d9be7f8 47 }
RichellBooyink 6:30698d9be7f8 48 if (final_filter4 >= flrmax) {
RichellBooyink 6:30698d9be7f8 49 flrmax=final_filter4;
RichellBooyink 6:30698d9be7f8 50 }
RichellBooyink 6:30698d9be7f8 51 }
RichellBooyink 6:30698d9be7f8 52 }
RichellBooyink 6:30698d9be7f8 53
RichellBooyink 3:61f0fc41f3bc 54
RichellBooyink 3:61f0fc41f3bc 55 // Standaard formule voor het biquad filter
RichellBooyink 3:61f0fc41f3bc 56 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 57
RichellBooyink 6:30698d9be7f8 58 {
RichellBooyink 3:61f0fc41f3bc 59 double v = u - a1*v1 - a2*v2;
RichellBooyink 3:61f0fc41f3bc 60 double y = b0*v + b1*v1 + b2*v2;
RichellBooyink 3:61f0fc41f3bc 61 v2=v1;
RichellBooyink 3:61f0fc41f3bc 62 v1=v;
RichellBooyink 3:61f0fc41f3bc 63 return y;
RichellBooyink 3:61f0fc41f3bc 64 }
RichellBooyink 3:61f0fc41f3bc 65
RichellBooyink 5:e78295b978ab 66 void Filters_bicepsleft()
RichellBooyink 0:2ded24549b70 67 {
RichellBooyink 4:8db85182a00d 68 u1 = EMG_bicepsleft.read();
RichellBooyink 2:e3259334299e 69 //Highpass
RichellBooyink 3:61f0fc41f3bc 70 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 71 u2 = y1;
RichellBooyink 3:61f0fc41f3bc 72 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 73 //Lowpass
RichellBooyink 2:e3259334299e 74 u3 = y2;
RichellBooyink 3:61f0fc41f3bc 75 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 76 u4 = y3;
RichellBooyink 3:61f0fc41f3bc 77 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 78 // Notch
RichellBooyink 2:e3259334299e 79 u5 = y4;
RichellBooyink 3:61f0fc41f3bc 80 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 81 u6 = y5;
RichellBooyink 3:61f0fc41f3bc 82 y6 = biquad (u6, fno2_v1, fno2_v2, fno2_a1, fno2_a2, fno2_b0, fno2_b1, fno2_b2);
RichellBooyink 2:e3259334299e 83 u7 = y6;
RichellBooyink 3:61f0fc41f3bc 84 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 85 // Rectify sample
RichellBooyink 4:8db85182a00d 86 y8 = fabs(y7);
RichellBooyink 6:30698d9be7f8 87 // Make it smooth
RichellBooyink 6:30698d9be7f8 88 u8 = y8;
RichellBooyink 4:8db85182a00d 89 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 90 u9 = y9;
RichellBooyink 6:30698d9be7f8 91 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 6:30698d9be7f8 92 }
RichellBooyink 6:30698d9be7f8 93
RichellBooyink 6:30698d9be7f8 94 void Filters_bicepsright()
RichellBooyink 5:e78295b978ab 95 {
RichellBooyink 5:e78295b978ab 96 u1 = EMG_bicepsright.read();
RichellBooyink 5:e78295b978ab 97 //Highpass
RichellBooyink 5:e78295b978ab 98 y1 = biquad (u1, fh1_v1, fh1_v2, fh1_a1, fh1_a2, fh1_b0*0.924547, fh1_b1*0.924547, fh1_b2*0.924547);
RichellBooyink 5:e78295b978ab 99 u2 = y1;
RichellBooyink 5:e78295b978ab 100 y2 = biquad (u2, fh2_v1, fh2_v2, fh2_a1, fh2_a2, fh2_b0*0.918885, fh2_b1*0.918885, fh2_b2*0.918885);
RichellBooyink 5:e78295b978ab 101 //Lowpass
RichellBooyink 5:e78295b978ab 102 u3 = y2;
RichellBooyink 5:e78295b978ab 103 y3 = biquad (u3, fl1_v1, fl1_v2, fl1_a1, fl1_a2, fl1_b0*0.165103, fl1_b1*0.165103, fl1_b2*0.165103);
RichellBooyink 5:e78295b978ab 104 u4 = y3;
RichellBooyink 5:e78295b978ab 105 y4 = biquad (u4, fl2_v1, fl2_v2, fl2_a1, fl2_a2, fl2_b0*0.031617, fl2_b1*0.031617, fl2_b2*0.031617);
RichellBooyink 5:e78295b978ab 106 // Notch
RichellBooyink 5:e78295b978ab 107 u5 = y4;
RichellBooyink 5:e78295b978ab 108 y5 = biquad (u5, fno1_v1, fno1_v2, fno1_a1, fno1_a2, fno1_b0*1.004206, fno1_b1*1.004206, fno1_b2*1.004206);
RichellBooyink 5:e78295b978ab 109 u6 = y5;
RichellBooyink 5:e78295b978ab 110 y6 = biquad (u6, fno2_v1, fno2_v2, fno2_a1, fno2_a2, fno2_b0, fno2_b1, fno2_b2);
RichellBooyink 5:e78295b978ab 111 u7 = y6;
RichellBooyink 5:e78295b978ab 112 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 113 // Rectify sample
RichellBooyink 5:e78295b978ab 114 y8 = fabs(y7);
RichellBooyink 6:30698d9be7f8 115 // Make it smooth
RichellBooyink 6:30698d9be7f8 116 u8 = y8;
RichellBooyink 5:e78295b978ab 117 y9 = biquad (u8, flp1_v1, flp1_v2, flp1_a1, flp1_a2, flp1_b0* 0.010386, flp1_b1* 0.010386, flp1_b2* 0.010386);
RichellBooyink 5:e78295b978ab 118 u9 = y9;
RichellBooyink 6:30698d9be7f8 119 final_filter2 = biquad(u9, flp2_v1, flp2_v2, flp2_a1, flp2_a2, flp2_b0*0.000109, flp2_b1*0.000109, flp2_b2*0.000109);
RichellBooyink 6:30698d9be7f8 120 }
RichellBooyink 6:30698d9be7f8 121 void Filters_legleft()
RichellBooyink 5:e78295b978ab 122 {
RichellBooyink 5:e78295b978ab 123 u1 = EMG_legleft.read();
RichellBooyink 5:e78295b978ab 124 //Highpass
RichellBooyink 5:e78295b978ab 125 y1 = biquad (u1, fh1_v1, fh1_v2, fh1_a1, fh1_a2, fh1_b0*0.924547, fh1_b1*0.924547, fh1_b2*0.924547);
RichellBooyink 5:e78295b978ab 126 u2 = y1;
RichellBooyink 5:e78295b978ab 127 y2 = biquad (u2, fh2_v1, fh2_v2, fh2_a1, fh2_a2, fh2_b0*0.918885, fh2_b1*0.918885, fh2_b2*0.918885);
RichellBooyink 5:e78295b978ab 128 //Lowpass
RichellBooyink 5:e78295b978ab 129 u3 = y2;
RichellBooyink 5:e78295b978ab 130 y3 = biquad (u3, fl1_v1, fl1_v2, fl1_a1, fl1_a2, fl1_b0*0.165103, fl1_b1*0.165103, fl1_b2*0.165103);
RichellBooyink 5:e78295b978ab 131 u4 = y3;
RichellBooyink 5:e78295b978ab 132 y4 = biquad (u4, fl2_v1, fl2_v2, fl2_a1, fl2_a2, fl2_b0*0.031617, fl2_b1*0.031617, fl2_b2*0.031617);
RichellBooyink 5:e78295b978ab 133 // Notch
RichellBooyink 5:e78295b978ab 134 u5 = y4;
RichellBooyink 5:e78295b978ab 135 y5 = biquad (u5, fno1_v1, fno1_v2, fno1_a1, fno1_a2, fno1_b0*1.004206, fno1_b1*1.004206, fno1_b2*1.004206);
RichellBooyink 5:e78295b978ab 136 u6 = y5;
RichellBooyink 5:e78295b978ab 137 y6 = biquad (u6, fno2_v1, fno2_v2, fno2_a1, fno2_a2, fno2_b0, fno2_b1, fno2_b2);
RichellBooyink 5:e78295b978ab 138 u7 = y6;
RichellBooyink 5:e78295b978ab 139 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 140 // Rectify sample
RichellBooyink 5:e78295b978ab 141 y8 = fabs(y7);
RichellBooyink 6:30698d9be7f8 142 // Make it smooth
RichellBooyink 6:30698d9be7f8 143 u8 = y8;
RichellBooyink 5:e78295b978ab 144 y9 = biquad (u8, flp1_v1, flp1_v2, flp1_a1, flp1_a2, flp1_b0* 0.010386, flp1_b1* 0.010386, flp1_b2* 0.010386);
RichellBooyink 5:e78295b978ab 145 u9 = y9;
RichellBooyink 6:30698d9be7f8 146 final_filter3 = biquad(u9, flp2_v1, flp2_v2, flp2_a1, flp2_a2, flp2_b0*0.000109, flp2_b1*0.000109, flp2_b2*0.000109);
RichellBooyink 6:30698d9be7f8 147 }
RichellBooyink 6:30698d9be7f8 148 void Filters_legright()
RichellBooyink 5:e78295b978ab 149 {
RichellBooyink 5:e78295b978ab 150 u1 = EMG_legright.read();
RichellBooyink 5:e78295b978ab 151 //Highpass
RichellBooyink 5:e78295b978ab 152 y1 = biquad (u1, fh1_v1, fh1_v2, fh1_a1, fh1_a2, fh1_b0*0.924547, fh1_b1*0.924547, fh1_b2*0.924547);
RichellBooyink 5:e78295b978ab 153 u2 = y1;
RichellBooyink 5:e78295b978ab 154 y2 = biquad (u2, fh2_v1, fh2_v2, fh2_a1, fh2_a2, fh2_b0*0.918885, fh2_b1*0.918885, fh2_b2*0.918885);
RichellBooyink 5:e78295b978ab 155 //Lowpass
RichellBooyink 5:e78295b978ab 156 u3 = y2;
RichellBooyink 5:e78295b978ab 157 y3 = biquad (u3, fl1_v1, fl1_v2, fl1_a1, fl1_a2, fl1_b0*0.165103, fl1_b1*0.165103, fl1_b2*0.165103);
RichellBooyink 5:e78295b978ab 158 u4 = y3;
RichellBooyink 5:e78295b978ab 159 y4 = biquad (u4, fl2_v1, fl2_v2, fl2_a1, fl2_a2, fl2_b0*0.031617, fl2_b1*0.031617, fl2_b2*0.031617);
RichellBooyink 5:e78295b978ab 160 // Notch
RichellBooyink 5:e78295b978ab 161 u5 = y4;
RichellBooyink 5:e78295b978ab 162 y5 = biquad (u5, fno1_v1, fno1_v2, fno1_a1, fno1_a2, fno1_b0*1.004206, fno1_b1*1.004206, fno1_b2*1.004206);
RichellBooyink 5:e78295b978ab 163 u6 = y5;
RichellBooyink 5:e78295b978ab 164 y6 = biquad (u6, fno2_v1, fno2_v2, fno2_a1, fno2_a2, fno2_b0, fno2_b1, fno2_b2);
RichellBooyink 5:e78295b978ab 165 u7 = y6;
RichellBooyink 5:e78295b978ab 166 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 167 // Rectify sample
RichellBooyink 5:e78295b978ab 168 y8 = fabs(y7);
RichellBooyink 6:30698d9be7f8 169 // Make it smooth
RichellBooyink 6:30698d9be7f8 170 u8 = y8;
RichellBooyink 5:e78295b978ab 171 y9 = biquad (u8, flp1_v1, flp1_v2, flp1_a1, flp1_a2, flp1_b0* 0.010386, flp1_b1* 0.010386, flp1_b2* 0.010386);
RichellBooyink 5:e78295b978ab 172 u9 = y9;
RichellBooyink 6:30698d9be7f8 173 final_filter4 = biquad(u9, flp2_v1, flp2_v2, flp2_a1, flp2_a2, flp2_b0*0.000109, flp2_b1*0.000109, flp2_b2*0.000109);
RichellBooyink 6:30698d9be7f8 174 }
RichellBooyink 5:e78295b978ab 175 // Send signals to HIDScope
RichellBooyink 6:30698d9be7f8 176 void Filters_EMG ()
RichellBooyink 6:30698d9be7f8 177 {
RichellBooyink 6:30698d9be7f8 178 scope.set (0,final_filter1);
RichellBooyink 6:30698d9be7f8 179 scope.set (1,final_filter2);
RichellBooyink 6:30698d9be7f8 180 scope.set (2,final_filter3);
RichellBooyink 6:30698d9be7f8 181 scope.set (3,final_filter4);
RichellBooyink 1:16165e207e70 182 scope.send ();
RichellBooyink 6:30698d9be7f8 183 }
RichellBooyink 6:30698d9be7f8 184
RichellBooyink 2:e3259334299e 185 Ticker filter;
RichellBooyink 0:2ded24549b70 186 int main ()
RichellBooyink 6:30698d9be7f8 187 {
RichellBooyink 6:30698d9be7f8 188 filter.attach_us(Filters_EMG,1e3);
RichellBooyink 6:30698d9be7f8 189 }
RichellBooyink 0:2ded24549b70 190
RichellBooyink 0:2ded24549b70 191
RichellBooyink 0:2ded24549b70 192
RichellBooyink 2:e3259334299e 193
RichellBooyink 2:e3259334299e 194