File content as of revision 1:99f469d63b9a:

#include "mbed.h"
#include "fct.h"
#include "Pixy.h"
#include "PixyLink.h"
#include "SRF05.h"
#include <CMPS03.h>

Serial pc (USBTX,USBRX);
int main()
{
    init();
    Pixy pixy= Pixy(p28,p27);
    pixy.setSerialOutput(&pc);
    pc.printf("alive");

    while(1) {
        lecture_blanc();    // recupere valeurs capteurs ligne blanche
        lecture_us();   // recupere valeurs capteurs us
        lecture_boussole(); //gBoussole
        bout=bp.read();
        blocks = pixy.getBlocks();
        //pc.printf("etat=%d\n\r",etat);
        //pc.printf("etat=%d,capt1= %d, capt2= %d, us_ar=%f, bp=%d, boussole=%f\n\r",etat,captL1,captL3,us_arriere,bp.read(),gBoussole);
        if (blocks) {
            taille=pixy.blocks[j].width*pixy.blocks[j].height;
            errorX = (160-pixy.blocks[j].x);
            //pc.printf("taille=%f,sig: %d x: %d y: %d width: %d height: %d\n\r",taille, pixy.blocks[j].signature, pixy.blocks[j].x, pixy.blocks[j].y, pixy.blocks[j].width, pixy.blocks[j].height);
            //pc.printf("OK");
        }
        
        switch(etat)
        {
            case(0) :
            
                if(bout==0)
                {
                    etat=100;
                    cap_set=gBoussole;
                }
                if((captL1>=1)||(captL3>=1))
                {
                    etat=3;
                }
                break;
            
            case(100):
            
                if((blocks)&&(taille>50))
                {
                    etat=1;
                }
                
                else
                {
                    etat=21;
                }
                if((captL1>=1)||(captL3>=1))
                {
                    etat=3;
                }
                break;
                 
            case(1) :
            
                if(!blocks)
                {
                    flag++;
                }
                if(flag>40)
                {
                    etat=2;
                    tempo.stop();
                    tempo.reset();
                    tempo.start();
                    flag=0;
                }
                
                if((captL1>=1)||(captL3>=1))
                {
                    etat=3;
                    flag=0;
                }
                break;
            case(2) :
            
                if(tempo.read()>0.5)
                {
                    etat=3;
                }
                if((captL1>=1)||(captL3>=1))
                {
                    etat=3;
                }
                break;
            case(3) :
            
                if(us_arriere<25)
                {
                    etat=100;
                }
                break;
            case(21) :
                cap_rot=cap_set+30;
                if(cap_rot>360)
                {
                    cap_rot=cap_rot-360;
                }
                
                if((gBoussole>cap_rot)&&(gBoussole<cap_rot+180))
                {
                    etat=22;
                }
                if(blocks)
                {
                    etat=1;
                }
                break;
            case(22) :
                cap_rot=cap_set-30;
                if(cap_rot<0)
                {
                    cap_rot=cap_rot+360;
                }
                
                if((gBoussole<cap_rot)&&(gBoussole>cap_rot-180))
                {
                    etat=21;
                }
                if(blocks)
                {
                    etat=1;
                }
                break;
            
        }
        
        switch(etat)
        {
            case(0) : vitmoteur(0,0);break;
            
            case(1) : vitmoteur(V_max-K*errorX,V_max+K*errorX);break;
            
            case(2) : vitmoteur(V_max*1.5,V_max*1.5);break;
            
            case(3) : 
            
                    cap_arriere=cap_set;
                    erreur_ar=cap_arriere-gBoussole;
                    
                    if(erreur_ar>20)
                    {
                        erreur_ar=20;
                    }
                    if(erreur_ar<-20)
                    {
                        erreur_ar=-20;
                    }
                    vitmoteur(-V_moy+erreur_ar*0.2,-V_moy-erreur_ar*0.2);break;
            
            case(21) : vitmoteur(V_recherche,-V_recherche);break;           
            
            case(22) : vitmoteur(-V_recherche,V_recherche);break;
            
            case(100) : break;
        } 
       
    }
}