mirs_esp_pio
読み取り中…
検索中…
一致する文字列を見つけられません
OdometryCalculator.cpp
[詳解]
2#include <math.h>
3
4#ifndef M_PI
5#define M_PI 3.14159265358979323846
6#endif
7
8namespace {
9// [-PI, PI] に正規化する
10float normalizeAngle(float angle) {
11 return atan2f(sinf(angle), cosf(angle));
12}
13} // namespace
14
16 : left_(nullptr), right_(nullptr)
17 , left_reversed_(false), right_reversed_(false)
18{
19}
20
22 bool left_reversed, bool right_reversed)
23 : left_(&left), right_(&right)
24 , left_reversed_(left_reversed), right_reversed_(right_reversed)
25{
26}
27
29 if (left_) left_->begin();
30 if (right_) right_->begin();
31 if (left_) last_l_ = left_->getCount();
32 if (right_) last_r_ = right_->getCount();
33}
34
35void OdometryCalculator::setWheelBase(double wheel_base) {
36 wheel_base_ = wheel_base;
37}
38
39void OdometryCalculator::setWheelParams(double wheel_radius, double wheel_base) {
40 wheel_radius_ = wheel_radius;
41 wheel_base_ = wheel_base;
42}
43
44void OdometryCalculator::setReversed(bool left_reversed, bool right_reversed) {
45 left_reversed_ = left_reversed;
46 right_reversed_ = right_reversed;
47}
48
50 if (left_) left_->reset();
51 if (right_) right_->reset();
52 last_l_ = 0;
53 last_r_ = 0;
54 x = 0.0f;
55 y = 0.0f;
56 theta = 0.0f;
57 v_linear = 0.0f;
58 v_angular = 0.0f;
59 dist_l_ = 0.0;
60 dist_r_ = 0.0;
61}
62
63void OdometryCalculator::updateWithDistance(double dist_left, double dist_right, double dt_sec) {
64 dist_l_ = dist_left;
65 dist_r_ = dist_right;
66
67 const double d = (dist_left + dist_right) / 2.0;
68 const double dtheta = (dist_right - dist_left) / wheel_base_;
69
70 // 中点法(Runge-Kutta 2次相当): ステップ中間の姿勢で並進を積分
71 const double mid = static_cast<double>(theta) + dtheta / 2.0;
72 theta = normalizeAngle(static_cast<float>(static_cast<double>(theta) + dtheta));
73 x += static_cast<float>(d * cos(mid));
74 y += static_cast<float>(d * sin(mid));
75
76 if (dt_sec > 0.0) {
77 v_linear = static_cast<float>(d / dt_sec);
78 v_angular = static_cast<float>(dtheta / dt_sec);
79 }
80}
81
82void OdometryCalculator::update(double dt_sec) {
83 int32_t cl = 0, cr = 0;
84 int64_t dl = 0, dr = 0;
85 sampleAndUpdate(cl, cr, dl, dr, dt_sec);
86}
87
88void OdometryCalculator::sampleAndUpdate(int32_t &count_l, int32_t &count_r,
89 int64_t &delta_l, int64_t &delta_r,
90 double dt_sec) {
91 if (!left_ || !right_) {
92 return;
93 }
94 const int64_t raw_l = left_->getCount();
95 const int64_t raw_r = right_->getCount();
96
97 int64_t dl = raw_l - last_l_;
98 int64_t dr = raw_r - last_r_;
99 last_l_ = raw_l;
100 last_r_ = raw_r;
101
102 // ハードウェア取付方向の反転補正
103 if (left_reversed_) {
104 dl = -dl;
105 }
106 if (right_reversed_) {
107 dr = -dr;
108 }
109
110 delta_l = dl;
111 delta_r = dr;
112 count_l = static_cast<int32_t>(left_reversed_ ? -raw_l : raw_l);
113 count_r = static_cast<int32_t>(right_reversed_ ? -raw_r : raw_r);
114
115 const double cpr_l = static_cast<double>(left_->countsPerRev());
116 const double cpr_r = static_cast<double>(right_->countsPerRev());
117
118 const double dist_left = (dl / cpr_l) * 2.0 * M_PI * wheel_radius_;
119 const double dist_right = (dr / cpr_r) * 2.0 * M_PI * wheel_radius_;
120
121 updateWithDistance(dist_left, dist_right, dt_sec);
122}
#define M_PI
エンコーダの増分を左右車輪の移動距離に換算する。
AMT103-V 直交エンコーダ(PCNT ハードウェア4逓倍計数)。
void setWheelBase(double wheel_base)
void setReversed(bool left_reversed, bool right_reversed)
void setWheelParams(double wheel_radius, double wheel_base)
void sampleAndUpdate(int32_t &count_l, int32_t &count_r, int64_t &delta_l, int64_t &delta_r, double dt_sec=0.015)
左右エンコーダからサンプリングし、差分・カウントを出力しつつオドメトリを更新する。
void update(double dt_sec=0.015)
左右エンコーダからサンプリングし、移動距離・位置姿勢・速度を同時更新する。
void updateWithDistance(double dist_left, double dist_right, double dt_sec)
外部から距離を与えてオドメトリを直接更新する場合(テスト・シミュレーション用)