Nahuel Manterola
/
EMG_Controller_5
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 QEI biquadFilter mbed
Fork of
EMG_Controller
by 
Pascal van Baardwijk
Embed:
(wiki syntax)
Show/hide line numbers
emg.h
00001 #include "mbed.h" 00002 #include "BiQuad.h" 00003 //#include "HIDScope.h" 00004 //Enum with states 00005 enum states {STATE_DEFAULT , STATE_CALIBRATION, STATE_RUN}; 00006 00007 //Variable called 'state' 00008 states state = STATE_DEFAULT; 00009 00010 //Creating two scope channels 00011 //HIDScope scope(3); 00012 00013 //Notch filter 00014 BiQuadChain notch_50_0; 00015 BiQuadChain notch_50_1; 00016 BiQuadChain notch_50_2; 00017 BiQuad bq03( 0.98116526140, 0.00000000044, 0.98116526140, 0.00000000043, 0.95391787621); 00018 BiQuad bq04( 0.97224232015, 0.00000000043, 0.97224232015, -0.04036799459, 0.97670000725); 00019 BiQuad bq05( 1.00000000000, 0.00000000044, 1.00000000000, 0.04036799547, 0.9767000072); 00020 BiQuad bq13( 0.98116526140, 0.00000000044, 0.98116526140, 0.00000000043, 0.95391787621); 00021 BiQuad bq14( 0.97224232015, 0.00000000043, 0.97224232015, -0.04036799459, 0.97670000725); 00022 BiQuad bq15( 1.00000000000, 0.00000000044, 1.00000000000, 0.04036799547, 0.9767000072); 00023 BiQuad bq23( 0.98116526140, 0.00000000044, 0.98116526140, 0.00000000043, 0.95391787621); 00024 BiQuad bq24( 0.97224232015, 0.00000000043, 0.97224232015, -0.04036799459, 0.97670000725); 00025 BiQuad bq25( 1.00000000000, 0.00000000044, 1.00000000000, 0.04036799547, 0.9767000072); 00026 00027 //High pass filter 00028 BiQuadChain high_pass_0; 00029 BiQuadChain high_pass_1; 00030 BiQuadChain high_pass_2; 00031 BiQuad bq06( 0.80254782780,-1.60509565560, 0.80254782780, -1.58011656361, 0.63006219630); 00032 BiQuad bq07( 0.90006571973,-1.80013143945, 0.900065719734, -1.77213098592, 0.8281459694); 00033 BiQuad bq16( 0.80254782780,-1.60509565560, 0.80254782780, -1.58011656361, 0.63006219630); 00034 BiQuad bq17( 0.90006571973,-1.80013143945, 0.900065719734, -1.77213098592, 0.8281459694); 00035 BiQuad bq26( 0.80254782780,-1.60509565560, 0.80254782780, -1.58011656361, 0.63006219630); 00036 BiQuad bq27( 0.90006571973,-1.80013143945, 0.900065719734, -1.77213098592, 0.8281459694); 00037 //BiQuad bq8( 0.92490714701,-1.84981429401, 0.92490714701, -1.90032503529, 0.9352152620); 00038 00039 //Low pass filter 00040 BiQuadChain low_pass_0; 00041 BiQuadChain low_pass_1; 00042 BiQuadChain low_pass_2; 00043 BiQuad bq9( 0.00801840797, 0.01603681594, 0.00801840797,-1.65212256130, 0.68416767240); 00044 BiQuad bq10( 0.00836524486, 0.01673048973, 0.00836524486,-1.72511837232, 0.75857933411); 00045 BiQuad bq11( 0.00905039996, 0.01810079992, 0.00905039996,-1.86807725180, 0.9043110909); 00046 BiQuad bq19( 0.00801840797, 0.01603681594, 0.00801840797,-1.65212256130, 0.68416767240); 00047 BiQuad bq110( 0.00836524486, 0.01673048973, 0.00836524486,-1.72511837232, 0.75857933411); 00048 BiQuad bq111( 0.00905039996, 0.01810079992, 0.00905039996,-1.86807725180, 0.9043110909); 00049 BiQuad bq29( 0.00801840797, 0.01603681594, 0.00801840797,-1.65212256130, 0.68416767240); 00050 BiQuad bq210( 0.00836524486, 0.01673048973, 0.00836524486,-1.72511837232, 0.75857933411); 00051 BiQuad bq211( 0.00905039996, 0.01810079992, 0.00905039996,-1.86807725180, 0.9043110909); 00052 00053 //Ticker 00054 Ticker emgSampleTicker; 00055 00056 //Timeout to change state after 5 seconds 00057 Timeout change_state; 00058 00059 //Timeout to change state after 15 seconds 00060 Timeout change_state2; 00061 00062 //LED 00063 DigitalOut led(LED_RED); 00064 00065 //Emg input 00066 AnalogIn emg0( A0 ); 00067 AnalogIn emg1( A1 ); 00068 AnalogIn emg2( A2 ); 00069 00070 //Go flag for emg sample 00071 bool go_emgSample; 00072 00073 //Variables for intermediate filter values 00074 double emg_sample[3]; 00075 double emg_low_passed_200[3]; 00076 double emg_notch[3]; 00077 double emg_high_passed[3]; 00078 double emg_low_passed[3]; 00079 double min_emg[3]; 00080 double max_emg[3]; 00081 00082 //Calculating normalized outputs 00083 double Norm_EMG_0; 00084 double Norm_EMG_1; 00085 double Norm_EMG_2; 00086 00087 //count for emg min max 00088 int start_calibration = 0; 00089 00090 void emgSample() { 00091 go_emgSample = true; 00092 } 00093 00094 void calibrate() { 00095 state = STATE_CALIBRATION; 00096 led.write(0); 00097 } 00098 00099 void run() { 00100 state = STATE_RUN; 00101 led.write(1); 00102 } 00103 00104 void EMG_filter(); 00105 00106 void EMG_filter() { 00107 if(go_emgSample == true){ 00108 //read the emg signal 00109 emg_sample[0] = emg0.read(); 00110 emg_sample[1] = emg1.read(); 00111 emg_sample[2] = emg2.read(); 00112 //pc.printf("%f - %f - %f \r\n", emg_sample[0], emg_sample[1], emg_sample[2]); 00113 //filter out the 50Hz components with a notch filter 00114 emg_notch[0] = notch_50_0.step(emg_sample[0]); 00115 emg_notch[1] = notch_50_1.step(emg_sample[1]); 00116 emg_notch[2] = notch_50_2.step(emg_sample[2]); 00117 //high pass the signal (removing motion artifacts and offset) 00118 emg_high_passed[0] = high_pass_0.step(emg_notch[0]); 00119 emg_high_passed[1] = high_pass_1.step(emg_notch[1]); 00120 emg_high_passed[2] = high_pass_2.step(emg_notch[2]); 00121 float emg_fabs[3]; 00122 emg_fabs[0] = fabs(emg_high_passed[0]); 00123 emg_fabs[1] = fabs(emg_high_passed[1]); 00124 emg_fabs[2] = fabs(emg_high_passed[2]); 00125 //low pass the rectified emg signal 00126 emg_low_passed[0] = low_pass_0.step(emg_fabs[0]); 00127 emg_low_passed[1] = low_pass_1.step(emg_fabs[1]); 00128 emg_low_passed[2] = low_pass_2.step(emg_fabs[2]); 00129 00130 //pc.printf("%f - %f - %f \r\n", emg_low_passed[0], emg_low_passed[1], emg_low_passed[2]); 00131 //Calculating min value and max value of emg signal 00132 if(state == STATE_CALIBRATION) 00133 { 00134 if (start_calibration == 0) { 00135 min_emg[0] = emg_low_passed[0]; 00136 max_emg[0] = emg_low_passed[0]; 00137 min_emg[1] = emg_low_passed[1]; 00138 max_emg[1] = emg_low_passed[1]; 00139 min_emg[2] = emg_low_passed[2]; 00140 max_emg[2] = emg_low_passed[2]; 00141 start_calibration++; 00142 } 00143 else { 00144 //finding min and max of emg0 00145 00146 if (emg_low_passed[0] < min_emg[0]) { 00147 min_emg[0] = emg_low_passed[0]; 00148 } 00149 else if (emg_low_passed[0] > max_emg[0]) { 00150 max_emg[0] = emg_low_passed[0]; 00151 } 00152 00153 //finding min and max of emg1 00154 if (emg_low_passed[1] < min_emg[1]) { 00155 min_emg[1] = emg_low_passed[1]; 00156 } 00157 else if (emg_low_passed[1] > max_emg[1]) { 00158 max_emg[1] = emg_low_passed[1]; 00159 } 00160 00161 //finding min and max of emg2 00162 if (emg_low_passed[2] < min_emg[2]) { 00163 min_emg[2] = emg_low_passed[2]; 00164 } 00165 else if (emg_low_passed[2] > max_emg[2]) { 00166 max_emg[2] = emg_low_passed[2]; 00167 } 00168 } 00169 } 00170 00171 //calculating input_forces for controller 00172 Norm_EMG_0 = (emg_low_passed[0] - min_emg[0])/(max_emg[0]-min_emg[0]); 00173 Norm_EMG_1 = (emg_low_passed[1] - min_emg[1])/(max_emg[1]-min_emg[1]); 00174 Norm_EMG_2 = (emg_low_passed[2] - min_emg[2])/(max_emg[2]-min_emg[2]); 00175 00176 //Send scope data 00177 //scope.set(0,Norm_EMG_0); 00178 //scope.set(1,Norm_EMG_1); 00179 //scope.set(2,Norm_EMG_2); 00180 00181 go_emgSample = false; 00182 } 00183 } 00184
Generated on Tue Jul 12 2022 22:39:24 by
1.7.2