File content as of revision 11:ad99b62b119f:

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


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