Tk A / Mbed 2 deprecated f3rc3

fdlsj

Dependencies:   mbed

Fork of f3rc2 by Tk A

locate.h@2:1a2984dfac3e, 2016-08-30 (annotated)

Committer:
sakanakuuun
Date:
Tue Aug 30 07:05:09 2016 +0000
Revision:
2:1a2984dfac3e
Parent:
0:df2659fd8031 
fe;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
choutin 0:df2659fd8031 1 #ifndef Locate_H
choutin 0:df2659fd8031 2 #define Locate_H
choutin 0:df2659fd8031 3
choutin 0:df2659fd8031 4 #include <math.h>
choutin 0:df2659fd8031 5 #include"mbed.h"
choutin 0:df2659fd8031 6
choutin 0:df2659fd8031 7 #define OUTERRING_D 140 //外輪間距離(mm)
choutin 0:df2659fd8031 8 #define INNERRING_D 136 //内輪間距離(mm)
choutin 0:df2659fd8031 9 #define PI 3.14159 //π
choutin 0:df2659fd8031 10 #define RESOLUSION 200 //P/R(分解能)
choutin 0:df2659fd8031 11 #define DIAMETER 31.8 //タイヤの直径(mm)
choutin 0:df2659fd8031 12 #define LOCATE_STEP (DIAMETER*PI / RESOLUSION) // エンコーダの１ステップあたりの距離(mm)
choutin 0:df2659fd8031 13 #define TIRE_DISTANCE ((OUTERRING_D + INNERRING_D) / 2) //タイヤ間距離(mm)
choutin 0:df2659fd8031 14 #define ROUND (PI * DIAMETER / (RESOLUSION * TIRE_DISTANCE)) //機体が１回転するために必要なステップ数の”逆数”
choutin 0:df2659fd8031 15
choutin 0:df2659fd8031 16 /*
choutin 0:df2659fd8031 17 エンコーダから、現在のステップ数（＝タイヤがどれだけ回ったか）を得られる
choutin 0:df2659fd8031 18 今回は分解能が200だから、(タイヤ1回転) = (200ステップ)
choutin 0:df2659fd8031 19 */
choutin 0:df2659fd8031 20
choutin 0:df2659fd8031 21 int pr = 0, pl = 0; //前回のステップ数
choutin 0:df2659fd8031 22 char dl = 0, dr = 0; //エンコーダの初期ズレ
choutin 0:df2659fd8031 23 short v = 0; //ステップ速度
choutin 0:df2659fd8031 24 float x = 0, y = 0; //xy方向に進んだ距離(m換算なし)
choutin 0:df2659fd8031 25 float theta = 0; //機体角度、x軸正の向きを0とする
choutin 0:df2659fd8031 26
choutin 0:df2659fd8031 27 void setup(int r, int l) //エンコーダの初期のズレ(dr,dl)を出す、最初に一回だけ行う
choutin 0:df2659fd8031 28 {
choutin 0:df2659fd8031 29 pr = dr = r;
choutin 0:df2659fd8031 30 pl = dl = l;
choutin 0:df2659fd8031 31 }
choutin 0:df2659fd8031 32
choutin 0:df2659fd8031 33 void update (int r, int l)
choutin 0:df2659fd8031 34 //位置情報を更新する。r,lはエンコーダから
choutin 0:df2659fd8031 35 {
choutin 0:df2659fd8031 36 r -= dr;
choutin 0:df2659fd8031 37 l -= dl;
choutin 0:df2659fd8031 38 theta = (r - l) * ROUND;
choutin 0:df2659fd8031 39 v = (r - pr + l - pl);
choutin 0:df2659fd8031 40
choutin 0:df2659fd8031 41 x += v * cos(theta);
choutin 0:df2659fd8031 42 y += v * sin(theta);
choutin 0:df2659fd8031 43
choutin 0:df2659fd8031 44 pr = r;
choutin 0:df2659fd8031 45 pl = l;
choutin 0:df2659fd8031 46 }
choutin 0:df2659fd8031 47
choutin 0:df2659fd8031 48 short coordinateX()
choutin 0:df2659fd8031 49 //xをmm換算して整数値として返す
choutin 0:df2659fd8031 50 {
choutin 0:df2659fd8031 51 return x * LOCATE_STEP / 2;
choutin 0:df2659fd8031 52 }
choutin 0:df2659fd8031 53
choutin 0:df2659fd8031 54 short coordinateX(int r, int l)
choutin 0:df2659fd8031 55 //上のupdate()も一緒にやってくれる版
choutin 0:df2659fd8031 56 {
choutin 0:df2659fd8031 57 update(r, l);
choutin 0:df2659fd8031 58 return x * LOCATE_STEP / 2;
choutin 0:df2659fd8031 59 }
choutin 0:df2659fd8031 60
choutin 0:df2659fd8031 61 short coordinateY()
choutin 0:df2659fd8031 62 //yをmm換算して整数値として返す
choutin 0:df2659fd8031 63 {
choutin 0:df2659fd8031 64 return y * LOCATE_STEP / 2;
choutin 0:df2659fd8031 65 }
choutin 0:df2659fd8031 66
choutin 0:df2659fd8031 67 short coordinateY(int r, int l)
choutin 0:df2659fd8031 68 //上のupdate()も一緒にやってくれる版
choutin 0:df2659fd8031 69 {
choutin 0:df2659fd8031 70 update(r, l);
choutin 0:df2659fd8031 71 return y * LOCATE_STEP / 2;
choutin 0:df2659fd8031 72 }
choutin 0:df2659fd8031 73
choutin 0:df2659fd8031 74 float coordinateTheta()
choutin 0:df2659fd8031 75 //thetaを返す
choutin 0:df2659fd8031 76 {
choutin 0:df2659fd8031 77 return theta;
choutin 0:df2659fd8031 78 }
choutin 0:df2659fd8031 79
choutin 0:df2659fd8031 80 #endif
choutin 0:df2659fd8031 81