File content as of revision 7:d4fcddd78fce:

#ifndef Locate_H
#define Locate_H

#define OUTERRING_D     264             //外輪間距離(mm)
#define INNERRING_D     224             //内輪間距離(mm)
#define PI              3.14159         //π
#define RESOLUSION      200             //P/R(分解能)
#define DIAMETER        50.0            //タイヤの直径(mm)
#define LOCATE_STEP     (DIAMETER*PI / RESOLUSION)                  // エンコーダの１ステップあたりの距離(mm)
#define TIRE_DISTANCE   ((OUTERRING_D + INNERRING_D) / 2)           //タイヤ間距離(mm)
#define ROUND           (DIAMETER / (RESOLUSION * TIRE_DISTANCE))   //機体が１回転するために必要なステップ数の”逆数”

/*
エンコーダから、現在のステップ数（＝タイヤがどれだけ回ったか）を得られる
今回は分解能が200だから、(タイヤ1回転) = (200ステップ)
*/


class Locate
{
//引数のr,lはエンコーダから受け取るステップ数で、rは右車輪、lは左車輪
//x, y

private:
    int pr, pl;     //前回のステップ数
    char dl, dr;    //エンコーダの初期ズレ

public:
    float rad;      //機体角度、x軸正の向きを0とする
    float x, y;     //xy方向に進んだ距離(m換算なし)
    short v;        //ステップ速度

    Locate (int fx, int fy);        //fx,fyは初期位置
    
    void  setup(int r, int l);      //エンコーダの初期のズレ(dr,dl)を出す、最初に一回だけ行う
    
    void  update (int r, int l);    //位置情報を更新する
    
    short getX();                   //x, yをmm換算して整数値として返す
    short getY();
};



/*使用例
#include "mbed.h"
#include "QEI.h"
#include "Locate.h"

DigitalOut rv(PA_6), lv(PA_14);
QEI right(PA_7, PA_5, NC, 100, QEI::X2_ENCODING);
QEI left(PA_13, PA_15, NC, 100, QEI::X2_ENCODING);

Locate machine(0,0);

int main()
{
    rv = 1;lv = 1;
    
    machine.setup(right.getPulses(), left.getPulses());
    while(1) 
    {
        machine.update(right.getPulses(), left.getPulses());
        
        (mainで行う処理)
        {
            if(machine.getX() > 1000)
                *********
            if(machine.rad > 2)
                **********
            **************
        }
    }
}

*/
#endif