Labyrinte.cpp

00001 #include "mbed.h"
00002 #include "SRF05.h"
00003 #include "fonction.h"
00004 #define ARRET 0
00005 #define ROULE 1
00006 #define DROITE 3
00007 #define DIST 2
00008 #include "SRF05.h"
00009 
00010 float srf;
00011 
00012 int main()
00013 {
00014     float cptG,cptD,E,k=19;
00015     PWMG.period_us(100);
00016     PWMD.period_us(100);
00017     int etat=ARRET;
00018    
00019     while(1) {
00020 
00021         
00022         srf=SRF.read();
00023         srf2=SRF2.read();
00024         
00025         switch(etat) {
00026             case ARRET :                            // A l'arret
00027                 if(srf>=15) etat=ROULE;
00028                 break;
00029             case ROULE :
00030                 if(srf2>10) etat=DROITE;                          // Jack débranché => robot avance
00031                 if(srf<=25) etat=ARRET;
00032                 break;
00033             case DIST :                            // A l'arret
00034                 if(srf>=25) etat=ROULE;
00035                 break;
00036 
00037 
00038 
00039         }
00040 
00041         switch(etat) {
00042 
00043             case ARRET :                            // A l'arret
00044                 roule_motdroit(0,0);
00045                 roule_motgauche(0,0);
00046                 break;
00047 
00048             case ROULE :
00049                 roule_motgauche(0,30);
00050                 roule_motdroit(0,30));                                  // Roule
00051                 printf("Measured : %.1f\n\r", srf);
00052                 break;
00053             
00054             case DROITE :                            // A l'arret
00055                 roule_motdroit(0,20);
00056                 roule_motgauche(0,35);
00057                 break;
00058 
00059                 
00060             case DIST :
00061                 roule_motgauche(0,(int)(EG-10));
00062                 roule_motdroit(0,(int)(ED-10));
00063                 break;
00064         }
00065 
00066     }
00067 
00068 }