File content as of revision 10:bb350e855c59:

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

DigitalIn   bp          (PC_13);
DigitalOut  led1        (PA_5);
DigitalOut  led2        (PD_2);



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;


//speed entre 0 et 1300
void movement_rotate(int speed, int relative_angle_degree)
{
    double destination_angle_degree = (relative_angle_degree * PI) / 180.0f;
    double starting_angle_rad = theta;

    if (relative_angle_degree > 0.0f) {
        while (theta - starting_angle_rad < destination_angle_degree) {
            motor.setSpeed(-speed,speed);
            motor.getPosition (&x,&y, &theta);
            motor.getSpeed (&vG, &vD);
        }
    } else {
        while (theta - starting_angle_rad > destination_angle_degree) {
            motor.setSpeed(speed,-speed);
            motor.getPosition (&x,&y, &theta);
            motor.getSpeed (&vG, &vD);
        }
    }
    motor.setSpeed(0,0);
}

//speed entre 0 et 1300
void movement_linear(int speed, int relative_distance_mm)
{
    double starting_traveled_distance = motor.getTraveledDistance_mm();
    
    if (relative_distance_mm > 0.0f) {
        while (motor.getTraveledDistance_mm() - starting_traveled_distance < relative_distance_mm) {
            motor.setSpeed(speed,speed);
            motor.getPosition (&x,&y, &theta);
            motor.getSpeed (&vG, &vD);
        }
    }
    else {
        while (motor.getTraveledDistance_mm() - starting_traveled_distance > relative_distance_mm) {
            motor.setSpeed(-speed,-speed);
            motor.getPosition (&x,&y, &theta);
            motor.getSpeed (&vG, &vD);
        }
    }

    motor.setSpeed(0,0);
}

//speed entre 0 et 1300
//L'acceleration et la decelleration ne sont indispensab le qu'a partir des vitesses superieur a 300
//calcul de la distance parcourue lors de l'acceleration: (speed/100)*acceleration_distance_mm [en mm]
void movement_acceleration(int speed, int acceleration_distance_mm) {
    int acceleration_steps = speed / 100.0f;
    for (int i = 0; i < acceleration_steps ; i++) {
        movement_linear(100*(i+1), acceleration_distance_mm);
    }
}

//speed entre 0 et 1300
void movement_deceleration(int speed, int deceleration_distance_mm) {
    int acceleration_steps = speed / 100.0f;
    for (int i = acceleration_steps; i > 0 ; i--) {
        movement_linear(100*i, deceleration_distance_mm);
    }
}

main ()
{


    pc.printf ("\n\rinit\n");

    motor.resetPosition();

    while (1) {
        movement_acceleration(1300, 50);
        movement_linear(1300, 1000);
        movement_deceleration(1300, 30);
        wait(0.5);
        
        movement_linear(300, 100);
        
        movement_rotate(200, 90);
        wait(0.5);
        
        movement_acceleration(1300, -50);
        movement_linear(1300, -1000);
        movement_deceleration(1300, -30);
        wait(0.5);
    }

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