yuki honma / UnderBody

下半身制御用ライブラリ Odometry...自己位置推定 Mecanum...メカナムホイール用 Bezier...ベジエ曲線 RoboClaw...MD用

Dependents:   TOUTEKI_all_mbed mbed_test_program

Odometry.h@1:698586aa0c5a, 2018-09-02 (annotated)

Committer:
yuki17100
Date:
Sun Sep 02 10:47:46 2018 +0000
Revision:
1:698586aa0c5a
Parent:
0:62707e16531a 
?????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
yuki17100 0:62707e16531a 1 #ifndef ODOMETRY_H
yuki17100 0:62707e16531a 2 #define ODOMETRY_H
yuki17100 0:62707e16531a 3
yuki17100 0:62707e16531a 4 #include "mbed.h"
yuki17100 0:62707e16531a 5
yuki17100 0:62707e16531a 6 /*
yuki17100 0:62707e16531a 7 * 3輪のエンコーダを用いて自己位置推定します
yuki17100 0:62707e16531a 8 *
yuki17100 0:62707e16531a 9 */
yuki17100 0:62707e16531a 10
yuki17100 0:62707e16531a 11 //中心からエンコーダ位置までの方向[rad] 基本的にDIRと同一
yuki17100 0:62707e16531a 12 #define ENC1POSI -PI/2
yuki17100 0:62707e16531a 13 #define ENC2POSI PI/6
yuki17100 0:62707e16531a 14 #define ENC3POSI PI*5/6
yuki17100 0:62707e16531a 15
yuki17100 0:62707e16531a 16 //中心からエンコーダ位置までの距離[mm]
yuki17100 0:62707e16531a 17 #define ENC1DIS 180
yuki17100 0:62707e16531a 18 #define ENC2DIS 180
yuki17100 0:62707e16531a 19 #define ENC3DIS 180
yuki17100 0:62707e16531a 20
yuki17100 0:62707e16531a 21 //エンコーダの回転軸の方向[rad]
yuki17100 0:62707e16531a 22 #define ENC1DIR -PI/2
yuki17100 0:62707e16531a 23 #define ENC2DIR PI/6
yuki17100 0:62707e16531a 24 #define ENC3DIR PI*5/6
yuki17100 0:62707e16531a 25
yuki17100 0:62707e16531a 26 //車輪直径[mm]
yuki17100 0:62707e16531a 27 #define WHL1DIM 40
yuki17100 0:62707e16531a 28 #define WHL2DIM 40
yuki17100 0:62707e16531a 29 #define WHL3DIM 40
yuki17100 0:62707e16531a 30
yuki17100 0:62707e16531a 31 //エンコーダ分解能
yuki17100 0:62707e16531a 32 #define ENCRES 800
yuki17100 0:62707e16531a 33
yuki17100 0:62707e16531a 34 #define PI 3.14159265358979
yuki17100 0:62707e16531a 35
yuki17100 0:62707e16531a 36 class Odometry{
yuki17100 0:62707e16531a 37
yuki17100 0:62707e16531a 38 public:
yuki17100 0:62707e16531a 39 Odometry();
yuki17100 0:62707e16531a 40
yuki17100 0:62707e16531a 41 void update(int enc1,int enc2,int enc3);
yuki17100 0:62707e16531a 42 /* 位置情報を更新する関数
yuki17100 0:62707e16531a 43 * 引数はエンコーダのカウント値 前回からの差分
yuki17100 0:62707e16531a 44 */
yuki17100 0:62707e16531a 45
yuki17100 0:62707e16531a 46 void reset();
yuki17100 0:62707e16531a 47 // 自己位置を初期化する
yuki17100 0:62707e16531a 48
yuki17100 0:62707e16531a 49 void set(double nX, double nY, double nTheta);
yuki17100 0:62707e16531a 50 // 自己位置をセットする
yuki17100 0:62707e16531a 51
yuki17100 0:62707e16531a 52 void correct(double dX, double dY, double dT);
yuki17100 0:62707e16531a 53 // 自己位置を引数に取った増分だけ補正する
yuki17100 0:62707e16531a 54
yuki17100 0:62707e16531a 55 double x(),y(),theta();
yuki17100 0:62707e16531a 56 // 呼び出し用の関数
yuki17100 0:62707e16531a 57
yuki17100 0:62707e16531a 58 double length();
yuki17100 1:698586aa0c5a 59 // 前回からの移動量を返す関数
yuki17100 0:62707e16531a 60
yuki17100 0:62707e16531a 61 private:
yuki17100 0:62707e16531a 62
yuki17100 0:62707e16531a 63 double X, Y, Theta;
yuki17100 0:62707e16531a 64 double Len;
yuki17100 0:62707e16531a 65 // 自己位置
yuki17100 0:62707e16531a 66
yuki17100 0:62707e16531a 67 };
yuki17100 0:62707e16531a 68
yuki17100 0:62707e16531a 69
yuki17100 0:62707e16531a 70 #endif