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
theta???????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sakanakuuun 1:2bded5a5f636 1 #ifndef Locate_H
sakanakuuun 3:633ea1e7c681 2 #define Locate_H
sakanakuuun 3:633ea1e7c681 3
sakanakuuun 3:633ea1e7c681 4 #define OUTERRING_D 264 //外輪間距離(mm)
sakanakuuun 3:633ea1e7c681 5 #define INNERRING_D 224 //内輪間距離(mm)
sakanakuuun 3:633ea1e7c681 6 #define PI 3.14159 //π
sakanakuuun 3:633ea1e7c681 7 #define RESOLUSION 200 //P/R(分解能)
sakanakuuun 3:633ea1e7c681 8 #define DIAMETER 50.0 //タイヤの直径(mm)
sakanakuuun 3:633ea1e7c681 9 #define LOCATE_STEP (DIAMETER*PI / RESOLUSION) // エンコーダの1ステップあたりの距離(mm)
sakanakuuun 3:633ea1e7c681 10 #define TIRE_DISTANCE ((OUTERRING_D + INNERRING_D) / 2) //タイヤ間距離(mm)
sakanakuuun 5:f7adb05270e8 11 #define ROUND (DIAMETER / (RESOLUSION * TIRE_DISTANCE)) //機体が1回転するために必要なステップ数の”逆数”
sakanakuuun 3:633ea1e7c681 12
sakanakuuun 3:633ea1e7c681 13 /*
sakanakuuun 3:633ea1e7c681 14 エンコーダから、現在のステップ数(=タイヤがどれだけ回ったか)を得られる
sakanakuuun 3:633ea1e7c681 15 今回は分解能が200だから、(タイヤ1回転) = (200ステップ)
sakanakuuun 3:633ea1e7c681 16 */
sakanakuuun 3:633ea1e7c681 17
sakanakuuun 1:2bded5a5f636 18
sakanakuuun 3:633ea1e7c681 19 class Locate
sakanakuuun 3:633ea1e7c681 20 {
sakanakuuun 3:633ea1e7c681 21 //引数のr,lはエンコーダから受け取るステップ数で、rは右車輪、lは左車輪
sakanakuuun 7:d4fcddd78fce 22 //x, y
sakanakuuun 3:633ea1e7c681 23
sakanakuuun 3:633ea1e7c681 24 private:
sakanakuuun 3:633ea1e7c681 25 int pr, pl; //前回のステップ数
sakanakuuun 3:633ea1e7c681 26 char dl, dr; //エンコーダの初期ズレ
sakanakuuun 1:2bded5a5f636 27
sakanakuuun 1:2bded5a5f636 28 public:
sakanakuuun 9:b00a8fba58c6 29 float theta; //機体角度、x軸正の向きを0とする
sakanakuuun 7:d4fcddd78fce 30 float x, y; //xy方向に進んだ距離(m換算なし)
sakanakuuun 7:d4fcddd78fce 31 short v; //ステップ速度
sakanakuuun 7:d4fcddd78fce 32
sakanakuuun 8:a5093de00347 33 Locate (); //fx,fyは初期位置
sakanakuuun 1:2bded5a5f636 34
sakanakuuun 3:633ea1e7c681 35 void setup(int r, int l); //エンコーダの初期のズレ(dr,dl)を出す、最初に一回だけ行う
sakanakuuun 3:633ea1e7c681 36
sakanakuuun 3:633ea1e7c681 37 void update (int r, int l); //位置情報を更新する
sakanakuuun 3:633ea1e7c681 38
sakanakuuun 9:b00a8fba58c6 39 int getX(); //x, yをmm換算して整数値として返す
sakanakuuun 9:b00a8fba58c6 40 int getY();
sakanakuuun 9:b00a8fba58c6 41 float getTheta();
sakanakuuun 1:2bded5a5f636 42 };
sakanakuuun 1:2bded5a5f636 43
sakanakuuun 1:2bded5a5f636 44
sakanakuuun 7:d4fcddd78fce 45
sakanakuuun 7:d4fcddd78fce 46 /*使用例
sakanakuuun 7:d4fcddd78fce 47 #include "mbed.h"
sakanakuuun 7:d4fcddd78fce 48 #include "QEI.h"
sakanakuuun 7:d4fcddd78fce 49 #include "Locate.h"
sakanakuuun 7:d4fcddd78fce 50
sakanakuuun 7:d4fcddd78fce 51 DigitalOut rv(PA_6), lv(PA_14);
sakanakuuun 7:d4fcddd78fce 52 QEI right(PA_7, PA_5, NC, 100, QEI::X2_ENCODING);
sakanakuuun 7:d4fcddd78fce 53 QEI left(PA_13, PA_15, NC, 100, QEI::X2_ENCODING);
sakanakuuun 7:d4fcddd78fce 54
sakanakuuun 7:d4fcddd78fce 55 Locate machine(0,0);
sakanakuuun 7:d4fcddd78fce 56
sakanakuuun 7:d4fcddd78fce 57 int main()
sakanakuuun 7:d4fcddd78fce 58 {
sakanakuuun 7:d4fcddd78fce 59 rv = 1;lv = 1;
sakanakuuun 7:d4fcddd78fce 60
sakanakuuun 7:d4fcddd78fce 61 machine.setup(right.getPulses(), left.getPulses());
sakanakuuun 7:d4fcddd78fce 62 while(1)
sakanakuuun 7:d4fcddd78fce 63 {
sakanakuuun 7:d4fcddd78fce 64 machine.update(right.getPulses(), left.getPulses());
sakanakuuun 7:d4fcddd78fce 65
sakanakuuun 7:d4fcddd78fce 66 (mainで行う処理)
sakanakuuun 7:d4fcddd78fce 67 {
sakanakuuun 7:d4fcddd78fce 68 if(machine.getX() > 1000)
sakanakuuun 7:d4fcddd78fce 69 *********
sakanakuuun 9:b00a8fba58c6 70 if(machine.theta > 2)
sakanakuuun 7:d4fcddd78fce 71 **********
sakanakuuun 7:d4fcddd78fce 72 **************
sakanakuuun 7:d4fcddd78fce 73 }
sakanakuuun 7:d4fcddd78fce 74 }
sakanakuuun 7:d4fcddd78fce 75 }
sakanakuuun 7:d4fcddd78fce 76
sakanakuuun 7:d4fcddd78fce 77 */
sakanakuuun 1:2bded5a5f636 78 #endif