File content as of revision 12:09932ddcb089:

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


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 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 (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 ()
{
    motor.resetPosition();
    
    automate_evitemment(400, 50, 40);
    
    while (1);
    
    /*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);
    }*/
}