Module 9 Super Team
/
EMG_Kevin
EMG_filter zonder moving average
Fork of EMG by
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 #include "mbed.h" 00002 #include "HIDScope.h" 00003 00004 #define MOV_AVG_NUM 100 00005 00006 //Define objects 00007 AnalogIn b_emg(A0); //Analog input bicep 00008 AnalogIn t_emg(A1); //Analog input tricep 00009 AnalogIn p_emg(A2); //Analog input bicep for push 00010 Ticker sample_timer; 00011 HIDScope scope(6); 00012 00013 double b_highV[4]; 00014 double b_lowV[4]; 00015 double t_highV[4]; 00016 double t_lowV[4]; 00017 double p_highV[4]; 00018 double p_lowV[4]; 00019 00020 //double lastOutputs[MOV_AVG_NUM-1]; 00021 00022 double filter(double input, double coeff_input[], double coeff_output[], double prev_outputs[]) 00023 { 00024 double new_input = input; 00025 for(int i=1; i<5; i++) 00026 new_input -= coeff_input[i] * prev_outputs[i-1]; 00027 00028 double new_output = coeff_output[0] * new_input; 00029 for(int i=1; i<5; i++) 00030 new_output += coeff_output[i] * prev_outputs[i-1]; 00031 00032 // Set the new output as the first value of the 'recent outputs' 00033 for(int i = 3; i > 0; i--) 00034 prev_outputs[i] = prev_outputs[i-1]; 00035 prev_outputs[0] = new_input; 00036 return new_output; 00037 } 00038 double fh_b[]= {0.7602, -3.0406, 4.5609, -3.0406, 0.7602}; 00039 double fh_a[]= {1, -3.4532, 4.5041, -2.6273, 0.5778}; 00040 double b_highpass_filter(double u) 00041 { 00042 return filter(u, fh_a, fh_b, t_highV); 00043 } 00044 double t_highpass_filter(double u) 00045 { 00046 return filter(u, fh_a, fh_b, b_highV); 00047 } 00048 double p_highpass_filter(double u) 00049 { 00050 return filter(u, fh_a, fh_b, p_highV); 00051 } 00052 00053 double fl_b[]= {0.00001329, 0.00005317, 0.00007976, 0.00005317, 0.00001329}; 00054 double fl_a[]= {1.0000, -3.6717, 5.0680, -3.1160, 0.7199}; 00055 double b_lowpass_filter(double u) 00056 { 00057 return filter(u, fl_a, fl_b, t_lowV); 00058 } 00059 double t_lowpass_filter(double u) 00060 { 00061 00062 return filter(u, fl_a, fl_b, b_lowV); 00063 } 00064 double p_lowpass_filter(double u) 00065 { 00066 00067 return filter(u, fl_a, fl_b, p_lowV); 00068 } 00069 00070 double last_biceps = 0; 00071 double last_triceps = 0; 00072 double last_push = 0; 00073 /** Sample function 00074 * this function samples the emg and sends it to HIDScope 00075 **/ 00076 void sample() 00077 { 00078 double b_input = b_emg.read(); 00079 double output1 = b_highpass_filter(b_input); 00080 double output2 = fabs(output1); 00081 double output3 = b_lowpass_filter(output2); 00082 00083 double t_input = t_emg.read(); 00084 double output4 = t_highpass_filter(t_input); 00085 double output5 = fabs(output4); 00086 double output6 = t_lowpass_filter(output5); 00087 00088 double p_input = p_emg.read(); 00089 double output7 = p_highpass_filter(p_input); 00090 double output8 = fabs(output7); 00091 double output9 = p_lowpass_filter(output8); 00092 00093 /*double tot = output3; 00094 for(int i=0; i<MOV_AVG_NUM-1; i++) { 00095 tot += lastOutputs[i]; 00096 if(i != 0) lastOutputs[i] = lastOutputs[i-1]; 00097 } 00098 lastOutputs[0] = output3; 00099 output3 = tot/MOV_AVG_NUM;*/ 00100 00101 /* Second, set the sampled emg value in channel zero (the first channel) in the 'HIDScope' variable named 'scope' */ 00102 00103 double bicout; 00104 double tricout; 00105 double pushout; 00106 if (last_biceps == 0 and output3 > 0.06) { 00107 bicout = 1; 00108 } else if (last_biceps == 1 and output3 < 0.045) { 00109 bicout = 0; 00110 } else { 00111 bicout = last_biceps; 00112 } 00113 last_biceps = bicout; 00114 00115 /*if (output3>0.06) {//This is the output for the right bicep 00116 bicout=1; 00117 } else if(output3>0.04) { 00118 bicout=0.5; 00119 } else { 00120 bicout=0; 00121 }*/ 00122 00123 /*if (output6>0.0561) {//this is the output for the right tricep 00124 tricout=1; 00125 } else if(output6>0.0380) { 00126 tricout=0.5; 00127 } else { 00128 tricout=0; 00129 }*/ 00130 00131 if (last_triceps == 0 and output6 > 0.045) { 00132 tricout = 1; 00133 } else if(last_triceps == 1 and output6 < 0.04) { 00134 tricout = 0; 00135 } else { 00136 tricout = last_triceps; 00137 } 00138 last_triceps = tricout; 00139 00140 if (output9>0.05) {//this is the output for the left bicep (the push motion) 00141 pushout=1; 00142 } else { 00143 pushout=0; 00144 } 00145 00146 scope.set(0,output3); 00147 scope.set(1,output6); 00148 scope.set(2,output9); 00149 scope.set(3,bicout); 00150 scope.set(4,tricout); 00151 scope.set(5,pushout); 00152 /* Repeat the step above if required for more channels (channel 0 up to 5 = 6 channels) */ 00153 /* Finally, send all channels to the PC at once */ 00154 scope.send(); 00155 } 00156 00157 int main() 00158 { 00159 /**Attach the 'sample' function to the timer 'sample_timer'. 00160 * this ensures that 'sample' is executed every... 0.002 seconds 00161 */ 00162 sample_timer.attach(&sample, 0.002); 00163 00164 /*empty loop, sample() is executed periodically*/ 00165 while(1) {} 00166 }
Generated on Fri Jul 22 2022 20:54:11 by 1.7.2