mirs_esp_pio
読み取り中…
検索中…
一致する文字列を見つけられません
OdometryCalculator.hpp
[詳解]
1#pragma once
2#include <stdint.h>
3#include "Encoder.hpp"
4
12public:
15 bool left_reversed = false, bool right_reversed = false);
16
17 void begin();
18 void setWheelBase(double wheel_base);
19 void setWheelParams(double wheel_radius, double wheel_base);
20 void setReversed(bool left_reversed, bool right_reversed);
21 void reset();
22
27 void update(double dt_sec = 0.015);
28
32 void snapshot(int32_t &count_l, int32_t &count_r) const {
33 count_l = static_cast<int32_t>(left_ ? (left_reversed_ ? -left_->getCount() : left_->getCount()) : 0);
34 count_r = static_cast<int32_t>(right_ ? (right_reversed_ ? -right_->getCount() : right_->getCount()) : 0);
35 }
36
45 void sampleAndUpdate(int32_t &count_l, int32_t &count_r,
46 int64_t &delta_l, int64_t &delta_r,
47 double dt_sec = 0.015);
48
49 void sample(int32_t &count_l, int32_t &count_r,
50 int64_t &delta_l, int64_t &delta_r) {
51 sampleAndUpdate(count_l, count_r, delta_l, delta_r, 0.015);
52 }
53
55 void updateWithDistance(double dist_left, double dist_right, double dt_sec);
56 void update(double dist_left, double dist_right, double dt_sec) {
57 updateWithDistance(dist_left, dist_right, dt_sec);
58 }
59
60 // 積算位置・姿勢
61 float x = 0.0f;
62 float y = 0.0f;
63 float theta = 0.0f;
64
65 // 最新速度
66 float v_linear = 0.0f;
67 float v_angular = 0.0f;
68
69 // 直近ステップの左右移動距離 [m]
70 double distLeft() const { return dist_l_; }
71 double distRight() const { return dist_r_; }
72 double wheelBase() const { return wheel_base_; }
73 double wheelRadius() const { return wheel_radius_; }
74
75private:
76 Encoder *left_ = nullptr;
77 Encoder *right_ = nullptr;
78 bool left_reversed_ = false;
79 bool right_reversed_ = false;
80
81 int64_t last_l_ = 0;
82 int64_t last_r_ = 0;
83
84 double wheel_radius_ = 0.0391; // [m]
85 double wheel_base_ = 0.39; // [m]
86 double dist_l_ = 0.0;
87 double dist_r_ = 0.0;
88};
ESP32のパルス計数機能で左右のエンコーダ信号を数える。
AMT103-V 直交エンコーダ(PCNT ハードウェア4逓倍計数)。
void setWheelBase(double wheel_base)
void setReversed(bool left_reversed, bool right_reversed)
void snapshot(int32_t &count_l, int32_t &count_r) const
反転補正済み最新カウントスナップショットを取得する。
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 dist_left, double dist_right, double dt_sec)
void sample(int32_t &count_l, int32_t &count_r, int64_t &delta_l, int64_t &delta_r)
void update(double dt_sec=0.015)
左右エンコーダからサンプリングし、移動距離・位置姿勢・速度を同時更新する。
void updateWithDistance(double dist_left, double dist_right, double dt_sec)
外部から距離を与えてオドメトリを直接更新する場合(テスト・シミュレーション用)