File content as of revision 13:9b6578fa1669:

#include "mbed.h"
#include "CMPS03.h"
#include "CNY70.h"
#include "VMA306.h"
#include "Pixy.h"
#include "PID.h"


#define PI  3.1415926535898

#define NSpeed  800

Serial      pc          (PA_2, PA_3, 921600);
PID         motor       (TIM3, TIM4, PA_8, PA_9, PC_6, PC_5, PC_9, PC_8);
VMA306      UltraSon    (PB_13, PB_2, PB_14, PC_7, PB_15, PA_6);

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

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

void end_game()
{
    motor.setSpeed(0, 0);
    while (1) {
        led1 = 1;
        led2 = 0;
        wait(0.3);
        led1 = 0;
        led2 = 1;
        wait(0.3);
    }
}

void automate_evitemment(int speed, int seuil_face, int seuil_cote)
{
    motor.setSpeed(speed, speed);

    double val_us_gauche = 0, val_us_face = 0, val_us_droite = 0;
    while (1) {
        if (timer.read() > 80.0f)
            end_game();

        if (UltraSon.isUSGReady()) val_us_gauche = UltraSon.readUSG();
        if (UltraSon.isUSBReady()) val_us_face = UltraSon.readUSB();
        if (UltraSon.isUSDReady()) val_us_droite = UltraSon.readUSD();

        if (val_us_face < seuil_face)  {
            movement_rotate(speed, 80);
        } else if (val_us_gauche < seuil_cote) {
            movement_rotate(speed, -40);
        } else if (val_us_droite < seuil_cote) {
            movement_rotate(speed, 40);
        }

        motor.setSpeed(speed, speed);
        wait(0.005);
    }
}




main ()
{
    timer.start();

    motor.resetPosition();
    
    
    /*movement_acceleration(1000, 100);
    movement_linear(1000, 1850 - 100*10 - 100*10); //bas
    movement_deceleration(1000, 100);*/
    
    movement_linear(300, 1850); //bas
    wait(0.5);

    movement_rotate(200, -90);
    wait(0.5);

    movement_linear(300, 2275); //mid
    wait(0.5);

    /*movement_rotate(100, -47.5);
    wait(0.5);

    movement_linear(300, 2950); //mid
    wait(0.5);*/

    movement_rotate(100, -30);
    wait(0.5);

    movement_linear(300, 75); // riz
    wait(0.25);

    movement_rotate(100, 60);
    wait(0.25);

    movement_rotate(100, -60);
    wait(0.25);

    movement_linear(300, 75); // riz
    wait(0.25);

    movement_rotate(100, 60);
    wait(0.25);

    movement_rotate(100, -60);
    wait(0.25);

    movement_linear(300, 75); // riz
    wait(0.25);

    movement_rotate(100, 60);
    wait(0.25);

    movement_rotate(100, -60);
    wait(0.25);

    movement_linear(300, 75); // riz
    wait(0.25);

    movement_rotate(100, 60);
    wait(0.25);

    movement_rotate(100, -60);
    wait(0.25);

    movement_linear(300, 75); // riz
    wait(0.25);

    movement_rotate(100, 60);
    wait(0.25);

    movement_rotate(100, -30);
    wait(0.25);

    automate_evitemment(400, 50, 40);

    while (1);

}