File content as of revision 0:86c5a1f6e21d:

#include "mbed.h"
#include "Nucleo_Encoder_16_bits.h"
#include "math.h"
#include "PID.h"

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

Serial      pc          (PA_2, PA_3, 921600);                                   // Create a serial link to PC for communication

//DigitalOut              led1        (PA_5);                                     // Added Led1 for test purpose
DigitalOut              led2        (PD_2);                                     // Added Led2 for test purpose
DigitalOut              disquette   (PA_12);                                    // Added baloon destructor command (without it, you might see baloon destructor motor be set to full speed)

main ()
{
    int                 etatmvt = 0;
    double              x, y, theta, vG, vD;

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

    wait (5);

    while (1) {

        // 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", etatmvt, x, y, theta, vG, vD);

        switch (etatmvt) {
            case 0 :
                // On avance de 50cm (coordonnés X = 500, Y = 0)
                motor.setSpeed (400,400);
                if (x >= 500) etatmvt = 1;
                break;
            case 1 :
                // On tourne à droite de 90° (sens antitrigo => négatif) => Angle = -PI/2
                motor.setSpeed (400,-400);
                if (theta <= -PI/2.0) etatmvt = 2;
                break;
            case 2 :
                // On avance de 50cm (coordonnés X = 500, Y = -500)
                motor.setSpeed (400,400);
                if (y <= -500) 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 (400,-400);
                if (theta >= 0) etatmvt = 4;
                break;
            case 4 :
                // On avance de 50cm (coordonnés X = 0, Y = -500)
                motor.setSpeed (400,400);
                if (x <= 0) etatmvt = 5;
                break;
            case 5 :
                // On tourne à droite de 90° (sens antitrigo => négatif) => Angle = PI/2
                motor.setSpeed (400,-400);
                if (theta <= PI/2.0) etatmvt = 6;
                break;
            case 6 :
                // On avance de 50cm (coordonnés X = 0, Y = 0)
                motor.setSpeed (400,400);
                if (y <= 0) etatmvt = 7;
                break;
            case 7 :
                // On tourne à droite de 90° (sens antitrigo => négatif) => Angle = 0
                motor.setSpeed (400,-400);
                if (theta <= 0) etatmvt = 0;
                break;
        }

//        led1 = !led1;
        led2 = !led2;
        wait (0.2);
    }
}