suivi.cpp

00001 #include "mbed.h"
00002 #include "SRF05.h"
00003 #include "fonction.h"
00004 #define ARRET 0
00005 #define ROULE 1
00006 
00007 
00008 SRF05 SRFC(p8,p16);//Trig puis echo
00009 SRF05 SRFA(p17,p18);
00010 DigitalOut SensG(p12);//Sens Gauche
00011 DigitalOut SensD(p11);//Sens Droite
00012 PwmOut PWMG(p23); //Sortie moteur gauche
00013 PwmOut PWMD(p22); //Sortie moteur droit
00014 DigitalIn BpD(p29);
00015 DigitalIn BpG(p30);  
00016 AnalogIn CPTG (p20);
00017 AnalogIn CPTD (p19);
00018 AnalogIn BAT (p15);
00019 
00020 float srfa, srfc;
00021 
00022 int main()  
00023 {
00024 
00025     PWMG.period_us(100);
00026     PWMD.period_us(100);
00027     int /*J,S,*/etat=0/*,bpg,bpd*/;
00028     //BpD.mode(PullUp);
00029     //BpG.mode(PullUp);
00030 
00031     while(1) {
00032         srfa=SRFA.read();
00033         srfc=SRFC.read();
00034          printf("Measured a: %.1f\n\r", srfa);
00035           printf("Measured c: %.1f\n\r", srfc);
00036 
00037         switch(etat) {
00038             case 0 :                            //avance
00039                 if(srfc<8) etat=2;
00040                 if(srfc>=8) etat=1;
00041                 if(srfc<=8 && srfa<=12) etat=3;
00042                 if(srfc>=12) etat=4;
00043                 break;
00044             case 1 :                            //ZIG (vers le mur)
00045                 if(srfc<=8) etat=2;
00046                 if(srfc<=8 && srfa<=12) etat=3;
00047                 if(srfc>=12) etat=4;
00048                 break;
00049             case 2 :                            //ZAG (éloigne du mur)
00050                 if(srfc>=8) etat=1;
00051                 if(srfc<=8 && srfa<=12) etat=3;
00052                 if(srfc>=12) etat=4;
00053                 break;
00054             case 3 :                            //MUR (impasse)
00055                 if(srfa>=10) etat=0;
00056                 break;
00057             case 4:                              //!MUR virage droite
00058                 if(srfc<=8) etat=0;
00059         }
00060 
00061         switch(etat) {
00062 
00063             case 0 :                    //avance
00064                 roule_motgauche(0,30);
00065                 roule_motdroit(0,30);                                  
00066                 break;
00067             case 1 :                    //ZIG (vers le mur)
00068                 roule_motgauche(0,35);
00069                 roule_motdroit(0,22);
00070                 break;
00071             case 2 :                    //ZAG (éloigne du mur)
00072                 roule_motgauche(0,22);
00073                 roule_motdroit(0,35);
00074                 break;
00075             case 3 :                    //MUR (impasse)
00076                 roule_motgauche(1,35);
00077                 roule_motdroit(0,35);
00078                 break;
00079             case 4 :                    //!MUR virage droite
00080                 roule_motgauche(0,35);
00081                 roule_motdroit(1,35);
00082                 break;
00083         }
00084 
00085     }
00086 
00087 }
00088 
00089