File content as of revision 0:c3b54b05bf77:

#include "mbed.h"
#include "Pixy.h"
#include "PID.h"

Serial          pc              (PA_2, PA_3, 921600);

PID             motor           (TIM4, TIM3, PA_9, PA_8, PC_9, PC_8, PC_6, PC_5);

InterruptIn     boussole        (PC_4);

AnalogIn        CNY70ligneD     (PC_3);
AnalogIn        CNY70ligneG     (PC_2);
AnalogIn        CNY70exterior   (PA_7);

AnalogIn        GP2sensorL      (PA_4);
AnalogIn        GP2sensorS      (PB_0);

DigitalOut      trig1           (PB_15);
InterruptIn     echo1           (PA_6);
DigitalOut      trig2           (PB_14);
InterruptIn     echo2           (PC_7);
DigitalOut      trig3           (PB_13);
InterruptIn     echo3           (PB_2);

//PwmOut          ballon          (PB_10);
//PwmOut          verrou          (PA_15);

PIXY            pixy            (PA_0, PA_1, 230400);

DigitalIn       bp              (PC_13);

DigitalOut      led1            (PA_5);
DigitalOut      led2            (PD_2);
DigitalOut      disquette       (PA_12);

Timer           temps;
Ticker          tick;

bool            CMPS03flag, VMA306Flag1, VMA306Flag2, VMA306Flag3;
long            CMPS03startTime, VMA306startTime1, VMA306startTime2, VMA306startTime3;
double          CMPS03bearing, VMA306dist1, VMA306dist2, VMA306dist3;
double          _Kp, _Ki, _Kd;
double          _SpeedG, _SpeedD;
double          _measDistG, _measDistD;
int             _WheelStuckG, _WheelStuckD;
double          _X, _Y, _THETA;

void VMA306rise1(void)
{
    VMA306startTime1 = temps.read_us();
}

void VMA306fall1(void)
{
    VMA306dist1 = (double)(temps.read_us() - VMA306startTime1)/58.0;
    VMA306Flag1 = 1;
}

// Clear and start the timer at the begining of the echo pulse
void VMA306rise2(void)
{
    VMA306startTime2 = temps.read_us();
}

// Stop and read the timer at the end of the pulse
void VMA306fall2(void)
{
    VMA306dist2 = (double)(temps.read_us() - VMA306startTime2)/58.0;
    VMA306Flag2 = 1;
}

// Clear and start the timer at the begining of the echo pulse
void VMA306rise3(void)
{
    VMA306startTime3 = temps.read_us();
}

// Stop and read the timer at the end of the pulse
void VMA306fall3(void)
{
    VMA306dist3 = (double)(temps.read_us() - VMA306startTime3)/58.0;
    VMA306Flag3 = 1;
}

void CMPS03rise (void)
{
    CMPS03startTime = temps.read_us();
}

void CMPS03fall (void)
{
    CMPS03bearing = ((double)(temps.read_us() - CMPS03startTime - 1000)/100.0);
}

double getGP2Distance (double sensorValue, double slope)
{
    return  slope/(sensorValue * 3.3);
}

bool whatIsUnderMyCNY (double sensorValue)
{
    return (sensorValue > 0.5) ? 0 : 1;
}

void baisser (PwmOut *ptr)
{
    ptr->pulsewidth_ms(2);
}

void lever (PwmOut *ptr)
{
    ptr->pulsewidth_ms(1);
}


main ()
{
    int                 nbCC, nbNM, objet = 0;
    T_pixyCCBloc        CCBuf[20];
    T_pixyNMBloc        NMBuf[20];
    double              topDepart, capAuDepart;
    double              x, y, theta;
    double              speedG = 1000, speedD = 1000;
    PwmOut              *ptr;

    boussole.rise       (&CMPS03rise);
    boussole.fall       (&CMPS03fall);

    echo1.rise          (&VMA306rise1);
    echo1.fall          (&VMA306fall1);
    echo2.rise          (&VMA306rise2);
    echo2.fall          (&VMA306fall2);
    echo3.rise          (&VMA306rise3);
    echo3.fall          (&VMA306fall3);

    temps.start();

/*    ptr = new PwmOut(PB_10);
    ptr->period_ms(20);
    baisser(ptr);
    wait (2);
    delete ptr;
    
    ptr = new PwmOut(PA_15);
    ptr->period_ms(20);
    lever(ptr);
    wait (2);
    delete ptr;*/

    pc.printf ("\n\rHelloWorld\n");
    led1 = 1;
    led2 = 0;
    disquette = 0;
    motor.resetPosition();
    wait(3);
    
    pc.printf("\rcheck Pixy\n");
    if (pixy.checkPixy()) led2=1;
    else {
        pc.printf("\rLa pixy ne parle pas\n");
        while(1);
    }
    pc.printf("\rApprochez une balle\n");
    do {
        pixy.detectedObject(&nbNM, &nbCC);
    } while(nbNM!=1);
    pc.printf("\rOK !\n");
    led1 = 0;

    pc.printf("\rApprochez une balise\n");
    do {
        pixy.detectedObject(&nbNM, &nbCC);
    } while(nbCC!=1);
    pc.printf("\rOK !\n");
    led2 = 0;

    pc.printf("\n\rcheck Moteur\n");
    while(bp.read());
    led1 = 1;

    //Avance
    motor.setSpeed (80,80);
    do {
        motor.getPosition (&x,&y, &theta);
    } while (x<50);
    motor.setSpeed (0,0);
    wait_ms(10);

    //Recule
    motor.setSpeed (-80,-80);
    do {
        motor.getPosition (&x,&y, &theta);
    } while (x>5);
    motor.setSpeed (0,0);
    wait_ms(10);

    //Tourne à droite
    motor.setSpeed (80,-80);
    do {
        motor.getPosition (&x,&y, &theta);
    } while (theta > -PI/2.0);

    motor.setSpeed (0,0);
    pc.printf("\rMoteurs OK\n");
    while (bp.read());
    
    pc.printf("\rScan de la zone\n");

    motor.setSpeed (-50,50);
    do {
        pixy.detectedObject(&nbNM, &nbCC);
    } while (nbNM != 1);

    pc.printf("\rBalle trouvee\n");
    pc.printf("\rAlignement\n");

    motor.setSpeed (-30,30);
    do {
        pixy.detectedObject(&nbNM, &nbCC);
        if (nbNM==1) {
            NMBuf[objet] = pixy.getNMBloc ();
            nbNM--;
        }
    } while (NMBuf[objet].x != 160);

    pc.printf("\rBalle alignee\n");
    pc.printf("\n\rREADY TO GO\n");

    do {
        led1 = !led1;
        wait(0.1);
    } while (bp);

    topDepart = temps.read();
    capAuDepart = CMPS03bearing;

    while (1) {
        do {
            motor.setSpeed (speedG, speedD);
            pixy.detectedObject(&nbNM, &nbCC);
            if (nbNM == 1) {
                NMBuf[0] = pixy.getNMBloc ();
                nbNM--;
            }
            speedG = 1000 - 10*(double)(NMBuf[0].x - 160);
            speedD = 1000 - 10*(double)(NMBuf[0].x + 160);
        } while (NMBuf[0].y < 170);
        
        


    }
}