Diff: locate.h

Revision:

0:df2659fd8031

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/locate.h	Sun Aug 28 17:03:34 2016 +0000
@@ -0,0 +1,81 @@
+#ifndef Locate_H
+#define Locate_H
+ 
+#include <math.h>
+#include"mbed.h"
+ 
+#define OUTERRING_D     140             //外輪間距離(mm)
+#define INNERRING_D     136             //内輪間距離(mm)
+#define PI              3.14159         //π
+#define RESOLUSION      200             //P/R(分解能)
+#define DIAMETER        31.8            //タイヤの直径(mm)
+#define LOCATE_STEP     (DIAMETER*PI / RESOLUSION)                  // エンコーダの１ステップあたりの距離(mm)
+#define TIRE_DISTANCE   ((OUTERRING_D + INNERRING_D) / 2)           //タイヤ間距離(mm)
+#define ROUND           (PI * DIAMETER / (RESOLUSION * TIRE_DISTANCE))   //機体が１回転するために必要なステップ数の”逆数”
+ 
+/*
+エンコーダから、現在のステップ数（＝タイヤがどれだけ回ったか）を得られる
+今回は分解能が200だから、(タイヤ1回転) = (200ステップ)
+*/
+ 
+int pr = 0, pl = 0;     //前回のステップ数
+char dl = 0, dr = 0;    //エンコーダの初期ズレ
+short v = 0;        //ステップ速度
+float x = 0, y = 0;     //xy方向に進んだ距離(m換算なし)
+float theta = 0;    //機体角度、x軸正の向きを0とする
+ 
+void  setup(int r, int l)      //エンコーダの初期のズレ(dr,dl)を出す、最初に一回だけ行う
+{
+    pr = dr = r;
+    pl = dl = l;
+}
+ 
+void  update (int r, int l)
+//位置情報を更新する。r,lはエンコーダから
+{
+    r -= dr;
+    l -= dl;
+    theta = (r - l) * ROUND;
+    v = (r - pr + l - pl);
+ 
+    x += v * cos(theta);
+    y += v * sin(theta);
+ 
+    pr = r;
+    pl = l;
+}
+ 
+short coordinateX()
+//xをmm換算して整数値として返す
+{
+    return x * LOCATE_STEP / 2;
+}
+ 
+short coordinateX(int r, int l)
+//上のupdate()も一緒にやってくれる版
+{
+    update(r, l);
+    return x * LOCATE_STEP / 2;
+}
+ 
+short coordinateY()
+//yをmm換算して整数値として返す
+{
+    return y * LOCATE_STEP / 2;
+}
+ 
+short coordinateY(int r, int l)
+//上のupdate()も一緒にやってくれる版
+{
+    update(r, l);
+    return y * LOCATE_STEP / 2;
+}
+ 
+float coordinateTheta()
+//thetaを返す
+{
+    return theta;
+}
+ 
+#endif
+            
\ No newline at end of file