mirs_esp_pio
Toggle main menu visibility
読み取り中…
検索中…
一致する文字列を見つけられません
OdometryCalculator.cpp
[詳解]
1
#include "
OdometryCalculator.hpp
"
2
#include <math.h>
3
4
#ifndef M_PI
5
#define M_PI 3.14159265358979323846
6
#endif
7
8
namespace
{
9
// [-PI, PI] に正規化する
10
float
normalizeAngle(
float
angle) {
11
return
atan2f(sinf(angle), cosf(angle));
12
}
13
}
// namespace
14
15
OdometryCalculator::OdometryCalculator
()
16
: left_(nullptr), right_(nullptr)
17
, left_reversed_(false), right_reversed_(false)
18
{
19
}
20
21
OdometryCalculator::OdometryCalculator
(
Encoder
&left,
Encoder
&right,
22
bool
left_reversed,
bool
right_reversed)
23
: left_(&left), right_(&right)
24
, left_reversed_(left_reversed), right_reversed_(right_reversed)
25
{
26
}
27
28
void
OdometryCalculator::begin
() {
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
35
void
OdometryCalculator::setWheelBase
(
double
wheel_base) {
36
wheel_base_ = wheel_base;
37
}
38
39
void
OdometryCalculator::setWheelParams
(
double
wheel_radius,
double
wheel_base) {
40
wheel_radius_ = wheel_radius;
41
wheel_base_ = wheel_base;
42
}
43
44
void
OdometryCalculator::setReversed
(
bool
left_reversed,
bool
right_reversed) {
45
left_reversed_ = left_reversed;
46
right_reversed_ = right_reversed;
47
}
48
49
void
OdometryCalculator::reset
() {
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
63
void
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
82
void
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
88
void
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
}
M_PI
#define M_PI
定義
OdometryCalculator.cpp:5
OdometryCalculator.hpp
エンコーダの増分を左右車輪の移動距離に換算する。
Encoder
AMT103-V 直交エンコーダ(PCNT ハードウェア4逓倍計数)。
定義
Encoder.hpp:15
OdometryCalculator::v_linear
float v_linear
定義
OdometryCalculator.hpp:66
OdometryCalculator::setWheelBase
void setWheelBase(double wheel_base)
定義
OdometryCalculator.cpp:35
OdometryCalculator::setReversed
void setReversed(bool left_reversed, bool right_reversed)
定義
OdometryCalculator.cpp:44
OdometryCalculator::setWheelParams
void setWheelParams(double wheel_radius, double wheel_base)
定義
OdometryCalculator.cpp:39
OdometryCalculator::reset
void reset()
定義
OdometryCalculator.cpp:49
OdometryCalculator::sampleAndUpdate
void sampleAndUpdate(int32_t &count_l, int32_t &count_r, int64_t &delta_l, int64_t &delta_r, double dt_sec=0.015)
左右エンコーダからサンプリングし、差分・カウントを出力しつつオドメトリを更新する。
定義
OdometryCalculator.cpp:88
OdometryCalculator::y
float y
定義
OdometryCalculator.hpp:62
OdometryCalculator::update
void update(double dt_sec=0.015)
左右エンコーダからサンプリングし、移動距離・位置姿勢・速度を同時更新する。
定義
OdometryCalculator.cpp:82
OdometryCalculator::updateWithDistance
void updateWithDistance(double dist_left, double dist_right, double dt_sec)
外部から距離を与えてオドメトリを直接更新する場合(テスト・シミュレーション用)
定義
OdometryCalculator.cpp:63
OdometryCalculator::theta
float theta
定義
OdometryCalculator.hpp:63
OdometryCalculator::v_angular
float v_angular
定義
OdometryCalculator.hpp:67
OdometryCalculator::begin
void begin()
定義
OdometryCalculator.cpp:28
OdometryCalculator::OdometryCalculator
OdometryCalculator()
定義
OdometryCalculator.cpp:15
OdometryCalculator::x
float x
定義
OdometryCalculator.hpp:61
lib
OdometryCalc
OdometryCalculator.cpp
構築:
1.18.0