main.cpp

00001 #include "mbed.h"
00002 #include "HIDScope.h"
00003 #include "biquadFilter.h"        // Require the HIDScope library
00004 #include "MODSERIAL.h"
00005 #include "QEI.h"
00006 //Define objects
00007 AnalogIn    emg1(A0); //Analog of EMG input
00008 AnalogIn    emg2(A1);
00009 Ticker      sample_timer1;
00010 Ticker      sample_timer2;
00011 Ticker      motor_timer1;
00012 Ticker      motor_timer2;
00013 Ticker      cal_timer;
00014 HIDScope    scope(5);        // Instantize a 2-channel HIDScope object
00015 DigitalIn button1(PTA4);//test button for starting motor 1
00016 DigitalOut led1(LED_RED);
00017 DigitalOut led2(LED_BLUE);
00018 DigitalOut led3(LED_GREEN);
00019 MODSERIAL pc(USBTX,USBRX);
00020 // motor objects
00021 QEI motor1(D13,D12,NC, 624);//encoder for motor 1
00022 QEI motor2(D11,D10,NC, 624);//encoder for motor 2
00023 DigitalOut direction1(D7);//direction input for motor 1
00024 DigitalOut direction2(D4);//direction input for motor 2
00025 PwmOut speed1(D6);//speed input for motor 1
00026 PwmOut speed2(D5);//speed input for motor 2
00027 
00028 /*The biquad filters required to transform the EMG signal into an usable signal*/
00029 biquadFilter filterhigh1(-1.4542, 0.5741, 0.7571, -1.5142, 0.7571);
00030 biquadFilter filterlow1(0.4395, 0.2059, 0.4114, 0.8227, 0.4114);
00031 biquadFilter notch(-1.0958, 0.9723, 0.9862, -1.0958, 0.9862);
00032 biquadFilter filterlow2(-1.9722, 0.9726, 9.50600294492288e-05, 0.000190120058898458, 9.50600294492288e-05);
00033 
00034 biquadFilter filterhigh12(-1.4542, 0.5741, 0.7571, -1.5142, 0.7571);
00035 biquadFilter filterlow12(0.4395, 0.2059, 0.4114, 0.8227, 0.4114);
00036 biquadFilter notch2(-1.0958, 0.9723, 0.9862, -1.0958, 0.9862);
00037 biquadFilter filterlow22(-1.9722, 0.9726, 9.50600294492288e-05, 0.000190120058898458, 9.50600294492288e-05);
00038 
00039 double emg_value1;
00040 double signalpart11;
00041 double signalpart12;
00042 double signalpart13;
00043 double signalpart14;
00044 double signalfinal1;
00045 double onoffsignal1;
00046 
00047 double emg_value2;
00048 double signalpart21;
00049 double signalpart22;
00050 double signalpart23;
00051 double signalpart24;
00052 double signalfinal2;
00053 double onoffsignal2;
00054 
00055 double test;
00056 
00057 
00058 double maxcal=1;        
00059 bool calyes=1;
00060 bool motor1_dir=0;//set the direction of motor 1
00061 bool motor2_dir = 0;//set the direction of motor 1
00062 
00063 float cycle = 0.3;//define the speed of the motor
00064    bool motor1_on = 1;//set the on variable of motor 1
00065    bool motor2_on =1;//set the on variable of motor 2
00066    int n1=1;//numeric conditions to determine if the speed needs to be increased
00067    int n2=1;
00068 
00069 void changedirmotor1(){
00070         motor1_dir = !motor1_dir;//code for changing direction of motor 1
00071         }
00072 void changedirmotor2(){
00073         motor2_dir = !motor2_dir;//code for changing direction of motor 2
00074         }        
00075 
00076 /* 
00077  */
00078 void filter1(){
00079         if(calyes==1){
00080         emg_value1 = emg1.read();//read the emg value from the elektrodes
00081         signalpart11 = notch.step(emg_value1);//Highpass filter for removing offset and artifacts
00082         signalpart12 = filterhigh1.step(signalpart11);//rectify the filtered signal
00083         signalpart13 = fabs(signalpart12);//low pass filter to envelope the emg
00084         signalpart14 = filterlow1.step(signalpart13);//notch filter to remove 50Hz signal
00085         signalfinal1 = filterlow2.step(signalpart14);//2nd low pass filter to envelope the emg
00086         onoffsignal1=signalfinal1/maxcal;//divide the emg signal by the max EMG to calibrate the signal per person
00087         //pc.printf("the emg 1 signal is: %f \n",onoffsignal1); 
00088         /*emg_value2 = emg2.read();//read the emg value from the elektrodes
00089         signalpart21 = notch.step(emg_value2);//Highpass filter for removing offset and artifacts
00090         signalpart22 = filterhigh1.step(signalpart21);//rectify the filtered signal
00091         signalpart23 = abs(signalpart22);//low pass filter to envelope the emg
00092         signalpart24 = filterlow1.step(signalpart23);//notch filter to remove 50Hz signal
00093         signalfinal2 = filterlow2.step(signalpart24);//2nd low pass filter to envelope the emg
00094         onoffsignal2=signalfinal2/maxcal;*///divide the emg signal by the max EMG to calibrate the signal per person
00095         //test=onoffsignal2-onoffsignal1;
00096         scope.set(0,emg_value1);//set emg signal to scope in channel 1
00097         scope.set(1,onoffsignal1);//set filtered signal to scope in channel 2
00098         //scope.set(2,emg_value2);//set emg signal to scope in channel 3
00099         //scope.set(3,onoffsignal2);//set filtered signal to scope in channel 4
00100         //scope.set(4,test);
00101         //scope.send();//send the signals to the scope
00102 
00103         //pc.printf("the difference between the 2 signals is: %f \n",test); 
00104 
00105         //pc.printf("emg signal %f, filtered signal %f \n",emg_value,onoffsignal);
00106 }
00107 }
00108 
00109 
00110 void filter2(){
00111         if(calyes==1){
00112         emg_value2 = emg2.read();//read the emg value from the elektrodes
00113         signalpart21 = notch2.step(emg_value2);//Highpass filter for removing offset and artifacts
00114         signalpart22 = filterhigh12.step(signalpart21);//rectify the filtered signal
00115         signalpart23 = fabs(signalpart22);//low pass filter to envelope the emg
00116         signalpart24 = filterlow12.step(signalpart23);//notch filter to remove 50Hz signal
00117         signalfinal2 = filterlow22.step(signalpart24);//2nd low pass filter to envelope the emg
00118         onoffsignal2=signalfinal2/maxcal;//divide the emg signal by the max EMG to calibrate the signal per person
00119         scope.set(2,emg_value2);//set emg signal to scope in channel 1
00120         scope.set(3,onoffsignal2);//set filtered signal to scope in channel 2
00121         //pc.printf("the emg 2 signal is: %f \n",onoffsignal2); 
00122         scope.send();//send the signals to the scope
00123         //pc.printf("emg signal %f, filtered signal %f \n",emg_value,onoffsignal);
00124 }
00125 }
00126 
00127 
00128 
00129 /*
00130 void checkmotor1(){//check the normalized signal and set actions if a threshold is passed not needed in ticker move to main function
00131     if(calyes==1){
00132     if(onoffsignal1 >= 0.05){
00133                  led1.write(0);
00134                  led2.write(1);
00135             while(n1 == 1){
00136                 changedirmotor1();
00137                 speed1.write(cycle);//write speed only on first run through the loop
00138                 direction1.write(motor1_dir);//turn motor CCW or CW 
00139                   
00140                 n1=0;
00141                   }
00142              }
00143     else if(onoffsignal1<=0.03){
00144              led1.write(1);
00145              led2.write(0);
00146              
00147             while(n1==0){//check if the first run was done
00148                  speed1.write(0);//if so set speed to 0 and reset the run counter
00149                  n1=1;
00150              }
00151             }
00152     
00153         }
00154 }
00155 
00156 void checkmotor2(){//check the normalized signal and set actions if a threshold is passed
00157     if(calyes==1){
00158     if(onoffsignal2 >= 0.05){
00159                  led1.write(0);
00160                  led2.write(1);
00161             while(n2 == 1){
00162                 changedirmotor2();
00163                 speed2.write(cycle);//write speed only on first run through the loop
00164                 direction2.write(motor2_dir);//turn motor CCW or CW 
00165                   
00166                 n2=0;
00167                   }
00168              }
00169     else if(onoffsignal2<=0.03){
00170              led1.write(1);
00171              led2.write(0);
00172              
00173             while(n2==0){//check if the first run was done
00174                  speed2.write(0);//if so set speed to 0 and reset the run counter
00175                  n2=1;
00176              }
00177             }
00178     
00179         }
00180 }
00181 
00182 void calibri(){//calibration function
00183     if(button1.read()==false){
00184         for(int n =0; n<100000;n++){//read for 5000 samples as calibration
00185             emg_value = emg1.read();//read the emg value from the electrodes
00186             signalpart1 = notch.step(emg_value);//Highpass filter for removing offset and artifacts
00187             signalpart2 = filterhigh1.step(signalpart1);//rectify the filtered signal
00188             signalpart3 = abs(signalpart2);//low pass filter to envelope the emg
00189             signalpart4 = filterlow1.step(signalpart3);//notch filter to remove 50Hz signal
00190             signalfinal = filterlow2.step(signalpart4);//2nd low pass filter to envelope the emg
00191             double signalmeasure = signalfinal;
00192             if (signalmeasure > maxcal){//determine what the highest reachable emg signal is
00193                 maxcal = signalmeasure;
00194                 }
00195             }
00196         calyes=1;
00197     }  
00198     }
00199     */
00200 int main()
00201 {
00202     pc.baud(115200);
00203     led1.write(1);
00204     led2.write(1);
00205     led3.write(1);
00206     speed1.write(0);
00207     speed2.write(0);
00208         sample_timer1.attach(filter1, 0.003125);//continously execute the EMG reader and filter
00209         sample_timer2.attach(filter2, 0.003125);//continously execute the EMG reader and filter
00210         //motor_timer1.attach(&checkmotor1, 0.002);//continously execute the motor controller
00211         //motor_timer2.attach(&checkmotor2, 0.002);//continously execute the motor controller
00212       //  cal_timer.attach(&calibri, 0.002);//ticker to check if motor is being calibrated
00213         
00214 
00215 while(1){
00216 if(calyes==1){
00217     if(onoffsignal1 >= 0.05){
00218                  led1.write(0);
00219                  led2.write(1);
00220             while(n1 == 1){
00221                 changedirmotor1();
00222                 speed1.write(cycle);//write speed only on first run through the loop
00223                 direction1.write(motor1_dir);//turn motor CCW or CW 
00224                   
00225                 n1=0;
00226                   }
00227              }
00228     else if(onoffsignal1<=0.03){
00229              led1.write(1);
00230              led2.write(0);
00231              
00232             while(n1==0){//check if the first run was done
00233                  speed1.write(0);//if so set speed to 0 and reset the run counter
00234                  n1=1;
00235              }
00236             }
00237     
00238         }
00239         
00240         if(calyes==1){
00241     if(onoffsignal2 >= 0.05){
00242                  led1.write(0);
00243                  led2.write(1);
00244             while(n2 == 1){
00245                 changedirmotor2();
00246                 speed2.write(cycle);//write speed only on first run through the loop
00247                 direction2.write(motor2_dir);//turn motor CCW or CW 
00248                   
00249                 n2=0;
00250                   }
00251              }
00252     else if(onoffsignal2<=0.03){
00253              led1.write(1);
00254              led2.write(0);
00255              
00256             while(n2==0){//check if the first run was done
00257                  speed2.write(0);//if so set speed to 0 and reset the run counter
00258                  n2=1;
00259              }
00260             }
00261     
00262         }
00263 }     
00264 }