下半身制御用ライブラリ Odometry...自己位置推定 Mecanum...メカナムホイール用 Bezier...ベジエ曲線 RoboClaw...MD用
Dependents: TOUTEKI_all_mbed mbed_test_program
Odometry.cpp
- Committer:
- yuki17100
- Date:
- 2018-09-04
- Revision:
- 3:9c3f2662974e
- Parent:
- 0:62707e16531a
File content as of revision 3:9c3f2662974e:
#include "Odometry.h" Odometry::Odometry(){ reset(); Len = 0; } void Odometry::update(int enc1, int enc2, int enc3){ // 単位変換 [mm] double amount1 = (double)enc1/ENCRES*PI*WHL1DIM; double amount2 = (double)enc2/ENCRES*PI*WHL2DIM; double amount3 = (double)enc3/ENCRES*PI*WHL3DIM; // ローカル座標 double localT = (amount1/ENC1DIS/cos(ENC1DIR-ENC1POSI) + amount2/ENC2DIS/cos(ENC2DIR-ENC2POSI) + amount3/ENC3DIS/cos(ENC3DIR-ENC3POSI))*2/3; // 単位円周上での旋回量(AVG of amounts/radius[mm]) の中心角(/単位円周長(単位直径*PI)[mm]) =[rev] の弧度法(*2*PI) =[rad] double localX = (amount1*cos(ENC1DIR) + amount2*cos(ENC2DIR) + amount3*cos(ENC3DIR)); double localY = (amount1*sin(ENC1DIR) + amount2*sin(ENC2DIR) + amount3*sin(ENC3DIR)); // グローバル座標 Theta += localT; double ang = Theta - localT/2; //while(T > PI/2){ T -= PI/2; } //while(T < -PI/2){ T += PI/2; } X += cos(ang)*localX + sin(ang)*localY; Y += sin(ang)*localX + cos(ang)*localY; Len = hypot(localX,localY); return; } void Odometry::reset(){ X = 0; Y = 0; Theta = 0; } void Odometry::set(double nX, double nY, double nT){ X = nX; Y = nY; Theta = nT; } void Odometry::correct(double dX, double dY, double dT){ X += dX; Y += dY; Theta += dT; } double Odometry::x(){ return X; } double Odometry::y(){ return Y; } double Odometry::theta(){ return Theta; } double Odometry::length(){ return Len; }