main.cpp

00001 #include "mbed.h"
00002 #include "BiQuad.h"
00003 #include "MODSERIAL.h"
00004 #include <iostream>
00005 #include "MovingAverage.h"
00006 #define NSAMPLE 100
00007 
00008 DigitalOut led1(LED_GREEN);
00009 DigitalOut led2(LED_RED);
00010 DigitalOut led3(LED_BLUE);
00011 MODSERIAL pc(USBTX, USBRX);
00012 
00013 // Tickers
00014 Ticker sample_ticker; //ticker voor filteren met 1000Hz
00015 Ticker threshold_check_ticker; //ticker voor het checken van de threshold met 1000Hz
00016 Timer timer_calibration; //timer voor EMG Kalibratie
00017 double ts = 0.001; //tijdsstap !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
00018 double calibration_time = 37; //Kalibratie tijd
00019 
00020 //Input
00021 AnalogIn    emg1( A1 ); //Duim
00022 AnalogIn    emg2( A2 ); //Bicep
00023 AnalogIn    emg3( A3 ); //Dorsaal
00024 AnalogIn    emg4( A4 ); //Palmair
00025 
00026 // GLOBALS EMG
00027 //Gefilterde EMG signalen
00028 volatile double emg1_filtered, emg2_filtered, emg3_filtered, emg4_filtered;
00029 
00030 bool thresholdreach1 = false;
00031 bool thresholdreach2 = false;
00032 bool thresholdreach3 = false;
00033 bool thresholdreach4 = false;
00034 
00035 volatile double temp_highest_emg1 = 0; //Hoogste waarde gevonden tijdens kalibratie
00036 volatile double temp_highest_emg2 = 0;
00037 volatile double temp_highest_emg3 = 0;
00038 volatile double temp_highest_emg4 = 0;
00039 
00040 //Percentage van de hoogste waarde waar de bovenste treshold gezet moet worden
00041 double Duim_p_t = 0.5; 
00042 double Bicep_p_t = 0.4; 
00043 double Dorsaal_p_t = 0.6; 
00044 double Palmair_p_t = 0.5;
00045 
00046 //Percentage van de hoogste waarde waar de onderste treshold gezet moet worden
00047 double Duim_p_tL = 0.5; 
00048 double Bicep_p_tL = 0.4; 
00049 double Dorsaal_p_tL = 0.5; 
00050 double Palmair_p_tL = 0.5;
00051 
00052 // Waarde bovenste treshold waar het signaal overheen moet om de arm te activeren
00053 volatile double threshold1;
00054 volatile double threshold2;
00055 volatile double threshold3;
00056 volatile double threshold4;
00057 
00058 // Waarde onderste treshold waar het signaal onder moet om de arm te deactiveren
00059 volatile double threshold1L;
00060 volatile double threshold2L;
00061 volatile double threshold3L;
00062 volatile double threshold4L;
00063 
00064 // thresholdreads bools
00065 int Duim;
00066 int Bicep;
00067 int Dorsaal;
00068 int Palmair;
00069 
00070 //  EMG OUTPUT
00071 int EMGxplus;
00072 int EMGxmin ;
00073 int EMGyplus;
00074 int EMGymin ;
00075 
00076 
00077 //EMG1!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
00078 //Highpass vierde orde cutoff 20Hz, Band filter om 49, 50, 51Hz eruit te filteren
00079 BiQuadChain highp1;
00080 BiQuad highp1_1( 0.8485, -1.6969, 0.8485, 1.0000, -1.7783, 0.7924 );
00081 BiQuad highp1_2( 1.0000, -2.0000, 1.0000, 1.0000, -1.8934, 0.9085 );
00082 BiQuad notch1_1( 0.9907, -1.8843, 0.9907, 1.0000, -1.8843, 0.9813 );
00083 
00084 //Lowpass first order cutoff 0.4Hz
00085 BiQuad lowp1( 0.0013, 0.0013, 0, 1.0000, -0.9975, 0 );
00086 
00087 //EMG2!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
00088 //Highpass vierde orde cutoff 20Hz, Band filter om 49, 50, 51Hz eruit te filteren
00089 BiQuadChain highp2;
00090 BiQuad highp2_1( 0.8485, -1.6969, 0.8485, 1.0000, -1.7783, 0.7924 );
00091 BiQuad highp2_2( 1.0000, -2.0000, 1.0000, 1.0000, -1.8934, 0.9085 );
00092 BiQuad notch2_1( 0.9907, -1.8843, 0.9907, 1.0000, -1.8843, 0.9813 );
00093 
00094 //Lowpass first order cutoff 0.4Hz
00095 BiQuad lowp2( 0.0013, 0.0013, 0, 1.0000, -0.9975, 0 );
00096 
00097 //EMG3!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
00098 //Highpass vierde orde cutoff 20Hz, Band filter om 49, 50, 51Hz eruit te filteren
00099 BiQuadChain highp3;
00100 BiQuad highp3_1( 0.8485, -1.6969, 0.8485, 1.0000, -1.7783, 0.7924 );
00101 BiQuad highp3_2( 1.0000, -2.0000, 1.0000, 1.0000, -1.8934, 0.9085 );
00102 BiQuad notch3_1( 0.9907, -1.8843, 0.9907, 1.0000, -1.8843, 0.9813 );
00103 
00104 //Lowpass first order cutoff 0.4Hz
00105 BiQuad lowp3( 0.0013, 0.0013, 0, 1.0000, -0.9975, 0 );
00106 
00107 //EMG4!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
00108 //Highpass vierde orde cutoff 20Hz, Band filter om 49, 50, 51Hz eruit te filteren
00109 BiQuadChain highp4;
00110 BiQuad highp4_1( 0.8485, -1.6969, 0.8485, 1.0000, -1.7783, 0.7924 );
00111 BiQuad highp4_2( 1.0000, -2.0000, 1.0000, 1.0000, -1.8934, 0.9085 );
00112 BiQuad notch4_1( 0.9907, -1.8843, 0.9907, 1.0000, -1.8843, 0.9813 );
00113 
00114 //Lowpass first order cutoff 0.4Hz
00115 BiQuad lowp4( 0.0013, 0.0013, 0, 1.0000, -0.9975, 0 );
00116 
00117 // ~~~~~~~~~~~~~~~~~~~EMG FUNCTIONS~~~~~~~~~~~~~~~~~~
00118 void emgsample()
00119 {
00120     
00121     // EMG signaal lezen
00122     double emgread1 = emg1.read();
00123     double emgread2 = emg2.read();
00124     double emgread3 = emg3.read();
00125     double emgread4 = emg4.read();
00126     
00127     // Vierde orde highpass filter + notch filter
00128     double emg1_highpassed = highp1.step(emgread1);
00129     double emg2_highpassed = highp2.step(emgread2);
00130     double emg3_highpassed = highp3.step(emgread3);
00131     double emg4_highpassed = highp4.step(emgread4);
00132 
00133     //Rectificatie
00134     double emg1_abs = abs(emg1_highpassed);
00135     double emg2_abs = abs(emg2_highpassed);
00136     double emg3_abs = abs(emg3_highpassed);
00137     double emg4_abs = abs(emg4_highpassed);
00138 
00139     //All EMG abs into lowpass
00140     emg1_filtered = lowp1.step(emg1_abs);
00141     emg2_filtered = lowp2.step(emg2_abs);
00142     emg3_filtered = lowp3.step(emg3_abs);
00143     emg4_filtered = lowp4.step(emg4_abs);
00144 
00145 }
00146 
00147 void CalibrationEMG()
00148 {
00149     timer_calibration.reset();
00150     timer_calibration.start();
00151     while(timer_calibration<20) {                           //Duim
00152         if(timer_calibration>0 && timer_calibration<20) {
00153             led1=!led1;
00154             if(emg1_filtered>temp_highest_emg1) {
00155                 temp_highest_emg1= emg1_filtered;
00156                 pc.printf("Highest value Duim= %f \r\n", temp_highest_emg1);
00157             }
00158         
00159             if(emg2_filtered>temp_highest_emg2) {
00160                 temp_highest_emg2= emg2_filtered;
00161                 pc.printf("Highest value Bicep= %f \r\n", temp_highest_emg2);
00162             }
00163         
00164             if(emg3_filtered>temp_highest_emg3) {
00165                 temp_highest_emg3= emg3_filtered;
00166                 pc.printf("Highest value Dorsaal= %f \r\n", temp_highest_emg3);
00167             }
00168         
00169             if(emg4_filtered>temp_highest_emg4) {
00170                 temp_highest_emg4= emg4_filtered;
00171                 pc.printf("Highest value Palmair= %f \r\n", temp_highest_emg4);
00172             }
00173             
00174         }
00175         led1=1;
00176         led2=1;
00177         led3=1;
00178 
00179     }
00180     pc.printf("threshold calculation\r\n");
00181     threshold1 = temp_highest_emg1*Duim_p_t;  
00182     threshold2 = temp_highest_emg2*Bicep_p_t; 
00183     threshold3 = temp_highest_emg3*Dorsaal_p_t;  
00184     threshold4 = temp_highest_emg4*Palmair_p_t;
00185     
00186     threshold1L = temp_highest_emg1*Duim_p_tL;
00187     threshold2L = temp_highest_emg2*Bicep_p_tL; 
00188     threshold3L = temp_highest_emg3*Dorsaal_p_tL; 
00189     threshold4L = temp_highest_emg4*Palmair_p_tL; 
00190 }
00191 
00192 //Check of emg_filtered boven de threshold is
00193 void threshold_check()
00194 {
00195 
00196 // EMG1 Check    
00197 if (thresholdreach1 == false){            //Als emg_filtered nog niet boven de bovenste threshold is geweest
00198     //bovenste threshold check
00199     if(emg1_filtered>threshold1) {
00200         Duim = 1;
00201         thresholdreach1 = true;
00202         
00203     } else {
00204         Duim= 0;
00205     }
00206     }
00207 else{                                   //Als emg_filtered boven de bovenste threshold is geweest
00208     //onderste threshold check
00209     if(emg1_filtered<threshold1L) {
00210         Duim = 0;
00211         thresholdreach1 = false;
00212         
00213     } else {
00214         Duim= 1;
00215     }
00216     }
00217 
00218 // EMG2 Check    
00219 if (thresholdreach2 == false){            //Als emg_filtered nog niet boven de bovenste threshold is geweest
00220     //bovenste threshold check
00221     if(emg2_filtered>threshold2) {
00222         Bicep = 1;
00223         thresholdreach2 = true;
00224     } else {
00225         Bicep= 0;
00226     }
00227     }
00228 else{                                   //Als emg_filtered boven de bovenste threshold is geweest
00229     //onderste threshold check
00230     if(emg2_filtered<threshold2L) {
00231         Bicep = 0;
00232         thresholdreach2 = false;
00233         
00234     } else {
00235         Bicep= 1;
00236     }
00237     }
00238 
00239 // EMG3 Check    
00240 if (thresholdreach3 == false){            //Als emg_filtered nog niet boven de bovenste threshold is geweest
00241     //bovenste threshold check
00242     if(emg3_filtered>threshold3) {
00243         Dorsaal = 1;
00244         thresholdreach3 = true;
00245     } else {
00246         Dorsaal= 0;
00247     }
00248     }
00249 else{                                   //Als emg_filtered boven de bovenste threshold is geweest
00250     //onderste threshold check
00251     if(emg3_filtered<threshold3L) {
00252         Dorsaal = 0;
00253         thresholdreach3 = false;
00254         
00255     } else {
00256         Dorsaal= 1;
00257     }
00258     }
00259 
00260 // EMG4 Check    
00261 if (thresholdreach4 == false){            //Als emg_filtered nog niet boven de bovenste threshold is geweest
00262     //bovenste threshold check
00263     if(emg4_filtered>threshold4) {
00264         Palmair = 1;
00265         thresholdreach4 = true;
00266     } else {
00267         Palmair= 0;
00268     }
00269     }
00270 else{                                   //Als emg_filtered boven de bovenste threshold is geweest
00271     //onderste threshold check
00272     if(emg4_filtered<threshold4L) {
00273         Palmair = 0;
00274         thresholdreach4 = false;
00275         
00276     } else {
00277         Palmair= 1;
00278     }
00279     }
00280 
00281     }
00282 
00283 
00284 
00285    
00286 
00287 
00288 
00289 
00290 Ticker      sample_timer;
00291 
00292 
00293 
00294 void sample()
00295 {
00296 pc.printf("Duim Right = %i\r\n", Duim);
00297 pc.printf("Bicep Right = %i\r\n",Bicep);
00298 pc.printf("Dorsaal Left = %i\r\n", Dorsaal);
00299 pc.printf("Palmair Left = %i\r\n", Palmair);           
00300         
00301 }
00302 
00303 
00304 int main()
00305 {   
00306                 led1 = 1;
00307                 led2 = 1;
00308                 led3 = 1;
00309     sample_ticker.attach(&emgsample, 0.001);            // Leest het ruwe EMG signaal af met een frequentie van 1000Hz
00310     pc.baud(115200);
00311     
00312 //BiQuad Chain add
00313     highp1.add( &highp1_1 ).add( &highp1_2 ).add( &notch1_1 );
00314     highp2.add( &highp2_1 ).add( &highp2_2 ).add( &notch2_1 );
00315     highp3.add( &highp3_1 ).add( &highp3_2 ).add( &notch3_1 );
00316     highp4.add( &highp4_1 ).add( &highp4_2 ).add( &notch4_1 );
00317 
00318     temp_highest_emg1 = 0; //highest detected value right Biceps
00319     temp_highest_emg2 = 0;
00320     temp_highest_emg3 = 0;
00321     temp_highest_emg4 = 0;
00322 
00323 
00324     CalibrationEMG();
00325     pc.printf("threshold1 = %i, threshold1L = %f\r\n", threshold1, threshold1L);
00326     threshold_check_ticker.attach(&threshold_check, 0.01);
00327     led1 = 1;
00328     led2 = 1;
00329     led3 = 1;
00330     sample_timer.attach(&sample, 0.1);
00331     pc.printf("sample timer attached\r\n");
00332 
00333     timer_calibration.stop();
00334 
00335 
00336 
00337     /*empty loop, sample() is executed periodically*/
00338     while(1) {
00339         wait(0.01);
00340         }
00341 }