MoJo / ER2_Robot_gnuarmeclipse_lpc1768-V2

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-02-15
Revision:
0:1a801a2a7b4b
Child:
1:96ef3513d0d5

File content as of revision 0:1a801a2a7b4b:

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

SRF05 SRF(p13,p14);
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 srf;

int main()
{
    float cptG,cptD,E,k=18,EG,ED/*,bat*/;
    PWMG.period_us(100);
    PWMD.period_us(100);
    int /*J,S,*/etat=ROULE/*,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 :                            // Jack débranché => robot avance
               if(srf<=25) etat=ARRET; 
                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;
        }

}

}