File content as of revision 89:46730de0d013:

#include "entete.h"

void GotoDist(float timer) {
    Timer t;
    
    roboclaw.SpeedAccelM1(accel_dista, vitesse_dista);
    roboclaw.SpeedAccelM2(accel_dista, vitesse_dista);
    
    t.reset();
    t.start();
    
    while (t.read() < timer) {
        if (Ravance != 1) {
            roboclaw.ForwardM1(0);
            roboclaw.ForwardM2(0);
            t.stop();
            while (Ravance != 1);
            roboclaw.SpeedAccelM1(accel_dista, vitesse_dista);
            roboclaw.SpeedAccelM2(accel_dista, vitesse_dista);
            t.start();
        }
    }
    
    roboclaw.ForwardM1(0);
    roboclaw.ForwardM2(0);
    
    t.stop();
    t.reset();
    
    wait(waiting_time);
}

void GotoThet (float timer, int signe) {
    Timer t;
    
    if (signe == GAUCHE) {
        //roboclaw.SpeedAccelM1(accel_angle, vitesse_angle);
        //roboclaw.SpeedAccelM2(accel_angle, -vitesse_angle);
        roboclaw.SpeedM1(vitesse_angle);
        roboclaw.SpeedM2(-vitesse_angle);
    }
    if (signe == DROITE) {
        roboclaw.SpeedAccelM1(accel_angle, -vitesse_angle);
        roboclaw.SpeedAccelM2(accel_angle, vitesse_angle);
    }
    
    t.reset();
    t.start();
    
    while (t.read() < timer) {
        if (Ravance != 1) {
            roboclaw.ForwardM1(0);
            roboclaw.ForwardM2(0);
            t.stop();
            while (Ravance != 1);
            t.start();
        }
        if (signe < 0) {
            roboclaw.SpeedAccelM1(accel_dista, vitesse_dista);
            roboclaw.SpeedAccelM2(-accel_dista, -vitesse_dista);
        }
        else {
            roboclaw.SpeedAccelM1(-accel_dista, -vitesse_dista);
            roboclaw.SpeedAccelM2(accel_dista, vitesse_dista);
        }
    }
    
    roboclaw.ForwardM1(0);
    roboclaw.ForwardM2(0);
    
    t.stop();
    t.reset();
    
    wait(waiting_time);
}

void GotoArr(float timer) {
    Timer t;
    
    roboclaw.SpeedAccelM1(accel_dista, -vitesse_dista);
    roboclaw.SpeedAccelM2(accel_dista, -vitesse_dista);
    
    t.reset();
    t.start();
    
    while (t.read() < timer) {
        if (Ravance != 1) {
            roboclaw.ForwardM1(0);
            roboclaw.ForwardM2(0);
            t.stop();
            while (Ravance != 1);
            roboclaw.SpeedAccelM1(accel_dista, vitesse_dista);
            roboclaw.SpeedAccelM2(accel_dista, vitesse_dista);
            t.start();
        }
    }
    
    roboclaw.ForwardM1(0);
    roboclaw.ForwardM2(0);
    
    t.stop();
    t.reset();
    
    wait(waiting_time);
}

void GotoThet(double theta_) {
    roboclaw.ResetEnc();
    float diameter_left = 61.7;
    float diameter_right = 61.8;
    int distance_ticks_left = -theta_*ENTRAXE/(2*(diameter_left*3.14159/4096));
    int distance_ticks_right = theta_*ENTRAXE/(2*(diameter_right*3.14159/4096));

    if (theta_ < 0)
        roboclaw.SpeedAccelDeccelPositionM1M2(-accel_angle, vitesse_angle, deccel_angle, distance_ticks_right, accel_angle, vitesse_angle, deccel_angle, distance_ticks_left, 1);
    else
        roboclaw.SpeedAccelDeccelPositionM1M2(-accel_angle, vitesse_angle, deccel_angle, distance_ticks_right, -accel_angle, vitesse_angle, deccel_angle, distance_ticks_left, 1);

    wait(10);
    roboclaw.ForwardM1(0);
    roboclaw.ForwardM2(0);
}