Tk A
for f3rc
使い方
まず、"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();
となる
Diff: Locate.cpp
- Revision:
- 3:633ea1e7c681
- Parent:
- 2:f20408555e83
- Child:
- 6:835fbc52feb0
diff -r f20408555e83 -r 633ea1e7c681 Locate.cpp
--- a/Locate.cpp Sat Jul 02 02:36:39 2016 +0000
+++ b/Locate.cpp Thu Aug 11 04:07:42 2016 +0000
@@ -1,18 +1,41 @@
-#include <mathe.h>
+#include <math.h>
#include "Locate.h"
-Locate::Locate () {
- theta = 0;
- x = 0;
- y = 0;
+Locate::Locate (int fx, int fy)
+{
+ x = fx;
+ y = fy;
+}
+
+void Locate::setup(int r, int l)
+{
+ pr = dr = r; //(今(最初)のステップ数) = (初期ズレ) = (エンコーダから受け取るステップ数)
+ pl = dl = l; //上の説明は意味不明なので無視してください
}
-void Locate::update (short int r, short int l) {
- theta = (r - l) * ROUND;
- v = (r + l) / 2;
- x += v * sin(theta);
- y += v * cos(theta);
- //r = 0; l = 0; //受け取らずにループしても変化しないようにするため
+void Locate::update (int r, int l)
+{
+ r -= dr; l -= dl; //ズレを修正
+
+ theta = (r - l) * ROUND;
+ v = (r-pr + l-pl) / 2; //r-pr = 前回から右車輪が進んだステップ数
+
+ x += v * (sin(theta));
+ y += v * (cos(theta));
+
+ pr = r; //今回のステップ数を保存
+ pl = l;
+}
+
+
+short Locate::getX()
+{
+ return x * LOCATE_STEP;
+}
+
+short Locate::getY()
+{
+ return y * LOCATE_STEP;
}
\ No newline at end of file