File content as of revision 94:86b9bd6d5c28:

#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_) {
    roboclaw.ResetEnc();
    wait_ms(10);
    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(3);
    roboclaw.ForwardM1(0);
    roboclaw.ForwardM2(0);
}

void Odometry::GotoDistPos(double distance)
{
    int32_t pos_initiale_right = m_pulses_right, pos_initiale_left = m_pulses_left;

    int32_t distance_ticks_right = (int32_t) distance/m_distPerTick_right + pos_initiale_right;
    int32_t distance_ticks_left = (int32_t) distance/m_distPerTick_left + pos_initiale_left;

    roboclaw.SpeedAccelDeccelPositionM1M2(accel_dista, avant ? vitesse_dista : 0 - vitesse_dista, deccel_dista, distance_ticks_right, accel_dista, avant ? vitesse_dista : 0 - vitesse_dista, deccel_dista, distance_ticks_left, 1);

    //logger.printf("IniR:%6d\tDistR:%6d\tIniL:%6d\tDistL:%6d\n\r", pos_initiale_right, distance_ticks_right, pos_initiale_left, distance_ticks_left);
    
    while((m_pulses_right != distance_ticks_right)&&(m_pulses_left != distance_ticks_left)); //logger.printf("PR:%6d\tIR:%6d\tDR:%6d\tPL:%6d\tIL:%6d\tDL:%6d\n\r",m_pulses_right, pos_initiale_right, distance_ticks_right, m_pulses_left, pos_initiale_left, distance_ticks_left);
    
    wait(0.4);
    //logger.printf("arrivey %d\n\r",pos_prog);
    //logger.printf("X : %3.2f\tY : %3.2f\tTheta : %3.2f\n\r", getX(), getY(), getTheta()*180/PI);
    logger.printf("End\n\r");
}