main.cpp

00001 #include "mbed.h"
00002 #include "BiQuad.h"
00003 #include "HIDScope.h"
00004 //#define SERIAL_BAUD 115200  // baud rate for serial communication
00005 
00006 //Serial communication with PC
00007 //Serial pc(USBTX,USBRX);
00008 AnalogIn    emg0( A0 );
00009 AnalogIn    emg1( A1 );
00010 HIDScope    scope( 5 );
00011 
00012 Ticker      filter_timer, send_timer;
00013 DigitalOut  led1(LED_RED);
00014 DigitalOut  led2(LED_BLUE);
00015 
00016 volatile bool filter_timer_go=false,send_timer_go=false;
00017 
00018 //double EMGright, EMGleft, inR;
00019 int EMGgain=1; 
00020 //set initial conditions
00021 //double biceps_l = 0;
00022 //double biceps_r = 0;
00023 //double outRenvelope, outLenvelope;
00024 int T=4;
00025 double threshold_l=0.025;
00026 double threshold_r = 0.025;
00027 
00028 
00029 
00030 void filter_timer_act(){filter_timer_go=true;};
00031 void send_timer_act(){send_timer_go=true;};
00032 
00033 BiQuadChain bcq1R;
00034 BiQuadChain bcq2R;
00035 // Notch filter wo=50; bw=wo/35
00036 BiQuad bq1R(9.9821e-01,-1.9807e+00,9.9821e-01,-1.9807e+00,9.9642e-01);
00037 // High pass Butterworth filter 2nd order, Fc=10;
00038 BiQuad bq2R(9.8239e-01,-1.9648e+00,9.8239e-01,-1.9645e+00,9.6508e-01);
00039 // Low pass Butterworth filter 2nd order, Fc = 8;
00040 BiQuad bq3R(5.6248e-05,1.1250e-04,5.6248e-05,-1.9787e+00,9.7890e-01);
00041 
00042 BiQuadChain bcq1L;
00043 BiQuadChain bcq2L;
00044 // Notch filter wo=50; bw=wo/35
00045 BiQuad bq1L(9.9821e-01,-1.9807e+00,9.9821e-01,-1.9807e+00,9.9642e-01);
00046 // High pass Butterworth filter 2nd order, Fc=10;
00047 BiQuad bq2L(9.8239e-01,-1.9648e+00,9.8239e-01,-1.9645e+00,9.6508e-01);
00048 // Low pass Butterworth filter 2nd order, Fc = 8;
00049 BiQuad bq3L(5.6248e-05,1.1250e-04,5.6248e-05,-1.9787e+00,9.7890e-01);
00050 
00051 // In the following: R is used for right arm, L is used for left arm!
00052 
00053 void FilteredSample(int &Tout)
00054 {   
00055     double inLout = emg0.read();
00056     double inRout = emg1.read();
00057     
00058     double outRfilter1 = bcq1R.step(inRout);
00059     double outRrect= fabs(outRfilter1);
00060     double envelopeR = bcq2R.step(outRrect);
00061     
00062     double outLfilter1 = bcq1L.step(inLout);
00063     double outLrect = fabs(outLfilter1);
00064     double envelopeL = bcq2L.step(outLrect);
00065     
00066     double biceps_l = (double) envelopeL * EMGgain; //emg0.read();     //velocity or reference position change, EMG with a gain
00067     double biceps_r = (double) envelopeR * EMGgain; //emg1.read();
00068     if (biceps_l > threshold_l && biceps_r > threshold_r){
00069         //both arms activated: stamp moves down
00070         //pc.printf("Stamp down   ");
00071         //pc.printf("right: %f    ",biceps_r);
00072         //pc.printf("left:  %f\n\r",biceps_l);
00073         //wait(0.5);
00074         Tout = -1;
00075         //pc.printf("T=%d\n\r",T);
00076         led1=!led1;//blink purple
00077         led2=!led2;
00078         }
00079     else if (biceps_l > threshold_l && biceps_r <= threshold_r){
00080         //arm 1 activated, move left
00081         //pc.printf("Move left    ");
00082         //pc.printf("right: %f    ",biceps_r);
00083         //pc.printf("left:  %f\n\r",biceps_l);
00084         //wait(0.5);
00085         Tout  = -2;
00086         //pc.printf("T=%d\n\r",T);
00087         led2=1;//off
00088         led1=0;//on    red
00089         }
00090     else if (biceps_l <= threshold_l && biceps_r > threshold_r){
00091         //arm 1 activated, move right
00092         //pc.printf("Move right   "); 
00093         //pc.printf("right: %f    ",biceps_r);
00094         //pc.printf("left:  %f\n\r",biceps_l);
00095         //wait(0.5);
00096         Tout = 2;
00097         //pc.printf("T=%d\n\r",T);
00098         led2=0;//on    blue
00099         led1=1;//off
00100         }
00101     else{
00102         //wait(0.2);
00103         led1 = 1;
00104         led2 = 1;  //off
00105         //pc.printf("Nothing...   ");
00106         //wait(0.5);
00107         Tout = 5;
00108         //pc.printf("right: %f    ",biceps_r);
00109         //pc.printf("left:  %f\n\r",biceps_l);
00110         }
00111 
00112     /*pc.printf("EMG right = %f\n\r",inRout);
00113     pc.printf("EMG left =  %f\n\r",inLout);
00114     pc.printf("envelope EMG right = %f\n\r",envelopeR);
00115     pc.printf("envelope EMG left =  %f\n\r",envelopeL);*/
00116     
00117     scope.set(0, inRout);
00118     scope.set(1, inLout);
00119     scope.set(2, envelopeR);
00120     scope.set(3, envelopeL);
00121     scope.set(4, Tout);
00122     
00123     
00124     scope.send();
00125     // To indicate that the function is working, the LED is toggled*/
00126     //led2 = !led2;
00127 }
00128 
00129 
00130 /*void sendValues( double outRenvelope, double outLenvelope){
00131     
00132     biceps_l = (double) outLenvelope * EMGgain; //emg0.read();     //velocity or reference position change, EMG with a gain
00133     biceps_r = (double) outRenvelope * EMGgain; //emg1.read();
00134     if (biceps_l > 0.2 && biceps_r > 0.2){
00135         //both arms activated: stamp moves down
00136         pc.printf("Stamp down\n\r");
00137         pc.printf("right: %f\n\r",biceps_r);
00138         pc.printf("left:  %f\n\r",biceps_l);
00139         //wait(0.5);
00140         led1=!led1;//blink purple
00141         led2=!led2;
00142         }
00143     else if (biceps_l > 0.2 && biceps_r <= 0.2){
00144         //arm 1 activated, move left
00145         pc.printf("Move left\n\r");
00146         pc.printf("right: %f\n\r",biceps_r);
00147         pc.printf("left:  %f\n\r",biceps_l);
00148         //wait(0.5);
00149         led2=1;//off
00150         led1=0;//on    red
00151         }
00152     else if (biceps_l <= 0.2 && biceps_r > 0.2){
00153         //arm 1 activated, move right
00154         pc.printf("Move right\n\r");
00155         pc.printf("right: %f\n\r",biceps_r);
00156         pc.printf("left:  %f\n\r",biceps_l);
00157         //wait(0.5);
00158         led2=0;//on    blue
00159         led1=1;//off
00160         }
00161     else{
00162         wait(0.2);
00163         led1 = 1;
00164         led2 = 1;  //off
00165         pc.printf("Nothing...\n\r");
00166         //wait(0.5);
00167         }
00168     // To indicate that the function is working, the LED is toggled
00169     //led2 = !led2; // blue
00170     
00171 }*/
00172 
00173 
00174 
00175 int main()
00176 {   
00177     //pc.baud(SERIAL_BAUD);
00178     led1=1;
00179     led2=1; 
00180     led1=0; //red
00181 
00182     bcq1R.add(&bq1R).add(&bq2R);
00183     bcq2R.add(&bq3R);
00184     
00185     bcq1L.add(&bq1L).add(&bq2L);
00186     bcq2L.add(&bq3L);
00187       
00188     filter_timer.attach(&filter_timer_act, 0.0004); //2500Hz (same as with filter coefficients on matlab!!! Thus adjust!)
00189     //send_timer.attach(&send_timer_act, 0.0004);
00190     //pc.printf("\rMain-loop\n\r");
00191         
00192         while(1)
00193         {
00194         if (filter_timer_go){
00195                 filter_timer_go=false;
00196                 FilteredSample(T);}
00197         /*if (send_timer_go){
00198                 send_timer_go=false;
00199                 sendValues(outRenvelope, outLenvelope);}*/
00200         }
00201 }