From 089b6fdae92dfdd76004a20201780680b8e0b269 Mon Sep 17 00:00:00 2001 From: Kevin79111 <2894694006@qq.com> Date: Wed, 18 Nov 2026 22:33:03 +0800 Subject: [PATCH] =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E9=A6=96=E5=8F=91=E5=92=8C?= =?UTF-8?q?=E5=B8=B8=E8=A7=84=E5=BB=B6=E8=BF=9F=E6=A3=80=E6=B5=8B=EF=BC=8C?= =?UTF-8?q?=E4=BF=AE=E6=94=B9=E6=9D=BE=E5=BC=80=E6=89=B3=E6=9C=BA=E5=90=8E?= =?UTF-8?q?safe=E6=A8=A1=E5=BC=8F=E7=A0=B4=E5=9D=8F=E5=B7=B2=E6=A0=87?= =?UTF-8?q?=E5=AE=9A=E5=BC=B9=E4=BD=8D=E7=AD=89=E9=97=AE=E9=A2=98=E3=80=82?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- InfantryLauncher.hpp | 163 ++++++++++++++++++---- README.md | 318 ++++++++++++++++++++++--------------------- 2 files changed, 299 insertions(+), 182 deletions(-) diff --git a/InfantryLauncher.hpp b/InfantryLauncher.hpp index 868039b..d43cd26 100644 --- a/InfantryLauncher.hpp +++ b/InfantryLauncher.hpp @@ -20,6 +20,7 @@ module_description: 步兵发射机构模块:控制两个摩擦轮和一个拨 #include #include #include +#include #include "CMD.hpp" #include "Motor.hpp" @@ -146,6 +147,12 @@ class InfantryLauncher ///< trigger speed reference limit }; + struct HeatParam + { + float offline_heat_limit = 240.0f; // 首次有效裁判参数前的热量上限。 + float offline_cooling_rate = 40.0f; // 首次有效裁判参数前的冷却速率。 + }; + /** * @brief 热量控制状态。 * Heat control state. @@ -188,6 +195,7 @@ class InfantryLauncher const char* launcher_ref_topic_name; ///< 订阅的裁判发射数据 Topic 名称 ///< Name of the subscribed referee ///< launcher data Topic + HeatParam heat_param{}; }; /** @@ -248,7 +256,9 @@ class InfantryLauncher .num_trig_tooth = 10}, .thread_priority = LibXR::Thread::Priority::HIGH, .launcher_cmd_topic_name = "launcher_cmd", - .launcher_ref_topic_name = "launcher_ref"}) + .launcher_ref_topic_name = "launcher_ref", + .heat_param = {.offline_heat_limit = 240.0f, + .offline_cooling_rate = 40.0f}}) : motor_fric_0_(&motor_fric_0), motor_fric_1_(&motor_fric_1), motor_trig_(&motor_trig), @@ -257,8 +267,13 @@ class InfantryLauncher pid_fric_0_(param.pid_param_fric_speed_0), pid_fric_1_(param.pid_param_fric_speed_1), param_(param.launcher_param), + heat_param_(param.heat_param), referee_(referee) { + ASSERT(std::isfinite(heat_param_.offline_heat_limit) && + heat_param_.offline_heat_limit > 0.0f); + ASSERT(std::isfinite(heat_param_.offline_cooling_rate) && + heat_param_.offline_cooling_rate >= 0.0f); launcher_cmd_topic_name_ = param.launcher_cmd_topic_name; launcher_ref_topic_name_ = param.launcher_ref_topic_name; thread_.Create(this, ThreadFunc, "LauncherThread", param.task_stack_depth, param.thread_priority); @@ -329,33 +344,42 @@ class InfantryLauncher static void ThreadFunc(InfantryLauncher* self) { LibXR::Topic::ASyncSubscriber cmd_sub(self->launcher_cmd_topic_name_); - LibXR::Topic::ASyncSubscriber launcher_ref( - self->launcher_ref_topic_name_); + std::optional> launcher_ref; cmd_sub.StartWaiting(); - launcher_ref.StartWaiting(); self->last_online_time_ = LibXR::Timebase::GetMicroseconds(); while (true) { + // 裁判 Topic 出现后再订阅,离线启动时不等待。 + if (!launcher_ref) + { + auto topic = LibXR::Topic::Find(self->launcher_ref_topic_name_); + if (topic != nullptr) + { + launcher_ref.emplace(LibXR::Topic(topic)); + launcher_ref->StartWaiting(); + } + } + auto now = LibXR::Timebase::GetMicroseconds(); self->dt_ = (now - self->last_online_time_).ToSecondf(); self->last_online_time_ = now; + self->mutex_.Lock(); if (cmd_sub.Available()) { self->launcher_cmd_ = cmd_sub.GetData(); cmd_sub.StartWaiting(); } - if (launcher_ref.Available()) + if (launcher_ref && launcher_ref->Available()) { - const auto ref_pack = launcher_ref.GetData(); - self->ref_data_.heat_limit = ref_pack.rs.shooter_heat_limit; - self->ref_data_.cooling_rate = ref_pack.rs.shooter_cooling_value; - self->ref_data_.current_heat_17 = ref_pack.ph.launcher_id1_17_heat; + const auto ref_pack = launcher_ref->GetData(); + self->UpdateRefereeHeat(ref_pack.rs.shooter_heat_limit, + ref_pack.rs.shooter_cooling_value, + ref_pack.ph.launcher_id1_17_heat); self->ref_data_.bullet_speed = ref_pack.ld.bullet_speed; self->robot_level_ = ref_pack.rs.robot_level; - launcher_ref.StartWaiting(); + launcher_ref->StartWaiting(); } - self->mutex_.Lock(); self->Update(); self->RunStateMachine(); self->mutex_.Unlock(); @@ -505,6 +529,13 @@ class InfantryLauncher calibration_pending_ = false; target_shot_index_ = 0; is_reverse_ = false; + first_shot_start_time_ = 0; + first_shot_delay_ms_ = 0; + delay_time_ = 0; + start_fire_time_ = 0; + finish_fire_time_ = 0; + real_launch_delay_ = 0; + mark_launch_ = true; } break; } @@ -535,6 +566,13 @@ class InfantryLauncher press_continue_ = false; calibrated_ = false; calibration_pending_ = false; + first_shot_start_time_ = 0; + first_shot_delay_ms_ = 0; + delay_time_ = 0; + start_fire_time_ = 0; + finish_fire_time_ = 0; + real_launch_delay_ = 0; + mark_launch_ = true; trigger_step_active_ = false; target_shot_index_ = 0; is_reverse_ = false; @@ -604,6 +642,7 @@ class InfantryLauncher LibXR::PID pid_fric_1_; LauncherParam param_; + HeatParam heat_param_; Referee* referee_ = nullptr; LibXR::Event launcher_event; const char* launcher_cmd_topic_name_ = nullptr; @@ -624,6 +663,9 @@ class InfantryLauncher float target_trig_angle_ = 0.0f; float last_motor_angle_ = 0.0f; float first_shot_angle_ = 0.0f; + uint32_t first_shot_delay_ms_ = 0; // 首发拨弹到掉速校准成功的耗时(毫秒)。 + uint32_t delay_time_ = 0; // 首发校准等待的控制循环次数。 + uint32_t real_launch_delay_ = 0; // 常规拨弹到掉速检测出弹的耗时(毫秒)。 float fric_speed_peak_ = 0.0f; float jam_target_angle_ = 0.0f; int32_t target_shot_index_ = 0; @@ -637,6 +679,7 @@ class InfantryLauncher bool trigger_step_active_ = false; bool calibrated_ = false; bool calibration_pending_ = false; + bool mark_launch_ = true; // 常规延迟观测已完成,或尚未开始。 bool ui_layer_cleared_ = false; bool ui_fric_text_initialized_ = false; bool ui_fire_mode_text_initialized_ = false; @@ -646,6 +689,9 @@ class InfantryLauncher float shot_progress_ = 0.0f; LibXR::MillisecondTimestamp fire_press_time_ = 0; + LibXR::MillisecondTimestamp first_shot_start_time_ = 0; + LibXR::MillisecondTimestamp start_fire_time_ = 0; + LibXR::MillisecondTimestamp finish_fire_time_ = 0; LibXR::MillisecondTimestamp last_trig_time_ = 0; LibXR::MillisecondTimestamp last_jam_time_ = 0; LibXR::MillisecondTimestamp last_heat_time_ = 0; @@ -667,18 +713,45 @@ class InfantryLauncher }; LibXR::Mutex mutex_; - void UpdateLauncherState() + void UpdateRefereeHeat(float heat_limit, float cooling_rate, float current_heat) { - if (param_trig_.torque > launcher::param::JAM_TORQUE) + // 无效数据不覆盖最后有效裁判参数。 + if (!std::isfinite(heat_limit) || heat_limit <= 0.0f || + !std::isfinite(cooling_rate) || cooling_rate < 0.0f || + !std::isfinite(current_heat) || current_heat < 0.0f) { - launcher_state_ = LauncherState::JAMMED; return; } + ref_data_.heat_limit = heat_limit; + ref_data_.cooling_rate = cooling_rate; + ref_data_.current_heat_17 = current_heat; + } + + float EffectiveHeatLimit() const + { + return ref_data_.heat_limit > 0.0f ? ref_data_.heat_limit + : heat_param_.offline_heat_limit; + } + + float EffectiveCoolingRate() const + { + return ref_data_.heat_limit > 0.0f ? ref_data_.cooling_rate + : heat_param_.offline_cooling_rate; + } + + void UpdateLauncherState() + { + // 非 READY 时不进入脱卡流程。 if (launcher_event_ != LauncherEvent::SET_FRICMODE_READY) { launcher_state_ = LauncherState::RELAX; return; } + if (param_trig_.torque > launcher::param::JAM_TORQUE) + { + launcher_state_ = LauncherState::JAMMED; + return; + } if (!heat_limit_.allow_fire) { @@ -686,7 +759,8 @@ class InfantryLauncher return; } - launcher_state_ = launcher_cmd_.isfire ? LauncherState::NORMAL : LauncherState::STOP; + // 松开扳机不取消当前拨弹目标。 + launcher_state_ = LauncherState::NORMAL; } void RunStateMachine() @@ -716,7 +790,13 @@ class InfantryLauncher break; case LauncherState::NORMAL: - if (continue_mode_) + if (!launcher_cmd_.isfire) + { + // 松手后完成当前拨弹。 + press_continue_ = false; + trig_mode_ = TrigMode::SINGLE; + } + else if (continue_mode_) { press_continue_ = true; trig_mode_ = TrigMode::CONTINUE; @@ -753,6 +833,13 @@ class InfantryLauncher const bool fric_ready = fabsf(param_fric_0_.velocity) >= ready_rpm && fabsf(param_fric_1_.velocity) >= ready_rpm; + // 超过 500 ms 未检测到出弹,按新版重新计时。 + if (!mark_launch_ && start_fire_time_ > 0 && + (now - start_fire_time_).ToMillisecond() > 500) + { + start_fire_time_ = now; + } + auto indexed_target = [&]() { return first_shot_angle_ + step * static_cast(target_shot_index_); }; @@ -771,16 +858,32 @@ class InfantryLauncher last_trig_time_ = now; }; + if (calibration_pending_ && !calibrated_) + { + ++delay_time_; + } + if (trigger_step_active_) { fric_speed_peak_ = std::max(fric_speed_peak_, fric_speed); - if (calibration_pending_ && !calibrated_ && fric_speed_peak_ >= ready_rpm && - fric_speed_peak_ - fric_speed >= launcher::param::FRIC_DROP_RPM) + // 首发校准和常规延迟观测使用相同的掉速判定。 + const bool SHOT_DETECTED = + fric_speed_peak_ >= ready_rpm && + fric_speed_peak_ - fric_speed >= launcher::param::FRIC_DROP_RPM; + if (!mark_launch_ && SHOT_DETECTED) + { + finish_fire_time_ = now; + mark_launch_ = true; + real_launch_delay_ = (finish_fire_time_ - start_fire_time_).ToMillisecond(); + } + + if (calibration_pending_ && !calibrated_ && SHOT_DETECTED) { calibrated_ = true; calibration_pending_ = false; first_shot_angle_ = trig_angle_; + first_shot_delay_ms_ = (now - first_shot_start_time_).ToMillisecond(); target_trig_angle_ = indexed_target(); heat_limit_.current_heat = std::max(heat_limit_.current_heat, ref_data_.current_heat_17) + @@ -814,8 +917,7 @@ class InfantryLauncher const float current_heat = std::max(heat_limit_.current_heat, ref_data_.current_heat_17); const float shot_heat = heat_limit_.single_heat * static_cast(shot_count_); - if (ref_data_.heat_limit <= 0.0f || - current_heat + shot_heat + heat_limit_.merge > ref_data_.heat_limit) + if (current_heat + shot_heat + heat_limit_.merge > EffectiveHeatLimit()) { return; } @@ -824,10 +926,14 @@ class InfantryLauncher { target_shot_index_ += static_cast(shot_count_); target_trig_angle_ = indexed_target(); + start_fire_time_ = now; + mark_launch_ = false; } else { calibration_pending_ = true; + delay_time_ = 0; + first_shot_start_time_ = now; target_shot_index_ = static_cast(shot_count_) - 1; fric_speed_peak_ = fric_speed; target_trig_angle_ = trig_angle_ + step * static_cast(shot_count_); @@ -852,8 +958,9 @@ class InfantryLauncher { recover_from_jam(); } - else if (last_trig_mode_ == TrigMode::SAFE || last_trig_mode_ == TrigMode::RELAX) + else if (launcher_cmd_.isfire && !last_fire_notify_) { + // 按下扳机时发起一次拨弹。 start_shot(); } break; @@ -867,7 +974,8 @@ class InfantryLauncher { recover_from_jam(); } - else if (!trigger_step_active_ && since_last >= interval_s) + else if (launcher_cmd_.isfire && !trigger_step_active_ && + since_last >= interval_s) { start_shot(); } @@ -951,10 +1059,11 @@ class InfantryLauncher } last_heat_time_ = now; + const float MAX_HEAT = EffectiveHeatLimit(); + const float cooling_rate = EffectiveCoolingRate(); float current_heat = std::max(heat_limit_.current_heat, ref_data_.current_heat_17); - float residuary_heat = ref_data_.heat_limit - current_heat - heat_limit_.merge; - heat_limit_.allow_fire = - ref_data_.heat_limit > 0.0f && residuary_heat >= heat_limit_.single_heat; + float residuary_heat = MAX_HEAT - current_heat - heat_limit_.merge; + heat_limit_.allow_fire = residuary_heat >= heat_limit_.single_heat; if (!heat_limit_.allow_fire) { @@ -964,7 +1073,7 @@ class InfantryLauncher if (residuary_heat <= heat_limit_.single_heat * heat_limit_.heat_threshold) { - float safe_freq = ref_data_.cooling_rate / heat_limit_.single_heat; + float safe_freq = cooling_rate / heat_limit_.single_heat; float ratio = residuary_heat / (heat_limit_.single_heat * heat_limit_.heat_threshold); trig_freq_ = ratio * (expect_trig_freq_ - safe_freq) + safe_freq; @@ -991,7 +1100,7 @@ class InfantryLauncher if (delta_time > 0.0f) { - heat_limit_.current_heat -= ref_data_.cooling_rate * delta_time; + heat_limit_.current_heat -= EffectiveCoolingRate() * delta_time; } if (heat_limit_.current_heat <= 0.0f) { diff --git a/README.md b/README.md index 051a1d4..2031a42 100644 --- a/README.md +++ b/README.md @@ -1,191 +1,199 @@ # InfantryLauncher -步兵发射机构模块:控制两个摩擦轮和一个拨弹盘,支持单发、三连发和连发,按裁判系统热量限制射频 / Infantry launcher Module controlling two friction wheels and a trigger disc, with single, three-round and continuous fire and a fire rate limited by the referee system heat +基于 LibXR / XRobot 1.0 的 RoboMaster 步兵发射模块,控制两个摩擦轮和一个拨弹盘, +支持单发、三连发、持续连发、首发弹位校准、卡弹恢复和裁判系统 UI。 +主要实现位于 [InfantryLauncher.hpp](InfantryLauncher.hpp)。 -## 1. 模块作用 / Purpose +当前版本使用拨弹角度环与速度环串级 PID、两路摩擦轮速度 PID, +通过 `Motor::ControlMode::MODE_CURRENT` 输出电流指令。 +构造函数已移除 `Sliding*` 参数,模块依赖中也不再包含 `local/Sliding`。 -构造时,InfantryLauncher 创建线程 `LauncherThread`(栈深 `param.task_stack_depth`,优先级 `param.thread_priority`)。线程每轮依次刷新电机反馈、执行热量计算与状态机、设定拨弹目标、计算 PID 输出并下发,然后休眠 2 ms。电机以 `MODE_CURRENT` 下发;电机 `state == 0` 时先 `Enable()`,`state` 不为 0 与 1 时 `ClearError()`。 +## 依赖与接口 -事件:`GetEvent()` 返回的 `LibXR::Event` 注册了 `InfantryLauncher::LauncherEvent` 的各个值。 +模块 ID 为 `QDU-Robomaster/InfantryLauncher`,使用 C++20。 +`MODULE MANIFEST V2` 声明以下依赖,均采用 `same-or-dev` 引用: -- 摩擦轮模式 `SET_FRICMODE_RELAX`、`SET_FRICMODE_SAFE`、`SET_FRICMODE_READY`:`RELAX` 时三个电机 `Relax()`;`SAFE` 时摩擦轮目标转速为 0,输出缩小为 1/50;`READY` 时两个摩擦轮以同一目标转速运行。离开 `READY` 时清除拨弹盘标定。 -- 射击模式 `SET_SHOTMODE_SINGLE`(每次 1 发)、`SET_SHOTMODE_BOOST_3`(每次 3 发)、`SET_SHOTMODE_CONTINUE`(按住连发)。 -- CMD 的 `CMD_EVENT_START_CTRL` 切换到 `SET_FRICMODE_RELAX`;`CMD_EVENT_LOST_CTRL` 复位全部状态,失能拨弹电机并放松摩擦轮。 +- `QDU-Robomaster/CMD`:发射命令和控制连接事件。 +- `QDU-Robomaster/RMMotor`:摩擦轮及拨弹电机。 +- `QDU-Robomaster/Motor`:电机反馈和控制接口。 +- `QDU-Robomaster/Referee`:裁判数据与 UI。 -摩擦轮转速:目标转速初值为 `fric1_setpoint_speed`(rpm)。`READY` 下每当裁判系统回传的弹速变化时,弹速高于 `target_bullet_speed - bullet_speed_tolerance` 则目标降低 70 rpm,低于 `target_bullet_speed - 2.2 × bullet_speed_tolerance` 则升高 50 rpm;回传弹速不在 0 至 30 m/s 时按 `target_bullet_speed - 2 × bullet_speed_tolerance` 处理。两个摩擦轮的转速都不低于目标减 200 rpm 时才允许发射。 - -拨弹:`launcher_cmd` 的 `isfire` 在 `READY` 且热量允许时进入发射状态。单发与三连发模式下,上升沿发射 `shot_count` 发,按住超过 0.5 s 转为连发;连发模式下按住即按当前射频连续发射。拨弹盘每发转过 2π/10。进入 `READY` 后的第一次发射,以摩擦轮转速相对峰值下降不小于 150 rpm 判定出弹,并据此标定拨弹盘的发射位置,之后按该位置分度。拨弹由角度环与速度环串联,速度参考限幅为 1.5 × 2π × 射频 / `num_trig_tooth`。 - -卡弹:拨弹电机扭矩大于 0.028 N·m 时进入卡弹处理,每 0.1 s 在“后退 0.3 格”与原目标之间往复,扭矩恢复且仍在发射时重新对齐到下一格并继续。 - -热量:单发热量按 10 计,热量估计取本地累计(按拨弹盘转过的格数计发射数,按冷却值衰减)与裁判系统 17 mm 枪管热量的较大值。裁判系统热量上限为 0(尚未收到 `launcher_ref`)或剩余热量小于 10 时禁止发射;剩余热量不超过 60 时射频从 15 Hz 线性降到冷却值 / 10,否则射频为 15 Hz;某次发射会使热量超过上限时,该次发射不执行。 - -裁判系统 UI:`referee` 非空时,定时任务每 80 ms 在图层 1 上轮流绘制落点圆圈、`FRIC ON` / `FRIC OFF` 文字和射击模式文字(`SING`、`BOOST_3`、`CONT`)。 - -Upon construction, InfantryLauncher creates the thread `LauncherThread` (stack depth `param.task_stack_depth`, priority `param.thread_priority`). Each iteration refreshes the motor feedback, runs the heat calculation and the state machine, sets the trigger target, computes the PID outputs and sends them, and then sleeps for 2 ms. The motors are sent in `MODE_CURRENT`; a motor with `state == 0` is `Enable()`d first, and `ClearError()` is called when `state` is neither 0 nor 1. - -Events: `GetEvent()` returns a `LibXR::Event` on which every value of `InfantryLauncher::LauncherEvent` is registered. - -- Friction wheel modes `SET_FRICMODE_RELAX`, `SET_FRICMODE_SAFE` and `SET_FRICMODE_READY`: in `RELAX` the three motors are `Relax()`ed; in `SAFE` the friction wheel target speed is 0 and the output is scaled down to 1/50; in `READY` both friction wheels run at the same target speed. Leaving `READY` clears the trigger disc calibration. -- Shot modes `SET_SHOTMODE_SINGLE` (1 round each time), `SET_SHOTMODE_BOOST_3` (3 rounds each time) and `SET_SHOTMODE_CONTINUE` (continuous fire while held). -- The CMD event `CMD_EVENT_START_CTRL` switches to `SET_FRICMODE_RELAX`; `CMD_EVENT_LOST_CTRL` resets all states, disables the trigger motor and relaxes the friction wheels. - -Friction wheel speed: the initial target speed is `fric1_setpoint_speed` (rpm). In `READY`, whenever the bullet speed reported by the referee system changes, a speed above `target_bullet_speed - bullet_speed_tolerance` lowers the target by 70 rpm and a speed below `target_bullet_speed - 2.2 × bullet_speed_tolerance` raises it by 50 rpm; a reported bullet speed outside 0 to 30 m/s is treated as `target_bullet_speed - 2 × bullet_speed_tolerance`. Firing is allowed only when the speeds of both friction wheels are at least the target minus 200 rpm. - -Trigger: with `READY` and heat available, `isfire` of `launcher_cmd` enters the firing state. In the single and three-round modes, a rising edge fires `shot_count` rounds and holding for more than 0.5 s switches to continuous fire; in the continuous mode, holding fires continuously at the current fire rate. The trigger disc advances 2π/10 per round. The first shot after entering `READY` is detected as a round leaving when the friction wheel speed falls at least 150 rpm from its peak, which calibrates the firing position of the trigger disc, and later rounds are indexed from that position. The trigger runs an angle loop in series with a speed loop, and the speed reference is limited to 1.5 × 2π × fire rate / `num_trig_tooth`. - -Jam: a trigger motor torque above 0.028 N·m enters jam handling, which alternates every 0.1 s between "back off 0.3 tooth" and the original target; once the torque recovers and firing continues, the disc realigns to the next tooth and goes on. - -Heat: one round counts as 10 heat, and the heat estimate is the larger of the local accumulation (rounds counted from the teeth the trigger disc advances, decayed by the cooling value) and the referee system 17 mm barrel heat. Firing is disabled when the referee system heat limit is 0 (no `launcher_ref` received yet) or the remaining heat is below 10; when the remaining heat is at most 60, the fire rate decreases linearly from 15 Hz to the cooling value / 10, otherwise the fire rate is 15 Hz; a shot that would push the heat over the limit is not fired. - -Referee system UI: when `referee` is not null, a timer task draws in turn, every 80 ms on layer 1, the impact point circle, the `FRIC ON` / `FRIC OFF` text and the shot mode text (`SING`, `BOOST_3`, `CONT`). - -## 2. 构造接口 / Constructor +构造接口如下,`param` 的默认初始化值省略,详见头文件: ```cpp -InfantryLauncher(RMMotor& motor_fric_0, - RMMotor& motor_fric_1, - RMMotor& motor_trig, - CMD& cmd, - Referee* referee, - const Param& param = {...}); // 节选 / excerpt +InfantryLauncher(RMMotor& motor_fric_0, RMMotor& motor_fric_1, + RMMotor& motor_trig, CMD& cmd, Referee* referee, + const Param& param); ``` -依赖: +`CMD::LauncherCMD::isfire` 表示是否按住扳机。命令和裁判数据分别通过 +`launcher_cmd_topic_name`、`launcher_ref_topic_name` 指定的 Topic 订阅。 +`referee` 指针用于绘制 UI;传入 `nullptr` 时不创建 UI 定时任务, +裁判数据订阅仍由 Topic 名称决定。 -- `motor_fric_0`、`motor_fric_1`:`RMMotor`,两个摩擦轮电机。 -- `motor_trig`:`RMMotor`,拨弹电机。 -- `cmd`:`CMD` 实例。 -- `referee`:`Referee*`,用于 UI 绘制,为 `nullptr` 时不创建 UI 定时任务;热量与弹速数据来自 `launcher_ref_topic_name` 指定的 Topic。 +电机、CMD 以及需要引用的 Referee 实例应先于发射器构造。 +`GetEvent()` 提供事件绑定入口,控制循环由模块自身创建的线程执行。 -配置参数(`Param`;PID 为 `LibXR::PID::Param`,字段为 `k, p, i, d, i_limit, out_limit, cycle`): +## XRobot 配置 -- `task_stack_depth`:线程栈深,默认 4096。 -- `pid_param_trig_angle`:拨弹角度环,默认 `{.k = 1.0f, .p = 4000.0f, .i = 0.0f, .d = 0.0f, .i_limit = 0.0f, .out_limit = 4000.0f, .cycle = false}`。 -- `pid_param_trig_speed`:拨弹速度环,默认 `{.k = 1.0f, .p = 0.0012f, .i = 0.0005f, .d = 0.0f, .i_limit = 1.0f, .out_limit = 1.0f, .cycle = false}`。 -- `pid_param_fric_speed_0`、`pid_param_fric_speed_1`:摩擦轮速度环,默认均为 `{.k = 1.0f, .p = 0.002f, .i = 0.0f, .d = 0.0f, .i_limit = 0.0f, .out_limit = 1.0f, .cycle = false}`。 -- `launcher_param.fric1_setpoint_speed`:摩擦轮目标转速初值,单位 rpm,默认 6500。 -- `launcher_param.target_bullet_speed`:目标弹速,单位 m/s,默认 25。 -- `launcher_param.bullet_speed_tolerance`:弹速容差,单位 m/s,默认 1.5。 -- `launcher_param.trig_gear_ratio`:拨弹电机减速比,默认 36。 -- `launcher_param.num_trig_tooth`:拨弹盘齿数,用于拨弹速度参考限幅,默认 10。 -- `thread_priority`:线程优先级,默认 `LibXR::Thread::Priority::HIGH`。 -- `launcher_cmd_topic_name`:订阅的发射控制命令 Topic 名称,默认 `"launcher_cmd"`,与 CMD 的 `launcher_cmd_topic_name` 一致。 -- `launcher_ref_topic_name`:订阅的裁判系统发射数据 Topic 名称,默认 `"launcher_ref"`,与 Referee 的 `referee_launcher_tp_name` 一致。 - -Dependencies: - -- `motor_fric_0`, `motor_fric_1`: `RMMotor` objects for the two friction wheel motors. -- `motor_trig`: the `RMMotor` of the trigger motor. -- `cmd`: the `CMD` instance. -- `referee`: `Referee*`, used for UI drawing; the UI timer task is created only when it is not `nullptr`. The heat and bullet speed data come from the Topic named by `launcher_ref_topic_name`. - -Configuration parameters (`Param`; the PIDs are `LibXR::PID::Param` with fields `k, p, i, d, i_limit, out_limit, cycle`): - -- `task_stack_depth`: thread stack depth, default 4096. -- `pid_param_trig_angle`: trigger angle loop, default `{.k = 1.0f, .p = 4000.0f, .i = 0.0f, .d = 0.0f, .i_limit = 0.0f, .out_limit = 4000.0f, .cycle = false}`. -- `pid_param_trig_speed`: trigger speed loop, default `{.k = 1.0f, .p = 0.0012f, .i = 0.0005f, .d = 0.0f, .i_limit = 1.0f, .out_limit = 1.0f, .cycle = false}`. -- `pid_param_fric_speed_0`, `pid_param_fric_speed_1`: friction wheel speed loops, both default to `{.k = 1.0f, .p = 0.002f, .i = 0.0f, .d = 0.0f, .i_limit = 0.0f, .out_limit = 1.0f, .cycle = false}`. -- `launcher_param.fric1_setpoint_speed`: initial friction wheel target speed in rpm, default 6500. -- `launcher_param.target_bullet_speed`: target bullet speed in m/s, default 25. -- `launcher_param.bullet_speed_tolerance`: bullet speed tolerance in m/s, default 1.5. -- `launcher_param.trig_gear_ratio`: trigger motor reduction ratio, default 36. -- `launcher_param.num_trig_tooth`: number of trigger disc teeth, used for the trigger speed reference limit, default 10. -- `thread_priority`: thread priority, default `LibXR::Thread::Priority::HIGH`. -- `launcher_cmd_topic_name`: name of the subscribed launcher command Topic, default `"launcher_cmd"`, matching the `launcher_cmd_topic_name` of CMD. -- `launcher_ref_topic_name`: name of the subscribed referee launcher data Topic, default `"launcher_ref"`, matching the `referee_launcher_tp_name` of Referee. - -## 3. Topic - -| Topic | 方向 | 类型 | 说明 | -| --- | --- | --- | --- | -| `param.launcher_cmd_topic_name`(默认 `launcher_cmd`) | 订阅 | `CMD::LauncherCMD` | 发射命令 `isfire` | -| `param.launcher_ref_topic_name`(默认 `launcher_ref`) | 订阅 | `Referee::LauncherPack` | 热量上限、冷却值、17 mm 热量、弹速 | - -| Topic | Direction | Type | Meaning | -| --- | --- | --- | --- | -| `param.launcher_cmd_topic_name` (default `launcher_cmd`) | Subscribe | `CMD::LauncherCMD` | Fire command `isfire` | -| `param.launcher_ref_topic_name` (default `launcher_ref`) | Subscribe | `Referee::LauncherPack` | Heat limit, cooling value, 17 mm heat, bullet speed | - -## 4. 配置示例 / Configuration Example - -`xrobot instance add QDU-Robomaster/InfantryLauncher` 写入的实例,依赖填写为其他模块实例的 id:`motor_fric_0`、`motor_fric_1`、`motor_trig` 取自 `QDU-Robomaster/RMMotor` 实例,`cmd` 取自 `QDU-Robomaster/CMD` 实例,`referee` 取自 `QDU-Robomaster/Referee` 实例的地址(`'&ref'`),它们须在本实例之前列出。模式事件通常由 `EventBinder` 从遥控器事件绑定到本实例(`InfantryLauncher::LauncherEvent::SET_FRICMODE_READY` 等)。 - -An instance written by `xrobot instance add QDU-Robomaster/InfantryLauncher`, with the dependencies set to the ids of other Module instances: `motor_fric_0`, `motor_fric_1` and `motor_trig` come from `QDU-Robomaster/RMMotor` instances, `cmd` from a `QDU-Robomaster/CMD` instance, and `referee` from the address of a `QDU-Robomaster/Referee` instance (`'&ref'`); they are listed before this instance. The mode events are usually bound from the remote controller events to this instance by `EventBinder` (`InfantryLauncher::LauncherEvent::SET_FRICMODE_READY` and others). +以下为发射器实例配置片段,参数采用头文件中的默认值。 +其中 `motor_fric_0`、`motor_fric_1`、`motor_trig`、`cmd`、`ref` 应替换为 +工程中已定义的实例 ID;PID、转速和减速比需按实际机构配置。 ```yaml modules: - module: QDU-Robomaster/InfantryLauncher id: launcher args: - - motor_fric_0: motor_fric_front_left - - motor_fric_1: motor_fric_front_right + - motor_fric_0: motor_fric_0 + - motor_fric_1: motor_fric_1 - motor_trig: motor_trig - cmd: cmd - referee: '&ref' - param: - task_stack_depth: 1536 - pid_param_trig_angle: - k: 1.0f - p: 40.0f - i: 0.0f - d: 0.0f - i_limit: 0.0f - out_limit: 0.0f - cycle: false - pid_param_trig_speed: - k: 1.0f - p: 0.15f - i: 0.0f - d: 0.0f - i_limit: 0.0f - out_limit: 0.0f - cycle: false - pid_param_fric_speed_0: - k: 0.8f - p: 0.0008f - i: 0.0f - d: 0.0f - i_limit: 0.0f - out_limit: 0.6f - cycle: false - pid_param_fric_speed_1: - k: 0.8f - p: 0.0008f - i: 0.0f - d: 0.0f - i_limit: 0.0f - out_limit: 0.6f - cycle: false + task_stack_depth: 4096 + pid_param_trig_angle: '{.k = 1.0f, .p = 4000.0f, .i = 0.0f, .d = 0.0f, .i_limit = 0.0f, .out_limit = 4000.0f, .cycle = false}' + pid_param_trig_speed: '{.k = 1.0f, .p = 0.0012f, .i = 0.0005f, .d = 0.0f, .i_limit = 1.0f, .out_limit = 1.0f, .cycle = false}' + pid_param_fric_speed_0: '{.k = 1.0f, .p = 0.002f, .i = 0.0f, .d = 0.0f, .i_limit = 0.0f, .out_limit = 1.0f, .cycle = false}' + pid_param_fric_speed_1: '{.k = 1.0f, .p = 0.002f, .i = 0.0f, .d = 0.0f, .i_limit = 0.0f, .out_limit = 1.0f, .cycle = false}' launcher_param: - fric1_setpoint_speed: 5800.0f - target_bullet_speed: 21.0f - bullet_speed_tolerance: 1.0f + fric1_setpoint_speed: 6500.0f + target_bullet_speed: 25.0f + bullet_speed_tolerance: 1.5f trig_gear_ratio: 36.0f num_trig_tooth: 10 thread_priority: LibXR::Thread::Priority::HIGH launcher_cmd_topic_name: "launcher_cmd" launcher_ref_topic_name: "launcher_ref" + heat_param: + offline_heat_limit: 240.0f + offline_cooling_rate: 40.0f ``` -## 5. 依赖与硬件 / Dependencies and Hardware +从旧的滑模配置迁移时,删除发射器 `args` 中的 `sliding` 项。 +工程中遗留的 Sliding 实例不会再参与本模块的拨弹控制。 -依赖: +`num_trig_tooth` 当前用于拨弹速度参考限幅;每发拨弹角度与热量计数使用的 +`launcher::param::TRIG_STEP` 固定为 `2π / 10`(36°)。 +仅修改 YAML 中的齿数不会改变这个步进角。 + +## 模式与扳机行为 -- `QDU-Robomaster/RMMotor`:摩擦轮与拨弹电机。 -- `QDU-Robomaster/Motor`:电机命令与反馈类型。 -- `QDU-Robomaster/CMD`:发射命令类型与 CMD 事件。 -- `QDU-Robomaster/Referee`:热量与弹速数据类型,以及 UI 绘制。 -- LibXR。 +通过 `GetEvent()` 绑定 `InfantryLauncher::LauncherEvent`: -硬件:两个摩擦轮电机和一个拨弹电机,均通过 `RMMotor` 实例接入。 - -Dependencies: - -- `QDU-Robomaster/RMMotor`: friction wheel and trigger motors. -- `QDU-Robomaster/Motor`: motor command and feedback types. -- `QDU-Robomaster/CMD`: launcher command type and CMD events. -- `QDU-Robomaster/Referee`: heat and bullet speed data types, and UI drawing. -- LibXR. +| 事件 | 行为 | +| --- | --- | +| `SET_FRICMODE_RELAX` | 放松摩擦轮和拨弹电机,清除校准与延迟观测状态 | +| `SET_FRICMODE_SAFE` | 摩擦轮目标转速设为 0,摩擦轮 PID 输出除以 50,退出拨弹与脱卡流程 | +| `SET_FRICMODE_READY` | 启动摩擦轮,允许按扳机命令和热量条件拨弹 | +| `SET_SHOTMODE_SINGLE` | 每次按下扳机请求一发,长按超过 0.5 秒进入持续拨弹 | +| `SET_SHOTMODE_CONTINUE` | 按住扳机持续拨弹,无需等待长按阈值 | +| `SET_SHOTMODE_BOOST_3` | 每次按下扳机请求三发;长按逻辑同样适用,连续请求仍使用三发步进 | + +射击模式选择与摩擦轮模式分别保存。只有摩擦轮处于 `READY`,且速度、热量条件满足时, +才会启动一次正常拨弹。两个摩擦轮的速度绝对值均须达到期望转速减 200 rpm。 + +单发和三连发使用 `isfire` 从 `false` 变为 `true` 的上升沿触发。 +若该次触发时条件不满足,不会排队保存这次短按请求。 +连发则在持续按住期间检查:当前拨弹完成,且距上次启动已达到 `1 / trig_freq_` 秒, +才启动下一次拨弹。内部期望射频初值为 `15.0f`,实际节奏还受热量及到位时间影响。 + +松开扳机后停止发起后续拨弹,正常状态下继续完成当前拨弹目标并保持目标位置; +摩擦轮继续按所选模式运行。连发模式选择会保留,再次按下仍进入连发。 +热量不足、模式切换、卡弹或失控会改变这一正常完成流程。 + +`CMD_EVENT_LOST_CTRL` 会复位状态、禁用拨弹电机并放松摩擦轮; +`CMD_EVENT_START_CTRL` 将摩擦轮模式设为 `RELAX`,需重新选择 `READY`。 + +## 校准与卡弹恢复 + +首发拨弹期间记录两个摩擦轮速度绝对值的平均值及其峰值。 +当峰值达到就绪门限,且平均转速从峰值下降至少 `FRIC_DROP_RPM = 150 rpm` 时, +判定检测到出弹,记录当前拨弹角度作为弹位基准。 +常规发射延迟观测使用同一掉速条件。 + +拨弹到位容差为 `TRIGGER_SETTLE_ANGLE = 0.2 * TRIG_STEP`,即约 7.2°。 +未完成校准就到位时会结束本次校准等待,下一次发射可重新尝试校准。 + +仅在 `READY` 模式下,拨弹电机反馈扭矩大于 `JAM_TORQUE = 0.028f` 时进入脱卡。 +当前判断使用正向扭矩值。脱卡过程以 0.1 秒为间隔,在回退约 0.3 齿与恢复目标之间切换。 +松开扳机不会单独取消 `READY` 状态下的脱卡流程。 + +## 热量管理与发射计数 + +| 配置字段 | 默认值 | 使用条件 | +| --- | --- | --- | +| `heat_param.offline_heat_limit` | `240.0f` | 首次有效裁判参数到达前的热量上限,必须有限且大于 0 | +| `heat_param.offline_cooling_rate` | `40.0f` | 首次有效裁判参数到达前每秒冷却量,必须有限且不小于 0 | + +裁判 Topic 尚未出现时,控制线程使用本地参数继续运行,待 Topic 出现后再订阅。 +裁判热量上限必须大于 0,冷却速率和当前热量必须不小于 0,三者均须为有限值; +整组数据通过校验后才更新缓存。收到过有效参数后,继续使用最后一次有效值, +断流时不会自动切回离线参数。冷却速率为 0 是有效值。 + +`CurrentHeat()` 每轮控制循环结算冷却量和拨弹进度。 +当前实现每轮都会将本地热量与缓存的裁判热量取较大值: +若裁判数据停止更新,缓存热量仍会作为本地估计的下限。 +`UpdateHeatControl()` 以至少 50 ms 的间隔更新开火许可和射频限制。 +每次新拨弹还会检查剩余热量是否足以容纳本次一发或三发请求。 + +`heat_limit_.launched_num` 表示本轮按拨弹角度计算出的发射数: + +1. 初始化之后,每轮进入 `CurrentHeat()` 时先清零。 +2. `READY` 状态下累计角度进度 `shot_progress_`,以一齿为一发。 +3. 进度达到一发时取整数部分写入 `launched_num`,扣除已结算进度,保留小数部分。 +4. 将 `single_heat * launched_num` 加入本地热量;当前 `single_heat = 10.0f`。 + +因此 `launched_num` 通常只在一个控制周期内为 `1`,下一轮又变为 `0`。 +累计热量保存在 `current_heat` 中;累计发射总数需另设变量记录。 +该计数根据拨弹运动估算,空转也可能被计入;首发校准成功时还会单独补计一发热量, +并清零角度进度,因此不能直接把 `launched_num` 的累计值当作实际出弹总数。 +当前接口没有 `GetOverheatCount()`。 + +## 调试变量、线程与 UI + +以下为调试器可观察的内部变量: + +| 变量 | 含义 | +| --- | --- | +| `first_shot_delay_ms_` | 首发拨弹开始到掉速校准成功的耗时,单位 ms | +| `delay_time_` | 首发校准等待的控制循环次数 | +| `real_launch_delay_` | 常规发射计时起点到检测到掉速的耗时,单位 ms | +| `trigger_step_active_` | 当前是否仍在执行一次拨弹 | +| `target_shot_index_` | 弹位目标序号,与校准基准共同确定目标角度 | +| `heat_limit_.current_heat` | 当前本地估计热量 | +| `heat_limit_.launched_num` | 本轮角度进度结算出的发射数 | + +常规发射超过 500 ms 仍未检测到出弹时,当前代码会重新设置计时起点。 +此时 `real_launch_delay_` 后续记录的是距最近一次计时起点的时间, +不代表从最初拨弹开始累计的全部等待时间。 +离开 `READY` 或失控时会清除相关延迟观测状态。 + +控制线程每轮末尾休眠 2 ms,实际周期还包含执行与调度时间。 +命令读取、裁判数据更新和状态机在互斥锁内执行,随后解锁并调用 `Control()` 输出电机指令。 +事件回调和 UI 状态快照也使用该锁。 + +提供有效 `referee` 指针且获得非零机器人 ID 后,UI 在图层 1 上显示 +`FRIC ON/OFF`、`SING/CONT/BOOST_3` 和固定位置的落点参考圆圈。 +定时任务间隔为 80 ms,三类图形分时处理,因此同一类图形通常每 240 ms 处理一次。 +`FRIC ON` 表示选择了 `READY` 模式,实际能否拨弹仍由速度和热量条件决定。 + +## 在 BSP 工程中使用 + +在 BSP 的 `Modules/modules.yaml` 中指定模块分支,确认模块源索引指向所需仓库。 +模块代码提交并推送后,在 BSP 根目录执行: + +```sh +xrobot setup --update QDU-Robomaster/InfantryLauncher +xrobot sync +xrobot format +xrobot format --check +xrobot gen -c User/xrobot.yaml +cmake --preset debug +cmake --build --preset debug +``` -Hardware: two friction wheel motors and one trigger motor, all attached through `RMMotor` instances. +上述配置路径与构建预设适用于配套 BSP;其他工程使用各自的配置和工具链。 +若更新后报告旧 `sliding` 构造参数不匹配,先删除该参数,再运行同步和生成命令。 +模块目录是独立 Git 仓库,源码和本文档应在模块仓库提交; +BSP 的 `xrobot.lock` 由 `xrobot setup` 生成,不手工修改。