suivi.cpp

00001 #include "mbed.h"
00002 #include "SRF05.h"
00003 #include "fonction.h"
00004 #define ARRET 0
00005 #define ROULE 1
00006 #define DIST 2
00007 #include "SRF05.h"
00008 
00009 float srf;
00010 
00011 int main()
00012 {
00013     float cptG,cptD,E,k=19,EG,ED/*,bat*/;
00014     PWMG.period_us(100);
00015     PWMD.period_us(100);
00016     int /*J,S,*/etat=ARRET/*,bpg,bpd*/;
00017     //BpD.mode(PullUp);
00018     //BpG.mode(PullUp);
00019   
00020     while(1) {
00021         
00022         wait(0.01);
00023         //bat=BAT.read()*4*3.3;
00024         //bat=7/bat;
00025         //J=Jack.read();
00026         //S=Switch.read();
00027                 cptG=CPTG.read();          // Lis les valeurs capteurs
00028                 cptD=CPTD.read();          // Lis les valeurs capteurs
00029                 srf=SRF.read();
00030     
00031        E=cptG-cptD;
00032        EG=40/**bat*/+k*E;
00033        ED=40/**bat*/-k*E;
00034        if(EG<0) EG=0;
00035        if(EG>100) EG=100;
00036        if(ED<0) ED=0;
00037        if(ED>100) ED=100;
00038 
00039         switch(etat) {
00040             case ARRET :                            // A l'arret
00041                 if(srf>=25) etat=ROULE;
00042                 break;                       
00043             case ROULE :  
00044                 if(25<srf<=29) etat=DIST;                          // Jack débranché => robot avance
00045                if(srf<=25) etat=ARRET; 
00046                 break;
00047             case DIST :                            // A l'arret
00048                 if(srf>=25) etat=ROULE;
00049                 break;   
00050             
00051 
00052 
00053         }
00054 
00055         switch(etat) {
00056             
00057             case ARRET :                            // A l'arret
00058                 roule_motdroit(0,0);
00059                 roule_motgauche(0,0);
00060                 break;                     
00061             
00062             case ROULE :
00063                 roule_motgauche(0,(int)(EG));
00064                 roule_motdroit(0,(int)(ED));                                  // Roule 
00065                 printf("Measured : %.1f\n\r", srf);
00066                 break;
00067             case DIST :                           
00068                 roule_motgauche(0,(int)(EG-10));
00069                 roule_motdroit(0,(int)(ED-10));
00070                 break;          
00071         }
00072 
00073 }
00074 
00075 }
00076 
00077