File content as of revision 2:1a2984dfac3e:

#ifndef Locate_H
#define Locate_H
 
#include <math.h>
#include"mbed.h"
 
#define OUTERRING_D     140             //外輪間距離(mm)
#define INNERRING_D     136             //内輪間距離(mm)
#define PI              3.14159         //π
#define RESOLUSION      200             //P/R(分解能)
#define DIAMETER        31.8            //タイヤの直径(mm)
#define LOCATE_STEP     (DIAMETER*PI / RESOLUSION)                  // エンコーダの１ステップあたりの距離(mm)
#define TIRE_DISTANCE   ((OUTERRING_D + INNERRING_D) / 2)           //タイヤ間距離(mm)
#define ROUND           (PI * DIAMETER / (RESOLUSION * TIRE_DISTANCE))   //機体が１回転するために必要なステップ数の”逆数”
 
/*
エンコーダから、現在のステップ数（＝タイヤがどれだけ回ったか）を得られる
今回は分解能が200だから、(タイヤ1回転) = (200ステップ)
*/
 
int pr = 0, pl = 0;     //前回のステップ数
char dl = 0, dr = 0;    //エンコーダの初期ズレ
short v = 0;        //ステップ速度
float x = 0, y = 0;     //xy方向に進んだ距離(m換算なし)
float theta = 0;    //機体角度、x軸正の向きを0とする
 
void  setup(int r, int l)      //エンコーダの初期のズレ(dr,dl)を出す、最初に一回だけ行う
{
    pr = dr = r;
    pl = dl = l;
}
 
void  update (int r, int l)
//位置情報を更新する。r,lはエンコーダから
{
    r -= dr;
    l -= dl;
    theta = (r - l) * ROUND;
    v = (r - pr + l - pl);
 
    x += v * cos(theta);
    y += v * sin(theta);
 
    pr = r;
    pl = l;
}
 
short coordinateX()
//xをmm換算して整数値として返す
{
    return x * LOCATE_STEP / 2;
}
 
short coordinateX(int r, int l)
//上のupdate()も一緒にやってくれる版
{
    update(r, l);
    return x * LOCATE_STEP / 2;
}
 
short coordinateY()
//yをmm換算して整数値として返す
{
    return y * LOCATE_STEP / 2;
}
 
short coordinateY(int r, int l)
//上のupdate()も一緒にやってくれる版
{
    update(r, l);
    return y * LOCATE_STEP / 2;
}
 
float coordinateTheta()
//thetaを返す
{
    return theta;
}
 
#endif