4#include <freertos/FreeRTOS.h>
5#include <freertos/task.h>
33constexpr uint32_t WATCHDOG_TIMEOUT_MS = 1000;
35constexpr double ZERO_CMD_EPS = 1e-9;
37 "COUNTS_PER_REV must match the encoder configuration");
52DiffMotors motors(motor_l, motor_r,
false,
true);
71void fetchRosCommands() {
89double velocityToDuty(
double velocity) {
100 if (p == applied_params) {
127MotionSample estimateMotion(
double dt_sec) {
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(),
139WheelCmd computeAndDrive(
double dt_sec) {
140 const WheelCmd cmd{robot_ctrl.getLeftVelCmd(), robot_ctrl.getRightVelCmd()};
147 l_pwm = velocityToDuty(cmd.left);
148 r_pwm = velocityToDuty(cmd.right);
152 r_pwm = pid_right.compute(cmd.right, estimator.velR(), dt_sec);
153 l_pwm = pid_left.compute(cmd.left, estimator.velL(), dt_sec);
157 if (std::fabs(cmd.right) < ZERO_CMD_EPS) {
161 if (std::fabs(cmd.left) < ZERO_CMD_EPS) {
166 motors.setBoth(l_pwm, r_pwm);
172void publishTelemetry(
const MotionSample &s,
const WheelCmd &cmd) {
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);
192void control_loop(
double dt_sec) {
196 applyParamsIfChanged(p);
199 robot_ctrl.update(readRcControlInput(), millis());
201 const MotionSample s = estimateMotion(dt_sec);
207 const WheelCmd cmd = computeAndDrive(dt_sec);
208 publishTelemetry(s, cmd);
220 motors.begin(20000, 8);
228 TickType_t last_wake = xTaskGetTickCount();
229 TickType_t prev_tick = last_wake;
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;
239 control_loop(dt_sec);
左右モータの出力をまとめて設定し、配線に応じて回転方向の符号を合わせる。
ESP32のパルス計数機能で左右のエンコーダ信号を数える。
一つのモータドライバへPWMと回転方向を出力する。
目標値と計測値の差から、上限を考慮した制御出力を計算する。時間間隔は秒で受け取る。
車輪とIMUの計測から、車輪速度と機体の位置・向きをまとめて更新する。
プロポ受信機のパルス幅を計測し、中立をゼロとする操作量に変換する。
プロポとROS 2の操作元を切り替え、左右車輪の目標速度を求める。通信途絶時の指令ゼロも処理する。
制御・通信・センサのタスク間で、指令と計測値を排他制御して受け渡す。
差動用モータペアのインターフェース層。正逆極性の吸収が仕事。
AMT103-V 直交エンコーダ(PCNT ハードウェア4逓倍計数)。
static constexpr uint32_t MULTIPLIER
static constexpr uint32_t PULSES_PER_REV
単一MD10Cチャンネルのデバイス抽象。duty出力のみ。
static constexpr int DUTY_MAX
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入力)
bool popRosCmd(RosVelocityCmd &cmd)
void setMotion(const SharedMotion &m)
ロボット上位制御。RC/ROS2速度指令の統合・モード切替・ウォッチドッグ。
void requestOdometryReset()
void controlTask(void *arg)
static std::atomic< bool > odom_reset_req
指令の受付、操作元の切替、速度推定、モータ出力を周期的に実行する。
ESP32のピン、RC信号の範囲、速度上限と周期を定義する。
#define RC_SIGNAL_TIMEOUT_MS
#define MAX_ANGULAR_SPEED
#define TIMER_INTERVAL_MS