Dan August
Werkt. (hopelijk)
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 #include "mbed.h" 00002 #include "MODSERIAL.h" 00003 00004 //Define objects 00005 AnalogIn emg_biceps(PTB0); 00006 AnalogIn emg_triceps(PTB1); 00007 AnalogIn emg_flexoren(PTB2); 00008 AnalogIn emg_extensoren(PTB3); //Analog input 00009 PwmOut red(LED_RED); //PWM output 00010 Ticker timer; 00011 MODSERIAL pc(USBTX,USBRX,64,1024); 00012 00013 #define offset_biceps 0 // offset ruwe invoer met adapter motoren, waar toepassen? 00014 00015 //high pass filter constantes 15Hz cutoff 4e orde, Fs = 312,5Hz (geeft een wat mooiere waarde voor periode en is geen veelvoud van 50Hz) 00016 #define NUM0 0.6731 // constante 00017 #define NUM1 -2.6925 // z^-1 00018 #define NUM2 4.0388 // z^-2etc. 00019 #define NUM3 -2.6925 00020 #define NUM4 0.6731 00021 00022 #define DEN0 1 // constante 00023 #define DEN1 -3.2133 00024 #define DEN2 3.9348 00025 #define DEN3 -2.1689 00026 #define DEN4 0.4531 00027 00028 //lowpass filter constantes 4Hz 4e orde, Fs = 312,5 Hz 00029 #define NUM0_3 0.00000236 // constante 00030 #define NUM1_3 0.00000944 // z^-1 00031 #define NUM2_3 0.00001415 // z^-2etc. 00032 #define NUM3_3 0.00000944 00033 #define NUM4_3 0.00000236 00034 00035 #define DEN0_3 1 // constante 00036 #define DEN1_3 -3.7899 00037 #define DEN2_3 5.3914 00038 #define DEN3_3 -3.4119 00039 #define DEN4_3 0.8104 00040 00041 float std_dev (float input, int sig_number){ 00042 // define variables 00043 static int startcount=0; 00044 float sum; 00045 int size = 20; 00046 float sig_out; 00047 float mean; 00048 static int count_biceps=0; 00049 static int count_triceps=0; 00050 static int count_flexoren=0; 00051 static int count_extensoren=0; 00052 static float keeper_biceps[20]; 00053 static float keeper_triceps[20]; 00054 static float keeper_flexoren[20]; 00055 static float keeper_extensoren[20]; 00056 00057 if (startcount >= size) 00058 {//ja, dan gewoon std dev nemen, want keeper is al vol 00059 switch(sig_number){ 00060 case 1: 00061 keeper_biceps[count_biceps]=input; 00062 count_biceps++; 00063 if(count_biceps >= size) 00064 count_biceps=0; 00065 //sizeof(keeper_biceps)/sizeof(float); 00066 for(int i=0; i < size; i++) 00067 {sum+=keeper_biceps[i]; 00068 } 00069 mean=sum/size; 00070 sum=0; 00071 for(int i=0; i < size; i++) 00072 {sum+=(keeper_biceps[i]-mean)*(keeper_biceps[i]-mean); 00073 } 00074 sig_out=sqrt(sum/size); 00075 sum=0; 00076 break; 00077 case 2: 00078 keeper_triceps[count_triceps]=input; 00079 count_triceps++; 00080 if(count_triceps==size) 00081 count_triceps=0; 00082 //sizeof(keeper_triceps) / sizeof(float); 00083 for(int i=0; i < size; i++){ 00084 sum+=keeper_triceps[i]; 00085 } 00086 mean=sum/size; 00087 sum=0; 00088 for(int i=0; i < size; i++){ 00089 sum+=(keeper_triceps[i]-mean)*(keeper_triceps[i]-mean); 00090 } 00091 sig_out=sqrt(sum/size); 00092 sum=0; 00093 break; 00094 case 3: 00095 keeper_flexoren[count_flexoren]=input; 00096 count_flexoren++; 00097 if(count_flexoren==size) count_flexoren=0; 00098 00099 //size=sizeof(keeper_flexoren)/sizeof(float); 00100 for(int i=0; i < size; i++){ 00101 sum+=keeper_flexoren[i]; 00102 } 00103 mean=sum/size; 00104 sum=0; 00105 for(int i=0; i < size; i++){ 00106 sum+=(keeper_flexoren[i]-mean)*(keeper_flexoren[i]-mean); 00107 } 00108 sig_out=sqrt(sum/size); 00109 sum=0; 00110 break; 00111 case 4: 00112 keeper_extensoren[count_extensoren]=input; 00113 count_extensoren++; 00114 if(count_extensoren==size) count_extensoren=0; 00115 00116 //size=sizeof(keeper_extensoren)/sizeof(float); 00117 for(int i=0; i < size; i++){ 00118 sum+=keeper_extensoren[i]; 00119 } 00120 mean=sum/size; 00121 sum=0; 00122 for(int i=0; i < size; i++){ 00123 sum+=(keeper_extensoren[i]-mean)*(keeper_extensoren[i]-mean); 00124 } 00125 sig_out=sqrt(sum/size); 00126 sum=0; 00127 break; 00128 } // einde switch 00129 return sig_out; 00130 } // einde if startcount ... 00131 else 00132 {startcount+=1; 00133 switch(sig_number){ 00134 case 1: 00135 keeper_biceps[count_biceps]=input; 00136 count_biceps++; 00137 if(count_biceps >= size) 00138 count_biceps=0; 00139 break; 00140 case 2: 00141 keeper_triceps[count_triceps]=input; 00142 count_triceps++; 00143 if(count_triceps >= size) 00144 count_triceps=0; 00145 break; 00146 case 3: 00147 keeper_flexoren[count_flexoren]=input; 00148 count_flexoren++; 00149 if(count_flexoren >= size) 00150 count_flexoren=0; 00151 break; 00152 case 4: 00153 keeper_extensoren[count_extensoren]=input; 00154 count_extensoren++; 00155 if(count_extensoren >= size) 00156 count_extensoren=0; 00157 break; 00158 } // einde switch 00159 } // einde else 00160 } // einde std_dev 00161 00162 float filter(int sig_number){ 00163 float sig_out; 00164 // eerst variabelen definieren 00165 00166 //biceps 00167 //filter 1 00168 float in0_biceps =0; 00169 static float in1_biceps =0, in2_biceps = 0, in3_biceps = 0, in4_biceps = 0; 00170 static float out0_biceps = 0, out1_biceps = 0 , out2_biceps = 0, out3_biceps = 0, out4_biceps = 0; 00171 00172 //filter 3 00173 float in0_3_biceps =0; 00174 static float in1_3_biceps =0, in2_3_biceps = 0, in3_3_biceps = 0, in4_3_biceps = 0; 00175 static float out0_3_biceps = 0, out1_3_biceps = 0 , out2_3_biceps = 0, out3_3_biceps = 0, out4_3_biceps = 0; 00176 00177 00178 //triceps 00179 //filter 1 00180 float in0_triceps =0; 00181 static float in1_triceps =0, in2_triceps = 0, in3_triceps = 0, in4_triceps = 0; 00182 static float out0_triceps = 0, out1_triceps = 0 , out2_triceps = 0, out3_triceps = 0, out4_triceps = 0; 00183 00184 //filter 3 00185 float in0_3_triceps =0; 00186 static float in1_3_triceps =0, in2_3_triceps = 0, in3_3_triceps = 0, in4_3_triceps = 0; 00187 static float out0_3_triceps = 0, out1_3_triceps = 0 , out2_3_triceps = 0, out3_3_triceps = 0, out4_3_triceps = 0; 00188 00189 00190 //flexoren 00191 //filter 1 00192 float in0_flexoren =0; 00193 static float in1_flexoren =0, in2_flexoren = 0, in3_flexoren = 0, in4_flexoren = 0; 00194 static float out0_flexoren = 0, out1_flexoren = 0 , out2_flexoren = 0, out3_flexoren = 0, out4_flexoren = 0; 00195 00196 //filter 3 00197 float in0_3_flexoren =0; 00198 static float in1_3_flexoren =0, in2_3_flexoren = 0, in3_3_flexoren = 0, in4_3_flexoren = 0; 00199 static float out0_3_flexoren = 0, out1_3_flexoren = 0 , out2_3_flexoren = 0, out3_3_flexoren = 0, out4_3_flexoren = 0; 00200 00201 00202 //extensoren 00203 //filter 1 00204 float in0_extensoren =0; 00205 static float in1_extensoren =0, in2_extensoren = 0, in3_extensoren = 0, in4_extensoren = 0; 00206 static float out0_extensoren = 0, out1_extensoren = 0 , out2_extensoren = 0, out3_extensoren = 0, out4_extensoren = 0; 00207 00208 //filter 3 00209 float in0_3_extensoren =0; 00210 static float in1_3_extensoren =0, in2_3_extensoren = 0, in3_3_extensoren = 0, in4_3_extensoren = 0; 00211 static float out0_3_extensoren = 0, out1_3_extensoren = 0 , out2_3_extensoren = 0, out3_3_extensoren = 0, out4_3_extensoren = 0; 00212 00213 00214 00215 switch(sig_number){ 00216 case 1: 00217 // signaal filteren op 15 Hz HIGHPASS 00218 in4_biceps = in3_biceps; in3_biceps = in2_biceps; in2_biceps = in1_biceps; in1_biceps = in0_biceps; 00219 in0_biceps = emg_biceps.read() - offset_biceps; 00220 out4_biceps = out3_biceps; out3_biceps = out2_biceps; out2_biceps = out1_biceps; out1_biceps = out0_biceps; 00221 out0_biceps = (NUM0*in0_biceps + NUM1*in1_biceps + NUM2*in2_biceps + NUM3*in3_biceps + NUM4*in4_biceps - DEN1*out1_biceps - DEN2*out2_biceps - DEN3*out3_biceps - DEN4*out4_biceps ) / DEN0; 00222 /* 00223 //signaal filteren op 5Hz LOWPASS 00224 in4_3_biceps = in3_3_biceps; in3_3_biceps = in2_3_biceps; in2_3_biceps = in1_3_biceps; in1_3_biceps = in0_3_biceps; 00225 in0_3_biceps = abs(out0_biceps); // ruw - offset -> filter 1 -> stdev (-> filter 3) 00226 out4_3_biceps = out3_3_biceps; out3_3_biceps = out2_3_biceps; out2_3_biceps = out1_3_biceps; out1_3_biceps = out0_3_biceps; 00227 out0_3_biceps = (NUM0_3*in0_3_biceps + NUM1_3*in1_3_biceps + NUM2_3*in2_3_biceps + NUM3_3*in3_3_biceps + NUM4_3*in4_3_biceps - DEN1_3*out1_3_biceps - DEN2_3*out2_3_biceps - DEN3_3*out3_3_biceps - DEN4_3*out4_3_biceps ) / DEN0_3; 00228 */ 00229 00230 sig_out = out0_biceps; 00231 break; 00232 case 2: 00233 // signaal filteren op 15 Hz HIGHPASS 00234 in4_triceps = in3_triceps; in3_triceps = in2_triceps; in2_triceps = in1_triceps; in1_triceps = in0_triceps; 00235 in0_triceps = emg_triceps.read() - offset_biceps; 00236 out4_triceps = out3_triceps; out3_triceps = out2_triceps; out2_triceps = out1_triceps; out1_triceps = out0_triceps; 00237 out0_triceps = (NUM0*in0_triceps + NUM1*in1_triceps + NUM2*in2_triceps + NUM3*in3_triceps + NUM4*in4_triceps - DEN1*out1_triceps - DEN2*out2_triceps - DEN3*out3_triceps - DEN4*out4_triceps ) / DEN0; 00238 00239 00240 //signaal filteren op 5Hz LOWPASS 00241 /*in4_3_triceps = in3_3_triceps; in3_3_triceps = in2_3_triceps; in2_3_triceps = in1_3_triceps; in1_3_triceps = in0_3_triceps; 00242 in0_3_triceps = abs(out0_triceps); 00243 out4_3_triceps = out3_3_triceps; out3_3_triceps = out2_3_triceps; out2_3_triceps = out1_3_triceps; out1_3_triceps = out0_3_triceps; 00244 out0_3_triceps = (NUM0_3*in0_3_triceps + NUM1_3*in1_3_triceps + NUM2_3*in2_3_triceps + NUM3_3*in3_3_triceps + NUM4_3*in4_3_triceps - DEN1_3*out1_3_triceps - DEN2_3*out2_3_triceps - DEN3_3*out3_3_triceps - DEN4_3*out4_3_triceps ) / DEN0_3; 00245 */ 00246 00247 sig_out = out0_triceps; 00248 break; 00249 case 3: 00250 // signaal filteren op 15 Hz HIGHPASS 00251 in4_flexoren = in3_flexoren; in3_flexoren = in2_flexoren; in2_flexoren = in1_flexoren; in1_flexoren = in0_flexoren; 00252 in0_flexoren = emg_flexoren.read(); 00253 out4_flexoren = out3_flexoren; out3_flexoren = out2_flexoren; out2_flexoren = out1_flexoren; out1_flexoren = out0_flexoren; 00254 out0_flexoren = (NUM0*in0_flexoren + NUM1*in1_flexoren + NUM2*in2_flexoren + NUM3*in3_flexoren + NUM4*in4_flexoren - DEN1*out1_flexoren - DEN2*out2_flexoren - DEN3*out3_flexoren - DEN4*out4_flexoren ) / DEN0; 00255 /* 00256 00257 //signaal filteren op 5Hz LOWPASS 00258 in4_3_flexoren = in3_3_flexoren; in3_3_flexoren = in2_3_flexoren; in2_3_flexoren = in1_3_flexoren; in1_3_flexoren = in0_3_flexoren; 00259 in0_3_flexoren = abs(out0_2_flexoren); 00260 out4_3_flexoren = out3_3_flexoren; out3_3_flexoren = out2_3_flexoren; out2_3_flexoren = out1_3_flexoren; out1_3_flexoren = out0_3_flexoren; 00261 out0_3_flexoren = (NUM0_3*in0_3_flexoren + NUM1_3*in1_3_flexoren + NUM2_3*in2_3_flexoren + NUM3_3*in3_3_flexoren + NUM4_3*in4_3_flexoren - DEN1_3*out1_3_flexoren - DEN2_3*out2_3_flexoren - DEN3_3*out3_3_flexoren - DEN4_3*out4_3_flexoren ) / DEN0_3; 00262 00263 00264 */ 00265 sig_out = out0_flexoren; 00266 break; 00267 case 4: 00268 // signaal filteren op 15 Hz HIGHPASS 00269 in4_extensoren = in3_extensoren; in3_extensoren = in2_extensoren; in2_extensoren = in1_extensoren; in1_extensoren = in0_extensoren; 00270 in0_extensoren = emg_extensoren.read(); 00271 out4_extensoren = out3_extensoren; out3_extensoren = out2_extensoren; out2_extensoren = out1_extensoren; out1_extensoren = out0_extensoren; 00272 out0_extensoren = (NUM0*in0_extensoren + NUM1*in1_extensoren + NUM2*in2_extensoren + NUM3*in3_extensoren + NUM4*in4_extensoren - DEN1*out1_extensoren - DEN2*out2_extensoren - DEN3*out3_extensoren - DEN4*out4_extensoren ) / DEN0; 00273 /* 00274 00275 //signaal filteren op 5Hz LOWPASS 00276 in4_3_extensoren = in3_3_extensoren; in3_3_extensoren = in2_3_extensoren; in2_3_extensoren = in1_3_extensoren; in1_3_extensoren = in0_3_extensoren; 00277 in0_3_extensoren = abs(out0_2_extensoren); 00278 out4_3_extensoren = out3_3_extensoren; out3_3_extensoren = out2_3_extensoren; out2_3_extensoren = out1_3_extensoren; out1_3_extensoren = out0_3_extensoren; 00279 out0_3_extensoren = (NUM0_3*in0_3_extensoren + NUM1_3*in1_3_extensoren + NUM2_3*in2_3_extensoren + NUM3_3*in3_3_extensoren + NUM4_3*in4_3_extensoren - DEN1_3*out1_3_extensoren - DEN2_3*out2_3_extensoren - DEN3_3*out3_3_extensoren - DEN4_3*out4_3_extensoren ) / DEN0_3; 00280 00281 00282 */ 00283 sig_out = out0_extensoren; 00284 break; 00285 } 00286 return sig_out; 00287 } 00288 00289 void looper() 00290 { float emg_value_biceps; 00291 float emg_value_triceps; 00292 float emg_value_flexoren; 00293 float emg_value_extensoren; 00294 float dy; 00295 //static int sig_count = 1; 00296 emg_value_biceps=std_dev(filter(1),1); 00297 emg_value_triceps=std_dev(filter(2),2); 00298 emg_value_flexoren = std_dev(filter(3),3); 00299 emg_value_extensoren = std_dev(filter(4),4); 00300 00301 if(emg_value_biceps < 4.5) 00302 emg_value_biceps=0; 00303 else if (emg_value_biceps > 13) 00304 emg_value_biceps = 13; 00305 //emg_value_biceps = emg_value_biceps; 00306 00307 if(emg_value_triceps < 2.5) 00308 emg_value_biceps=0; 00309 else if (emg_value_biceps > 10) 00310 emg_value_biceps = 10; 00311 //emg_value_triceps = emg_value_triceps; 00312 00313 if(emg_value_flexoren < 2) 00314 emg_value_flexoren=0; 00315 else if (emg_value_flexoren > 8) 00316 emg_value_flexoren = 8; 00317 emg_value_flexoren = 2*emg_value_flexoren; 00318 00319 if(emg_value_extensoren < 5) 00320 emg_value_extensoren=0; 00321 else if (emg_value_extensoren > 13) 00322 emg_value_extensoren = 13; 00323 00324 dy = emg_value_biceps - emg_value_triceps; 00325 dx = 2*(emg_value_flexoren - emg_value_extensoren); 00326 00327 if(pc.rxBufferGetSize(0)-pc.rxBufferGetCount() > 30) 00328 pc.printf("%.6f, %.6f, %.6f\n",emg_value_biceps, emg_value_triceps, dy); 00329 /**When not using the LED, the above could also have been done this way: 00330 * pc.printf("%.6\n", emg0.read()); 00331 */ 00332 } 00333 00334 int main() 00335 { 00336 /*setup baudrate. Choose the same in your program on PC side*/ 00337 pc.baud(115200); 00338 /*set the period for the PWM to the red LED*/ 00339 red.period_ms(2); 00340 /**Here you attach the 'void looper(void)' function to the Ticker object 00341 * The looper() function will be called every 1/Fs seconds. 00342 * Please mind that the parentheses after looper are omitted when using attach. 00343 */ 00344 timer.attach(looper,0.0032); //invullen in seconde. .0032 is niet eens afgerond, dus vandaar die frequentie. 00345 while(1) //Loop 00346 { 00347 /*Empty!*/ 00348 /*Everything is handled by the interrupt routine now!*/ 00349 } 00350 }
Generated on Thu Jul 21 2022 05:20:13 by
1.7.2