下半身制御用ライブラリ Odometry...自己位置推定 Mecanum...メカナムホイール用 Bezier...ベジエ曲線 RoboClaw...MD用
Dependents: TOUTEKI_all_mbed mbed_test_program
Odometry.h
- Committer:
- yuki17100
- Date:
- 2018-09-01
- Revision:
- 0:62707e16531a
- Child:
- 1:698586aa0c5a
File content as of revision 0:62707e16531a:
#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