下半身制御用ライブラリ 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?
User | Revision | Line number | New 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; } |