EMG controlling calibration + filters biorobotics group 13 BMT
Dependencies: HIDScope biquadFilter mbed
main.cpp
- Committer:
- RichellBooyink
- Date:
- 2015-10-20
- Revision:
- 10:e51b4c73131a
- Parent:
- 9:21e427de5ada
File content as of revision 10:e51b4c73131a:
#include "mbed.h" #include "HIDScope.h" AnalogIn EMG_bicepsleft(A0); AnalogIn EMG_bicepsright (A1); AnalogIn EMG_legleft (A2); AnalogIn EMG_legright (A3); HIDScope scope(4); Timer Timer_Calibration; Ticker EMG_Control; bool Cali = false; double TimeCali = 5; // Filter1 = High pass filter tot 20 Hz double fh1_v1=0, fh1_v2=0, fh2_v1=0, fh2_v2=0; const double fh1_a1=-0.84909054461, fh1_a2=0.00000000000, fh1_b0= 1, fh1_b1=-1, fh1_b2=0; const double fh2_a1=-1.82553264091, fh2_a2=0.85001416809, fh2_b0= 1, fh2_b1=-2, fh2_b2=1; // Filter2 = Low pass filter na 60 Hz double fl1_v1=0, fl1_v2=0, fl2_v1=0, fl2_v2=0; const double fl1_a1=-0.66979455390, fl1_a2=0.00000000000, fl1_b0= 1, fl1_b1=1, fl1_b2=0; const double fl2_a1=-1.55376616139, fl2_a2=0.68023470431, fl2_b0= 1, fl2_b1=2, fl2_b2=1; // Filter3 = Notch filter at 50 Hz double fno1_v1=0, fno1_v2=0, fno2_v1=0, fno2_v2=0, fno3_v1=0, fno3_v2=0; const double fno1_a1 = -1.87934916386, fno1_a2= 0.97731851355, fno1_b0= 1, fno1_b1= -1.90090686046, fno1_b2= 1; const double fno2_a1 = -1.88341028603, fno2_a2= 0.98825147717, fno2_b0= 1, fno2_b1= -1.90090686046, fno2_b2= 1; const double fno3_a1 = -1.89635403726, fno3_a2= 0.98894004849, fno3_b0= 1, fno3_b1= -1.90090686046, fno3_b2= 1; // Filter4 = Lowpass filter at 5 Hz double flp1_v1=0, flp1_v2=0, flp2_v1=0, flp2_v2=0; const double flp1_a1=-0.97922725527, flp1_a2=0.00000000000, flp1_b0= 1, flp1_b1=1, flp1_b2=0; const double flp2_a1=-1.97879353121, flp2_a2=0.97922951943, flp2_b0= 1, flp2_b1=2, flp2_b2=1; 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; 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; double final_filter1, final_filter2, final_filter3, final_filter4; double fblmax = 0; double fbrmax = 0; double fllmax = 0; double flrmax = 0; double Tbl, Tbr, Tll, Tlr; // Standaard formule voor het biquad filter double biquad(double u, double &v1, double &v2, const double a1, const double a2, const double b0, const double b1, const double b2) { double v = u - a1*v1 - a2*v2; double y = b0*v + b1*v1 + b2*v2; v2=v1; v1=v; return y; } void MyController() { // Biceps left u1 = EMG_bicepsleft.read(); //Highpass y1 = biquad (u1, fh1_v1, fh1_v2, fh1_a1, fh1_a2, fh1_b0*0.924547, fh1_b1*0.924547, fh1_b2*0.924547); u2 = y1; y2 = biquad (u2, fh2_v1, fh2_v2, fh2_a1, fh2_a2, fh2_b0*0.918885, fh2_b1*0.918885, fh2_b2*0.918885); //Lowpass u3 = y2; y3 = biquad (u3, fl1_v1, fl1_v2, fl1_a1, fl1_a2, fl1_b0*0.165103, fl1_b1*0.165103, fl1_b2*0.165103); u4 = y3; y4 = biquad (u4, fl2_v1, fl2_v2, fl2_a1, fl2_a2, fl2_b0*0.031617, fl2_b1*0.031617, fl2_b2*0.031617); // Notch u5 = y4; y5 = biquad (u5, fno1_v1, fno1_v2, fno1_a1, fno1_a2, fno1_b0*1.004206, fno1_b1*1.004206, fno1_b2*1.004206); u6 = y5; y6 = biquad (u6, fno2_v1, fno2_v2, fno2_a1, fno2_a2, fno2_b0, fno2_b1, fno2_b2); u7 = y6; y7 = biquad (u7, fno3_v1, fno3_v2, fno3_a1, fno3_a2, fno3_b0*0.973227, fno3_b1*0.973227, fno3_b2*0.973227); // Rectify sample y8 = fabs(y7); // Make it smooth u8 = y8; y9 = biquad (u8, flp1_v1, flp1_v2, flp1_a1, flp1_a2, flp1_b0* 0.010386, flp1_b1* 0.010386, flp1_b2* 0.010386); u9 = y9; final_filter1 = biquad(u9, flp2_v1, flp2_v2, flp2_a1, flp2_a2, flp2_b0*0.000109, flp2_b1*0.000109, flp2_b2*0.000109); //Biceps right u10 = EMG_bicepsright.read(); //Highpass y10 = biquad (u10, fh1_v1, fh1_v2, fh1_a1, fh1_a2, fh1_b0*0.924547, fh1_b1*0.924547, fh1_b2*0.924547); u11 = y10; y11 = biquad (u11, fh2_v1, fh2_v2, fh2_a1, fh2_a2, fh2_b0*0.918885, fh2_b1*0.918885, fh2_b2*0.918885); //Lowpass u12 = y11; y12 = biquad (u12, fl1_v1, fl1_v2, fl1_a1, fl1_a2, fl1_b0*0.165103, fl1_b1*0.165103, fl1_b2*0.165103); u13 = y12; y13 = biquad (u13, fl2_v1, fl2_v2, fl2_a1, fl2_a2, fl2_b0*0.031617, fl2_b1*0.031617, fl2_b2*0.031617); // Notch u14 = y13; y14 = biquad (u14, fno1_v1, fno1_v2, fno1_a1, fno1_a2, fno1_b0*1.004206, fno1_b1*1.004206, fno1_b2*1.004206); u15 = y14; y15 = biquad (u15, fno2_v1, fno2_v2, fno2_a1, fno2_a2, fno2_b0, fno2_b1, fno2_b2); u16 = y15; y16 = biquad (u16, fno3_v1, fno3_v2, fno3_a1, fno3_a2, fno3_b0*0.973227, fno3_b1*0.973227, fno3_b2*0.973227); // Rectify sample y17 = fabs(y16); // Make it smooth u17 = y17; y18 = biquad (u17, flp1_v1, flp1_v2, flp1_a1, flp1_a2, flp1_b0* 0.010386, flp1_b1* 0.010386, flp1_b2* 0.010386); u18 = y18; final_filter2 = biquad(u18, flp2_v1, flp2_v2, flp2_a1, flp2_a2, flp2_b0*0.000109, flp2_b1*0.000109, flp2_b2*0.000109); /// EMG Filter left leg u19 = EMG_legleft.read(); //Highpass y19 = biquad (u19, fh1_v1, fh1_v2, fh1_a1, fh1_a2, fh1_b0*0.924547, fh1_b1*0.924547, fh1_b2*0.924547); u20 = y19; y20 = biquad (u20, fh2_v1, fh2_v2, fh2_a1, fh2_a2, fh2_b0*0.918885, fh2_b1*0.918885, fh2_b2*0.918885); //Lowpass u21 = y20; y21 = biquad (u21, fl1_v1, fl1_v2, fl1_a1, fl1_a2, fl1_b0*0.165103, fl1_b1*0.165103, fl1_b2*0.165103); u22 = y21; y22 = biquad (u22, fl2_v1, fl2_v2, fl2_a1, fl2_a2, fl2_b0*0.031617, fl2_b1*0.031617, fl2_b2*0.031617); // Notch u23 = y22; y23 = biquad (u23, fno1_v1, fno1_v2, fno1_a1, fno1_a2, fno1_b0*1.004206, fno1_b1*1.004206, fno1_b2*1.004206); u24 = y23; y24 = biquad (u24, fno2_v1, fno2_v2, fno2_a1, fno2_a2, fno2_b0, fno2_b1, fno2_b2); u25 = y24; y25 = biquad (u25, fno3_v1, fno3_v2, fno3_a1, fno3_a2, fno3_b0*0.973227, fno3_b1*0.973227, fno3_b2*0.973227); // Rectify sample y26 = fabs(y7); // Make it smooth u26 = y26; y27 = biquad (u26, flp1_v1, flp1_v2, flp1_a1, flp1_a2, flp1_b0* 0.010386, flp1_b1* 0.010386, flp1_b2* 0.010386); u27 = y27; final_filter3 = biquad(u27, flp2_v1, flp2_v2, flp2_a1, flp2_a2, flp2_b0*0.000109, flp2_b1*0.000109, flp2_b2*0.000109); // EMG filter right leg u28 = EMG_legright.read(); //Highpass y28 = biquad (u8, fh1_v1, fh1_v2, fh1_a1, fh1_a2, fh1_b0*0.924547, fh1_b1*0.924547, fh1_b2*0.924547); u29 = y28; y29 = biquad (u29, fh2_v1, fh2_v2, fh2_a1, fh2_a2, fh2_b0*0.918885, fh2_b1*0.918885, fh2_b2*0.918885); //Lowpass u30 = y29; y30 = biquad (u30, fl1_v1, fl1_v2, fl1_a1, fl1_a2, fl1_b0*0.165103, fl1_b1*0.165103, fl1_b2*0.165103); u31 = y30; y31 = biquad (u31, fl2_v1, fl2_v2, fl2_a1, fl2_a2, fl2_b0*0.031617, fl2_b1*0.031617, fl2_b2*0.031617); // Notch u32 = y31; y32 = biquad (u32, fno1_v1, fno1_v2, fno1_a1, fno1_a2, fno1_b0*1.004206, fno1_b1*1.004206, fno1_b2*1.004206); u33 = y32; y33 = biquad (u33, fno2_v1, fno2_v2, fno2_a1, fno2_a2, fno2_b0, fno2_b1, fno2_b2); u34 = y33; y34 = biquad (u34, fno3_v1, fno3_v2, fno3_a1, fno3_a2, fno3_b0*0.973227, fno3_b1*0.973227, fno3_b2*0.973227); // Rectify sample y35 = fabs(y34); // Make it smooth u35 = y35; y36 = biquad (u35, flp1_v1, flp1_v2, flp1_a1, flp1_a2, flp1_b0* 0.010386, flp1_b1* 0.010386, flp1_b2* 0.010386); u36 = y36; final_filter4 = biquad(u36, flp2_v1, flp2_v2, flp2_a1, flp2_a2, flp2_b0*0.000109, flp2_b1*0.000109, flp2_b2*0.000109); scope.set (0,final_filter1); scope.set (1,final_filter2); scope.set (2,final_filter3); scope.set (3,final_filter4); scope.send (); if (Cali == true) { if (final_filter1 >= fblmax) { fblmax=final_filter1; } if (final_filter2 >= fbrmax) { fbrmax = final_filter2; } if (final_filter3 >= fllmax) { fllmax=final_filter3; } if (final_filter4 >= flrmax) { flrmax=final_filter4; } Tbl = 0.8*fblmax; Tbr = 0.8*fbrmax; Tll = 0.8*fllmax; Tlr = 0.8*flrmax; wait (3); } } int main () { EMG_Control.attach_us(MyController,1e3); Timer_Calibration.start(); if (Timer_Calibration < TimeCali) { Cali = true; } else { Cali = false; } Timer_Calibration.stop(); Timer_Calibration.reset(); }