File content as of revision 97:42167cfeb8d7:

#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);
            wait_ms(1);
            t.stop();
            while (Ravance != 1);
            wait_ms(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 GotoArr(float timer) {
    Timer t;
    
    roboclaw.SpeedAccelM1(accel_dista, -vitesse_dista);
    roboclaw.SpeedAccelM2(accel_dista, -vitesse_dista);
    wait_ms(1);
    
    t.reset();
    t.start();
    
    while (t.read() < timer) {
        if (Ravance != 1) {
            roboclaw.ForwardM1(0);
            wait_ms(1);
            roboclaw.ForwardM2(0);
            t.stop();
            while (Ravance != 1);
            wait_ms(1);
            roboclaw.SpeedAccelM1(accel_dista, -vitesse_dista);
            roboclaw.SpeedAccelM2(accel_dista, -vitesse_dista);
            t.start();
        }
    }
    
    roboclaw.ForwardM1(0);
    wait_ms(1);
    roboclaw.ForwardM2(0);
    
    t.stop();
    t.reset();
    
    wait(waiting_time);
}

void GotoThet(double theta_) {
    wait_ms(10);
    roboclaw.ResetEnc();
    wait_ms(10);
    int distance_ticks_left = -theta_*ENTRAXE/(2*(DIAMETRE_ROUE_GAUCHE*3.14159/4096));
    int distance_ticks_right = theta_*ENTRAXE/(2*(DIAMETRE_ROUE_DROITE*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(3);
    roboclaw.ForwardM1(0);
    wait_ms(10);
    roboclaw.ForwardM2(0);
    wait_ms(10);
}

void GotoDistPos(double distance)
{
    wait_ms(10);
    roboclaw.ResetEnc();
    wait_ms(10);
    int32_t distance_ticks_left = distance/((DIAMETRE_ROUE_GAUCHE*3.14156)/4096);
    int32_t distance_ticks_right = distance/((DIAMETRE_ROUE_GAUCHE*3.14156)/4096);

    roboclaw.SpeedAccelDeccelPositionM1M2(accel_dista, vitesse_dista, deccel_dista, distance_ticks_right, accel_dista, vitesse_dista, deccel_dista, distance_ticks_left, 1);
    
    wait_ms(10);
    int EncM1 = roboclaw.ReadEncM1();
    wait_ms(10);
    int EncM2 = roboclaw.ReadEncM2();
    
    
    while(abs(EncM1 - distance_ticks_right) > 20 || abs(EncM2 - distance_ticks_left) > 20)
    {
        wait_ms(10);
        EncM1 = roboclaw.ReadEncM1();
        wait_ms(10);
        EncM2 = roboclaw.ReadEncM2();
    }
    
    wait(0.1);
    roboclaw.ForwardM1(0);
    wait_ms(10);
    roboclaw.ForwardM2(0);
    wait_ms(10);
}