main.cpp

00001 //libraries
00002 #include "mbed.h"
00003 #include "HIDScope.h"
00004 #include "BiQuad.h"  
00005 #include "MODSERIAL.h"
00006 #include "QEI.h"
00007 
00008 //Define objects
00009    //EMG
00010     AnalogIn    emg_biceps_right_in( A0);              //analog in to get EMG biceps  (r) in to c++
00011     AnalogIn    emg_triceps_right_in(A1);               //analog in to get EMG triceps (r) in to c++
00012     AnalogIn    emg_biceps_left_in  (A2);               //analog in to get EMG biceps  (l) in to c++
00013 
00014     //Encoder
00015     DigitalIn encoder1A(D13);
00016     DigitalIn encoder1B(D12);
00017     DigitalIn encoder2A(D11); 
00018     DigitalIn encoder2B(D10);
00019     
00020     //callibration buttons
00021     DigitalIn   button_calibration_biceps  (SW3);                //button to start calibration biceps
00022     DigitalIn   button_calibration_triceps (SW2);               // button to start calibration tricps
00023     DigitalIn   buttontest                  (D9);
00024     DigitalIn   buttontest2                 (PTC12);    
00025     //tickers
00026     Ticker      sample_timer;               //ticker
00027     Ticker      switch_function;            //ticker
00028     Ticker      ticker_calibration_biceps;
00029     Ticker      ticker_calibration_triceps;
00030     Ticker      encoder_ticker;
00031 
00032 
00033 //everything for monitoring
00034     HIDScope    scope(5);                       //open 5 channels in hidscope
00035     MODSERIAL   pc(USBTX, USBRX);               //pc connection
00036     DigitalOut  red(LED_RED);
00037     DigitalOut  green(LED_GREEN);
00038     DigitalOut  blue(LED_BLUE);
00039  
00040 //motors
00041     DigitalOut direction_motor1(D4);
00042     PwmOut pwm_motor1(D5);
00043     DigitalOut direction_motor2(D7);
00044     PwmOut pwm_motor2(D6);
00045 
00046 //define variables
00047 
00048      //for motorcontrol
00049         const int cw = 0;                   // motor should turn clockwise
00050         const int ccw =1;                   // motor should turn counterclockwise
00051         const double gearboxratio=131.25;    // gearboxratio from encoder to motor
00052         const double rev_rond=64.0;          // revolutions per round of encoder
00053         int    onoffsignal_biceps=0;        // on/off signal: 1; biceps activation, 0: nothing, -1, triceps activation
00054         int    switch_signal_triceps=0;     // switching between motors. 
00055         
00056         volatile double cut_off_value_biceps_right =    0.04;       //tested, normal values. Can be changed by calibration
00057         volatile double cut_off_value_biceps_left  =    -0.04;      //volatiles becaused changen in interrupt
00058         volatile double cut_off_value_triceps=-0.03;       
00059         double signal_biceps_sum;
00060         double bicepstriceps_rightarm;
00061         int motorswitch=0;
00062         
00063         //encoders
00064          volatile double rev_counts_motor1=0;
00065          volatile double rev_counts_motor2=0;
00066          volatile double counts_encoder1;
00067          volatile double counts_encoder2;
00068          QEI Encoder1(D12,D13, NC, rev_rond,QEI::X4_ENCODING);  // maakt een encoder aan! D12/D13 ingangen, rev_rond zijn aantal pulsen per revolutie! Bovenaan in te stellen. 
00069          QEI Encoder2(D10,D11, NC, rev_rond,QEI::X4_ENCODING);
00070         
00071         //variables and constants for calibration
00072         const float percentage_max_triceps=0.3;
00073         const float percentage_max_biceps =0.3;
00074         double max_biceps;                          //calibration maximum biceps
00075         double max_triceps;                         //calibration maximum triceps
00076     
00077     //biceps  arm 1, right arm
00078     double emg_biceps_right;
00079     double emg_filtered_high_biceps_right;
00080     double emg_abs_biceps_right;
00081     double emg_filtered_biceps_right;
00082     double emg_filtered_high_notch_1_biceps_right;
00083     //double emg_filtered_high_notch_1_2_biceps_right;
00084     
00085     //triceps arm 1, right arm
00086     double emg_triceps_right;
00087     double emg_filtered_high_triceps_right;
00088     double emg_abs_triceps_right;
00089     double emg_filtered_triceps_right;
00090     double emg_filtered_high_notch_1_triceps_right;
00091     
00092     //biceps  arm 1, left arm
00093     double emg_biceps_left;
00094     double emg_filtered_high_biceps_left;
00095     double emg_abs_biceps_left;
00096     double emg_filtered_biceps_left;
00097     double emg_filtered_high_notch_1_biceps_left;
00098     
00099     //before abs filtering
00100     
00101     //b1 = biceps right arm
00102     BiQuad filterhigh_b1(9.5654e-01,-1.9131e+00,9.5654e-01,-1.9112e+00,9.1498e-01);
00103     BiQuad filternotch1_b1 (9.9376e-01 , -1.8902e-00,   9.9376e-01 , -1.8902e-00 ,  9.875e-01);
00104     
00105     //t1= triceps right arm
00106     BiQuad filterhigh_t1(9.5654e-01,-1.9131e+00,9.5654e-01,-1.9112e+00,9.1498e-01);
00107     BiQuad filternotch1_t1 (9.9376e-01 , -1.8902e-00,   9.9376e-01 , -1.8902e-00 ,  9.875e-01);
00108     
00109     //b2= biceps left arm
00110     BiQuad filterhigh_b2(9.5654e-01,-1.9131e+00,9.5654e-01,-1.9112e+00,9.1498e-01);
00111     BiQuad filternotch1_b2 (9.9376e-01 , -1.8902e-00,   9.9376e-01 , -1.8902e-00 ,  9.875e-01);
00112     
00113     //after abs filtering
00114     BiQuad filterlow_b1 (6.2942e-06, 1.2588e-05,6.2942e-06,-1.9929e+00,9.9292e-01);
00115     BiQuad filterlow_t1 (6.2942e-06, 1.2588e-05,6.2942e-06,-1.9929e+00,9.9292e-01);
00116     BiQuad filterlow_b2 (6.2942e-06, 1.2588e-05,6.2942e-06,-1.9929e+00,9.9292e-01);
00117 
00118 //function teller
00119 void encoder_working(){
00120        counts_encoder1 = Encoder1.getPulses(); 
00121        rev_counts_motor1=counts_encoder1/(gearboxratio*rev_rond);
00122        counts_encoder2 = Encoder2.getPulses(); 
00123        rev_counts_motor2=counts_encoder2/(gearboxratio*rev_rond);  }
00124 
00125 void SwitchN() {                        // maakt simpele functie die 1 bij n optelt
00126     if(switch_signal_triceps==1)
00127     {
00128         motorswitch++;
00129         
00130     if (motorswitch%2==0)  
00131       {pc.printf("If you contract the right arm, the robot will go right \r\n");
00132      pc.printf("If you contract biceps of the left arm, the robot will go left \r\n");
00133     pc.printf("\r\n");
00134         green=0;
00135         red=1;
00136           }
00137     
00138     else
00139     {pc.printf("If you contract the biceps of right arm, the robot will go up \r\n");
00140      pc.printf("If you contract the biceps of left arm, the robot will go down \r\n");
00141      pc.printf("\r\n");
00142         green=1;
00143         red=0;
00144    
00145     }    
00146 
00147     }
00148     }
00149     
00150 //functions which are called in ticker to sample the analog signal
00151 
00152 //callibration
00153 void calibration_biceps(){
00154         if (button_calibration_biceps==0){
00155         pc.printf("start of calibration biceps, contract maximal \n");
00156         red=1;
00157         green=1;
00158         blue=0;
00159         
00160         for(int n =0; n<1500;n++)                                                  //read for 2000 samples as calibration
00161                 {
00162         emg_biceps_right=emg_biceps_right_in.read();                            //read the emg value from the elektrodes
00163         emg_filtered_high_biceps_right= filterhigh_b1.step(emg_biceps_right);
00164         emg_filtered_high_notch_1_biceps_right=filternotch1_b1.step(emg_filtered_high_biceps_right);
00165         emg_abs_biceps_right=fabs(emg_filtered_high_notch_1_biceps_right); //fabs because float
00166         emg_filtered_biceps_right=filterlow_b1.step(emg_abs_biceps_right);
00167                                 
00168             if (emg_filtered_biceps_right > max_biceps)                    //determine what the highest reachable emg signal is
00169                 {
00170             max_biceps = emg_filtered_biceps_right;
00171             
00172                 }
00173                 wait(0.001f); //to sample at same freq; 1000Hz
00174                 }
00175             cut_off_value_biceps_right=percentage_max_biceps*max_biceps; 
00176             cut_off_value_biceps_left=-cut_off_value_biceps_right;
00177             //toggle lights
00178             blue=!blue;
00179             
00180             pc.printf(" end of calibration\r\n",cut_off_value_biceps_right );   
00181             pc.printf(" change of cv biceps: %f ",cut_off_value_biceps_right );
00182             
00183             wait(0.2f);
00184               
00185             if (motorswitch%2==0)
00186             {green=0;
00187             red=1;}
00188             
00189             else       {green=1;
00190             red=0;}
00191                 }
00192                 }
00193                 
00194 void calibration_triceps(){
00195         if(button_calibration_triceps==0){
00196         red=1;
00197         green=1;
00198         blue=0;
00199       
00200         pc.printf("start of calibration triceps\r\n");
00201 
00202         for(int n =0; n<1500;n++)                                                  //read for 2000 samples as calibration
00203                 {
00204         emg_triceps_right=emg_triceps_right_in.read();                            //read the emg value from the elektrodes
00205         emg_filtered_high_triceps_right= filterhigh_t1.step(emg_triceps_right);
00206         emg_filtered_high_notch_1_triceps_right=filternotch1_t1.step(emg_filtered_high_triceps_right);
00207         emg_abs_triceps_right=fabs(emg_filtered_high_notch_1_triceps_right); //fabs because float
00208         emg_filtered_triceps_right=filterlow_t1.step(emg_abs_triceps_right);
00209                               
00210             if (emg_filtered_triceps_right > max_triceps)                    //determine what the highest reachable emg signal is
00211                 {
00212             max_triceps = emg_filtered_triceps_right;
00213             
00214                 }
00215                 wait(0.001f); //to sample at same freq; 1000Hz
00216                 }
00217             cut_off_value_triceps=-percentage_max_triceps*max_triceps; 
00218             pc.printf(" end of calibration\r\n");   
00219             pc.printf(" change of cv triceps: %f ",cut_off_value_triceps ); 
00220             blue=!blue;
00221             wait(0.2f);
00222             if (motorswitch%2==0)
00223             {green=0;
00224             red=1;}
00225             
00226             else       {green=1;
00227             red=0;}  
00228                 }
00229                 }
00230 
00231 void filter(){
00232         //biceps right arm read+filtering
00233         emg_biceps_right=emg_biceps_right_in.read();                            //read the emg value from the elektrodes
00234         emg_filtered_high_biceps_right= filterhigh_b1.step(emg_biceps_right);
00235         emg_filtered_high_notch_1_biceps_right=filternotch1_b1.step(emg_filtered_high_biceps_right);
00236         emg_abs_biceps_right=fabs(emg_filtered_high_notch_1_biceps_right); //fabs because float
00237         emg_filtered_biceps_right=filterlow_b1.step(emg_abs_biceps_right);
00238         
00239         //triceps right arm read+filtering
00240         emg_triceps_right=emg_triceps_right_in.read();                            //read the emg value from the elektrodes
00241         emg_filtered_high_triceps_right= filterhigh_t1.step(emg_triceps_right);
00242         emg_filtered_high_notch_1_triceps_right=filternotch1_t1.step(emg_filtered_high_triceps_right);
00243         emg_abs_triceps_right=fabs(emg_filtered_high_notch_1_triceps_right); //fabs because float
00244         emg_filtered_triceps_right=filterlow_t1.step(emg_abs_triceps_right);
00245         
00246         //biceps left arm read+filtering
00247         emg_biceps_left=emg_biceps_left_in.read();                            //read the emg value from the elektrodes
00248         emg_filtered_high_biceps_left= filterhigh_b2.step(emg_biceps_left);
00249         emg_filtered_high_notch_1_biceps_left=filternotch1_b2.step(emg_filtered_high_biceps_left);
00250         emg_abs_biceps_left=fabs(emg_filtered_high_notch_1_biceps_left); //fabs because float
00251         emg_filtered_biceps_left=filterlow_b2.step(emg_abs_biceps_left);
00252           
00253         //signal substraction of filter biceps and triceps. right Biceps + left biceps -
00254         signal_biceps_sum=emg_filtered_biceps_right-emg_filtered_biceps_left;
00255         bicepstriceps_rightarm=emg_filtered_biceps_right-emg_filtered_triceps_right;       
00256         //creating of on/off signal with the created on/off signals, with if statement   for right arm!    
00257         if (signal_biceps_sum>cut_off_value_biceps_right)
00258         {onoffsignal_biceps=1;}
00259           
00260         else if (signal_biceps_sum<cut_off_value_biceps_left)
00261         {
00262         onoffsignal_biceps=-1;
00263         }    
00264         
00265         else
00266         {onoffsignal_biceps=0;}
00267                       
00268         //creating on/off signal for switch (left arm)
00269         
00270         if (bicepstriceps_rightarm<cut_off_value_triceps)
00271         {
00272         switch_signal_triceps=1;    
00273         }    
00274         
00275         else
00276         {
00277         switch_signal_triceps=0;              
00278         }
00279         
00280         //send signals  to scope
00281         scope.set(0, emg_filtered_biceps_right);            //set emg signal to scope in channel 0
00282         scope.set(1, emg_filtered_triceps_right);           // set emg signal to scope in channel 1
00283         scope.set(2, emg_filtered_biceps_left);                     // set emg signal to scope in channel 2
00284         scope.set(3, onoffsignal_biceps);
00285         
00286         scope.send();                       //send all the signals to the scope
00287                 }
00288 
00289 //program
00290 
00291 int main()
00292 {  
00293 pc.baud(115200); //connect with pc with baudrate 115200
00294 
00295 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
00296 switch_function.attach(&SwitchN,1.0);                   //switch is every second available
00297 ticker_calibration_biceps.attach (&calibration_biceps,2.0);   //to call calibration biceps, stop everything else
00298 ticker_calibration_triceps.attach(&calibration_triceps,2.0);  //to call calibration triceps, stop everything else
00299 
00300 
00301   if (motorswitch%2==0) {
00302         pc.printf("If you contract the right arm, the robot will go right \r\n");
00303         pc.printf("If you contract biceps of the left arm, the robot will go left \r\n");
00304         pc.printf("\r\n");
00305         green=0;
00306         red=1;
00307         blue=1;
00308           }
00309     
00310     else
00311     {pc.printf("If you contract the biceps of right arm, the robot will go up \r\n");
00312      pc.printf("If you contract the biceps of left arm, the robot will go down \r\n");
00313      pc.printf("\r\n");
00314     green=1;
00315     red=0;
00316     blue=1;
00317            
00318     }    
00319 //endless loop
00320 
00321 
00322     while (true) {                        // zorgt er voor dat de code oneindig doorgelopen wordt  
00323     
00324                   
00325     //motor control with muscles.
00326     if (buttontest==0)                           // als s ingedrukt wordt gebeurd het volgende
00327     {
00328          if (motorswitch%2==0 && rev_counts_motor1<3.4)                     // als s ingedrukt wordt en het getal is even gebeurd het onderstaande
00329          { 
00330            direction_motor1 = ccw; //right
00331            pwm_motor1 = 0.18; 
00332            pc.printf("ccw m1\r\n");
00333            encoder_working();
00334            
00335          } 
00336          
00337          else if (motorswitch%2!=0 && rev_counts_motor2<10.0)                        // als s is ingedrukt maar het getal is niet even (dus oneven) gebeurdt het onderstaande
00338          {
00339            direction_motor2 = ccw;  //down
00340            pwm_motor2 = 1;
00341            encoder_working();
00342           
00343          }      
00344               
00345     }
00346     else if (buttontest2==0)                     
00347     {
00348          if (motorswitch%2==0 && rev_counts_motor1>-3.4)    //restriction
00349          {
00350            direction_motor1 = cw; //left
00351            pwm_motor1 = 0.18;
00352            encoder_working();
00353                                     
00354         } 
00355          else if(motorswitch%2!=0 && rev_counts_motor2>-19.0)                          // als d is ingedrukt maar het getal is niet even (dus oneven) gebeurt het onderstaande
00356          {
00357            direction_motor2 = cw;
00358            pwm_motor2 = 1;
00359            encoder_working();
00360               
00361          }  
00362     }   
00363     else{
00364        
00365     pwm_motor2=0;
00366     pwm_motor1=0;
00367     encoder_working();
00368              
00369        }              
00370     pc.printf("rev_counts_motor1= %f \r\n",rev_counts_motor1);          
00371     pc.printf("rev_counts_motor2= %f \r\n",rev_counts_motor2);          
00372 
00373 }
00374  }