Tk A / MIYAZAKI

for f3rc

Fork of Encoder by Tk A

使い方

まず、"Locate.h"をインクルードする。 次に、Locate machine(x, y)で宣言する。（ｘとｙは初期位置。でも(0,0)推奨） 続いて位置情報を得る。　 位置情報を更新する関数は、machine.update()だが、位置情報を得る方法は2つある

(1)machine.update()を直接使う場合

一連の処理は、

machine.update((エンコーダの右), (エンコーダの左));

x = machine.getX();

y = machine.getY();

こんな感じ。

(2)machine.update()を直接使わない場合

一連の処理は、

x = machine.getX((エンコーダの右), (エンコーダの左));

y = machine.getY((エンコーダの右), (エンコーダの左));

こんな感じ。 あるいは、

x = machine.getX((エンコーダの右), (エンコーダの左));

y = machine.getY();

でも同じ結果を得られる。

まとめると、位置情報を得るときは、最初の関数にエンコーダで読み取った値を渡す必要がある。

ちなみにエンコーダの値を得るためには、"QEI.h"(https://developer.mbed.org/users/aberk/code/QEI/) を別途インクルードしたうえで、

QEI right (PA_7, PA_5, NC, lPR, QEI::X2_ENCODING);　　右側のエンコーダ

QEI left (PA_13, PA_15, NC, lPR, QEI::X2_ENCODING); 左側のエンコーダ

のように宣言する必要がある。 その後、例えば右側のエンコーダの値を得たいときには、right.getPulses()　を使う これを使うと、(1)は

machine.update(right.getPulses(), left.getPulses());

x = machine.getX();

y = machine.getX();

となる

Locate.cpp

Committer:
sakanakuuun
Date:
2016-08-16
Revision:
13:4dfe270cc4de
Parent:
12:c88621d7285f

File content as of revision 13:4dfe270cc4de:

#include <math.h>
#include "Locate.h"


Locate::Locate (int fx, int fy) 
{
    x = fx;
    y = fy;
    dr = 0; dl = 0;
    pr = 0; pl = 0;
}

void Locate::setup(int r, int l)
{
    pr = dr = r;    //(今(最初)のステップ数) = (初期ズレ) = (エンコーダから受け取るステップ数)
    pl = dl = l;    //上の説明は意味不明なので無視してください
}


void Locate::update (int r, int l) 
{
    r -= dr; l -= dl;   //ズレを修正
    
    theta = (r - l) * ROUND;   
    v = (r-pr + l-pl);     //r-pr = 前回から右車輪が進んだステップ数
    
    x += v * (cos(theta))   ;
    y += v * (sin(theta));
    
    pr = r;                 //今回のステップ数を保存
    pl = l;
}


short Locate::getX()
{
    return x * LOCATE_STEP / 2;   
}

short Locate::getX(int r, int l)
{
    update(r, l);
    return x * LOCATE_STEP / 2;   
}

short Locate::getY()
{
    return y * LOCATE_STEP / 2;   
}

short Locate::getY(int r, int l)
{
    update(r, l);
    return y * LOCATE_STEP / 2;   
}

float Locate::getTheta()
{
    return theta;
}