File content as of revision 2:48f7e5f79caa:

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

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

    while(1) {
        infra=1/(((SD_2.read()*3.3)-0.12766)/11.489);//valeur en cm
        if(infra<0||infra>25) {
            infra=25.0;
        }
        lecture_blanc();    // recupere valeurs capteurs ligne blanche
        lecture_us();   // recupere valeurs capteurs us
        lecture_boussole(); //gBoussole
        bout=bp.read();
        blocks = pixy.getBlocks();
        //pc.printf("infra : %f\n\r",infra);
        //pc.printf("us gauche= %f us droit= %f\n\r",us_gauche,us_droit);
        //wait(0.5);
        //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(200):
              if((blocks)&&(taille>50)) {
                    etat=1;
                }

                else {
                    etat=21;
                }
                if((captL1>=1)||(captL3>=1)) {
                    etat=3;
                }
                break;

            case(1) :

                if(infra<20) {


                    etat=2;
                }

                if((captL1>=1)||(captL3>=1)) {
                    etat=3;
                    flag=0;
                }
                break;
            case(2) :

                if(infra<5) {
                    etat=3;
                }
                if(us_gauche<15 || us_droit<15){
                    etat=200;
                    }
                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) :
                cap_avant=cap_set;
                erreur_av=cap_avant-gBoussole;
                
                if(erreur_av>20) {
                    erreur_av=20;
                }
                if(erreur_av<-20) {
                    erreur_av=-20;
                }
                vitmoteur(V_max*1.5+erreur_av*0.2,V_max*1.5-erreur_av*0.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;
            
            case(200):
            vitmoteur(0,20);
            wait(2);
            vitmoteur(20,20);
            wait(2);
            break;
            
        }

    }
}