File content as of revision 7:6d58adc26b78:

#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 NSpeed  100

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        ld1         (PA_4, 20, 150, 55);
GP2A        sd1         (PB_0, 5, 80, 10);

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

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

PIXY        Pixy        (PA_0, PA_1, 230400);

DigitalIn   bp          (PC_13);

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

main ()
{
    int                 etatmvt = 0;
    int                 sens = 1, nbiter = 0, nbCC, nbNM;
    double              pos = 0.5;
    T_pixyCCBloc        CCBuf;
    T_pixyNMBloc        NMBuf;
    double              x, y, theta, vG, vD;

    pc.printf ("\n\rHelloWorld\n");
    led1 = 1;
    led2 = 0;
    disquette = 0;
    
    //motor.resetPosition();

    wait (5);

    while (1) {
        pc.printf ("\rCap = %5.2lf\n", boussole.getBearing());

        if (exterior.whatAmIOn())   pc.printf("\rwhite");
        else                        pc.printf("\rblue ");
        if (ligneG.whatAmIOn())     pc.printf("\r\twhite");
        else                        pc.printf("\r\tblue ");
        if (ligneD.whatAmIOn())     pc.printf("\r\t\twhite\n");
        else                        pc.printf("\r\t\tblue\n");

        pc.printf ("\r%4.3lf\t %4.3lf\t %4.3lf\n", exterior.getVoltage(), ligneG.getVoltage(), ligneD.getVoltage());

        pc.printf("\rGP2 longue distance = %5.2f\n",ld1.getDistance ());
        pc.printf("\rGP2 courte distance = %5.2f\n",sd1.getDistance ());

        pc.printf("\rpos = %2.1lf\n",pos);
        ballon.write (pos);
        verrou.write (pos);
        nbiter++;
        if (nbiter%5==0) {
            if (sens) {
                pos += 0.1;
                if (pos>0.9) sens = 0;
            } else {
                pos-=0.1;
                if (pos<0.1) sens = 1;
            }
        }

        if (UltraSon.isUSGReady())     pc.printf ("\rusG = %5.2lf -", UltraSon.readUSG());
        if (UltraSon.isUSBReady())     pc.printf ("\r\t\t usB = %5.2lf -", UltraSon.readUSB());
        if (UltraSon.isUSDReady())     pc.printf ("\r\t\t\t\t usD = %5.2lf", UltraSon.readUSD());
        pc.printf ("\n");

        if (Pixy.checkNewImage()) {
            Pixy.detectedObject (&nbNM, &nbCC);
            pc.printf ("\rnbCC = %2d - nbNM = %2d\n", nbCC, nbNM);
            while (nbCC > 0) {
                CCBuf = Pixy.getCCBloc ();
                nbCC--;
                pc.printf ("\rCC %5d : x=%5d, y=%5d - w=%5d, h=%5d, a=%5d\n", CCBuf.signature, CCBuf.x, CCBuf.y, CCBuf.width, CCBuf.height, (short)CCBuf.angle);
            }
            while (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);
            }
        }

        // Square danse !!!
        motor.getPosition (&x,&y, &theta);
        motor.getSpeed (&vG, &vD);

        pc.printf ("\rEtape = %d : x = %5.1lf, y = %5.1lf, theta = %5.1lf - speedG = %5.1lf, speedD = %5.1lf\n", etatmvt, x, y, 180*theta/PI, vG, vD);

        switch (etatmvt) {
            case 0 :
                // On avance de 50cm (coordonnés X = 500, Y = 0)
                motor.setSpeed (NSpeed,NSpeed);
                if (x > 300) {
                    etatmvt = 1;
                }
                break;
            case 1 :
                // On tourne à droite de 90° (sens antitrigo => négatif) => Angle = -PI/2
                motor.setSpeed (NSpeed,-NSpeed);
                if (theta < (-PI/2.0)) {
                    etatmvt = 2;
                }
                break;
            case 2 :
                // On avance de 50cm (coordonnés X = 500, Y = -500)
                motor.setSpeed (NSpeed,NSpeed);
                if (y < -300)  {
                    etatmvt = 3;
                }
                break;
            case 3 :
                // On tourne à droite de 90° (sens antitrigo => négatif) => Angle = -PI (Attention comme -PI = +PI, on teste le rebouclage)
                motor.setSpeed (NSpeed,-NSpeed);
                if (theta > 0)  {
                    etatmvt = 4;
                }
                break;
            case 4 :
                // On avance de 50cm (coordonnés X = 0, Y = -500)
                motor.setSpeed (NSpeed,NSpeed);
                if (x < 0)  {
                    etatmvt = 5;
                }
                break;
            case 5 :
                // On tourne à droite de 90° (sens antitrigo => négatif) => Angle = PI/2
                motor.setSpeed (NSpeed,-NSpeed);
                if (theta < (PI/2.0))  {
                    etatmvt = 6;
                }
                break;
            case 6 :
                // On avance de 50cm (coordonnés X = 0, Y = 0)
                motor.setSpeed (NSpeed,NSpeed);
                if (y > 0)  {
                    etatmvt = 7;
                }
                break;
            case 7 :
                // On tourne à droite de 90° (sens antitrigo => négatif) => Angle = 0
                motor.setSpeed (NSpeed,-NSpeed);
                if (theta < 0) {
                    etatmvt = 0;
                }
                break;
        }

        pc.printf ("\n\n");
        led1 = !led1;
        led2 = !led2;
        wait (0.2);
    }
}