mirs_esp_pio
読み取り中…
検索中…
一致する文字列を見つけられません
VelocityCalculator.cpp
[詳解]
1
5
7
8namespace {
9constexpr double kPi = 3.14159265358979323846;
10}
11
12VelocityCalculator::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
20 wheel_radius_ = radius;
21}
22
24 dt_sec_ = dt_sec;
25}
26
27int64_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
43double 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
51double 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
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
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(double counts_per_rev, double wheel_radius, double dt_sec)
コンストラクタ
void setWheelRadius(double radius)
車輪半径を設定
double calculate(int32_t current_count, int32_t &prev_count)
現在のエンコーダーカウントから速度を計算
void setDeltaTime(double dt_sec)
サンプリング周期を設定
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)
左右両輪の速度を計算
static int64_t wrapDelta(int32_t current_count, int32_t last_count)
int32境界のラップを吸収したカウント差分を返す