main.cpp

00001 //=====================================================================================
00002 //libraries
00003 #include "mbed.h"           //mbed revision 113
00004 #include "HIDScope.h"       //Hidscope by Tom Lankhorst
00005 #include "BiQuad.h"         //BiQuad by Tom Lankhorst
00006 #include "MODSERIAL.h"      //Modserial
00007 
00008 
00009 //=====================================================================================
00010 //Define objects
00011 
00012 //EMG
00013 AnalogIn    emg_biceps_right_in( A0 );              //analog in to get EMG biceps  (r) in to c++
00014 AnalogIn    emg_triceps_right_in(A1);               //analog in to get EMG triceps (r) in to c++
00015 AnalogIn    emg_biceps_left_in  (A2);               //analog in to get EMG biceps  (l) in to c++
00016 
00017 //Tickers
00018 Ticker      sample_timer;               //ticker for EMG signal sampling, analog becomes digital
00019 Ticker      ticker_switch;              //ticker for switch, every second it is possible to switch
00020 
00021 //Monitoring
00022 HIDScope    scope(5);                   //open 5 channels in hidscope
00023 MODSERIAL pc(USBTX, USBRX);             //pc connection
00024 DigitalOut red(LED_RED);                //LED on K64F board, 1 is out; 0 is on
00025 DigitalOut green(LED_GREEN);            //LED on K64f board, 1 is out; o is on
00026 
00027 //motors
00028 DigitalOut richting_motor1(D7); //motor 1 connected to motor 1 at k64f board; for turningtable
00029 PwmOut pwm_motor1(D6);
00030 DigitalOut richting_motor2(D4); ///motor 2 connected to motor 2 at k64f board; for linear actuator
00031 PwmOut pwm_motor2(D5);
00032 
00033 //=====================================================================================
00034 //define variables
00035 
00036 //tresholds
00037 double treshold_biceps_right =0.04;                 //common values that work.
00038 double treshold_biceps_left=-0.04;                  // tested on multiple persons 
00039 double treshold_triceps=-0.04;                      //triceps and left biceps is specified negative, thus negative treshold
00040 
00041 //on/off and switch signals
00042 int switch_signal = 0; //start of counter, switch made by even and odd numbers
00043 int onoffsignal_biceps; 
00044 int switch_signal_triceps;
00045 
00046 //motorvariables
00047 float speedmotor1=0.18; //speed of motor 1 is 0.18pwm at start
00048 float speedmotor2=1.0;  //speed of motor 2 is 1.0 pwm at start
00049 
00050 int cw=0;               //clockwise direction
00051 int ccw=1;              //counterclockwise direction
00052  
00053 //=======================================
00054 //filter coefficients
00055 
00056 //b1 = biceps right arm
00057 BiQuad filterhigh_b1(9.5654e-01,-1.9131e+00,9.5654e-01,-1.9112e+00,9.1498e-01);                 // second order highpass filter, with frequency of?
00058 BiQuad filternotch1_b1 (9.9376e-01 , -1.8902e-00,   9.9376e-01 , -1.8902e-00 ,  9.875e-01);     // second order notch filter, with frequency of?
00059 
00060 //t1= triceps right arm
00061 BiQuad filterhigh_t1(9.5654e-01,-1.9131e+00,9.5654e-01,-1.9112e+00,9.1498e-01);                  // second order highpass filter, with frequency of?
00062 BiQuad filternotch1_t1 (9.9376e-01 , -1.8902e-00,   9.9376e-01 , -1.8902e-00 ,  9.875e-01);      // second order notch filter, with frequency of?
00063 
00064 //b2= biceps left arm
00065 BiQuad filterhigh_b2(9.5654e-01,-1.9131e+00,9.5654e-01,-1.9112e+00,9.1498e-01);                  // second order highpass filter, with frequency of?
00066 BiQuad filternotch1_b2 (9.9376e-01 , -1.8902e-00,   9.9376e-01 , -1.8902e-00 ,  9.875e-01);      // second order notch filter, with frequency of?
00067 
00068 //after abs filtering
00069 BiQuad filterlow_b1 (6.2942e-06, 1.2588e-05,6.2942e-06,-1.9929e+00,9.9292e-01);                 // second order lowpass filter, with frequency of?
00070 BiQuad filterlow_t1 (6.2942e-06, 1.2588e-05,6.2942e-06,-1.9929e+00,9.9292e-01);                 // second order lowpass filter, with frequency of?
00071 BiQuad filterlow_b2 (6.2942e-06, 1.2588e-05,6.2942e-06,-1.9929e+00,9.9292e-01);                 // second order lowpass filter, with frequency of?
00072 
00073 //======================================================================
00074 //voids
00075 //======================================================================
00076 
00077 //function teller
00078 void switch_function() {                        // The switch function. Makes it possible to switch between the motors. It simply adds one at switch_signal.
00079     if(switch_signal_triceps==1)
00080     {
00081         switch_signal++;
00082            
00083     // To monitor what is happening: we will show the text in putty  and change led color from red to green or vice versa.
00084     
00085         green=!green;
00086         red=!red;
00087         
00088     if (switch_signal%2==0)  
00089      {pc.printf("If you contract the biceps, the robot will go right \r\n");    
00090      pc.printf("If you contract the triceps, the robot will go left \r\n");
00091      pc.printf("\r\n");
00092      }
00093        
00094     
00095     else
00096      {pc.printf("If you contract the biceps, the robot will go up \r\n");
00097      pc.printf("If you contract the triceps, the robot will go down \r\n");
00098     pc.printf("\r\n");
00099      }
00100      
00101     }    
00102     }
00103  
00104 //======================================================================   
00105 //functions which are called in ticker to sample the analog signal and make the on/off and switch signal. 
00106 
00107 void filter(){
00108         //biceps right arm read+filtering
00109        double emg_biceps_right=emg_biceps_right_in.read();                                                      //read the emg value from the elektrodes 
00110        double emg_filtered_high_biceps_right= filterhigh_b1.step(emg_biceps_right);                             //high pass filter, to remove offset
00111        double emg_filtered_high_notch_1_biceps_right=filternotch1_b1.step(emg_filtered_high_biceps_right);      //notch filter, to remove noise
00112        double emg_abs_biceps_right=fabs(emg_filtered_high_notch_1_biceps_right);                                //rectify the signal, fabs because float   
00113        double emg_filtered_biceps_right=filterlow_b1.step(emg_abs_biceps_right);                                //low pass filter to envelope the signal
00114         
00115         //triceps right arm read+filtering
00116        double emg_triceps_right=emg_triceps_right_in.read();                                                    //read the emg value from the elektrodes
00117        double emg_filtered_high_triceps_right= filterhigh_t1.step(emg_triceps_right);                           //high pass filter, to remove offset
00118        double emg_filtered_high_notch_1_triceps_right=filternotch1_t1.step(emg_filtered_high_triceps_right);    //notch filter, to remove noise
00119        double emg_abs_triceps_right=fabs(emg_filtered_high_notch_1_triceps_right);                              //rectify the signal, fabs because float   
00120        double emg_filtered_triceps_right=filterlow_t1.step(emg_abs_triceps_right);                              //low pass filter to envelope the signal
00121         
00122         //biceps left arm read+filtering
00123        double emg_biceps_left=emg_biceps_left_in.read();                                                    //read the emg value from the elektrodes
00124        double emg_filtered_high_biceps_left= filterhigh_b2.step(emg_biceps_left);                           //high pass filter, to remove offset
00125        double emg_filtered_high_notch_1_biceps_left=filternotch1_b2.step(emg_filtered_high_biceps_left);    //notch filter, to remove noise
00126        double emg_abs_biceps_left=fabs(emg_filtered_high_notch_1_biceps_left);                              //rectify the signal, fabs because float   
00127        double emg_filtered_biceps_left=filterlow_b2.step(emg_abs_biceps_left);                              //low pass filter to envelope the signal
00128                
00129         //creating of on/off signal with the created on/off signals, with if statement   for right arm!    
00130        //signal substraction of filter biceps and triceps. right Biceps + left biceps -
00131        double signal_biceps_sum=emg_filtered_biceps_right-emg_filtered_biceps_left;
00132        double bicepstriceps_rightarm=emg_filtered_biceps_right-emg_filtered_triceps_right;       
00133         
00134         //creating of on/off signal with the created on/off signals, with if statement   for right arm!    
00135             if (signal_biceps_sum>treshold_biceps_right)
00136             {onoffsignal_biceps=1;}
00137           
00138             else if (signal_biceps_sum<treshold_biceps_left)
00139             {        onoffsignal_biceps=-1;       }    
00140         
00141             else
00142             {onoffsignal_biceps=0;}
00143                       
00144         //creating on/off signal for switch (left arm)
00145         
00146             if (bicepstriceps_rightarm<treshold_triceps)
00147         {        switch_signal_triceps=1;            }    
00148         
00149         else
00150         {        switch_signal_triceps=0;       }
00151         
00152         //send signals  to scope to monitor the EMG signals
00153         scope.set(0, emg_filtered_biceps_right);                    //set emg signal of right biceps to scope in channel 0
00154         scope.set(1, emg_filtered_triceps_right);                   // set emg signal of right triceps to scope in channel 1
00155         scope.set(2, emg_filtered_biceps_left);                     // set emg signal of left biceps to scope in channel 2
00156         scope.set(3, onoffsignal_biceps);                           // set on/off signal for the motors to scope in channel 3
00157         scope.set(4, switch_signal_triceps);                        // set the switch signal to scope in channel 4 
00158         
00159         scope.send();                       //send all the signals to the scope
00160                 }
00161 //======================================================================   
00162 //program
00163 //======================================================================   
00164 int main()
00165 {  
00166 
00167 pc.baud(115200); //connect with pc with baudrate 115200
00168 green=1;         //led is off (1), at beginning  
00169 red=0;           //led is on (0), at beginning
00170 
00171 //attach tickers to functions
00172 sample_timer.attach(&filter, 0.001);        //continously execute the EMG reader and filter, it ensures that filter and sampling is executed every 1/frequency seconds
00173 ticker_switch.attach(&switch_function,1.0);
00174 
00175 //Show the user what the starting motor will be and what will happen
00176 pc.printf("We will start the demonstration\r\n");
00177 pc.printf("\r\n\r\n\r\n");
00178 
00179 
00180     if (switch_signal%2==0)  
00181      {pc.printf("If you contract the biceps, the robot will go right \r\n");
00182      pc.printf("If you contract the triceps, the robot will go left \r\n");
00183       pc.printf("\r\n");
00184      }
00185        
00186     
00187     else
00188      {pc.printf("If you contract the biceps, the robot will go up \r\n");
00189      pc.printf("If you contract the triceps, the robot will go down \r\n");
00190       pc.printf("\r\n");
00191      }
00192      
00193 //==============================================================================================
00194 //endless loop
00195 
00196 
00197     while (true) {                        // neverending loop
00198         
00199     if (onoffsignal_biceps==-1)  //left biceps contracted                        
00200     {
00201          if (switch_signal%2==0) //switch even                    
00202          { 
00203            richting_motor1 = ccw;    //motor 1, left
00204            pwm_motor1 = speedmotor1; //speed of motor 1
00205                   
00206          } 
00207          
00208          else                       //switch odd        
00209          {
00210            richting_motor2 = ccw;   //motor 2, up 
00211            pwm_motor2 = speedmotor2;//speed of motor 2
00212            
00213          }      
00214               
00215     }
00216     else if (onoffsignal_biceps==1)                     //right biceps contracted
00217     {
00218          if (switch_signal%2==0)                     //switch signal even
00219          {
00220            richting_motor1 = cw;        //motor 1, right
00221            pwm_motor1 = speedmotor1;    //speed motor 1
00222            
00223         } 
00224          else                         //switch signal odd
00225          {
00226            richting_motor2 = cw;        //motor 2. down
00227            pwm_motor2 = speedmotor2;    //speed motor 2
00228                 
00229          }  
00230     }   
00231     else{ 
00232     //no contraction of biceps   
00233     pwm_motor2=0;
00234     pwm_motor1=0;
00235        }              
00236                
00237 }//while true closed
00238         
00239 } //int main closed
00240 
00241 //=============================================================================================1