Diff: Odometry.h

Revision:

0:62707e16531a

Child:

1:698586aa0c5a

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Odometry.h	Sat Sep 01 09:07:35 2018 +0000
@@ -0,0 +1,70 @@
+#ifndef ODOMETRY_H
+#define ODOMETRY_H
+
+#include "mbed.h"
+
+/*
+*   3輪のエンコーダを用いて自己位置推定します
+*   
+*/
+
+//中心からエンコーダ位置までの方向[rad] 基本的にDIRと同一
+#define ENC1POSI -PI/2
+#define ENC2POSI PI/6
+#define ENC3POSI PI*5/6
+
+//中心からエンコーダ位置までの距離[mm]
+#define ENC1DIS 180
+#define ENC2DIS 180
+#define ENC3DIS 180
+
+//エンコーダの回転軸の方向[rad]
+#define ENC1DIR -PI/2
+#define ENC2DIR PI/6
+#define ENC3DIR PI*5/6
+
+//車輪直径[mm]
+#define WHL1DIM 40
+#define WHL2DIM 40
+#define WHL3DIM 40
+
+//エンコーダ分解能
+#define ENCRES 800
+
+#define PI 3.14159265358979
+
+class Odometry{
+    
+public:
+    Odometry();
+    
+    void update(int enc1,int enc2,int enc3);
+    /*  位置情報を更新する関数
+    *   引数はエンコーダのカウント値 前回からの差分
+    */
+    
+    void reset();
+    //  自己位置を初期化する
+    
+    void set(double nX, double nY, double nTheta);
+    //  自己位置をセットする
+    
+    void correct(double dX, double dY, double dT);
+    //  自己位置を引数に取った増分だけ補正する
+    
+    double x(),y(),theta();
+    //  呼び出し用の関数
+    
+    double length();
+    //  
+    
+private:
+
+    double X, Y, Theta;
+    double Len;
+    // 自己位置
+    
+};
+
+
+#endif
\ No newline at end of file