File content as of revision 18:ede80eb1e6d5:

#include "mbed.h"
#include "CMPS03.h"
#include "CNY70.h"
#include "GP2A.h"
#include "VMA306.h"
#include "Pixy.h"
#include "PID.h"

#define PI  3.1415926535898

Serial      pc          (PA_2, PA_3, 115200);
PID         motor       (TIM4, TIM3, PA_9, PA_8, PC_9, PC_8, PC_6, PC_5);

CMPS03      boussole    (PC_4);

CNY70       ligneD      (PC_3);
CNY70       ligneG      (PC_2);
CNY70       exterior    (PA_7);

VMA306      ultraSon    (PB_15, PA_6, PB_14, PC_7, PB_13, PB_2);

PIXY        pixy        (PA_0, PA_1, 115200);

InterruptIn button          (PC_13);

DigitalOut  led1        (PA_5);
DigitalOut  led2        (PD_2);
DigitalOut  unused1     (PB_10);
DigitalOut  unused2     (PA_15);
DigitalOut  unused3     (PA_12);

DigitalIn   unused4     (PA_4, PullUp);
DigitalIn   unused5     (PB_0, PullUp);
DigitalIn   unused6     (PC_1, PullUp);
DigitalIn   unused7     (PC_0, PullUp);

Timer       timer;

typedef enum {IDLE,WAIT1,WAIT2,HOW,GET,TURN,ALIGN,FOLLOW} T_state;

main ()
{
    T_pixyNMBloc NMBloc;
    int nbNM,nbCC;
    double x,y,theta,VL=0,VR=0;
    T_state  state= IDLE;
    timer.start();
    while (1) {
        motor.getPosition(&x,&y,&theta);
        motor.setSpeed(VL,VR);
        float distance= ultraSon.readUSB();
        switch (state) {
            case IDLE:
                if (pixy.checkPixy()==0)state= WAIT1;
                break;
            case WAIT1:
                if (button==0) state= WAIT2;
                break;
            case WAIT2:
                VL=0;
                VR=0;
                if (pixy.checkNewImage()) state= HOW;

                break;
            case HOW:
                pixy.detectedObject(&nbNM,&nbCC);
                if(nbNM==1)state= GET;
                else state= WAIT2;
                break;
            case GET:
                NMBloc=pixy.getNMBloc();
                state=TURN;
                timer.reset();
                break;
            case TURN:
                VL=NMBloc.x-159;
                VR=159-NMBloc.x;
                if(pixy.checkNewImage()) state=HOW;
                if(timer.read()>0.5f) state= WAIT2;
                if(NMBloc.x > 155 && NMBloc.x < 165) state= FOLLOW;
                else state = WAIT2;

                break;
            case ALIGN:
                VL=NMBloc.x-159;
                VR=159-NMBloc.x;
                if(pixy.checkNewImage()) state=HOW;
                if(timer.read()>0.5f) state= WAIT2;
                if(NMBloc.x > 155 && NMBloc.x < 165) state= FOLLOW;
                else state = WAIT2;

                break;

            case FOLLOW :
                if(x<-300)state= WAIT2;
                // if(distance>20)state=WAIT2;
                VL=-100;
                VR=-100;
                break;
                  
                
        }
    }
}