mirs_esp_pio
読み取り中…
検索中…
一致する文字列を見つけられません
control_task.cpp
[詳解]
1#include "control_task.hpp"
2#include <Arduino.h>
3#include <cmath>
4#include <freertos/FreeRTOS.h>
5#include <freertos/task.h>
6
7#include "hardware_config.hpp"
8#include "Encoder.hpp"
10#include "PoseEstimator.hpp"
11#include "RcReceiver.hpp"
12#include "PIDController.hpp"
14#include "MotorDriver.hpp"
15#include "DiffMotors.hpp"
16#include "SystemContext.hpp"
17#include <atomic>
18
19// rosタスクからのリセット要求フラグ。control周期内で消費する。
20static std::atomic<bool> odom_reset_req{false};
21
23 odom_reset_req.store(true);
24}
25
26// 配線と実行順序のみ持つ。計算・調停・出力・共有は各層に委譲する。
27// パイプライン: 指令受付→パラメータ適用→調停→状態推定→制御・出力→テレメトリ
28namespace {
31
32// 設定値の唯一の出所はSharedParams既定値。ここでの二重定義はしない
33constexpr uint32_t WATCHDOG_TIMEOUT_MS = 1000; // [ms] cmd_vel 無受信でWatchdog発火
34constexpr double DT_SEC = TIMER_INTERVAL_MS * 0.001; // [s] 制御周期
35constexpr double ZERO_CMD_EPS = 1e-9; // [m/s] 速度指令ゼロ判定しきい値
37 "COUNTS_PER_REV must match the encoder configuration");
38
39// PCNTユニットはbegin時にドライバから自動割当される
42OdometryCalculator odom_calc(enc_l, enc_r, false, true); // 左正方向・右反転
43
44RcReceiver rc_receiver;
47RobotController robot_ctrl(
49 MAX_LINEAR_SPEED, MAX_ANGULAR_SPEED, WATCHDOG_TIMEOUT_MS);
52DiffMotors motors(motor_l, motor_r, false, true); // 右反転はDIR論理と一致
53// 状態推定ファサード。odom/EKF/速度の判断は内包し、タスク側は投入と取出しのみ行う
56
57// 最後に適用したパラメータ(初期=既定値のため初回適用はskip)
58SharedParams applied_params;
59
60// ステージ間受渡し用の値型
61struct MotionSample {
62 int32_t count_l = 0;
63 int32_t count_r = 0;
64};
65struct WheelCmd {
66 double left = 0.0; // [m/s]
67 double right = 0.0;
68};
69
70// ステージ1:指令受付(mailboxから最新の指令だけ使う)
71void fetchRosCommands() {
73 if (g_sys.popRosCmd(cmd)) {
74 robot_ctrl.updateRos2Command(cmd.linear_x, cmd.angular_z, cmd.stamp_ms);
75 }
76}
77
78RcControlInput readRcControlInput() {
79 RcControlInput input;
86 return input;
87}
88
89double velocityToDuty(double velocity) {
90 if (!(MAX_LINEAR_SPEED > 0.0f)) {
91 return 0.0;
92 }
93 return (velocity / static_cast<double>(MAX_LINEAR_SPEED)) * MotorDriver::DUTY_MAX;
94}
95
96// ステージ2:パラメータ適用(変更時のみ書込む)。
97// 適用先の一覧はここに集約する。新規consumer追加時は本関数への登録を忘れないこと。
98// 現行:estimator(半径/base)/robot_ctrl(diff逆運動学)/odom_calc(距離)/pid×2のgain
99void applyParamsIfChanged(const SharedParams &p) {
100 if (p == applied_params) {
101 return;
102 }
103 estimator.applyParams(p.wheel_radius, p.wheel_base);
104 robot_ctrl.setWheelBase(p.wheel_base);
105 odom_calc.setWheelParams(p.wheel_radius, p.wheel_base);
106 pid_right.setGains(p.rkp, p.rki, p.rkd);
107 pid_left.setGains(p.lkp, p.lki, p.lkd);
108 applied_params = p;
109}
110
111// SharedSensor→推定器入力の変換(配線)。FW非依存の値型に詰め替えるだけ。
112EstimatorImu toEstimatorImu(const SharedSensor &s) {
113 EstimatorImu imu;
114 imu.imu_ok = s.imu_ok;
115 imu.mag_ok = s.mag_ok;
116 imu.ax = s.ax;
117 imu.ay = s.ay;
118 imu.az = s.az;
119 imu.gz = s.gz;
120 imu.mx = s.mx;
121 imu.my = s.my;
122 imu.mz = s.mz;
123 return imu;
124}
125
126// ステージ3:状態推定。読取→投入→結果取出しのみ。判断はPoseEstimatorに委譲する。
127MotionSample estimateMotion(double dt_sec) {
128 // 単一読取でスナップショット・差分・移動距離を同時更新する(二重読み防止)
129 MotionSample s;
130 int64_t dl = 0, dr = 0;
131 odom_calc.sampleAndUpdate(s.count_l, s.count_r, dl, dr, dt_sec);
132 estimator.update(dl, dr, odom_calc.distLeft(), odom_calc.distRight(),
133 toEstimatorImu(g_sys.getSensor()), dt_sec);
134 return s;
135}
136
137// ステージ4:制御・出力(MANUALは開ループ直結、ROS2はPID閉ループ)。
138// 手動速度→duty換算はこの制御アダプター、極性吸収はDiffMotorsに委譲する。
139WheelCmd computeAndDrive(double dt_sec) {
140 const WheelCmd cmd{robot_ctrl.getLeftVelCmd(), robot_ctrl.getRightVelCmd()};
141 const bool manual = (robot_ctrl.getControlMode() == RobotController::MODE_MANUAL);
142
143 double l_pwm = 0.0;
144 double r_pwm = 0.0;
145 if (manual) {
146 // 開ループ直結。PID状態は使わないため積分器を保全する
147 l_pwm = velocityToDuty(cmd.left);
148 r_pwm = velocityToDuty(cmd.right);
149 pid_left.reset();
150 pid_right.reset();
151 } else {
152 r_pwm = pid_right.compute(cmd.right, estimator.velR(), dt_sec);
153 l_pwm = pid_left.compute(cmd.left, estimator.velL(), dt_sec);
154 }
155
156 // 速度指令ゼロ時はPWMと積分項をリセット
157 if (std::fabs(cmd.right) < ZERO_CMD_EPS) {
158 r_pwm = 0.0;
159 pid_right.reset();
160 }
161 if (std::fabs(cmd.left) < ZERO_CMD_EPS) {
162 l_pwm = 0.0;
163 pid_left.reset();
164 }
165
166 motors.setBoth(l_pwm, r_pwm);
167 return cmd;
168}
169
170// ステージ5:テレメトリ書込(ros taskが発行する)。
171// 推定結果の取出しと共有構造体への詰替えのみ。判断は持たない。
172void publishTelemetry(const MotionSample &s, const WheelCmd &cmd) {
173 SharedMotion m;
174 m.count_l = s.count_l;
175 m.count_r = s.count_r;
176 m.vel_l = estimator.velL();
177 m.vel_r = estimator.velR();
178 m.vel_cmd_l = static_cast<float>(cmd.left);
179 m.vel_cmd_r = static_cast<float>(cmd.right);
180 m.rc_pulse[0] = rc_receiver.getPulseWidth(CH_LEFT);
181 m.rc_pulse[1] = rc_receiver.getPulseWidth(CH_MODE_SW);
182 m.rc_pulse[2] = rc_receiver.getPulseWidth(CH_RIGHT);
183 m.ctrl_mode = (robot_ctrl.getControlMode() == RobotController::MODE_ROS2) ? 1 : 0;
184 m.odom_x = estimator.x();
185 m.odom_y = estimator.y();
186 m.odom_theta = estimator.theta();
187 m.lin_vel = estimator.v();
188 m.ang_vel = estimator.w();
189 g_sys.setMotion(m);
190}
191
192void control_loop(double dt_sec) {
193 fetchRosCommands();
194
195 const SharedParams p = g_sys.getParams();
196 applyParamsIfChanged(p);
197
198 // RC入力と制御モード更新(調停はRobotControllerに委譲)
199 robot_ctrl.update(readRcControlInput(), millis());
200
201 const MotionSample s = estimateMotion(dt_sec);
202 // 原点リセット要求があれば積算後にゼロ化する(参照継続のため速度に段差なし)。
203 // SLAM開始前の停止状態で使うこと。走行中の呼出しは軌跡を切断する。
204 if (odom_reset_req.exchange(false)) {
205 estimator.reset();
206 }
207 const WheelCmd cmd = computeAndDrive(dt_sec);
208 publishTelemetry(s, cmd);
209}
210
211} // namespace
212
213void controlTask(void *arg) {
214 (void)arg;
215
216 // エンコーダー・オドメトリ初期化 (PCNT 4逓倍計数開始)
217 odom_calc.begin();
218
219 // モーター制御初期化
220 motors.begin(20000, 8); // 20kHz, 8bit分解能
221
222 // RC レシーバー初期化
223 const uint8_t rc_pins[RC_NUM_CHANNELS] = {RC_LEFT_PIN, RC_MODE_SW_PIN, RC_RIGHT_PIN};
224 rc_receiver.begin(rc_pins, RC_NUM_CHANNELS);
225
226 // 非常停止は回路実装前のため未配線(TODO.mdで管理)
227
228 TickType_t last_wake = xTaskGetTickCount();
229 TickType_t prev_tick = last_wake;
230 for (;;) {
231 // 実測周期で速度・オドメ・PIDを駆動する(ジッタを無視しない)
232 const TickType_t now_tick = xTaskGetTickCount();
233 double dt_sec = static_cast<double>(now_tick - prev_tick) /
234 static_cast<double>(configTICK_RATE_HZ);
235 prev_tick = now_tick;
236 if (dt_sec <= 0.0) {
237 dt_sec = DT_SEC;
238 }
239 control_loop(dt_sec);
240 vTaskDelayUntil(&last_wake, pdMS_TO_TICKS(TIMER_INTERVAL_MS));
241 }
242}
左右モータの出力をまとめて設定し、配線に応じて回転方向の符号を合わせる。
ESP32のパルス計数機能で左右のエンコーダ信号を数える。
一つのモータドライバへPWMと回転方向を出力する。
エンコーダの増分を左右車輪の移動距離に換算する。
目標値と計測値の差から、上限を考慮した制御出力を計算する。時間間隔は秒で受け取る。
車輪とIMUの計測から、車輪速度と機体の位置・向きをまとめて更新する。
プロポ受信機のパルス幅を計測し、中立をゼロとする操作量に変換する。
プロポとROS 2の操作元を切り替え、左右車輪の目標速度を求める。通信途絶時の指令ゼロも処理する。
SystemContext g_sys
制御・通信・センサのタスク間で、指令と計測値を排他制御して受け渡す。
差動用モータペアのインターフェース層。正逆極性の吸収が仕事。
AMT103-V 直交エンコーダ(PCNT ハードウェア4逓倍計数)。
static constexpr uint32_t MULTIPLIER
static constexpr uint32_t PULSES_PER_REV
単一MD10Cチャンネルのデバイス抽象。duty出力のみ。
static constexpr int DUTY_MAX
差動二輪オドメトリ計算器。
汎用PID制御 (正規形。ゲインは周期不変)。
RCサーボPWM受信機リーダ (MR-8等、890〜2100us、中立1496us前提)。
void begin(const uint8_t ch_pins[], uint8_t num_channels)
uint16_t getPulseWidth(uint8_t channel)
static float pulseToNormalized(uint16_t pulse_us)
bool isSignalValid(uint8_t channel, uint32_t timeout_ms)
ロボット上位制御。RC/ROS2速度指令の統合・モード切替・ウォッチドッグ。
@ MODE_MANUAL
手動制御モード(RC入力)
@ MODE_ROS2
ROS2制御モード
SharedSensor getSensor()
bool popRosCmd(RosVelocityCmd &cmd)
void setMotion(const SharedMotion &m)
SharedParams getParams()
ロボット上位制御。RC/ROS2速度指令の統合・モード切替・ウォッチドッグ。
void requestOdometryReset()
void controlTask(void *arg)
static std::atomic< bool > odom_reset_req
指令の受付、操作元の切替、速度推定、モータ出力を周期的に実行する。
ESP32のピン、RC信号の範囲、速度上限と周期を定義する。
#define PIN_DIR_R
#define PIN_ENC_B_R
#define CH_MODE_SW
#define ENABLE_EKF
#define COUNTS_PER_REV
#define CH_RIGHT
#define RC_NUM_CHANNELS
#define PIN_ENC_A_L
#define MAX_LINEAR_SPEED
#define RC_SIGNAL_TIMEOUT_MS
#define CH_LEFT
#define PIN_PWM_R
#define PIN_PWM_L
#define RC_RIGHT_PIN
#define RC_MODE_SW_PIN
#define MAX_ANGULAR_SPEED
#define TIMER_INTERVAL_MS
#define PIN_ENC_B_L
#define PIN_ENC_A_R
#define RC_LEFT_PIN
#define PIN_DIR_L
推定器へのIMU入力(値型・FW非依存)。
正規化済みRC入力と各チャンネルの有効状態。
ROS速度指令メールボックス要素。
運動系テレメトリ。
車体・PID共有パラメータ。
センサ系テレメトリ。
正規化済みRC入力と各チャンネルの有効状態。