mirs_esp_pio
Toggle main menu visibility
読み取り中…
検索中…
一致する文字列を見つけられません
VelocityCalculator.cpp
[詳解]
1
5
6
#include "
VelocityCalculator.hpp
"
7
8
namespace
{
9
constexpr
double
kPi = 3.14159265358979323846;
10
}
11
12
VelocityCalculator::VelocityCalculator
(
double
counts_per_rev,
double
wheel_radius,
double
dt_sec)
13
: counts_per_rev_(counts_per_rev)
14
, wheel_radius_(wheel_radius)
15
, dt_sec_(dt_sec)
16
{
17
}
18
19
void
VelocityCalculator::setWheelRadius
(
double
radius) {
20
wheel_radius_ = radius;
21
}
22
23
void
VelocityCalculator::setDeltaTime
(
double
dt_sec) {
24
dt_sec_ = dt_sec;
25
}
26
27
int64_t
VelocityCalculator::wrapDelta
(int32_t current_count, int32_t last_count) {
28
// 符号付き減算のUBを避け、unsigned演算のラップで差分を求めて符号拡張する。
29
// 1周期の真の移動量が2^31未満ならラップ時も正しい差分になる
30
int64_t d =
static_cast<
int64_t
>
(
31
static_cast<
int32_t
>
(
static_cast<
uint32_t
>
(current_count) -
static_cast<
uint32_t
>
(last_count)));
32
// int32ラップ時はunsigned演算の折返しで正しい差分になる。
33
// 1制御周期の真の移動は数十カウント以下なので16bit窓にも畳み込む(最終防御)。
34
// 例: 32765→-32765は-65530ではなく+6と解釈する。
35
if
(d > 32767) {
36
d -= 65536;
37
}
else
if
(d < -32768) {
38
d += 65536;
39
}
40
return
d;
41
}
42
43
double
VelocityCalculator::calculate
(int64_t current_count, int64_t &prev_count) {
44
const
int64_t delta = current_count - prev_count;
45
prev_count = current_count;
46
47
// 速度 [m/s] = (カウント差 / カウント/回転) × 2π × 半径 / 時間
48
return
(
static_cast<
double
>
(delta) / counts_per_rev_) * 2.0 * kPi * wheel_radius_ / dt_sec_;
49
}
50
51
double
VelocityCalculator::calculate
(int32_t current_count, int32_t &prev_count) {
52
const
int64_t delta =
wrapDelta
(current_count, prev_count);
53
prev_count = current_count;
54
55
return
(
static_cast<
double
>
(delta) / counts_per_rev_) * 2.0 * kPi * wheel_radius_ / dt_sec_;
56
}
57
58
void
VelocityCalculator::calculateBothWheels
(
59
int64_t current_count_l, int64_t current_count_r,
60
int64_t &prev_count_l, int64_t &prev_count_r,
61
double
&vel_l,
double
&vel_r
62
) {
63
vel_l =
calculate
(current_count_l, prev_count_l);
64
vel_r =
calculate
(current_count_r, prev_count_r);
65
}
66
67
void
VelocityCalculator::calculateBothWheels
(
68
int32_t current_count_l, int32_t current_count_r,
69
int32_t &prev_count_l, int32_t &prev_count_r,
70
double
&vel_l,
double
&vel_r
71
) {
72
const
int64_t dl =
wrapDelta
(current_count_l, prev_count_l);
73
const
int64_t dr =
wrapDelta
(current_count_r, prev_count_r);
74
75
prev_count_l = current_count_l;
76
prev_count_r = current_count_r;
77
78
vel_l = (
static_cast<
double
>
(dl) / counts_per_rev_) * 2.0 * kPi * wheel_radius_ / dt_sec_;
79
vel_r = (
static_cast<
double
>
(dr) / counts_per_rev_) * 2.0 * kPi * wheel_radius_ / dt_sec_;
80
}
VelocityCalculator.hpp
車輪の移動距離と経過時間から速度を求める。
VelocityCalculator::VelocityCalculator
VelocityCalculator(double counts_per_rev, double wheel_radius, double dt_sec)
コンストラクタ
定義
VelocityCalculator.cpp:12
VelocityCalculator::setWheelRadius
void setWheelRadius(double radius)
車輪半径を設定
定義
VelocityCalculator.cpp:19
VelocityCalculator::calculate
double calculate(int32_t current_count, int32_t &prev_count)
現在のエンコーダーカウントから速度を計算
定義
VelocityCalculator.cpp:51
VelocityCalculator::setDeltaTime
void setDeltaTime(double dt_sec)
サンプリング周期を設定
定義
VelocityCalculator.cpp:23
VelocityCalculator::calculateBothWheels
void calculateBothWheels(int32_t current_count_l, int32_t current_count_r, int32_t &prev_count_l, int32_t &prev_count_r, double &vel_l, double &vel_r)
左右両輪の速度を計算
定義
VelocityCalculator.cpp:67
VelocityCalculator::wrapDelta
static int64_t wrapDelta(int32_t current_count, int32_t last_count)
int32境界のラップを吸収したカウント差分を返す
定義
VelocityCalculator.cpp:27
lib
VelocityCalc
VelocityCalculator.cpp
構築:
1.18.0