for f3rc

Fork of Encoder by Tk A

使い方

まず、"Locate.h"をインクルードする。 次に、Locate machine(x, y)で宣言する。(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();

となる

Committer:
sakanakuuun
Date:
Thu Aug 11 06:41:07 2016 +0000
Revision:
9:b00a8fba58c6
Parent:
8:a5093de00347
Child:
10:f742cecbda10
theta???????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sakanakuuun 3:633ea1e7c681 1 #include <math.h>
sakanakuuun 1:2bded5a5f636 2 #include "Locate.h"
sakanakuuun 1:2bded5a5f636 3
sakanakuuun 1:2bded5a5f636 4
sakanakuuun 8:a5093de00347 5 Locate::Locate ()
sakanakuuun 3:633ea1e7c681 6 {
sakanakuuun 8:a5093de00347 7 x = 0;
sakanakuuun 8:a5093de00347 8 y = 0;
sakanakuuun 8:a5093de00347 9 //一応全て初期化しとく
sakanakuuun 7:d4fcddd78fce 10 dr = 0; dl = 0;
sakanakuuun 7:d4fcddd78fce 11 pr = 0; pl = 0;
sakanakuuun 9:b00a8fba58c6 12 theta = 0;
sakanakuuun 7:d4fcddd78fce 13 v = 0;
sakanakuuun 3:633ea1e7c681 14 }
sakanakuuun 3:633ea1e7c681 15
sakanakuuun 3:633ea1e7c681 16 void Locate::setup(int r, int l)
sakanakuuun 3:633ea1e7c681 17 {
sakanakuuun 3:633ea1e7c681 18 pr = dr = r; //(今(最初)のステップ数) = (初期ズレ) = (エンコーダから受け取るステップ数)
sakanakuuun 3:633ea1e7c681 19 pl = dl = l; //上の説明は意味不明なので無視してください
sakanakuuun 1:2bded5a5f636 20 }
sakanakuuun 1:2bded5a5f636 21
sakanakuuun 1:2bded5a5f636 22
sakanakuuun 3:633ea1e7c681 23 void Locate::update (int r, int l)
sakanakuuun 3:633ea1e7c681 24 {
sakanakuuun 3:633ea1e7c681 25 r -= dr; l -= dl; //ズレを修正
sakanakuuun 3:633ea1e7c681 26
sakanakuuun 9:b00a8fba58c6 27 theta = (r - l) * ROUND;
sakanakuuun 3:633ea1e7c681 28 v = (r-pr + l-pl) / 2; //r-pr = 前回から右車輪が進んだステップ数
sakanakuuun 3:633ea1e7c681 29
sakanakuuun 9:b00a8fba58c6 30 x += v * (cos(theta));
sakanakuuun 9:b00a8fba58c6 31 y += v * (sin(theta));
sakanakuuun 3:633ea1e7c681 32
sakanakuuun 3:633ea1e7c681 33 pr = r; //今回のステップ数を保存
sakanakuuun 3:633ea1e7c681 34 pl = l;
sakanakuuun 3:633ea1e7c681 35 }
sakanakuuun 3:633ea1e7c681 36
sakanakuuun 3:633ea1e7c681 37
sakanakuuun 9:b00a8fba58c6 38 int Locate::getX()
sakanakuuun 3:633ea1e7c681 39 {
sakanakuuun 3:633ea1e7c681 40 return x * LOCATE_STEP;
sakanakuuun 3:633ea1e7c681 41 }
sakanakuuun 3:633ea1e7c681 42
sakanakuuun 9:b00a8fba58c6 43 int Locate::getY()
sakanakuuun 3:633ea1e7c681 44 {
sakanakuuun 3:633ea1e7c681 45 return y * LOCATE_STEP;
sakanakuuun 9:b00a8fba58c6 46 }
sakanakuuun 9:b00a8fba58c6 47
sakanakuuun 9:b00a8fba58c6 48 float Locate::getTheta()
sakanakuuun 9:b00a8fba58c6 49 {
sakanakuuun 9:b00a8fba58c6 50 return theta;
sakanakuuun 1:2bded5a5f636 51 }