yuki honma / UnderBody

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

Dependents:   TOUTEKI_all_mbed mbed_test_program

Odometry.cpp@3:9c3f2662974e, 2018-09-04 (annotated)

Committer:
yuki17100
Date:
Tue Sep 04 14:22:13 2018 +0000
Revision:
3:9c3f2662974e
Parent:
0:62707e16531a 
Mecanum????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
yuki17100 0:62707e16531a 1 #include "Odometry.h"
yuki17100 0:62707e16531a 2
yuki17100 0:62707e16531a 3
yuki17100 0:62707e16531a 4 Odometry::Odometry(){
yuki17100 0:62707e16531a 5 reset();
yuki17100 0:62707e16531a 6 Len = 0;
yuki17100 0:62707e16531a 7 }
yuki17100 0:62707e16531a 8
yuki17100 0:62707e16531a 9 void Odometry::update(int enc1, int enc2, int enc3){
yuki17100 0:62707e16531a 10
yuki17100 0:62707e16531a 11 // 単位変換 [mm]
yuki17100 0:62707e16531a 12 double amount1 = (double)enc1/ENCRES*PI*WHL1DIM;
yuki17100 0:62707e16531a 13 double amount2 = (double)enc2/ENCRES*PI*WHL2DIM;
yuki17100 0:62707e16531a 14 double amount3 = (double)enc3/ENCRES*PI*WHL3DIM;
yuki17100 0:62707e16531a 15
yuki17100 0:62707e16531a 16 // ローカル座標
yuki17100 0:62707e16531a 17 double localT = (amount1/ENC1DIS/cos(ENC1DIR-ENC1POSI)
yuki17100 0:62707e16531a 18 + amount2/ENC2DIS/cos(ENC2DIR-ENC2POSI)
yuki17100 0:62707e16531a 19 + amount3/ENC3DIS/cos(ENC3DIR-ENC3POSI))*2/3;
yuki17100 0:62707e16531a 20 // 単位円周上での旋回量(AVG of amounts/radius[mm]) の中心角(/単位円周長(単位直径*PI)[mm]) =[rev] の弧度法(*2*PI) =[rad]
yuki17100 0:62707e16531a 21
yuki17100 0:62707e16531a 22 double localX = (amount1*cos(ENC1DIR) + amount2*cos(ENC2DIR) + amount3*cos(ENC3DIR));
yuki17100 0:62707e16531a 23 double localY = (amount1*sin(ENC1DIR) + amount2*sin(ENC2DIR) + amount3*sin(ENC3DIR));
yuki17100 0:62707e16531a 24
yuki17100 0:62707e16531a 25 // グローバル座標
yuki17100 0:62707e16531a 26 Theta += localT;
yuki17100 0:62707e16531a 27 double ang = Theta - localT/2;
yuki17100 0:62707e16531a 28
yuki17100 0:62707e16531a 29 //while(T > PI/2){ T -= PI/2; }
yuki17100 0:62707e16531a 30 //while(T < -PI/2){ T += PI/2; }
yuki17100 0:62707e16531a 31
yuki17100 0:62707e16531a 32 X += cos(ang)*localX + sin(ang)*localY;
yuki17100 0:62707e16531a 33 Y += sin(ang)*localX + cos(ang)*localY;
yuki17100 0:62707e16531a 34
yuki17100 0:62707e16531a 35 Len = hypot(localX,localY);
yuki17100 0:62707e16531a 36
yuki17100 0:62707e16531a 37 return;
yuki17100 0:62707e16531a 38 }
yuki17100 0:62707e16531a 39
yuki17100 0:62707e16531a 40 void Odometry::reset(){
yuki17100 0:62707e16531a 41 X = 0;
yuki17100 0:62707e16531a 42 Y = 0;
yuki17100 0:62707e16531a 43 Theta = 0;
yuki17100 0:62707e16531a 44 }
yuki17100 0:62707e16531a 45
yuki17100 0:62707e16531a 46 void Odometry::set(double nX, double nY, double nT){
yuki17100 0:62707e16531a 47 X = nX;
yuki17100 0:62707e16531a 48 Y = nY;
yuki17100 0:62707e16531a 49 Theta = nT;
yuki17100 0:62707e16531a 50 }
yuki17100 0:62707e16531a 51
yuki17100 0:62707e16531a 52 void Odometry::correct(double dX, double dY, double dT){
yuki17100 0:62707e16531a 53 X += dX;
yuki17100 0:62707e16531a 54 Y += dY;
yuki17100 0:62707e16531a 55 Theta += dT;
yuki17100 0:62707e16531a 56 }
yuki17100 0:62707e16531a 57
yuki17100 0:62707e16531a 58 double Odometry::x(){ return X; }
yuki17100 0:62707e16531a 59 double Odometry::y(){ return Y; }
yuki17100 0:62707e16531a 60 double Odometry::theta(){ return Theta; }
yuki17100 0:62707e16531a 61 double Odometry::length(){ return Len; }