File content as of revision 9:234439133426:

#include "mbed.h"
#include "CMPS03.h"
#include "CNY70.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       (TIM3, TIM4, PA_8, PA_9, PC_6, PC_5, PC_9, PC_8);

CMPS03      boussole    (PC_4);

CNY70       ligneD      (PC_3);
CNY70       ligneG      (PC_2);
CNY70       exterior    (PA_7);


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

PIXY        Pixy        (PA_0, PA_1, 230400);

DigitalIn   bp          (PC_13);

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

main ()
{
    led1 = 0;
    led2 = 0;
    pc.printf ("\n\rHelloWorld\n");
    wait(0.5);
    led1 = 1;
    led2 = 0;
    
    int                 etatmvt = 00;
    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\rinit\n");
    led1 = 1;
    led2 = 0;
    
    motor.resetPosition();

    pc.printf ("\n\r3\n");
    wait (1);
    pc.printf ("\n\r2\n");
    wait (1);
    pc.printf ("\n\r1\n");
    wait (1);
    
    
    //motor.setSpeed (0,0);

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


        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 (1);
    }
}