main.cpp

00001 #include "mbed.h"
00002 #include "TextLCD.h"
00003 #include "Timer.h"
00004 #define moy 31      //rapport cyclique à baisser
00005 #define Kp 16
00006 #define Kd 1
00007 
00008 void vitG (int, int, float, float, float, float);
00009 void vitD (int, int, float, float, float, float);
00010 void Confettis();
00011 void Suivi_de_ligne(int,int);
00012 void Carre();
00013 
00014 DigitalIn fdc(D2);
00015 DigitalIn jack(D3);
00016 PwmOut moteur1(D6);
00017 PwmOut moteur2(D8);
00018 DigitalOut SensMot1(D7);
00019 DigitalOut SensMot2(D9);
00020 AnalogIn capteur1(A1);
00021 AnalogIn capteur2(A2);
00022 AnalogIn capteur3(A3);
00023 AnalogIn capteur4(A4);
00024 TextLCD lcd(D15,D14,D10,D11,D12,D13);
00025 DigitalIn Bouton1(D4);
00026 DigitalIn Bouton2(D5);
00027 
00028 int main()
00029 {
00030     int bout_av1=0;
00031     int bout_av2=0;
00032     int bout_fdc=0;
00033     int Prog=5, Sel=0, Regl=5, Vit=5, Coef=5;
00034     int Moy=moy, kp=Kp;
00035     while(1) {
00036         switch(Sel) {
00037             case 0 :
00038                 lcd.cls();
00039                 wait(0.001);
00040                 lcd.printf("Programm");
00041                 lcd.locate(0,1);
00042                 lcd.printf("e");
00043                 if(Bouton2.read() < bout_av2) {
00044                     Sel=1;
00045                     bout_av1=0;
00046                     wait(0.15);
00047                     break;
00048                 }
00049                 bout_av2=Bouton2.read();
00050                 wait_ms(10);
00051                 if(Bouton1.read() < bout_av1) {
00052                     Sel=1;
00053                     bout_av1=0;
00054                     wait(0.15);
00055                     break;
00056                 }
00057                 bout_av1=Bouton1.read();
00058                 wait_ms(10);
00059                 if(fdc.read() < bout_fdc) {
00060                     Sel=5;
00061                     Prog=0;
00062                     wait(0.15);
00063                     break;
00064                 }
00065                 bout_fdc=fdc.read();
00066                 wait_ms(10);
00067                 break;
00068             case 1 :
00069                 lcd.cls();
00070                 wait(0.001);
00071                 lcd.printf("Reglage");
00072 
00073                 if(Bouton2.read() < bout_av2) {
00074                     Sel=0;
00075                     bout_av1=0;
00076                     wait(0.15);
00077                     break;
00078                 }
00079                 bout_av2=Bouton2.read();
00080                 wait_ms(10);
00081                 if(Bouton1.read() < bout_av1) {
00082                     Sel=0;
00083                     bout_av1=0;
00084                     wait(0.15);
00085                     break;
00086                 }
00087                 bout_av1=Bouton1.read();
00088                 wait_ms(10);
00089                 if(fdc.read() < bout_fdc) {
00090                     Sel=5;
00091                     Regl=0;
00092                     wait(0.15);
00093                     break;
00094                 }
00095                 bout_fdc=fdc.read();
00096                 wait_ms(10);
00097                 break;
00098             default :
00099                 break;
00100         }
00101         switch(Prog) {
00102             case 0 :
00103                 lcd.cls();
00104                 wait(0.001);
00105                 lcd.printf("Confetti");
00106                 lcd.locate(0,1);
00107                 lcd.printf("s");
00108                 if(fdc.read() < bout_fdc) {
00109                     Confettis();
00110                     Prog=5;
00111                     Sel=0;
00112                     wait(0.15);
00113                     break;
00114                 }
00115                 bout_fdc=fdc.read();
00116                 wait_ms(10);
00117                 if(Bouton2.read() < bout_av2) {
00118                     Prog=3;
00119                     wait(0.15);
00120                     break;
00121                 }
00122                 bout_av2=Bouton2.read();
00123                 wait_ms(10);
00124                 if(Bouton1.read() < bout_av1) {
00125                     Prog=1;
00126                     wait(0.15);
00127                     break;
00128                 }
00129                 bout_av1=Bouton1.read();
00130                 wait_ms(10);
00131                 break;
00132             case 1 :
00133                 lcd.cls();
00134                 wait(0.001);
00135                 lcd.printf("Suivi de");
00136                 lcd.locate(0,1);
00137                 lcd.printf(" ligne");
00138                 if(fdc.read() < bout_fdc) {
00139                     Suivi_de_ligne(Moy,kp);
00140                     Prog=5;
00141                     Sel=0;
00142                     wait(0.15);
00143                     break;
00144                 }
00145                 bout_fdc=fdc.read();
00146                 wait_ms(10);
00147                 if(Bouton2.read() < bout_av2) {
00148                     Prog=0;
00149                     wait(0.15);
00150                     break;
00151                 }
00152                 bout_av2=Bouton2.read();
00153                 wait_ms(10);
00154                 if(Bouton1.read() < bout_av1) {
00155                     Prog=2;
00156                     wait(0.15);
00157                 }
00158                 bout_av1=Bouton1.read();
00159                 wait_ms(10);
00160                 break;
00161             case 2 :
00162                 lcd.cls();
00163                 wait(0.001);
00164                 lcd.printf("Carre");
00165                 if(fdc.read() < bout_fdc) {
00166                     Carre();
00167                     Prog=5;
00168                     Sel=0;
00169                     wait(0.15);
00170                     break;
00171                 }
00172                 bout_fdc=fdc.read();
00173                 wait_ms(10);
00174                 if(Bouton2.read() < bout_av2) {
00175                     Prog=1;
00176                     wait(0.15);
00177                     break;
00178                 }
00179                 bout_av2=Bouton2.read();
00180                 wait_ms(10);
00181                 if(Bouton1.read() < bout_av1) {
00182                     Prog=3;
00183                     wait(0.15);
00184                 }
00185                 bout_av1=Bouton1.read();
00186                 wait_ms(10);
00187                 break;
00188             case 3 :
00189                 lcd.cls();
00190                 wait(0.001);
00191                 lcd.printf("Quitter");
00192                 if(fdc.read() < bout_fdc) {
00193                     Prog=5;
00194                     Sel=0;
00195                     wait(0.15);
00196                     break;
00197                 }
00198                 bout_fdc=fdc.read();
00199                 wait_ms(10);
00200                 if(Bouton2.read() < bout_av2) {
00201                     Prog=2;
00202                     wait(0.15);
00203                     break;
00204                 }
00205                 bout_av2=Bouton2.read();
00206                 wait_ms(10);
00207                 if(Bouton1.read() < bout_av1) {
00208                     Prog=0;
00209                     wait(0.15);
00210                 }
00211                 bout_av1=Bouton1.read();
00212                 wait_ms(10);
00213                 break;
00214             default :
00215                 break;
00216         }
00217         switch(Regl) {
00218             case 0 :
00219                 lcd.cls();
00220                 wait(0.001);
00221                 lcd.printf("Vitesse");
00222                 if(fdc.read() < bout_fdc) {
00223                     Vit=0;
00224                     Regl=5;
00225                     wait(0.15);
00226                     break;
00227                 }
00228                 bout_fdc=fdc.read();
00229                 wait_ms(10);
00230                 if(Bouton2.read() < bout_av2) {
00231                     Regl=2;
00232                     wait(0.15);
00233                     break;
00234                 }
00235                 bout_av2=Bouton2.read();
00236                 wait_ms(10);
00237                 if(Bouton1.read() < bout_av1) {
00238                     Regl=1;
00239                     wait(0.15);
00240                     break;
00241                 }
00242                 bout_av1=Bouton1.read();
00243                 wait_ms(10);
00244                 break;
00245             case 1 :
00246                 lcd.cls();
00247                 wait(0.001);
00248                 lcd.printf("Coeffici");
00249                 lcd.locate(0,1);
00250                 lcd.printf("ent");
00251                 if(fdc.read() < bout_fdc) {
00252                     Coef=0;
00253                     Regl=5;
00254                     wait(0.15);
00255                     break;
00256                 }
00257                 bout_fdc=fdc.read();
00258                 wait_ms(10);
00259                 if(Bouton2.read() < bout_av2) {
00260                     Regl=0;
00261                     wait(0.15);
00262                     break;
00263                 }
00264                 bout_av2=Bouton2.read();
00265                 wait_ms(10);
00266                 if(Bouton1.read() < bout_av1) {
00267                     Regl=2;
00268                     wait(0.15);
00269                 }
00270                 bout_av1=Bouton1.read();
00271                 wait_ms(10);
00272                 break;
00273             case 2 :
00274                 lcd.cls();
00275                 wait(0.001);
00276                 lcd.printf("Quitter");
00277                 if(fdc.read() < bout_fdc) {
00278                     Regl=5;
00279                     Sel=0;
00280                     wait(0.15);
00281                     break;
00282                 }
00283                 bout_fdc=fdc.read();
00284                 wait_ms(10);
00285                 if(Bouton2.read() < bout_av2) {
00286                     Regl=1;
00287                     wait(0.15);
00288                     break;
00289                 }
00290                 bout_av2=Bouton2.read();
00291                 wait_ms(10);
00292                 if(Bouton1.read() < bout_av1) {
00293                     Regl=0;
00294                     wait(0.15);
00295                 }
00296                 bout_av1=Bouton1.read();
00297                 wait_ms(10);
00298                 break;
00299         }
00300         switch(Vit) {
00301             case 0 :
00302                 if(fdc.read() < bout_fdc) {
00303                     Vit=5;
00304                     Regl=0;
00305                     wait(0.15);
00306                     break;
00307                 }
00308                 bout_fdc=fdc.read();
00309                 wait_ms(10);
00310                 if(Bouton1.read() < bout_av1) {
00311                     Moy++;
00312                     if (Moy>=101) {
00313                         Moy=0;
00314                     }
00315                     lcd.cls();
00316                     wait(0.001);
00317                     lcd.printf("Vitesse:");
00318                     lcd.locate(0,1);
00319                     lcd.printf(" %d ",Moy);
00320                     wait(0.15);
00321 
00322                 }
00323                 bout_av1=Bouton1.read();
00324                 wait_ms(10);
00325                 if(Bouton2.read() < bout_av2) {
00326                     Moy--;
00327                     if (Moy<=0) {
00328                         Moy=100;
00329                     }
00330                     lcd.cls();
00331                     wait(0.001);
00332                     lcd.printf("Vitesse:");
00333                     lcd.locate(0,1);
00334                     lcd.printf(" %d ",Moy);
00335                     wait(0.15);
00336                 }
00337                 bout_av2=Bouton2.read();
00338                 wait_ms(10);
00339             default :
00340                 break;
00341         }
00342         switch(Coef) {
00343             case 0 :
00344                 if(fdc.read() < bout_fdc) {
00345                     Coef=5;
00346                     Regl=1;
00347                     wait(0.15);
00348                     break;
00349                 }
00350                 bout_fdc=fdc.read();
00351                 wait_ms(10);
00352                 if(Bouton1.read() < bout_av1) {
00353                     kp++;
00354                     if (kp>50) {
00355                         kp=0;
00356                     }
00357                     lcd.cls();
00358                     wait(0.001);
00359                     lcd.printf("Coeffici");
00360                     lcd.locate(0,1);
00361                     lcd.printf("ent : %d",kp);
00362 
00363                 }
00364                 bout_av1=Bouton1.read();
00365                 wait_ms(10);
00366                 if(Bouton2.read() < bout_av2) {
00367                     kp--;
00368                     if (kp<0) {
00369                         kp=50;
00370                     }
00371                     lcd.cls();
00372                     wait(0.001);
00373                     lcd.printf("Coeffici");
00374                     lcd.locate(0,1);
00375                     lcd.printf("ent : %d",Kp);
00376                 }
00377                 bout_av2=Bouton2.read();
00378                 wait_ms(10);
00379                 break;
00380             default :
00381                 break;
00382         }
00383     }
00384 }
00385 
00386 void Confettis()
00387 {
00388     moteur1.period_us(100);
00389     moteur2.period_us(100);
00390     int varJack ,etat=0 ;
00391     float varCapt2, varCapt3;
00392     lcd.cls();
00393     wait(0.001);
00394     lcd.printf("Confetti");
00395     lcd.locate(0,1);
00396     lcd.printf("s      V");
00397     while(etat!=3) {
00398         varJack=jack.read();
00399         varCapt2=capteur2.read();
00400         varCapt3=capteur3.read();
00401         switch(etat) {
00402             case 0 :
00403                 if ( varJack == 1 ) { /* il n'y a plus le jack */
00404                     etat = 1;
00405                 }
00406                 break;
00407             case 1 :
00408                 if ( varCapt2 <= 0.1 && varCapt3 <= 0.1 ) { /* Les capteurs détectent tout les deux LA zone "blanche" */
00409                     lcd.cls();
00410                     wait(0.001);
00411                     lcd.printf("Fini");
00412                     wait(0.15);
00413                     etat = 2;
00414                 }
00415                 break;
00416             case 2 :
00417                 if ( varJack == 0 ) {
00418                     etat = 3;
00419                 }
00420                 break;
00421         }
00422         switch(etat) {
00423             case 0 :
00424                 SensMot1=0;
00425                 SensMot2=0;
00426                 moteur1.pulsewidth_us(0);
00427                 moteur2.pulsewidth_us(0);  // moteurs à l'arrêt
00428                 printf("Je suis a l'aret( de bus lol)\n\r");
00429                 break;
00430             case 1 :
00431                 moteur1.pulsewidth_us(35);
00432                 moteur2.pulsewidth_us(35);   // fonctionnement des moteurs
00433                 printf("Je roule\n\r");
00434                 break;
00435             case 2 :
00436                 moteur1.pulsewidth_us(0);
00437                 moteur2.pulsewidth_us(0); // moteurs à l'arrêt
00438                 printf("J'attends\n\r");
00439                 break;
00440         }
00441     }
00442 }
00443 
00444 
00445 void Suivi_de_ligne(int Moy, int kp)
00446 {
00447     int etat=0;
00448     int varJack, varFDC;
00449     float Erreur;
00450     //int cpt=0;
00451     lcd.cls();
00452     moteur1.period_us(100);
00453     moteur2.period_us(100);
00454     // moteurs à l'arret
00455     moteur1.pulsewidth_us(0);
00456     moteur2.pulsewidth_us(0);
00457     wait(0.001);
00458     lcd.printf("Suivi de");
00459     lcd.locate(0,1);
00460     lcd.printf(" ligne V");
00461     Timer t;
00462     float dt,derive, Erreur_old;
00463     t.start();
00464     while(etat!=6) {
00465         dt=t.read_us();
00466         t.reset();
00467         varJack=jack.read();
00468         varFDC=fdc.read();
00469         Erreur = capteur2.read()- capteur3.read();
00470         derive=(Erreur-Erreur_old)/t;
00471         switch(etat) {
00472             case 0 : {
00473                 if ( varJack == 1 ) {
00474                     /* Il n'y a plus le jack */
00475                     etat = 1 ;
00476                     break;
00477                 }
00478                 break;
00479             }
00480             case 1 : {
00481                 if ( varFDC == 0 ) {
00482                     lcd.cls();
00483                     wait(0.001);
00484                     lcd.printf("Fini");
00485                     wait(0.15);
00486                     moteur1.pulsewidth_us(0);
00487                     moteur2.pulsewidth_us(0);
00488                     wait(0.15);
00489                     SensMot1=1;
00490                     wait(0.15);
00491                     moteur1.pulsewidth_us(25);
00492                     moteur2.pulsewidth_us(25);
00493                     wait(2);
00494                     moteur1.pulsewidth_us(0);
00495                     moteur2.pulsewidth_us(0);
00496                     SensMot1=0;
00497                     SensMot2=0;
00498                     etat = 2 ;
00499                     break;
00500                 }
00501                 if ( capteur1.read() <= 0.5 && capteur2.read() <= 0.5 && capteur3.read() <= 0.5 && capteur4.read() <0.5) {
00502                     break;
00503                 }
00504                 if ( capteur1.read() <= 0.5 && (capteur2.read() >= 0.5 || capteur3.read() >= 0.5)) {
00505                     etat = 3;
00506                     break;
00507                 }
00508                 if ( capteur4.read() <= 0.5 && (capteur2.read() >= 0.5 || capteur3.read() >= 0.5)) {
00509                     etat = 4;
00510                     break;
00511                 }
00512                 /*if ( capteur1.read() <= 0.5 && capteur2.read() <= 0.5 && capteur4.read() >= 0.5) {
00513                     cpt++;
00514                     if ( cpt == 2 ) {
00515                         etat = 5;
00516                         break;
00517                     }
00518                     wait(0.05);
00519                     break;
00520                 }*/
00521                 break;
00522             }
00523             case 2 : {
00524                 if ( varJack == 0 ) {
00525                     etat = 6 ;
00526                     break;
00527                 }
00528                 break;
00529             }
00530             case 3 : {
00531                 if ( capteur3.read()<=0.5 ) {
00532                     etat = 1;
00533                     break;
00534                 }
00535                 if ( varFDC == 0 ) {
00536                     etat = 2 ;
00537                     break;
00538                 }
00539                 break;
00540             }
00541             case 4 : {
00542                 if ( capteur2.read()<=0.5 ) {
00543                     etat = 1;
00544                     break;
00545                 }
00546                 if ( varFDC == 0 ) {
00547                     etat = 2 ;
00548                     break;
00549                 }
00550             }
00551             case 5 :
00552                 etat = 1;
00553                 break;
00554 
00555         }
00556 
00557         switch(etat) {
00558             case 0 :
00559                 moteur1.pulsewidth_us(0);
00560                 moteur2.pulsewidth_us(0);  // moteurs à l'arrêt
00561                 break;
00562             case 1 :
00563                 vitG (Moy,kp,Erreur,dt,t,derive);
00564                 vitD (Moy,kp,Erreur,dt,t,derive);
00565                 break;
00566             case 2 :
00567                 moteur1.pulsewidth_us(0);
00568                 moteur2.pulsewidth_us(0);  // moteurs à l'arrêt
00569                 break;
00570             case 3 :
00571                 moteur1.pulsewidth_us(10);
00572                 moteur2.pulsewidth_us(25);
00573                 break;
00574             case 4 :
00575                 moteur1.pulsewidth_us(25);
00576                 moteur2.pulsewidth_us(10);
00577                 break;
00578             /*case 5 :
00579                 moteur1.pulsewidth_us(0);
00580                 moteur2.pulsewidth_us(0);
00581                 wait(0.001);
00582                 SensMot1=1;
00583                 SensMot2=0;
00584                 moteur1.pulsewidth_us(25);
00585                 moteur2.pulsewidth_us(25);
00586                 wait(35*0.067);
00587                 moteur1.pulsewidth_us(0);
00588                 moteur2.pulsewidth_us(0);
00589                 SensMot1=0;
00590                 SensMot2=0;
00591                 break;*/
00592         }
00593         Erreur_old = Erreur;
00594 
00595     }
00596 }
00597 
00598 
00599 void vitG (int Moy, int kp, float erreur, float d, float t, float d_erreur)
00600 {
00601     int vitG;
00602     vitG = Moy + (erreur*kp);
00603     if ( vitG > 100 ) {
00604         vitG = 100;
00605     }
00606     if ( vitG < 0 ) {
00607         vitG = 0;
00608     }
00609     moteur1.pulsewidth_us((int)vitG);
00610 }
00611 
00612 void vitD (int Moy, int kp, float erreur, float d, float t, float d_erreur)
00613 {
00614     int vitD;
00615     vitD = Moy - (erreur*kp);
00616     if ( vitD > 100 ) {
00617         vitD = 100;
00618     }
00619     if ( vitD < 0 ) {
00620         vitD = 0;
00621     }
00622     moteur2.pulsewidth_us((int)vitD);
00623 }
00624 
00625 
00626 
00627 
00628 void Carre()
00629 {
00630     int bout_av1=0;
00631 
00632     int bout_av2=0;
00633     {
00634         moteur1.period_us(100); /* moteur gauche */
00635         moteur2.period_us(100); /* moteur droit */
00636         int varJack, etat=0,iTaille, i;
00637         lcd.cls();
00638         wait(0.001);
00639         lcd.printf("Taille :");
00640         iTaille=60;
00641         lcd.locate(0,1);
00642         lcd.printf(" %d ",iTaille);
00643         while(etat!=3) {
00644             varJack=jack.read();
00645             switch(etat) {
00646                 case 0 :
00647                     if(Bouton1.read() < bout_av1) {
00648                         iTaille++;
00649                         if (iTaille>200) {
00650                             iTaille=60;
00651                         }
00652 
00653                         lcd.cls();
00654                         wait(0.001);
00655                         lcd.printf("Taille :");
00656                         lcd.locate(0,1);
00657                         lcd.printf(" %d ",iTaille);
00658 
00659                     }
00660                     bout_av1=Bouton1.read();
00661                     wait_ms(10);
00662                     if(Bouton2.read() < bout_av2) {
00663                         iTaille--;
00664                         if (iTaille<60) {
00665                             iTaille=200;
00666                         }
00667                         lcd.cls();
00668                         wait(0.001);
00669                         lcd.printf("Taille :");
00670                         lcd.locate(0,1);
00671                         lcd.printf(" %d ",iTaille);
00672                     }
00673                     bout_av2=Bouton2.read();
00674                     wait_ms(10);
00675                     printf(" %d ",iTaille);
00676                     if ( varJack == 1) { /* Il n'y a plus le jack */
00677                         etat = 1;
00678                         break;
00679                     }
00680                     break;
00681                 case 1 :
00682                     if ( i == 4 ) {
00683                         lcd.cls();
00684                         wait(0.001);
00685                         lcd.printf("Fini");
00686                         wait(0.15);
00687                         etat = 2;
00688                         break;
00689                     }
00690                 case 2 :
00691                     if ( varJack == 0 ) {
00692                         etat = 3;
00693                         break;
00694                     }
00695             }
00696             switch(etat) {
00697                 case 0 :
00698                     moteur1.pulsewidth_us(0);
00699                     moteur2.pulsewidth_us(0);  // moteurs à l'arrêt
00700                     break;
00701                 case 1 :
00702                     for (i=0; i<4; i++) {
00703                         SensMot1=0;
00704                         SensMot2=0;
00705                         moteur1.pulsewidth_us(26);
00706                         moteur2.pulsewidth_us(25);   // fonctionnement des moteurs
00707                         wait(iTaille*0.018);  // la taille du carré x le nombre de secondes par centimètre à 9V (75%)
00708                         moteur1.pulsewidth_us(0);
00709                         moteur2.pulsewidth_us(0);
00710                         wait(0.5);
00711                         SensMot1=0;
00712                         SensMot2=1;
00713                         moteur1.pulsewidth_us(23);
00714                         moteur2.pulsewidth_us(25);
00715                         wait((((37/2)*0.034)/2)+0.08); // besoin de la distance entre les deux roues
00716                         moteur1.pulsewidth_us(0);
00717                         moteur2.pulsewidth_us(0);
00718                         wait(0.5);
00719                     }
00720                     break;
00721                 case 2 :
00722                     SensMot1=0;
00723                     SensMot2=0;
00724                     moteur1.pulsewidth_us(0);
00725                     moteur2.pulsewidth_us(0);  // moteurs à l'arrêt
00726                     break;
00727             }
00728         }
00729     }
00730 }