File content as of revision 3:9c3f2662974e:

#include "Odometry.h"


Odometry::Odometry(){
    reset();
    Len = 0;
}

void Odometry::update(int enc1, int enc2, int enc3){
    
    //  単位変換 [mm]
    double amount1 = (double)enc1/ENCRES*PI*WHL1DIM;
    double amount2 = (double)enc2/ENCRES*PI*WHL2DIM;
    double amount3 = (double)enc3/ENCRES*PI*WHL3DIM;
    
    //  ローカル座標
    double localT = (amount1/ENC1DIS/cos(ENC1DIR-ENC1POSI)
                  +  amount2/ENC2DIS/cos(ENC2DIR-ENC2POSI)
                  +  amount3/ENC3DIS/cos(ENC3DIR-ENC3POSI))*2/3;
    //      単位円周上での旋回量(AVG of amounts/radius[mm]) の中心角(/単位円周長(単位直径*PI)[mm]) =[rev] の弧度法(*2*PI) =[rad]
    
    double localX = (amount1*cos(ENC1DIR) + amount2*cos(ENC2DIR) + amount3*cos(ENC3DIR));
    double localY = (amount1*sin(ENC1DIR) + amount2*sin(ENC2DIR) + amount3*sin(ENC3DIR));
    
    //  グローバル座標
    Theta += localT;
    double ang = Theta - localT/2;
    
    //while(T >  PI/2){ T -= PI/2; }
    //while(T < -PI/2){ T += PI/2; }
    
    X += cos(ang)*localX + sin(ang)*localY;
    Y += sin(ang)*localX + cos(ang)*localY;
    
    Len = hypot(localX,localY);
    
    return;
}

void Odometry::reset(){
    X = 0;
    Y = 0;
    Theta = 0;
}

void Odometry::set(double nX, double nY, double nT){
    X = nX;
    Y = nY;
    Theta = nT;
}

void Odometry::correct(double dX, double dY, double dT){
    X += dX;
    Y += dY;
    Theta += dT;
}

double Odometry::x(){ return X; }
double Odometry::y(){ return Y; }
double Odometry::theta(){ return Theta; }
double Odometry::length(){ return Len; }