Rakul Iyngarathasan / Mbed 2 deprecated ROBOT_2-0

Dependencies:   mbed IHM_V2

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

main.cpp

00001 #include "IHM.h"                // contient mbed.h
00002 #include "Nboard.h"      
00003 #define ENFONCE 0
00004 #define CONSTANTE 0.7
00005 
00006 IHM ihm;
00007 Timer timer;
00008 char rar; 
00009 float vpot,capt_eg,capt_cg,capt_cd,capt_rac,capt_ed;
00010 
00011 void test();
00012 void course();
00013 void nouvel_suivi_automate();
00014 void raccourci();
00015 
00016 /*-------------------------------------------------------------------------------------------------*/
00017 int main(void)
00018 {
00019     unsigned char choix=0;
00020     
00021     ihm.LCD_clear();
00022     timer.reset();
00023     timer.start();
00024     
00025     while (timer.read()<3)
00026         {
00027             if (BP0==ENFONCE)
00028                 choix=1;
00029         }
00030         
00031     if (choix==1)
00032         {
00033             test();
00034         }
00035     
00036     else
00037         course();
00038     
00039     return 0;
00040 }
00041 /*----------------------------------------------------------------------------------------------------------*/
00042 void test(void)
00043 {
00044     
00045     ihm.LCD_gotoxy(0,0);
00046     ihm.LCD_printf("TEST Robot D3");
00047     wait(1.0);
00048     
00049     pwmg.period_us(50);
00050     pwmd.period_us(50);
00051     ihm.LCD_clear();
00052     
00053     while (1)
00054     {   
00055       BusSelectMux = Ana1;//centre gauche
00056       wait_us(1);
00057       capt_cg=AnaIn.read();
00058 
00059       BusSelectMux = Ana2;//centre droit
00060       wait_us(1);
00061       capt_cd=AnaIn.read();
00062 
00063       BusSelectMux = Ana0;//gauche
00064       wait_us(1);
00065       capt_eg=AnaIn.read();
00066 
00067       BusSelectMux = Ana5;//droite
00068       wait_us(1);
00069       capt_ed=AnaIn.read();
00070 
00071       BusSelectMux = Ana4;//raccourci
00072       wait_us(1);
00073       capt_rac=AnaIn.read();
00074       
00075       
00076             if (capt_rac>CONSTANTE)
00077                 led4=1;
00078             else
00079                 led4=0;
00080             
00081             if (capt_eg>CONSTANTE)
00082                 led3=1;
00083             else
00084                 led3=0;
00085             
00086             if (capt_cg>CONSTANTE)
00087                 led2=1;
00088             else
00089                 led2=0;
00090             
00091             if (capt_cd>CONSTANTE)
00092                 led1=1;
00093             else
00094                 led1=0;
00095             
00096             if (capt_ed>CONSTANTE)
00097                 LED0=1;
00098             else
00099                 LED0=0; 
00100       
00101 
00102       ihm.LCD_gotoxy(0,0);
00103       ihm.LCD_printf("%3.2f", capt_rac);//raccourci
00104       ihm.LCD_gotoxy(1,3);
00105       ihm.LCD_printf("%3.2f ", capt_eg);//gauche
00106       ihm.LCD_gotoxy(0,6);
00107       ihm.LCD_printf("%3.2f", capt_cg);//centre gauche
00108       ihm.LCD_gotoxy(1,9);
00109       ihm.LCD_printf("%3.2f", capt_cd);//centre droit
00110       ihm.LCD_gotoxy(0,12);
00111       ihm.LCD_printf("%3.2f", capt_ed);//droite
00112       
00113       //Test moteurs.
00114         if(BP2==0)//gauche 
00115             {
00116                 pwmg.write(0);
00117                 pwmd.write(0.25);          
00118                 wait(2);
00119                 pwmg.write(0);
00120                 pwmd.write(0);
00121             }
00122             if(BP1==0)//droite
00123             {              
00124                 pwmg.write(0.25);
00125                 pwmd.write(0);              
00126                 wait(2);
00127                 pwmg.write(0);
00128                 pwmd.write(0);
00129             } 
00130          if(BP0 == 0)
00131             {
00132                 while(BP0==0)
00133                 {
00134                     BusSelectMux = Vpot; 
00135                     wait_us(1);
00136                     vpot=AnaIn.read();
00137                     pwmd.write(vpot);
00138                     pwmg.write(vpot);                 
00139                 }
00140                 pwmg.write(0);
00141                 pwmd.write(0);  
00142             }
00143       //Test Bouton Poussoir.     
00144       if (BP==1)
00145             led7=1;
00146       else
00147             led7=0;
00148     
00149       if (Jack==0)
00150             course();
00151     }
00152 }
00153 /*----------------------------------------------------------------------------------------------------------*/
00154 void course(void)
00155     {
00156     ihm.LCD_clear();    
00157      LEDs=0x80;
00158         
00159     pwmg.period_us(50);
00160     pwmd.period_us(50);
00161     
00162     ihm.LCD_gotoxy(0,0);
00163     ihm.LCD_printf("COURSE D3: Jack?");
00164     
00165     //Attente du Jack.
00166    
00167     while (Jack==1)
00168     {
00169         BusSelectMux = Vpot;
00170         wait_us(1);
00171         vpot=AnaIn.read();
00172         
00173         ihm.LCD_gotoxy(1,0);
00174         ihm.LCD_printf("%3.2f",vpot);  //Affichage valeur potentiomètre.
00175         
00176         //Chenillard d'attente.
00177         LEDs=LEDs>>1;
00178         wait(0.5-vpot);
00179             if(LEDs==0x01)
00180              {
00181                     LEDs=0x80;
00182                      wait(0.5-vpot);
00183              }
00184     }
00185     
00186     //Lancement du timer.
00187     timer.reset();
00188     timer.start();
00189     
00190     //Départ du robot sur la ligne.
00191         
00192                 while(1)   
00193                 {
00194                     /*if (0<rar && rar<3)
00195                     {raccourci();}
00196                     else*/ 
00197                     nouvel_suivi_automate();
00198                      
00199                 }
00200     
00201 
00202  }
00203  /*--------------------------------------------------------------------------------------------------------------------------*/   
00204 void nouvel_suivi_automate(void)
00205 {
00206     typedef enum {ligne, sortieD, attente, sortieG,arret} type_suivi ;
00207     static type_suivi suivi = ligne ;
00208     
00209       BusSelectMux = Ana1;//centre gauche
00210       wait_us(1);
00211       capt_cg=AnaIn.read();
00212 
00213       BusSelectMux = Ana2;//centre droit
00214       wait_us(1);
00215       capt_cd=AnaIn.read();
00216 
00217       BusSelectMux = Ana0;//gauche
00218       wait_us(1);
00219       capt_eg=AnaIn.read();
00220 
00221       BusSelectMux = Ana5;//droite
00222       wait_us(1);
00223       capt_ed=AnaIn.read();
00224 
00225       BusSelectMux = Ana4;//raccourci
00226       wait_us(1);
00227       capt_rac=AnaIn.read();           
00228     
00229     
00230     switch(suivi)
00231     {
00232         case ligne:
00233                     pwmg.write(0.26+vpot);
00234                     pwmd.write(0.25+vpot); 
00235                   
00236                /* if(capt_rac>CONSTANTE &&  capt_ed<CONSTANTE)
00237                 {
00238                     rar=1;
00239                 }*/
00240                 if(capt_cg > CONSTANTE && capt_cd < CONSTANTE)
00241                 {
00242                     suivi = sortieD;
00243                 }  
00244                 else if(capt_cg < CONSTANTE && capt_cd > CONSTANTE)
00245                 {
00246                     suivi = sortieG;
00247                     
00248                 }
00249                  
00250                
00251                 break;   
00252         
00253     case sortieD: //correction sortie droite
00254                     pwmg.write(0.15+vpot);
00255                     pwmd.write(0.45+vpot);
00256                     
00257                     if(capt_cg > CONSTANTE && capt_cd > CONSTANTE)
00258                     {
00259                     suivi = ligne;
00260                     }        
00261                     
00262                   break;
00263                     
00264      case sortieG: //correction sortie gauche
00265                 pwmg.write(0.45+vpot);
00266                 pwmd.write(0.15+vpot);  
00267              
00268              if(capt_cg > CONSTANTE && capt_cd > CONSTANTE)
00269              {
00270                 suivi = ligne;
00271              } 
00272              break;
00273        case arret:
00274        
00275             timer.stop();
00276             ihm.LCD_gotoxy(1,7);
00277             ihm.LCD_printf("%.2fsec",timer.read());
00278               
00279             pwmg.write(0);
00280             pwmd.write(0);
00281             
00282             if (Jack==1)
00283             {
00284                 wait(0.2);
00285                 while(Jack==1);
00286                 course();
00287             } 
00288             break;
00289                
00290     }
00291     
00292     if (BP==1)
00293             suivi=arret;
00294 }
00295 /*---------------------------------------------------------------------------------------*/
00296 
00297 void raccourci(void)
00298 {
00299     typedef enum {ligne, sortieD, attente, sortieG,raccour} type_suivi ;
00300     static type_suivi suivi = ligne ;
00301     
00302       BusSelectMux = Ana1;//centre gauche
00303       wait_us(1);
00304       capt_cg=AnaIn.read();
00305 
00306       BusSelectMux = Ana2;//centre droit
00307       wait_us(1);
00308       capt_cd=AnaIn.read();
00309 
00310       BusSelectMux = Ana0;//gauche
00311       wait_us(1);
00312       capt_eg=AnaIn.read();
00313 
00314       BusSelectMux = Ana5;//droite
00315       wait_us(1);
00316       capt_ed=AnaIn.read();
00317 
00318       BusSelectMux = Ana4;//raccourci
00319       wait_us(1);
00320       capt_rac=AnaIn.read();
00321             
00322       BusSelectMux = Vpot;//potentiometre
00323       wait_us(1);
00324       vpot=AnaIn.read();
00325     
00326     
00327     switch(suivi)
00328     {
00329         case ligne:
00330                     pwmg.write(0.257);
00331                     pwmd.write(0.25); 
00332                    
00333                   
00334                 if(capt_cg < CONSTANTE && capt_cd > CONSTANTE)
00335                 {
00336                     suivi = sortieG;
00337                 }  
00338                 if(capt_cg > CONSTANTE && capt_cd < CONSTANTE)
00339              {
00340                 suivi = sortieD;
00341              }
00342                 break;   
00343         
00344     case sortieD: //correction sortie droite
00345                     pwmg.write(0.15);
00346                     pwmd.write(0.25);
00347                     
00348                     if(capt_cg > CONSTANTE && capt_cd >CONSTANTE)
00349                     {
00350                     suivi = ligne;
00351                     }        
00352                     
00353                   break;
00354                     
00355      case sortieG: //correction sortie gauche
00356                 pwmg.write(0.25);
00357                 pwmd.write(0.15);  
00358              
00359              if(capt_cg > CONSTANTE && capt_cd > CONSTANTE)
00360              {
00361                 suivi = ligne;
00362              }
00363             
00364                break;
00365      
00366      case attente: //correction sortie gauche
00367                 pwmg.write(0.257);
00368                 pwmd.write(0.25); 
00369                  
00370              
00371              if(capt_rac > CONSTANTE )
00372              {
00373                 suivi = raccour;
00374              }
00375             
00376                break;
00377      
00378      case raccour: //correction sortie gauche
00379                     while(capt_ed<CONSTANTE)
00380                         {
00381                             pwmg.write(0.01);
00382                             pwmd.write(0.2);
00383                         } 
00384                          suivi = ligne;
00385                          rar++;
00386 
00387                break; 
00388         if(capt_rac>0.8)
00389         {suivi =attente;} 
00390     }
00391 }