File content as of revision 0:4effd7a50c67:

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


#define PI  3.1415926535898


#define VMOY    300
#define VMOY2   120
#define k       0.84
#define ouvert  0.8
#define fermer  1.0

Serial      pc          (PA_2, PA_3, 921600);
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);

//GP2A        sd1         (PB_0, 5, 80, 10);
AnalogIn  sd1(PB_0);

RC_Servo    ballon      (PB_10, 1);
RC_Servo    verrou      (PA_15, 1);

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

PIXY        Pixy        (PA_0, PA_1, 115200);

DigitalIn   bp          (PC_13);


DigitalOut  disquette   (PA_12);

InterruptIn button1(USER_BUTTON);

Timer t1,t2;
int gietat=0;
double gCap=0;
double gBoussole=0;
double distance_petit=0;
double erreur_cap;
long                taille_balle;
void start()
{
    gietat=1;
    gCap=boussole.getBearing();
    t2.reset();
}

int main()
{

    int                 position_balle=0;
    int                 nbCC, nbNM;
    T_pixyCCBloc        CCBuf;
    T_pixyNMBloc        NMBuf;
    disquette = 0;
    motor.resetPosition();
    verrou.write(ouvert);
    button1.fall(&start);
    t1.start();
    t2.start();

    while(1) {
        gBoussole=boussole.getBearing();
        distance_petit=(10.00/(sd1.read()*3.30-0.50))-0.42;
        if(distance_petit>20.0)distance_petit=20;
        if(distance_petit<3.5)distance_petit=3.5;
        erreur_cap=gCap-gBoussole;
        //pc.printf("etat:%d \t erreur:%0.2f \t distance:%0.2fn\n\r",gietat,abs(erreur_cap),distance_petit);
        switch(gietat) {
            case 0:
                break;
            case 1:
                if (Pixy.checkNewImage()) {
                        gietat=2;
                    }
                if((exterior.getVoltage()<0.2) && (ligneG.getVoltage()<0.2) && (ligneD.getVoltage()<0.2)) {
                    gietat=6;
                    t1.reset();
                }
                if(t2.read()>=85) gietat=8;
                break;
            case 2:
                if( distance_petit<=4.50 && distance_petit>=3.8) {
                    gietat=30;
                    t1.reset();
                }
                if((exterior.getVoltage()<0.2) && (ligneG.getVoltage()<0.2) && (ligneD.getVoltage()<0.2)) {
                    gietat=6;
                    t1.reset();
                }
                if(t2.read()>=85) gietat=8;
                break;

            case 3:
                if(abs(erreur_cap)<=7.0) {
                    gietat=4;
                    verrou.write(ouvert);
                }
                if((exterior.getVoltage()<0.2) && (ligneG.getVoltage()<0.2) && (ligneD.getVoltage()<0.2)) {
                    gietat=6;
                    t1.reset();
                }
                if(t2.read()>=85) gietat=8;
                break;
            case 4:
                if((exterior.getVoltage()<0.2) && (ligneG.getVoltage()<0.2) && (ligneD.getVoltage()<0.2)) {
                    gietat=5;
                }
                if(t2.read()>=85) gietat=8;
                break;
            case 5:
                t1.reset();
                gietat=6;
                break;
            case 6:
                if(t1.read()>=2) {
                    gietat=7;
                    t1.reset();
                }
                if(t2.read()>=85) gietat=8;
                break;
            case 7:
                if (t1.read()>1.75) {
                    gietat=70;
                    t1.reset();
                }
                if(t2.read()>=85) gietat=8;
                break;
            case 70:
            if (Pixy.checkNewImage()) {
                        gietat=2;
                    }
                if (t1.read()>0.75) {
                    gietat=71;
                    t1.reset();
                }
                
                if(t2.read()>=85) gietat=8;
                break;
            case 71: 
                if (Pixy.checkNewImage()) {
                        gietat=2;
                    }
                if (t1.read()>0.75) {
                    gietat=71;
                    t1.reset();
                }
                if(t2.read()>=85) gietat=8;
                break;
            case 30:
                if(t1.read()>0.75) {
                    gietat=31;
                    t1.reset();
                }
                if(t2.read()>=85) gietat=8;
                break;
            case 31:
                if(t1.read()>0.2) {
                    gietat=3;
                    t1.reset();
                }
                if(t2.read()>=85) gietat=8;
                break;
        }
        switch(gietat) {
            case 0:
                motor.setSpeed(0,0);
                break;
            case 1:
                motor.setSpeed(-200,200);
                break;
            case 2:
                Pixy.detectedObject (&nbNM, &nbCC);
                if (nbNM > 0) {
                    NMBuf = Pixy.getNMBloc ();
                    nbNM--;
                    //pc.printf ("\rNM %4x : x=%5d, y=%5d - w=%5d, h=%5d\n", NMBuf.signature, NMBuf.x, NMBuf.y, NMBuf.width, NMBuf.height);
                    position_balle=NMBuf.x-160;
                    //pc.printf("position_balle:%d \n\r",position_balle);
                    motor.setSpeed((int)((VMOY+(k*position_balle))),(int)((VMOY-(k*position_balle))));
                }
                break;
            case 3:
                motor.setSpeed(-150,150);
                verrou.write(fermer);
                break;
            case 30:
                motor.setSpeed(25,25);
                break;
            case 31:
                motor.setSpeed(25,25);
                verrou.write(fermer);
                break;
            case 4:
                motor.setSpeed(600,600);
                break;
            case 5:
                motor.setSpeed(0,0);
                break;
            case 6:
                motor.setSpeed(-150,-150);
                break;
            case 7:
                motor.setSpeed(-150,150);
                break;
            case 8:
                motor.setSpeed(0,0);
                break;
            case 70:
                motor.setSpeed(-150,150);
                break;
            case 71:
                motor.setSpeed(150,-150);
                break;

        }
    }
}