MoJo / ER2_Labyrinthe_V4

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

ER2_Robot/suivi.cpp

Committer:
joehatier
Date:
2019-06-24
Revision:
5:b958d7fd9b77
Parent:
4:31025db8e579

File content as of revision 5:b958d7fd9b77:

#include "mbed.h"
#include "SRF05.h"
#include "fonction.h"
#define ARRET 0
#define ROULE 1


SRF05 SRFC(p8,p16);//Trig puis echo
SRF05 SRFA(p17,p18);
DigitalOut SensG(p12);//Sens Gauche
DigitalOut SensD(p11);//Sens Droite
PwmOut PWMG(p23); //Sortie moteur gauche
PwmOut PWMD(p22); //Sortie moteur droit
DigitalIn BpD(p29);
DigitalIn BpG(p30);  
AnalogIn CPTG (p20);        
AnalogIn CPTD (p19);
AnalogIn BAT (p15);

float srfa, srfc;

int main()  
{

    PWMG.period_us(100);
    PWMD.period_us(100);
    int /*J,S,*/etat=0/*,bpg,bpd*/;
    //BpD.mode(PullUp);
    //BpG.mode(PullUp);

    while(1) {
        srfa=SRFA.read();
        srfc=SRFC.read();
         printf("Measured a: %.1f\n\r", srfa);
          printf("Measured c: %.1f\n\r", srfc);

        switch(etat) {
            case 0 :                            //avance
                if(srfc<8) etat=2;
                if(srfc>=8) etat=1;
                if(srfc<=8 && srfa<=12) etat=3;
                if(srfc>=12) etat=4;
                break;
            case 1 :                            //ZIG (vers le mur)
                if(srfc<=8) etat=2;
                if(srfc<=8 && srfa<=12) etat=3;
                if(srfc>=12) etat=4;
                break;
            case 2 :                            //ZAG (éloigne du mur)
                if(srfc>=8) etat=1;
                if(srfc<=8 && srfa<=12) etat=3;
                if(srfc>=12) etat=4;
                break;
            case 3 :                            //MUR (impasse)
                if(srfa>=10) etat=0;
                break;
            case 4:                              //!MUR virage droite
                if(srfc<=8) etat=0;
        }

        switch(etat) {

            case 0 :                    //avance
                roule_motgauche(0,30);
                roule_motdroit(0,30);                                  
                break;
            case 1 :                    //ZIG (vers le mur)
                roule_motgauche(0,35);
                roule_motdroit(0,22);
                break;
            case 2 :                    //ZAG (éloigne du mur)
                roule_motgauche(0,22);
                roule_motdroit(0,35);
                break;
            case 3 :                    //MUR (impasse)
                roule_motgauche(1,35);
                roule_motdroit(0,35);
                break;
            case 4 :                    //!MUR virage droite
                roule_motgauche(0,35);
                roule_motdroit(1,35);
                break;
        }

    }

}