File content as of revision 4:3e6e78d6d3d9:

#include "Odometry.h"

// M1 = Moteur droit, M2 = Moteur gauche

Odometry::Odometry(double diameter_right, double diameter_left, double v, RoboClaw &rc) : roboclaw(rc){
    m_v = v;
    m_distPerTick_left = diameter_left*PI/37400;
    m_distPerTick_right = diameter_right*PI/37400;

    roboclaw.ForwardM1(ADR, 0);
    roboclaw.ForwardM2(ADR, 0);

    // Erreur autorisée sur le déplacement en angle
    erreur_ang = 0.01;

    m_pulses_right = 0;
    m_pulses_left = 0;
    wait_ms(100);
}

void Odometry::setPos(double x, double y, double theta){
    this->x = x;
    this->y = y;
    this->theta = theta;
}

void Odometry::setX(double x){
    this->x = x;
}

void Odometry::setY(double y){
    this->y = y;
}

void Odometry::setTheta(double theta){
    this->theta = theta;
}

void Odometry::update_odo(void){
    int32_t delta_right = roboclaw.ReadEncM1(ADR) - m_pulses_right;
    m_pulses_right = roboclaw.ReadEncM1(ADR);
    int32_t delta_left = roboclaw.ReadEncM2(ADR) - m_pulses_left;
    m_pulses_left = roboclaw.ReadEncM2(ADR);

    double deltaS = (m_distPerTick_left*delta_left + m_distPerTick_right*delta_right) / 2.0f;
    double deltaTheta = (m_distPerTick_right*delta_right - m_distPerTick_left*delta_left)*C / m_v;

    double radius = deltaS/deltaTheta;
    double xO = x - radius*sin(theta);
    double yO = y + radius*cos(theta);

    theta += deltaTheta;

    x = xO + radius*sin(theta);
    y = yO - radius*cos(theta);

    while(theta > PI) theta -= 2*PI;
    while(theta <= -PI) theta += 2*PI;
}

void Odometry::GotoXYT(double x_goal, double y_goal, double theta_goal){
    double theta_ = atan2(y_goal-y, x_goal-x);
    double dist_ = sqrt(carre(x_goal-x)+carre(y_goal-y));
    GotoThet(theta_);
    GotoDist(dist_);
}

void Odometry::GotoThet(double theta_) {
    double distance_ticks_left;
    double distance_ticks_right;

    // Le calcul d'erreur est bon (testé), tu peux le vérifier par dessin
    double erreur_theta = theta_ - getTheta();
    bool arrived = false;

    while(erreur_theta >= PI) erreur_theta -= 2*PI;
    while(erreur_theta <= -PI) erreur_theta += 2*PI;

    if(erreur_theta <= 0) {
        distance_ticks_left = -(erreur_theta*m_v/2)/m_distPerTick_left;
        distance_ticks_right = (erreur_theta*m_v/2)/m_distPerTick_right;
    } else {
        distance_ticks_left = (erreur_theta*m_v/2)/m_distPerTick_left;
        distance_ticks_right = -(erreur_theta*m_v/2)/m_distPerTick_right;
    }
    pc.printf("TV %3.2f\tTh %3.2f\tET %3.2f\n\r",theta_*180/PI,getTheta()*180/PI,erreur_theta*180/PI);
    roboclaw.SpeedAccelDeccelPositionM1M2(ADR, 150000, 150000, 100000, (int32_t)distance_ticks_right, 150000, 150000, 100000, (int32_t)distance_ticks_left, 1);
    // Il faut ici faire un espèce de bouclage pour vérifier qu'il est bien arrivé, j'avais pensé à :
    /*
    while(!arrived){
        
    }
    */
}

void Odometry::GotoDist(double distance) {
    double temp1 = roboclaw.ReadEncM1(ADR), temp2 = roboclaw.ReadEncM2(ADR);
    double distance_ticks_left = distance/m_distPerTick_left - temp2;
    double distance_ticks_right =  distance/m_distPerTick_right - temp1;
    roboclaw.SpeedAccelDeccelPositionM1M2(ADR, 150000, 200000, 150000, (int32_t)distance_ticks_right, 150000, 200000, 150000, (int32_t)distance_ticks_left, 1);
}

bool Odometry::isArrivedRot(double theta_) {
    if(abs_d(getTheta()) <= abs_d(theta_+erreur_ang)) return true;
    else if(abs_d(getTheta()) >= abs_d(theta_-erreur_ang)) return true;
    else return false;
}