Theo/Ludo/Joe / ER2_Labyrinthe_V3

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-05-24
Revision:
1:96ef3513d0d5
Parent:
0:1a801a2a7b4b
Child:
2:73e8dca28f51

File content as of revision 1:96ef3513d0d5:

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

float srf;

int main()
{
    float cptG,cptD,E,k=19,EG,ED/*,bat*/;
    PWMG.period_us(100);
    PWMD.period_us(100);
    int /*J,S,*/etat=ARRET/*,bpg,bpd*/;
    //BpD.mode(PullUp);
    //BpG.mode(PullUp);
  
    while(1) {
        
        wait(0.01);
        //bat=BAT.read()*4*3.3;
        //bat=7/bat;
        //J=Jack.read();
        //S=Switch.read();
                cptG=CPTG.read();          // Lis les valeurs capteurs
                cptD=CPTD.read();          // Lis les valeurs capteurs
                srf=SRF.read();
    
       E=cptG-cptD;
       EG=40/**bat*/+k*E;
       ED=40/**bat*/-k*E;
       if(EG<0) EG=0;
       if(EG>100) EG=100;
       if(ED<0) ED=0;
       if(ED>100) ED=100;

        switch(etat) {
            case ARRET :                            // A l'arret
                if(srf>=25) etat=ROULE;
                break;                       
            case ROULE :  
                if(25<srf<=29) etat=DIST;                          // Jack débranché => robot avance
               if(srf<=25) etat=ARRET; 
                break;
            case DIST :                            // A l'arret
                if(srf>=25) etat=ROULE;
                break;   
            


        }

        switch(etat) {
            
            case ARRET :                            // A l'arret
                roule_motdroit(0,0);
                roule_motgauche(0,0);
                break;                     
            
            case ROULE :
                roule_motgauche(0,(int)(EG));
                roule_motdroit(0,(int)(ED));                                  // Roule 
                printf("Measured : %.1f\n\r", srf);
                break;
            case DIST :                           
                roule_motgauche(0,(int)(EG-10));
                roule_motdroit(0,(int)(ED-10));
                break;          
        }

}

}