File content as of revision 1:81896d606b4e:

#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);

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, 180*theta/PI, vG, vD);

        switch (etatmvt) {
            case 0 :
                // On avance de 50cm (coordonnés X = 500, Y = 0)
                motor.setSpeed (300,300);
                if (x > 500) {
                    etatmvt = 1;
                    pc.printf("\n");
                }
                break;
            case 1 :
                // On tourne à droite de 90° (sens antitrigo => négatif) => Angle = -PI/2
                motor.setSpeed (150,-150);
                if (theta < (-PI/2.0)) {
                    etatmvt = 2;
                    pc.printf("\n");
                }
                break;
            case 2 :
                // On avance de 50cm (coordonnés X = 500, Y = -500)
                motor.setSpeed (300,300);
                if (y < -500)  {
                    etatmvt = 3;
                    pc.printf("\n");
                }
                break;
            case 3 :
                // On tourne à droite de 90° (sens antitrigo => négatif) => Angle = -PI (Attention comme -PI = +PI, on teste le rebouclage)
                motor.setSpeed (150,-150);
                if (theta > 0)  {
                    etatmvt = 4;
                    pc.printf("\n");
                }
                break;
            case 4 :
                // On avance de 50cm (coordonnés X = 0, Y = -500)
                motor.setSpeed (300,300);
                if (x < 0)  {
                    etatmvt = 5;
                    pc.printf("\n");
                }
                break;
            case 5 :
                // On tourne à droite de 90° (sens antitrigo => négatif) => Angle = PI/2
                motor.setSpeed (200,-200);
                if (theta < (PI/2.0))  {
                    etatmvt = 6;
                    pc.printf("\n");
                }
                break;
            case 6 :
                // On avance de 50cm (coordonnés X = 0, Y = 0)
                motor.setSpeed (300,300);
                if (y > 0)  {
                    etatmvt = 7;
                    pc.printf("\n");
                }
                break;
            case 7 :
                // On tourne à droite de 90° (sens antitrigo => négatif) => Angle = 0
                motor.setSpeed (200,-200);
                if (theta < 0) {
                    etatmvt = 8;
                    pc.printf("\n");
                }
                break;
            case 8 :
                // On avance de 50cm (coordonnés X = 500, Y = 0)
                motor.setSpeed (300,300);
                if (x > 500) {
                    etatmvt = 9;
                    pc.printf("\n");
                }
                break;
            case 9 :
                // On tourne à droite de 90° (sens antitrigo => négatif) => Angle = -PI/2
                motor.setSpeed (-150,150);
                if (theta > (PI/2.0)) {
                    etatmvt = 10;
                    pc.printf("\n");
                }
                break;
            case 10 :
                // On avance de 50cm (coordonnés X = 500, Y = -500)
                motor.setSpeed (300,300);
                if (y > 500)  {
                    etatmvt = 11;
                    pc.printf("\n");
                }
                break;
            case 11 :
                // On tourne à droite de 90° (sens antitrigo => négatif) => Angle = -PI (Attention comme -PI = +PI, on teste le rebouclage)
                motor.setSpeed (-150,150);
                if (theta < 0)  {
                    etatmvt = 12;
                    pc.printf("\n");
                }
                break;
            case 12 :
                // On avance de 50cm (coordonnés X = 0, Y = -500)
                motor.setSpeed (300,300);
                if (x < 0)  {
                    etatmvt = 13;
                    pc.printf("\n");
                }
                break;
            case 13 :
                // On tourne à droite de 90° (sens antitrigo => négatif) => Angle = PI/2
                motor.setSpeed (-200,200);
                if (theta > (-PI/2.0))  {
                    etatmvt = 14;
                    pc.printf("\n");
                }
                break;
            case 14 :
                // On avance de 50cm (coordonnés X = 0, Y = 0)
                motor.setSpeed (300,300);
                if (y < 0)  {
                    etatmvt = 15;
                    pc.printf("\n");
                }
                break;
            case 15 :
                // On tourne à droite de 90° (sens antitrigo => négatif) => Angle = 0
                motor.setSpeed (-200,200);
                if (theta > 0) {
                    etatmvt = 0;
                    pc.printf("\n");
                }
                break;
        }
        led1 = !led1;
        led2 = !led2;
        wait (0.005);
    }
}