File content as of revision 3:9c3f2662974e:

#ifndef ODOMETRY_H
#define ODOMETRY_H

#include "mbed.h"

/*
*   3輪のエンコーダを用いて自己位置推定します
*   
*/

//中心からエンコーダ位置までの方向[rad] 基本的にDIRと同一
#define ENC1POSI -PI/2
#define ENC2POSI PI/6
#define ENC3POSI PI*5/6

//中心からエンコーダ位置までの距離[mm]
#define ENC1DIS 180
#define ENC2DIS 180
#define ENC3DIS 180

//エンコーダの回転軸の方向[rad]
#define ENC1DIR -PI/2
#define ENC2DIR PI/6
#define ENC3DIR PI*5/6

//車輪直径[mm]
#define WHL1DIM 40
#define WHL2DIM 40
#define WHL3DIM 40

//エンコーダ分解能
#define ENCRES 800

#define PI 3.14159265358979

class Odometry{
    
public:
    Odometry();
    
    void update(int enc1,int enc2,int enc3);
    /*  位置情報を更新する関数
    *   引数はエンコーダのカウント値 前回からの差分
    */
    
    void reset();
    //  自己位置を初期化する
    
    void set(double nX, double nY, double nTheta);
    //  自己位置をセットする
    
    void correct(double dX, double dY, double dT);
    //  自己位置を引数に取った増分だけ補正する
    
    double x(),y(),theta();
    //  呼び出し用の関数
    
    double length();
    //  前回からの移動量を返す関数
    
private:

    double X, Y, Theta;
    double Len;
    // 自己位置
    
};


#endif