diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index 39e4917..acca4ac 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -1,170 +1,13 @@ -name: XRobot Module Build Test - +name: Module CI on: push: pull_request: - schedule: - - cron: '0 3 1 * *' # 每月1日凌晨3点(UTC)自动触发 - + workflow_dispatch: jobs: build: - runs-on: ubuntu-latest - container: - image: ghcr.io/xrobot-org/docker-image-linux:main - options: --user 0 - - env: - XR_MODULE_NAME: ${{ github.event.repository.name }} - - steps: - - name: Checkout current module repo to ./Modules/${{ env.XR_MODULE_NAME }} - uses: actions/checkout@v4 - with: - path: Modules/${{ env.XR_MODULE_NAME }} - - - name: Create main.cpp - run: | - cat > main.cpp <<'EOF' - #include - #include "xrobot_main.hpp" - #include "libxr.hpp" - - int main() { - LibXR::PlatformInit(); - LibXR::STDIO::Printf<"This is XRobot Module Template Test\n">(); - LibXR::HardwareContainer hw; - XRobotMain(hw); - return 0; - } - EOF - - - name: Create minimal CMakeLists.txt - run: | - cat > CMakeLists.txt <<'EOF' - project(xrobot_mod_test CXX) - set(CMAKE_CXX_STANDARD 20) - add_executable(xr_test main.cpp) - set(XROBOT_MODULES_DIR ${CMAKE_CURRENT_SOURCE_DIR}/Modules/) - add_subdirectory(libxr) - target_include_directories(xr_test PUBLIC $ ${CMAKE_SOURCE_DIR}/User) - target_link_libraries(xr_test PUBLIC xr) - EOF - - - name: Pull libxr to ./libxr - run: git clone --depth=1 https://github.com/Jiu-xiao/libxr ./libxr - - - name: Checkout Motor module - uses: actions/checkout@v4 - with: - repository: QDU-Robomaster/Motor - path: Modules/Motor - - - name: Checkout RMMotor module - uses: actions/checkout@v4 - with: - repository: QDU-Robomaster/RMMotor - ref: master - path: Modules/RMMotor - - - name: Checkout CMD module - uses: actions/checkout@v4 - with: - repository: QDU-Robomaster/CMD - path: Modules/CMD - - - name: Checkout PowerControl module - uses: actions/checkout@v4 - with: - repository: QDU-Robomaster/PowerControl - path: Modules/PowerControl - - - name: Checkout SuperPower module - uses: actions/checkout@v4 - with: - repository: QDU-Robomaster/SuperPower - path: Modules/SuperPower - - - name: Checkout Referee module - uses: actions/checkout@v4 - with: - repository: QDU-Robomaster/Referee - path: Modules/Referee - - - name: Setup Python & Install deps - run: | - python3 -m pip install --upgrade pip - pip3 install pyyaml requests - - - name: Add XRobot tools to PATH - run: | - echo "$HOME/.local/bin" >> $GITHUB_PATH - - - name: Install xrobot toolchain (assumes pyproject/tar.gz/pip install .) - run: | - pip3 install xrobot libxr - - - name: Run xrobot_setup - run: | - xrobot_setup || true - - - name: Run xrobot_init_mod - run: | - xrobot_init_mod - - - name: Add modules - run: | - xrobot_add_mod RMMotor --instance-id motor_wheel_0 - xrobot_add_mod RMMotor --instance-id motor_wheel_1 - xrobot_add_mod RMMotor --instance-id motor_wheel_2 - xrobot_add_mod RMMotor --instance-id motor_wheel_3 - xrobot_add_mod RMMotor --instance-id motor_steer_0 - xrobot_add_mod RMMotor --instance-id motor_steer_1 - xrobot_add_mod RMMotor --instance-id motor_steer_2 - xrobot_add_mod RMMotor --instance-id motor_steer_3 - xrobot_add_mod CMD --instance-id cmd - xrobot_add_mod SuperPower --instance-id superpower - xrobot_add_mod PowerControl --instance-id power_control - xrobot_add_mod Referee --instance-id ref - - - name: Add this repo module - run: | - xrobot_add_mod ${{ env.XR_MODULE_NAME }} --instance-id omni_chassis && cat User/xrobot.yaml - - - name: Generate main again - run: | - xrobot_setup - - - name: Build - run: | - mkdir -p build - cd build - cmake .. - make - - - name: Create Tag via GitHub API - if: ${{ success() && github.event_name != 'pull_request' }} - uses: actions/github-script@v7 - with: - github-token: ${{ secrets.GITHUB_TOKEN }} - script: | - const moduleTag = process.env.XR_MODULE_NAME; - const date = new Date(); - const yyyy = date.getUTCFullYear(); - const mm = String(date.getUTCMonth() + 1).padStart(2, '0'); - const dd = String(date.getUTCDate()).padStart(2, '0'); - const HH = String(date.getUTCHours()).padStart(2, '0'); - const MM = String(date.getUTCMinutes()).padStart(2, '0'); - const SS = String(date.getUTCSeconds()).padStart(2, '0'); - const tagName = `${moduleTag}-${yyyy}${mm}${dd}-${HH}${MM}${SS}`; - - // 获取当前 commit sha - const sha = process.env.GITHUB_SHA; - - // 创建 tag reference - await github.rest.git.createRef({ - owner: context.repo.owner, - repo: context.repo.repo, - ref: `refs/tags/${tagName}`, - sha: sha - }); - console.log(`Created tag: ${tagName} on sha: ${sha}`); + uses: xrobot-org/XRobot/.github/workflows/module-ci.yml@dev + with: + xrobot-ref: dev + libxr-ref: dev + dependency-ref: refs/heads/dev + template-args: '["Mecanum"]' diff --git a/Chassis.hpp b/Chassis.hpp index 9b6f95b..c11f9b5 100644 --- a/Chassis.hpp +++ b/Chassis.hpp @@ -2,194 +2,69 @@ // clang-format off /* === MODULE MANIFEST V2 === -module_description: No description provided -constructor_args: - - motor_wheel_0: '@nullptr' - - motor_wheel_1: '@nullptr' - - motor_wheel_2: '@nullptr' - - motor_wheel_3: '@nullptr' - - motor_steer_0: '@nullptr' - - motor_steer_1: '@nullptr' - - motor_steer_2: '@nullptr' - - motor_steer_3: '@nullptr' - - cmd: '@nullptr' - - power_control: '@nullptr' - - referee: '@nullptr' - - task_stack_depth: 1536 - - ChassisParam: - wheel_radius: 0.065 - wheel_to_center: 0.26 - gravity_height: 0.0 - reduction_ratio: 15.7647 - wheel_resistance: 0.0 - error_compensation: 0.0 - gravity: 230 - length: 0.0 - width: 0.0 - rotor_speed_scale: 0.95 - rotor_omega_min_scale: 0.55 - rotor_buffer_low_j: 35.0 - rotor_buffer_high_j: 70.0 - rotor_scale_lpf_alpha: 0.2 - - pid_follow_: - k: 1.0 - p: 20.0 - i: 0.0 - d: 0.0 - i_limit: 0.0 - out_limit: 30.0 - cycle: true - - pid_velocity_x_: - k: 1.0 - p: 20.0 - i: 0.0 - d: 0.0 - i_limit: 0.0 - out_limit: 30.0 - cycle: false - - pid_velocity_y_: - k: 1.0 - p: 20.0 - i: 0.0 - d: 0.0 - i_limit: 0.0 - out_limit: 30.0 - cycle: false - - pid_omega_: - k: 1.0 - p: 20.0 - i: 0.0 - d: 0.0 - i_limit: 0.0 - out_limit: 30.0 - cycle: false - - pid_wheel_speed_0_: - k: 1.0 - p: 0.3 - i: 0.0 - d: 0.0 - i_limit: 0.0 - out_limit: 5.0 - cycle: false - - pid_wheel_speed_1_: - k: 1.0 - p: 0.3 - i: 0.0 - d: 0.0 - i_limit: 0.0 - out_limit: 5.0 - cycle: false - - pid_wheel_speed_2_: - k: 1.0 - p: 0.3 - i: 0.0 - d: 0.0 - i_limit: 0.0 - out_limit: 5.0 - cycle: false - - pid_wheel_speed_3_: - k: 1.0 - p: 0.3 - i: 0.0 - d: 0.0 - i_limit: 0.0 - out_limit: 5.0 - cycle: false - - pid_steer_angle_0_: - k: 1.0 - p: 0.0 - i: 0.0 - d: 0.0 - i_limit: 0.0 - out_limit: 0.0 - cycle: false - - pid_steer_angle_1_: - k: 1.0 - p: 0.0 - i: 0.0 - d: 0.0 - i_limit: 0.0 - out_limit: 0.0 - cycle: false - - pid_steer_angle_2_: - k: 1.0 - p: 0.0 - i: 0.0 - d: 0.0 - i_limit: 0.0 - out_limit: 0.0 - cycle: false - - pid_steer_angle_3_: - k: 1.0 - p: 0.0 - i: 0.0 - d: 0.0 - i_limit: 0.0 - out_limit: 0.0 - cycle: false - - pid_steer_speed_0_: - k: 1.0 - p: 0.0 - i: 0.0 - d: 0.0 - i_limit: 0.0 - out_limit: 0.0 - cycle: false - - pid_steer_speed_1_: - k: 1.0 - p: 0.0 - i: 0.0 - d: 0.0 - i_limit: 0.0 - out_limit: 0.0 - cycle: false - - pid_steer_speed_2_: - k: 1.0 - p: 0.0 - i: 0.0 - d: 0.0 - i_limit: 0.0 - out_limit: 0.0 - cycle: false - - pid_steer_speed_3_: - k: 1.0 - p: 0.0 - i: 0.0 - d: 0.0 - i_limit: 0.0 - out_limit: 0.0 - cycle: false - - thread_priority: LibXR::Thread::Priority::HIGH -template_args: - - ChassisType: Omni -required_hardware: - - dr16 - - motor - - can - - bmi088 +module_description: 底盘控制模块:麦轮、全向轮与舵轮底盘的速度闭环、功率限制与模式切换 / Chassis control Module with velocity loops, power limiting and mode switching for mecanum, omnidirectional and swerve (helm) chassis depends: - - xrobot-org/BMI088 - - qdu-future/RMMotor - - qdu-future/CMD - - qdu-future/PowerControl - - qdu-future/Referee - - xrobot-org/MadgwickAHRS +- id: xrobot-org/BMI088 + ref: same-or-dev +- id: QDU-Robomaster/RMMotor + ref: same-or-dev +- id: QDU-Robomaster/CMD + ref: same-or-dev +- id: QDU-Robomaster/PowerControl + ref: same-or-dev +- id: QDU-Robomaster/Referee + ref: same-or-dev +- id: xrobot-org/MadgwickAHRS + ref: same-or-dev +- id: QDU-Robomaster/Motor + ref: same-or-dev +- id: QDU-Robomaster/SuperPower + ref: same-or-dev === END MANIFEST === */ // clang-format on #include -#include #include -/* 功率控制数组按当前最大底盘需求预留 */ +#include "thread.hpp" + +/// 功率控制数组中的电机数量上限 +/// Maximum motor count of the power control arrays static constexpr int CHASSIS_POWER_CONTROL_MAX_MOTOR_COUNT = 6; -struct MotorData { +/** + * @brief 提交给 PowerControl 的电机数据,3508 与 6020 各一组。 + * Motor data submitted to PowerControl, one set each for the 3508 and 6020 motors. + */ +struct MotorData +{ float output_current_3508[CHASSIS_POWER_CONTROL_MAX_MOTOR_COUNT] = {}; + ///< 3508 电机的输出电流(电机控制单位) + ///< Output current of the 3508 motors (motor control units) float rotorspeed_rpm_3508[CHASSIS_POWER_CONTROL_MAX_MOTOR_COUNT] = {}; + ///< 3508 电机的转子转速 (rpm) + ///< Rotor speed of the 3508 motors (rpm) float output_current_6020[CHASSIS_POWER_CONTROL_MAX_MOTOR_COUNT] = {}; + ///< 6020 电机的输出电流(电机控制单位) + ///< Output current of the 6020 motors (motor control units) float rotorspeed_rpm_6020[CHASSIS_POWER_CONTROL_MAX_MOTOR_COUNT] = {}; + ///< 6020 电机的转子转速 (rpm) + ///< Rotor speed of the 6020 motors (rpm) +}; + +/** + * @brief 底盘订阅的 Topic 名称。 + * Names of the Topics subscribed by the chassis. + */ +struct ChassisTopicNames +{ + const char* chassis_cmd = "chassis_cmd"; ///< 底盘控制命令 Topic + ///< Chassis command Topic + const char* chassis_ref = "chassis_ref"; ///< 裁判系统底盘数据 Topic + ///< Referee chassis data Topic + const char* gimbal_euler = "gimbal_euler"; ///< 云台欧拉角 Topic,由全向轮底盘使用 + ///< Gimbal Euler angle Topic, used by Omni }; #include "CMD.hpp" @@ -199,140 +74,246 @@ struct MotorData { #include "Omni.hpp" #include "RMMotor.hpp" #include "Referee.hpp" -#include "app_framework.hpp" #include "libxr_def.hpp" #include "pid.hpp" +/** + * @brief 底盘控制模块:模板参数选择麦轮、全向轮或舵轮实现。 + * Chassis control Module; the template parameter selects the mecanum, + * omnidirectional-wheel or helm (swerve) implementation. + * + * @tparam ChassisType 底盘实现:Mecanum、Omni 或 Helm。 + * Chassis implementation: Mecanum, Omni or Helm. + */ template -class Chassis : public LibXR::Application { +class Chassis +{ public: + /// 所选底盘实现的模式枚举 + /// Mode enum of the selected chassis implementation using ChassisMode = typename ChassisType::ChassisMode; - struct ChassisParam { - float wheel_radius = 0.0f; - float wheel_to_center = 0.0f; - float gravity_height = 0.0f; - float reduction_ratio = 0.0f; - float wheel_resistance = 0.0f; - float error_compensation = 0.0f; - float gravity = 0.0f; - float length = 0.0f; - float width = 0.0f; - float rotor_speed_scale = 1.0f; - float rotor_omega_min_scale = 0.55f; - float rotor_buffer_low_j = 35.0f; - float rotor_buffer_high_j = 70.0f; - float rotor_scale_lpf_alpha = 0.2f; - }; - Chassis( - LibXR::HardwareContainer& hw, LibXR::ApplicationManager& app, - Motor* motor_wheel_0, Motor* motor_wheel_1, Motor* motor_wheel_2, - Motor* motor_wheel_3, Motor* motor_steer_0, Motor* motor_steer_1, - Motor* motor_steer_2, Motor* motor_steer_3, CMD* cmd, - PowerControl* power_control, Referee* referee, uint32_t task_stack_depth, - ChassisParam chassis_param = {}, - LibXR::PID::Param pid_follow_ = {}, - LibXR::PID::Param pid_velocity_x_ = {}, - LibXR::PID::Param pid_velocity_y_ = {}, - LibXR::PID::Param pid_omega_ = {}, - LibXR::PID::Param pid_wheel_speed_0_ = {}, - LibXR::PID::Param pid_wheel_speed_1_ = {}, - LibXR::PID::Param pid_wheel_speed_2_ = {}, - LibXR::PID::Param pid_wheel_speed_3_ = {}, - LibXR::PID::Param pid_steer_angle_0_ = {}, - LibXR::PID::Param pid_steer_angle_1_ = {}, - LibXR::PID::Param pid_steer_angle_2_ = {}, - LibXR::PID::Param pid_steer_angle_3_ = {}, - LibXR::PID::Param pid_steer_speed_0_ = {}, - LibXR::PID::Param pid_steer_speed_1_ = {}, - LibXR::PID::Param pid_steer_speed_2_ = {}, - LibXR::PID::Param pid_steer_speed_3_ = {}, - LibXR::Thread::Priority thread_priority = LibXR::Thread::Priority::HIGH) - : referee_(referee) { - ASSERT(motor_wheel_0 != nullptr); - ASSERT(motor_wheel_1 != nullptr); - ASSERT(motor_wheel_2 != nullptr); - ASSERT(motor_wheel_3 != nullptr); - ASSERT(cmd != nullptr); - ASSERT(power_control != nullptr); - - if constexpr (std::is_same::value) { - ASSERT(motor_steer_0 != nullptr); - ASSERT(motor_steer_1 != nullptr); - ASSERT(motor_steer_2 != nullptr); - ASSERT(motor_steer_3 != nullptr); - } else if constexpr (std::is_same::value) { - ASSERT(referee != nullptr); - } + /** + * @brief 底盘几何、动力与小陀螺缩放参数。 + * Chassis geometry, dynamics and spin-scaling parameters. + */ + struct ChassisParam + { + float wheel_radius = 0.0f; ///< 轮半径 (m) + ///< Wheel radius (m) + float wheel_to_center = 0.0f; ///< 轮心到底盘中心的距离 (m) + ///< Distance from the wheel center to the chassis center (m) + float gravity_height = 0.0f; ///< 质心高度 (m),用于全向轮姿态前馈 + ///< Center-of-mass height (m), used by the Omni attitude feedforward + float reduction_ratio = 0.0f; ///< 轮电机减速比 + ///< Reduction ratio of the wheel motors + float wheel_resistance = 0.0f; ///< 轮阻 + ///< Wheel resistance + float error_compensation = 0.0f; ///< 误差补偿 + ///< Error compensation + float gravity = 0.0f; ///< 底盘重力 (N),用于全向轮姿态前馈 + ///< Chassis weight (N), used by the Omni attitude feedforward + float length = 0.0f; ///< 轮距的长 (m),用于全向轮姿态前馈 + ///< Length of the wheel layout (m), used by the Omni attitude feedforward + float width = 0.0f; ///< 轮距的宽 (m),用于全向轮姿态前馈 + ///< Width of the wheel layout (m), used by the Omni attitude feedforward + float rotor_speed_scale = 1.0f; ///< 平移输入下的小陀螺转速缩放比例 + ///< Spin speed scale under translation input + float rotor_omega_min_scale = 0.55f; ///< 小陀螺动态缩放的下限 + ///< Lower bound of the dynamic spin scale + float rotor_buffer_low_j = 35.0f; ///< 缓冲能量低阈值 (J) + ///< Low buffer energy threshold (J) + float rotor_buffer_high_j = 70.0f; ///< 缓冲能量高阈值 (J) + ///< High buffer energy threshold (J) + float rotor_scale_lpf_alpha = 0.2f; ///< 动态缩放的一阶低通系数 + ///< First-order low-pass coefficient of the dynamic scale + }; - chassis_.emplace( - hw, app, motor_wheel_0, motor_wheel_1, motor_wheel_2, motor_wheel_3, - motor_steer_0, motor_steer_1, motor_steer_2, motor_steer_3, cmd, - power_control, referee, task_stack_depth, - typename ChassisType::ChassisParam{ - chassis_param.wheel_radius, chassis_param.wheel_to_center, - chassis_param.gravity_height, chassis_param.reduction_ratio, - chassis_param.wheel_resistance, chassis_param.error_compensation, - chassis_param.gravity, chassis_param.length, chassis_param.width, - chassis_param.rotor_speed_scale, - chassis_param.rotor_omega_min_scale, - chassis_param.rotor_buffer_low_j, - chassis_param.rotor_buffer_high_j, - chassis_param.rotor_scale_lpf_alpha}, - pid_follow_, pid_velocity_x_, pid_velocity_y_, pid_omega_, - pid_wheel_speed_0_, pid_wheel_speed_1_, pid_wheel_speed_2_, - pid_wheel_speed_3_, pid_steer_angle_0_, pid_steer_angle_1_, - pid_steer_angle_2_, pid_steer_angle_3_, pid_steer_speed_0_, - pid_steer_speed_1_, pid_steer_speed_2_, pid_steer_speed_3_, - thread_priority); + /** + * @brief 底盘配置参数。 + * Chassis configuration parameters. + */ + struct Param + { + ChassisParam chassis_param; ///< 底盘几何、动力与小陀螺缩放参数 + ///< Chassis geometry, dynamics and spin-scaling parameters + LibXR::PID::Param pid_follow; ///< 跟随云台的角度环 PID + ///< Gimbal-following angle-loop PID + LibXR::PID::Param pid_velocity_x; ///< x 方向速度环 PID + ///< x velocity-loop PID + LibXR::PID::Param pid_velocity_y; ///< y 方向速度环 PID + ///< y velocity-loop PID + LibXR::PID::Param pid_omega; ///< 角速度环 PID + ///< Angular-velocity-loop PID + LibXR::PID::Param pid_wheel_speed_0; ///< 轮 0 速度环 PID + ///< Speed-loop PID of wheel 0 + LibXR::PID::Param pid_wheel_speed_1; ///< 轮 1 速度环 PID + ///< Speed-loop PID of wheel 1 + LibXR::PID::Param pid_wheel_speed_2; ///< 轮 2 速度环 PID + ///< Speed-loop PID of wheel 2 + LibXR::PID::Param pid_wheel_speed_3; ///< 轮 3 速度环 PID + ///< Speed-loop PID of wheel 3 + LibXR::PID::Param pid_steer_angle_0; ///< 舵 0 角度环 PID,Mecanum 履带速度环 + ///< Angle-loop PID of steering motor 0; Mecanum: track speed loop + LibXR::PID::Param pid_steer_angle_1; ///< 舵 1 角度环 PID + ///< Angle-loop PID of steering motor 1 + LibXR::PID::Param pid_steer_angle_2; ///< 舵 2 角度环 PID + ///< Angle-loop PID of steering motor 2 + LibXR::PID::Param pid_steer_angle_3; ///< 舵 3 角度环 PID + ///< Angle-loop PID of steering motor 3 + LibXR::PID::Param pid_steer_speed_0; ///< 舵 0 速度环 PID + ///< Speed-loop PID of steering motor 0 + LibXR::PID::Param pid_steer_speed_1; ///< 舵 1 速度环 PID + ///< Speed-loop PID of steering motor 1 + LibXR::PID::Param pid_steer_speed_2; ///< 舵 2 速度环 PID + ///< Speed-loop PID of steering motor 2 + LibXR::PID::Param pid_steer_speed_3; ///< 舵 3 速度环 PID + ///< Speed-loop PID of steering motor 3 + LibXR::Thread::Priority thread_priority; ///< 控制线程优先级 + ///< Control thread priority + const char* chassis_cmd_topic_name; ///< 订阅的底盘控制命令 Topic 名称 + ///< Name of the subscribed chassis command Topic + const char* chassis_ref_topic_name; ///< 订阅的裁判系统底盘数据 Topic 名称 + ///< Name of the subscribed referee chassis data Topic + const char* gimbal_euler_topic_name; ///< 订阅的云台欧拉角 Topic 名称(Omni 使用) + ///< Name of the subscribed gimbal Euler angle Topic (used by Omni) + }; + /** + * @brief 构造 Chassis,创建所选底盘的控制线程并注册模式事件。 + * Construct Chassis, create the control thread of the selected chassis and + * register the mode events. + * + * @param motor_wheel_0 第 0 个驱动轮电机。 + * Drive wheel motor 0. + * @param motor_wheel_1 第 1 个驱动轮电机。 + * Drive wheel motor 1. + * @param motor_wheel_2 第 2 个驱动轮电机。 + * Drive wheel motor 2. + * @param motor_wheel_3 第 3 个驱动轮电机。 + * Drive wheel motor 3. + * @param motor_steer_0 舵向电机 0;Mecanum 为履带电机,可为 nullptr;Helm 须非空; + * Omni 填 nullptr。 + * Steering motor 0; the track motor of Mecanum, may be nullptr; + * non-null for Helm; nullptr for Omni. + * @param motor_steer_1 舵向电机 1;Helm 须非空,其他底盘填 nullptr。 + * Steering motor 1; non-null for Helm, nullptr for the others. + * @param motor_steer_2 舵向电机 2;Helm 须非空,其他底盘填 nullptr。 + * Steering motor 2; non-null for Helm, nullptr for the others. + * @param motor_steer_3 舵向电机 3;Helm 须非空,其他底盘填 nullptr。 + * Steering motor 3; non-null for Helm, nullptr for the others. + * @param cmd 控制命令模块实例,提供控制命令、控制模式与 CMD 事件。 + * Control command Module instance, providing the control command, the + * control mode and the CMD events. + * @param power_control 功率控制模块实例。 + * Power control Module instance. + * @param referee 裁判系统模块实例。 + * Referee Module instance. + * @param task_stack_depth 控制线程栈深,默认 1536。 + * Control thread stack depth, default 1536. + * @param param 配置参数,默认值见 Param 与 ChassisParam。 + * Configuration parameters; see Param and ChassisParam for the defaults. + */ + Chassis(Motor& motor_wheel_0, Motor& motor_wheel_1, Motor& motor_wheel_2, + Motor& motor_wheel_3, Motor* motor_steer_0, Motor* motor_steer_1, + Motor* motor_steer_2, Motor* motor_steer_3, CMD& cmd, + PowerControl& power_control, Referee& referee, uint32_t task_stack_depth = 1536, + const Param& param = {.chassis_param = {}, + .pid_follow = {}, + .pid_velocity_x = {}, + .pid_velocity_y = {}, + .pid_omega = {}, + .pid_wheel_speed_0 = {}, + .pid_wheel_speed_1 = {}, + .pid_wheel_speed_2 = {}, + .pid_wheel_speed_3 = {}, + .pid_steer_angle_0 = {}, + .pid_steer_angle_1 = {}, + .pid_steer_angle_2 = {}, + .pid_steer_angle_3 = {}, + .pid_steer_speed_0 = {}, + .pid_steer_speed_1 = {}, + .pid_steer_speed_2 = {}, + .pid_steer_speed_3 = {}, + .thread_priority = LibXR::Thread::Priority::HIGH, + .chassis_cmd_topic_name = "chassis_cmd", + .chassis_ref_topic_name = "chassis_ref", + .gimbal_euler_topic_name = "gimbal_euler"}) + : chassis_(&motor_wheel_0, &motor_wheel_1, &motor_wheel_2, &motor_wheel_3, + motor_steer_0, motor_steer_1, motor_steer_2, motor_steer_3, &cmd, + &power_control, &referee, task_stack_depth, + typename ChassisType::ChassisParam{ + .wheel_radius = param.chassis_param.wheel_radius, + .wheel_to_center = param.chassis_param.wheel_to_center, + .gravity_height = param.chassis_param.gravity_height, + .reduction_ratio = param.chassis_param.reduction_ratio, + .wheel_resistance = param.chassis_param.wheel_resistance, + .error_compensation = param.chassis_param.error_compensation, + .gravity = param.chassis_param.gravity, + .length = param.chassis_param.length, + .width = param.chassis_param.width, + .rotor_speed_scale = param.chassis_param.rotor_speed_scale, + .rotor_omega_min_scale = param.chassis_param.rotor_omega_min_scale, + .rotor_buffer_low_j = param.chassis_param.rotor_buffer_low_j, + .rotor_buffer_high_j = param.chassis_param.rotor_buffer_high_j, + .rotor_scale_lpf_alpha = param.chassis_param.rotor_scale_lpf_alpha}, + param.pid_follow, param.pid_velocity_x, param.pid_velocity_y, + param.pid_omega, param.pid_wheel_speed_0, param.pid_wheel_speed_1, + param.pid_wheel_speed_2, param.pid_wheel_speed_3, + param.pid_steer_angle_0, param.pid_steer_angle_1, + param.pid_steer_angle_2, param.pid_steer_angle_3, + param.pid_steer_speed_0, param.pid_steer_speed_1, + param.pid_steer_speed_2, param.pid_steer_speed_3, param.thread_priority, + ChassisTopicNames{.chassis_cmd = param.chassis_cmd_topic_name, + .chassis_ref = param.chassis_ref_topic_name, + .gimbal_euler = param.gimbal_euler_topic_name}), + referee_(&referee) + { auto callback = LibXR::Callback::Create( - [](bool in_isr, Chassis* chassis, uint32_t event_id) { + [](bool in_isr, Chassis* chassis, uint32_t event_id) + { UNUSED(in_isr); chassis->EventHandler(event_id); }, this); - chassis_event_.Register(static_cast(ChassisMode::RELAX), - callback); + chassis_event_.Register(static_cast(ChassisMode::RELAX), callback); - chassis_event_.Register(static_cast(ChassisMode::INDEPENDENT), - callback); + chassis_event_.Register(static_cast(ChassisMode::INDEPENDENT), callback); - chassis_event_.Register(static_cast(ChassisMode::ROTOR), - callback); - chassis_event_.Register(static_cast(ChassisMode::FOLLOW), - callback); - /* - * TRACK_START 只属于麦轮 - * 编译期判断可以让 Omni 和 Helm 继续使用各自的枚举 - */ - if constexpr (std::is_same::value) { - chassis_event_.Register(static_cast(ChassisMode::TRACK_START), - callback); + chassis_event_.Register(static_cast(ChassisMode::ROTOR), callback); + chassis_event_.Register(static_cast(ChassisMode::FOLLOW), callback); + /* TRACK_START 仅 Mecanum 的 ChassisMode 提供,编译期判断后注册 */ + if constexpr (std::is_same::value) + { + chassis_event_.Register(static_cast(ChassisMode::TRACK_START), callback); } } /** - * @brief 获取底盘的事件处理器 - * @details 通过此事件处理器可以向底盘发送事件消息,控制底盘的行为模式 - * @return LibXR::Event& 事件处理器的引用 + * @brief 获取模式切换事件。 + * Get the mode-switching event. + * + * @details 事件上注册了 ChassisMode 的各个值,激活对应的事件 ID 即切换底盘模式。 + * Every value of ChassisMode is registered on the event; activating the + * corresponding event ID switches the chassis mode. + * + * @return 事件对象的引用。 + * Reference to the event object. */ LibXR::Event& GetEvent() { return chassis_event_; } /** - * @brief 事件处理器,根据传入的事件ID执行相应操作 - * @param event_id 触发的事件ID + * @brief 按事件 ID 设置底盘模式。 + * Set the chassis mode from an event ID. + * + * @param event_id 事件 ID,取 ChassisMode 的值。 + * Event ID, a value of ChassisMode. */ - void EventHandler(uint32_t event_id) { - ASSERT(chassis_.has_value()); - chassis_->SetMode(event_id); - } - - void OnMonitor() override {} + void EventHandler(uint32_t event_id) { chassis_.SetMode(event_id); } private: - std::optional chassis_; + ChassisType chassis_; LibXR::Event chassis_event_; Referee* referee_; }; diff --git a/Helm.hpp b/Helm.hpp index 3796431..93492e0 100644 --- a/Helm.hpp +++ b/Helm.hpp @@ -2,10 +2,7 @@ // clang-format off /* === MODULE MANIFEST V2 === -module_description: No description provided -constructor_args: [] -template_args: [] -required_hardware: [] +module_description: 舵轮底盘控制实现,由 Chassis 模板使用 / Helm (swerve) chassis controller used by the Chassis template depends: [] === END MANIFEST === */ // clang-format on @@ -21,86 +18,154 @@ depends: [] #include "PowerControl.hpp" #include "RMMotor.hpp" #include "Referee.hpp" -#include "app_framework.hpp" #include "cycle_value.hpp" #include "event.hpp" #include "libxr_def.hpp" #include "libxr_rw.hpp" #include "libxr_time.hpp" #include "pid.hpp" +#include "thread.hpp" #include "timebase.hpp" -#define M3508_NM_TO_LSB_RATIO \ - 52437.5f /* 3508转子扭矩转化为电机控制单位的比例 */ -#define GM6020_NM_TO_LSB_RATIO \ - 7370.0f /* 6020转子扭矩转化为电机控制单位的比例 */ +#define M3508_NM_TO_LSB_RATIO 52437.5f /* 3508转子扭矩转化为电机控制单位的比例 */ +#define GM6020_NM_TO_LSB_RATIO 7370.0f /* 6020转子扭矩转化为电机控制单位的比例 */ #define HELM_CHASSIS_MAX_POWER 100 template class Chassis; -class Helm { + +/** + * @brief 舵轮底盘控制实现,由 Chassis 模板使用。 + * Helm (swerve) chassis controller used by the Chassis template. + */ +class Helm +{ public: - struct ChassisParam { - float wheel_radius = 0.0f; - float wheel_to_center = 0.0f; - float gravity_height = 0.0f; - float reduction_ratio = 0.0f; - float wheel_resistance = 0.0f; - float error_compensation = 0.0f; - float gravity = 0.0f; - float length = 0.0f; - float width = 0.0f; - float rotor_speed_scale = 1.0f; /* 平移输入下的小陀螺缩放 */ - float rotor_omega_min_scale = 0.55f; /* 动态缩放下限 */ - float rotor_buffer_low_j = 35.0f; /* 缓冲能量低阈值 */ - float rotor_buffer_high_j = 70.0f; /* 缓冲能量高阈值 */ - float rotor_scale_lpf_alpha = 0.2f; /* 动态缩放低通系数 */ + /** + * @brief 底盘几何、动力与小陀螺缩放参数。 + * Chassis geometry, dynamics and spin-scaling parameters. + */ + struct ChassisParam + { + float wheel_radius = 0.0f; ///< 轮半径 (m) + ///< Wheel radius (m) + float wheel_to_center = 0.0f; ///< 轮心到底盘中心的距离 (m) + ///< Distance from the wheel center to the chassis center (m) + float gravity_height = 0.0f; ///< 质心高度 (m),用于全向轮姿态前馈 + ///< Center-of-mass height (m), used by the Omni attitude feedforward + float reduction_ratio = 0.0f; ///< 轮电机减速比 + ///< Reduction ratio of the wheel motors + float wheel_resistance = 0.0f; ///< 轮阻 + ///< Wheel resistance + float error_compensation = 0.0f; ///< 误差补偿 + ///< Error compensation + float gravity = 0.0f; ///< 底盘重力 (N),用于全向轮姿态前馈 + ///< Chassis weight (N), used by the Omni attitude feedforward + float length = 0.0f; ///< 轮距的长 (m),用于全向轮姿态前馈 + ///< Length of the wheel layout (m), used by the Omni attitude feedforward + float width = 0.0f; ///< 轮距的宽 (m),用于全向轮姿态前馈 + ///< Width of the wheel layout (m), used by the Omni attitude feedforward + float rotor_speed_scale = 1.0f; ///< 平移输入下的小陀螺转速缩放比例 + ///< Spin speed scale under translation input + float rotor_omega_min_scale = 0.55f; ///< 小陀螺动态缩放的下限 + ///< Lower bound of the dynamic spin scale + float rotor_buffer_low_j = 35.0f; ///< 缓冲能量低阈值 (J) + ///< Low buffer energy threshold (J) + float rotor_buffer_high_j = 70.0f; ///< 缓冲能量高阈值 (J) + ///< High buffer energy threshold (J) + float rotor_scale_lpf_alpha = 0.2f; ///< 动态缩放的一阶低通系数 + ///< First-order low-pass coefficient of the dynamic scale }; - enum class ChassisMode : uint8_t { - RELAX, - INDEPENDENT, - ROTOR, - FOLLOW, + /** + * @brief 底盘模式。 + * Chassis modes. + */ + enum class ChassisMode : uint8_t + { + RELAX, ///< 放松:全部电机放松 + ///< Relax: all motors relaxed + INDEPENDENT, ///< 独立:平移相对底盘坐标系 + ///< Independent: translation in the chassis frame + ROTOR, ///< 小陀螺:底盘旋转,平移相对云台坐标系 + ///< Spin: the chassis rotates, translation in the gimbal frame + FOLLOW ///< 跟随:底盘跟随云台 yaw,平移相对云台坐标系 + ///< Follow: the chassis follows the gimbal yaw, translation in the gimbal frame }; /** - * @brief 构造函数,初始化舵轮底盘控制对象 - * @param hw 硬件容器引用 - * @param app 应用管理器引用 - * @param cmd 控制命令引用 - * @param motor_wheel_0 第0个驱动轮电机指针 - * @param motor_wheel_1 第1个驱动轮电机指针 - * @param motor_wheel_2 第2个驱动轮电机指针 - * @param motor_wheel_3 第3个驱动轮电机指针 - * @param motor_steer_0 第0个舵向电机指针 - * @param motor_steer_1 第1个舵向电机指针 - * @param motor_steer_2 第2个舵向电机指针 - * @param motor_steer_3 第3个舵向电机指针 - * @param task_stack_depth 控制线程栈深度 - * @param chassis_param 舵轮底盘参数 - * @param pid_follow 跟随云台角度PID参数 - * @param pid_velocity_x X方向速度PID参数 - * @param pid_velocity_y Y方向速度PID参数 - * @param pid_omega 角速度PID参数 - * @param pid_wheel_omega_0 轮子0角速度PID参数 - * @param pid_wheel_omega_1 轮子1角速度PID参数 - * @param pid_wheel_omega_2 轮子2角速度PID参数 - * @param pid_wheel_omega_3 轮子3角速度PID参数 - * @param pid_steer_angle_0 舵机0角度PID参数 - * @param pid_steer_angle_1 舵机1角度PID参数 - * @param pid_steer_angle_2 舵机2角度PID参数 - * @param pid_steer_angle_3 舵机3角度PID参数 + * @brief 构造舵轮底盘控制对象,创建控制线程。 + * Construct the helm chassis controller and create the control thread. + * + * @param motor_wheel_0 第 0 个驱动轮电机。 + * Drive wheel motor 0. + * @param motor_wheel_1 第 1 个驱动轮电机。 + * Drive wheel motor 1. + * @param motor_wheel_2 第 2 个驱动轮电机。 + * Drive wheel motor 2. + * @param motor_wheel_3 第 3 个驱动轮电机。 + * Drive wheel motor 3. + * @param motor_steer_0 第 0 个舵向电机,非空。 + * Steering motor 0, non-null. + * @param motor_steer_1 第 1 个舵向电机,非空。 + * Steering motor 1, non-null. + * @param motor_steer_2 第 2 个舵向电机,非空。 + * Steering motor 2, non-null. + * @param motor_steer_3 第 3 个舵向电机,非空。 + * Steering motor 3, non-null. + * @param cmd 控制命令模块实例。 + * Control command Module instance. + * @param power_control 功率控制模块实例。 + * Power control Module instance. + * @param referee 裁判系统模块实例。 + * Referee Module instance. + * @param task_stack_depth 控制线程栈深。 + * Control thread stack depth. + * @param chassis_param 底盘几何与动力参数。 + * Chassis geometry and dynamics parameters. + * @param pid_follow 跟随云台的角度 PID 参数。 + * PID parameters of the gimbal-following angle loop. + * @param pid_velocity_x x 方向速度 PID 参数。 + * PID parameters of the x velocity loop. + * @param pid_velocity_y y 方向速度 PID 参数。 + * PID parameters of the y velocity loop. + * @param pid_omega 角速度 PID 参数。 + * PID parameters of the angular-velocity loop. + * @param pid_wheel_speed_0 轮 0 速度 PID 参数。 + * PID parameters of the speed loop of wheel 0. + * @param pid_wheel_speed_1 轮 1 速度 PID 参数。 + * PID parameters of the speed loop of wheel 1. + * @param pid_wheel_speed_2 轮 2 速度 PID 参数。 + * PID parameters of the speed loop of wheel 2. + * @param pid_wheel_speed_3 轮 3 速度 PID 参数。 + * PID parameters of the speed loop of wheel 3. + * @param pid_steer_angle_0 舵 0 角度 PID 参数。 + * PID parameters of the angle loop of steering motor 0. + * @param pid_steer_angle_1 舵 1 角度 PID 参数。 + * PID parameters of the angle loop of steering motor 1. + * @param pid_steer_angle_2 舵 2 角度 PID 参数。 + * PID parameters of the angle loop of steering motor 2. + * @param pid_steer_angle_3 舵 3 角度 PID 参数。 + * PID parameters of the angle loop of steering motor 3. + * @param pid_steer_speed_0 舵 0 速度 PID 参数。 + * PID parameters of the speed loop of steering motor 0. + * @param pid_steer_speed_1 舵 1 速度 PID 参数。 + * PID parameters of the speed loop of steering motor 1. + * @param pid_steer_speed_2 舵 2 速度 PID 参数。 + * PID parameters of the speed loop of steering motor 2. + * @param pid_steer_speed_3 舵 3 速度 PID 参数。 + * PID parameters of the speed loop of steering motor 3. + * @param thread_priority 控制线程优先级。 + * Control thread priority. + * @param topic_names 订阅的 Topic 名称。 + * Names of the subscribed Topics. */ - Helm(LibXR::HardwareContainer& hw, LibXR::ApplicationManager& app, - Motor* motor_wheel_0, Motor* motor_wheel_1, Motor* motor_wheel_2, + Helm(Motor* motor_wheel_0, Motor* motor_wheel_1, Motor* motor_wheel_2, Motor* motor_wheel_3, Motor* motor_steer_0, Motor* motor_steer_1, - Motor* motor_steer_2, Motor* motor_steer_3, CMD* cmd, - PowerControl* power_control, Referee* referee, uint32_t task_stack_depth, - ChassisParam chassis_param, LibXR::PID::Param pid_follow, - LibXR::PID::Param pid_velocity_x, - LibXR::PID::Param pid_velocity_y, - LibXR::PID::Param pid_omega, + Motor* motor_steer_2, Motor* motor_steer_3, CMD* cmd, PowerControl* power_control, + Referee* referee, uint32_t task_stack_depth, ChassisParam chassis_param, + LibXR::PID::Param pid_follow, LibXR::PID::Param pid_velocity_x, + LibXR::PID::Param pid_velocity_y, LibXR::PID::Param pid_omega, LibXR::PID::Param pid_wheel_speed_0, LibXR::PID::Param pid_wheel_speed_1, LibXR::PID::Param pid_wheel_speed_2, @@ -113,7 +178,8 @@ class Helm { LibXR::PID::Param pid_steer_speed_1, LibXR::PID::Param pid_steer_speed_2, LibXR::PID::Param pid_steer_speed_3, - LibXR::Thread::Priority thread_priority = LibXR::Thread::Priority::HIGH) + LibXR::Thread::Priority thread_priority = LibXR::Thread::Priority::HIGH, + ChassisTopicNames topic_names = {}) : PARAM(chassis_param), motor_wheel_0_(motor_wheel_0), motor_wheel_1_(motor_wheel_1), @@ -127,23 +193,30 @@ class Helm { pid_velocity_x_(pid_velocity_x), /* ↑ │ ↑ */ pid_velocity_y_(pid_velocity_y), /* wheel0 │ wheel1 */ pid_omega_(pid_omega), - pid_wheel_speed_{pid_wheel_speed_0, pid_wheel_speed_1, - pid_wheel_speed_2, pid_wheel_speed_3}, + pid_wheel_speed_{pid_wheel_speed_0, pid_wheel_speed_1, pid_wheel_speed_2, + pid_wheel_speed_3}, pid_steer_angle_{ pid_steer_angle_0, pid_steer_angle_1, pid_steer_angle_2, pid_steer_angle_3, }, - pid_steer_speed_{pid_steer_speed_0, pid_steer_speed_1, - pid_steer_speed_2, pid_steer_speed_3}, + pid_steer_speed_{pid_steer_speed_0, pid_steer_speed_1, pid_steer_speed_2, + pid_steer_speed_3}, cmd_(cmd), - power_control_(power_control) { - UNUSED(hw); - UNUSED(app); + power_control_(power_control) + { + topic_names_ = topic_names; UNUSED(referee); - for (int i = 0; i < 4; i++) { + /* 舵轮底盘控制线程无条件访问四个舵向电机 */ + ASSERT(motor_steer_0 != nullptr); + ASSERT(motor_steer_1 != nullptr); + ASSERT(motor_steer_2 != nullptr); + ASSERT(motor_steer_3 != nullptr); + + for (int i = 0; i < 4; i++) + { motor_wheel_cmd_[i].mode = Motor::ControlMode::MODE_TORQUE; motor_wheel_cmd_[i].reduction_ratio = chassis_param.reduction_ratio; motor_wheel_cmd_[i].torque = 0.0f; @@ -152,7 +225,8 @@ class Helm { motor_wheel_cmd_[i].kp = 0.0f; motor_wheel_cmd_[i].kd = 0.0f; } - for (int i = 0; i < 4; i++) { + for (int i = 0; i < 4; i++) + { motor_steer_cmd_[i].mode = Motor::ControlMode::MODE_TORQUE; motor_steer_cmd_[i].reduction_ratio = 1.0f; motor_steer_cmd_[i].torque = 0.0f; @@ -165,7 +239,8 @@ class Helm { thread_.Create(this, ThreadFunction, "HelmChassisThread", task_stack_depth, thread_priority); auto lost_ctrl_callback = LibXR::Callback::Create( - [](bool in_isr, Helm* helm, uint32_t event_id) { + [](bool in_isr, Helm* helm, uint32_t event_id) + { UNUSED(in_isr); UNUSED(event_id); helm->LostCtrl(); @@ -175,16 +250,23 @@ class Helm { } /** - * @brief 舵轮底盘控制线程函数 - * @param helm Helm对象指针 - * @details 控制线程主循环,负责接收控制指令、执行运动学解算和动力学控制输出 + * @brief 舵轮底盘控制线程函数。 + * Control thread function of the helm chassis. + * + * @details 线程主循环:接收控制命令,执行运动学解算并输出控制量。 + * Main loop: receives the control command, solves the kinematics and sends the + * outputs. + * + * @param helm Helm 对象指针。 + * Pointer to the Helm object. */ - static void ThreadFunction(Helm* helm) { + static void ThreadFunction(Helm* helm) + { helm->mutex_.Lock(); - LibXR::Topic::ASyncSubscriber cmd_suber("chassis_cmd"); + LibXR::Topic::ASyncSubscriber cmd_suber(helm->topic_names_.chassis_cmd); LibXR::Topic::ASyncSubscriber referee_suber( - "chassis_ref"); + helm->topic_names_.chassis_ref); LibXR::Topic::ASyncSubscriber yawmotor_angle_suber("yawmotor_angle"); cmd_suber.StartWaiting(); @@ -195,21 +277,24 @@ class Helm { auto last_time = LibXR::Timebase::GetMilliseconds(); helm->mutex_.Unlock(); - while (true) { - if (cmd_suber.Available()) { + while (true) + { + if (cmd_suber.Available()) + { helm->cmd_data_ = cmd_suber.GetData(); cmd_suber.StartWaiting(); } - if (referee_suber.Available()) { + if (referee_suber.Available()) + { helm->referee_chassis_pack_ = referee_suber.GetData(); helm->referee_last_rx_time_ = LibXR::Timebase::GetMilliseconds(); referee_suber.StartWaiting(); } - if (yawmotor_angle_suber.Available()) { - helm->current_yaw_ = - LibXR::CycleValue(yawmotor_angle_suber.GetData()); + if (yawmotor_angle_suber.Available()) + { + helm->current_yaw_ = LibXR::CycleValue(yawmotor_angle_suber.GetData()); helm->delta_yaw_ = -helm->current_yaw_; yawmotor_angle_suber.StartWaiting(); } @@ -226,15 +311,21 @@ class Helm { } } /** - * @brief 更新电机状态 - * @details 获取当前时间戳并更新所有驱动轮电机的状态 + * @brief 更新电机状态。 + * Update the motor states. + * + * @details 记录采样间隔,更新四个驱动轮电机与四个舵向电机并读取反馈。 + * Records the sampling interval, updates the four wheel motors and the four + * steering motors and reads their feedback. */ - void Update() { + void Update() + { auto now = LibXR::Timebase::GetMicroseconds(); dt_ = (now - last_online_time_).ToSecondf(); last_online_time_ = now; - for (int i = 0; i < 4; i++) { + for (int i = 0; i < 4; i++) + { motor_wheel_[i]->Update(); motor_steer_[i]->Update(); motor_wheel_feedback_[i] = motor_wheel_[i]->GetFeedback(); @@ -243,27 +334,35 @@ class Helm { } /** - * @brief 失去控制处理 - * + * @brief 失去控制时使全部电机放松。 + * Relax all motors when control is lost. */ - void LostCtrl() { - for (int i = 0; i < 4; i++) { + void LostCtrl() + { + for (int i = 0; i < 4; i++) + { motor_wheel_[i]->Relax(); motor_steer_[i]->Relax(); } } /** - * @brief 设置底盘模式 (由 Chassis 外壳调用) - * @param mode 要设置的新模式 + * @brief 设置底盘模式并复位速度、轮速与舵向 PID,由 Chassis 外壳调用。 + * Set the chassis mode and reset the velocity, wheel and steering PIDs; called + * by the Chassis shell. + * + * @param mode 新模式,取 ChassisMode 的值。 + * New mode, a value of ChassisMode. */ - void SetMode(uint32_t mode) { + void SetMode(uint32_t mode) + { mutex_.Lock(); chassis_event_ = static_cast(mode); pid_omega_.Reset(); pid_velocity_x_.Reset(); pid_velocity_y_.Reset(); - for (int i = 0; i < 4; i++) { + for (int i = 0; i < 4; i++) + { pid_wheel_speed_[i].Reset(); pid_steer_angle_[i].Reset(); pid_steer_speed_[i].Reset(); @@ -272,21 +371,25 @@ class Helm { } /** - * @brief 更新底盘控制指令状态 - * @details 从CMD获取底盘控制指令,速控底盘 + * @brief 从 CMD 命令更新目标速度。 + * Update the target velocities from the CMD command. */ - void UpdateCMD() { + void UpdateCMD() + { target_vx_ = cmd_data_.x; target_vy_ = cmd_data_.y; target_omega_ = cmd_data_.z; } /** - * @brief 功率控制更新 - * + * @brief 更新功率控制:提交反馈与期望输出,计算功率上限并读取限幅结果。 + * Update power control: submit the feedback and the requested outputs, compute + * the power limit and read the limited result. */ - void PowerControlUpdate() { - for (int i = 0; i < 4; i++) { + void PowerControlUpdate() + { + for (int i = 0; i < 4; i++) + { motor_data_.rotorspeed_rpm_3508[i] = motor_wheel_feedback_[i].velocity; motor_data_.output_current_3508[i] = motor_wheel_feedback_[i].torque * M3508_NM_TO_LSB_RATIO; @@ -307,52 +410,57 @@ class Helm { /* 计算 3508 速度跟踪误差 */ float speed_error_3508[4]; - for (int i = 0; i < 4; i++) { - float actual_speed = motor_reverse_[i] - ? -motor_wheel_feedback_[i].velocity - : motor_wheel_feedback_[i].velocity; + for (int i = 0; i < 4; i++) + { + float actual_speed = motor_reverse_[i] ? -motor_wheel_feedback_[i].velocity + : motor_wheel_feedback_[i].velocity; speed_error_3508[i] = target_speed_[i] - actual_speed; - motor_data_.output_current_3508[i] = std::clamp( - wheel_out_[i] * M3508_NM_TO_LSB_RATIO / PARAM.reduction_ratio, - -16384.0f, 16384.0f); - motor_data_.output_current_6020[i] = std::clamp( - steer_out_[i] * GM6020_NM_TO_LSB_RATIO, -16384.0f, 16384.0f); + motor_data_.output_current_3508[i] = + std::clamp(wheel_out_[i] * M3508_NM_TO_LSB_RATIO / PARAM.reduction_ratio, + -16384.0f, 16384.0f); + motor_data_.output_current_6020[i] = + std::clamp(steer_out_[i] * GM6020_NM_TO_LSB_RATIO, -16384.0f, 16384.0f); } /* 计算 6020 舵向速度跟踪误差 */ float speed_error_6020[4]; - for (int i = 0; i < 4; i++) { - speed_error_6020[i] = - steer_angle_[i] - motor_steer_feedback_[i].velocity * - static_cast(LibXR::TWO_PI) / 60.0f; + for (int i = 0; i < 4; i++) + { + speed_error_6020[i] = steer_angle_[i] - motor_steer_feedback_[i].velocity * + static_cast(LibXR::TWO_PI) / + 60.0f; } power_control_->SetMotorData3508(motor_data_.output_current_3508, - motor_data_.rotorspeed_rpm_3508, - speed_error_3508); + motor_data_.rotorspeed_rpm_3508, speed_error_3508); power_control_->SetMotorData6020(motor_data_.output_current_6020, - motor_data_.rotorspeed_rpm_6020, - speed_error_6020); + motor_data_.rotorspeed_rpm_6020, speed_error_6020); auto now_ms = LibXR::Timebase::GetMilliseconds(); bool referee_online = (now_ms - referee_last_rx_time_).ToSecondf() <= 1.0f; bool power_control_online = power_control_->IsOnline(); bool boost_mode = (cmd_data_.self_define == CMD::ChasStat::BOOST); - float max_power = - static_cast(referee_chassis_pack_.rs.chassis_power_limit); - if (!referee_online || max_power <= 1.0f) { + float max_power = static_cast(referee_chassis_pack_.rs.chassis_power_limit); + if (!referee_online || max_power <= 1.0f) + { max_power = HELM_CHASSIS_MAX_POWER; } - if (power_control_online && boost_mode) { + if (power_control_online && boost_mode) + { float cap_energy = power_control_->GetCapEnergy(); - if (cap_energy > 0.8f) { + if (cap_energy > 0.8f) + { max_power += 100.0f; - } else if (cap_energy > 0.5f) { + } + else if (cap_energy > 0.5f) + { max_power += 70.0f; - } else if (cap_energy > 0.25f) { + } + else if (cap_energy > 0.25f) + { max_power += 40.0f; } } @@ -362,28 +470,35 @@ class Helm { } /** - * @brief 速控底盘控制 - * @details 多模式控制底盘 + * @brief 按模式计算目标速度、舵角,以及轮与舵向电机的输出。 + * Compute the target velocity and steering angles for the current mode, and the + * outputs of the wheel and steering motors. */ - void Helmcontrol() { + void Helmcontrol() + { /* 计算目标平移速度 */ - switch (chassis_event_) { + switch (chassis_event_) + { case (ChassisMode::RELAX): target_vx_ = 0.0f; target_vy_ = 0.0f; break; - case (ChassisMode::INDEPENDENT): { + case (ChassisMode::INDEPENDENT): + { target_vx_ = cmd_data_.x; target_vy_ = cmd_data_.y; - } break; + } + break; case (ChassisMode::FOLLOW): - case (ChassisMode::ROTOR): { + case (ChassisMode::ROTOR): + { float beta = current_yaw_; float cos_beta = cosf(beta); float sin_beta = sinf(beta); target_vx_ = cos_beta * cmd_data_.x - sin_beta * cmd_data_.y; target_vy_ = sin_beta * cmd_data_.x + cos_beta * cmd_data_.y; - } break; + } + break; default: target_vx_ = 0.0f; @@ -393,7 +508,8 @@ class Helm { const float SQRT2 = 1.41421356237f; /* 计算 wz */ - switch (chassis_event_) { + switch (chassis_event_) + { case (ChassisMode::RELAX): target_omega_ = 0.0f; break; @@ -412,34 +528,43 @@ class Helm { target_omega_ = 0.0f; break; } - switch (chassis_event_) { + switch (chassis_event_) + { case (ChassisMode::RELAX): - for (int i = 0; i < 4; i++) { + for (int i = 0; i < 4; i++) + { target_speed_[i] = 0.0f; target_angle_[i] = 0.0; } break; case (ChassisMode::INDEPENDENT): case (ChassisMode::FOLLOW): - case (ChassisMode::ROTOR): { + case (ChassisMode::ROTOR): + { float x = 0, y = 0, wheel_pos = 0; - for (int i = 0; i < 4; i++) { + for (int i = 0; i < 4; i++) + { wheel_pos = -static_cast(i) * static_cast(M_PI_2) + static_cast(M_PI) / 4.0f * 3.0f; x = -sinf(wheel_pos) * target_omega_ * SQRT2 + target_vx_; y = -cosf(wheel_pos) * target_omega_ * SQRT2 + target_vy_; - if (fabsf(x) < 0.05f && fabsf(y) < 0.05f) { + if (fabsf(x) < 0.05f && fabsf(y) < 0.05f) + { target_angle_[i] = wheel_pos; target_speed_[i] = 0.0f; - } else { + } + else + { target_angle_[i] = M_PI_2 - atan2f(y, x); target_speed_[i] = motor_max_speed_ * (fmaxf(fabsf(x), fabsf(y))); } } - } break; + } + break; default: - for (int i = 0; i < 4; i++) { + for (int i = 0; i < 4; i++) + { target_speed_[i] = 0.0f; target_angle_[i] = 0.0; } @@ -447,72 +572,82 @@ class Helm { } /* 舵向差超过 90 度时反转轮电机, 减少舵向转动 */ - for (int i = 0; i < 4; i++) { - if (fabs( - LibXR::CycleValue(motor_steer_feedback_[i].abs_angle - zero_[i]) - - target_angle_[i]) > M_PI_2) { + for (int i = 0; i < 4; i++) + { + if (fabs(LibXR::CycleValue(motor_steer_feedback_[i].abs_angle - zero_[i]) - + target_angle_[i]) > M_PI_2) + { motor_reverse_[i] = true; - } else { + } + else + { motor_reverse_[i] = false; } } /* 根据反转状态计算轮电机和舵向电机输出 */ - for (int i = 0; i < 4; i++) { - if (motor_reverse_[i]) { + for (int i = 0; i < 4; i++) + { + if (motor_reverse_[i]) + { wheel_out_[i] = pid_wheel_speed_[i].Calculate( -target_speed_[i], motor_wheel_feedback_[i].velocity, dt_); steer_angle_[i] = pid_steer_angle_[i].Calculate( - LibXR::CycleValue(target_angle_[i] + - static_cast(M_PI) + zero_[i]), + LibXR::CycleValue(target_angle_[i] + static_cast(M_PI) + + zero_[i]), motor_steer_feedback_[i].abs_angle, dt_); steer_out_[i] = pid_steer_speed_[i].Calculate( steer_angle_[i], - motor_steer_feedback_[i].velocity * - static_cast(LibXR::TWO_PI) / 60.0f, + motor_steer_feedback_[i].velocity * static_cast(LibXR::TWO_PI) / 60.0f, dt_); - } else { + } + else + { wheel_out_[i] = pid_wheel_speed_[i].Calculate( target_speed_[i], motor_wheel_feedback_[i].velocity, dt_); steer_angle_[i] = pid_steer_angle_[i].Calculate( - target_angle_[i] + zero_[i], motor_steer_feedback_[i].abs_angle, - dt_); + target_angle_[i] + zero_[i], motor_steer_feedback_[i].abs_angle, dt_); steer_out_[i] = pid_steer_speed_[i].Calculate( steer_angle_[i], - motor_steer_feedback_[i].velocity * - static_cast(LibXR::TWO_PI) / 60.0f, + motor_steer_feedback_[i].velocity * static_cast(LibXR::TWO_PI) / 60.0f, dt_); } } } /** - * @brief 输出控制命令到电机 - * + * @brief 向电机下发控制命令;RELAX 模式下使全部电机放松。 + * Send the control commands to the motors; all motors are relaxed in RELAX mode. */ - void Output() { - if (power_control_data_.is_power_limited) { - for (int i = 0; i < 4; i++) { - wheel_out_[i] = - std::clamp(power_control_data_.new_output_current_3508[i] / - M3508_NM_TO_LSB_RATIO * PARAM.reduction_ratio, - -4.4f, 4.4f); - steer_out_[i] = static_cast( - std ::clamp(power_control_data_.new_output_current_6020[i] / - GM6020_NM_TO_LSB_RATIO * 1.0, - -2.5, 2.5)); + void Output() + { + if (power_control_data_.is_power_limited) + { + for (int i = 0; i < 4; i++) + { + wheel_out_[i] = std::clamp(power_control_data_.new_output_current_3508[i] / + M3508_NM_TO_LSB_RATIO * PARAM.reduction_ratio, + -4.4f, 4.4f); + steer_out_[i] = static_cast(std ::clamp( + power_control_data_.new_output_current_6020[i] / GM6020_NM_TO_LSB_RATIO * 1.0, + -2.5, 2.5)); } } - if (chassis_event_ == (ChassisMode::RELAX)) { + if (chassis_event_ == (ChassisMode::RELAX)) + { LostCtrl(); - } else { - for (int i = 0; i < 4; i++) { + } + else + { + for (int i = 0; i < 4; i++) + { motor_wheel_cmd_[i].torque = std::clamp(wheel_out_[i], -4.4f, 4.4f); motor_steer_cmd_[i].torque = std::clamp(steer_out_[i], -2.5f, 2.5f); } - for (int i = 0; i < 4; i++) { + for (int i = 0; i < 4; i++) + { motor_wheel_[i]->Control(motor_wheel_cmd_[i]); motor_steer_[i]->Control(motor_steer_cmd_[i]); } @@ -527,16 +662,15 @@ class Helm { float target_omega_ = 0.0f; bool motor_reverse_[4]{false, false, false, false}; - LibXR::CycleValue zero_[4] = {1.10063124, 2.05323339, 3.2244277, - 0.979446769}; + LibXR::CycleValue zero_[4] = {1.10063124, 2.05323339, 3.2244277, 0.979446769}; float current_yaw_ = 0.0f; float delta_yaw_ = 0.0f; - /* 转子的转速 */ + /* 目标轮速与目标舵角 */ float target_speed_[4] = {0.0f, 0.0f, 0.0f, 0.0f}; LibXR::CycleValue target_angle_[4] = {0.0f, 0.0f, 0.0f, 0.0f}; - /* 输出的电流值 */ + /* 轮与舵向电机的输出 */ float wheel_out_[4] = {0.0f, 0.0f, 0.0f, 0.0f}; float steer_out_[4] = {0.0f, 0.0f, 0.0f, 0.0f}; float steer_angle_[4] = {0.0, 0.0, 0.0, 0.0}; @@ -570,21 +704,18 @@ class Helm { LibXR::PID pid_velocity_y_; LibXR::PID pid_omega_; - LibXR::PID pid_wheel_speed_[4] = { - LibXR::PID(LibXR::PID::Param()), - LibXR::PID(LibXR::PID::Param()), - LibXR::PID(LibXR::PID::Param()), - LibXR::PID(LibXR::PID::Param())}; - LibXR::PID pid_steer_angle_[4] = { - LibXR::PID(LibXR::PID::Param()), - LibXR::PID(LibXR::PID::Param()), - LibXR::PID(LibXR::PID::Param()), - LibXR::PID(LibXR::PID::Param())}; - LibXR::PID pid_steer_speed_[4] = { - LibXR::PID(LibXR::PID::Param()), - LibXR::PID(LibXR::PID::Param()), - LibXR::PID(LibXR::PID::Param()), - LibXR::PID(LibXR::PID::Param())}; + LibXR::PID pid_wheel_speed_[4] = {LibXR::PID(LibXR::PID::Param()), + LibXR::PID(LibXR::PID::Param()), + LibXR::PID(LibXR::PID::Param()), + LibXR::PID(LibXR::PID::Param())}; + LibXR::PID pid_steer_angle_[4] = {LibXR::PID(LibXR::PID::Param()), + LibXR::PID(LibXR::PID::Param()), + LibXR::PID(LibXR::PID::Param()), + LibXR::PID(LibXR::PID::Param())}; + LibXR::PID pid_steer_speed_[4] = {LibXR::PID(LibXR::PID::Param()), + LibXR::PID(LibXR::PID::Param()), + LibXR::PID(LibXR::PID::Param()), + LibXR::PID(LibXR::PID::Param())}; CMD* cmd_; CMD::ChassisCMD cmd_data_; @@ -598,6 +729,7 @@ class Helm { Referee::ChassisPack referee_chassis_pack_{}; LibXR::MillisecondTimestamp referee_last_rx_time_ = 0; + ChassisTopicNames topic_names_{}; LibXR::Thread thread_; LibXR::Mutex mutex_; diff --git a/LICENSE b/LICENSE new file mode 100644 index 0000000..d645695 --- /dev/null +++ b/LICENSE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/Mecanum.hpp b/Mecanum.hpp index f239878..ee15b5c 100644 --- a/Mecanum.hpp +++ b/Mecanum.hpp @@ -2,10 +2,7 @@ // clang-format off /* === MODULE MANIFEST V2 === -module_description: No description provided -constructor_args: [] -template_args: [] -required_hardware: [] +module_description: 麦轮底盘控制实现,由 Chassis 模板使用 / Mecanum chassis controller used by the Chassis template depends: [] === END MANIFEST === */ // clang-format on @@ -22,7 +19,6 @@ depends: [] #include "RMMotor.hpp" #include "Referee.hpp" #include "SuperPower.hpp" -#include "app_framework.hpp" #include "event.hpp" #include "libxr_def.hpp" #include "libxr_rw.hpp" @@ -31,8 +27,7 @@ depends: [] #include "thread.hpp" #include "timebase.hpp" -#define M3508_NM_TO_LSB_RATIO \ - 52437.5f /* 3508转子扭矩转化为电机控制单位的比例 */ +#define M3508_NM_TO_LSB_RATIO 52437.5f /* 3508转子扭矩转化为电机控制单位的比例 */ #define MECANUM_MOTOR_MAX_OMEGA 52.0f /* 麦轮输出轴最大角速度 rad/s */ #define MECANUM_CHASSIS_MAX_POWER 70 /* 麦轮默认功率上限 W */ @@ -41,71 +36,141 @@ depends: [] template class Chassis; -class Mecanum { +/** + * @brief 麦轮底盘控制实现,由 Chassis 模板使用。 + * Mecanum chassis controller used by the Chassis template. + */ +class Mecanum +{ public: - struct ChassisParam { - float wheel_radius = 0.0f; - float wheel_to_center = 0.0f; - float gravity_height = 0.0f; - float reduction_ratio = 0.0f; - float wheel_resistance = 0.0f; - float error_compensation = 0.0f; - float gravity = 0.0f; - float length = 0.0f; - float width = 0.0f; - float rotor_speed_scale = - 1.0f; /* 小陀螺转速缩放比例,降低可给平移留出更多功率 */ - float rotor_omega_min_scale = 0.55f; /* 动态缩放下限 */ - float rotor_buffer_low_j = 35.0f; /* 缓冲能量低阈值 */ - float rotor_buffer_high_j = 70.0f; /* 缓冲能量高阈值 */ - float rotor_scale_lpf_alpha = 0.2f; /* 动态缩放低通系数 */ + /** + * @brief 底盘几何、动力与小陀螺缩放参数。 + * Chassis geometry, dynamics and spin-scaling parameters. + */ + struct ChassisParam + { + float wheel_radius = 0.0f; ///< 轮半径 (m) + ///< Wheel radius (m) + float wheel_to_center = 0.0f; ///< 轮心到底盘中心的距离 (m) + ///< Distance from the wheel center to the chassis center (m) + float gravity_height = 0.0f; ///< 质心高度 (m),用于全向轮姿态前馈 + ///< Center-of-mass height (m), used by the Omni attitude feedforward + float reduction_ratio = 0.0f; ///< 轮电机减速比 + ///< Reduction ratio of the wheel motors + float wheel_resistance = 0.0f; ///< 轮阻 + ///< Wheel resistance + float error_compensation = 0.0f; ///< 误差补偿 + ///< Error compensation + float gravity = 0.0f; ///< 底盘重力 (N),用于全向轮姿态前馈 + ///< Chassis weight (N), used by the Omni attitude feedforward + float length = 0.0f; ///< 轮距的长 (m),用于全向轮姿态前馈 + ///< Length of the wheel layout (m), used by the Omni attitude feedforward + float width = 0.0f; ///< 轮距的宽 (m),用于全向轮姿态前馈 + ///< Width of the wheel layout (m), used by the Omni attitude feedforward + float rotor_speed_scale = 1.0f; ///< 平移输入下的小陀螺转速缩放比例 + ///< Spin speed scale under translation input + float rotor_omega_min_scale = 0.55f; ///< 小陀螺动态缩放的下限 + ///< Lower bound of the dynamic spin scale + float rotor_buffer_low_j = 35.0f; ///< 缓冲能量低阈值 (J) + ///< Low buffer energy threshold (J) + float rotor_buffer_high_j = 70.0f; ///< 缓冲能量高阈值 (J) + ///< High buffer energy threshold (J) + float rotor_scale_lpf_alpha = 0.2f; ///< 动态缩放的一阶低通系数 + ///< First-order low-pass coefficient of the dynamic scale }; - enum class ChassisMode : uint8_t { - RELAX, - INDEPENDENT, - ROTOR, - FOLLOW, - TRACK_START, + /** + * @brief 底盘模式。 + * Chassis modes. + */ + enum class ChassisMode : uint8_t + { + RELAX, ///< 放松:全部电机放松 + ///< Relax: all motors relaxed + INDEPENDENT, ///< 独立:平移相对底盘坐标系 + ///< Independent: translation in the chassis frame + ROTOR, ///< 小陀螺:底盘旋转,平移相对云台坐标系 + ///< Spin: the chassis rotates, translation in the gimbal frame + FOLLOW, ///< 跟随:底盘跟随云台 yaw,平移相对云台坐标系 + ///< Follow: the chassis follows the gimbal yaw, translation in the gimbal frame + TRACK_START ///< 履带:履带负责前后,麦轮提供横移与辅助 + ///< Track: the track drives forward/backward, the wheels give lateral motion }; /** - * @brief 构造函数,初始化麦轮底盘控制对象 - * @param hw 硬件容器引用 - * @param app 应用管理器引用 - * @param cmd 控制命令引用 - * @param motor_wheel_0 第0个驱动轮电机指针 - * @param motor_wheel_1 第1个驱动轮电机指针 - * @param motor_wheel_2 第2个驱动轮电机指针 - * @param motor_wheel_3 第3个驱动轮电机指针 - * @param motor_steer_0 第0个舵向电机指针(本底盘用作track) - * @param motor_steer_1 第1个舵向电机指针(本底盘未使用) - * @param motor_steer_2 第2个舵向电机指针(本底盘未使用) - * @param motor_steer_3 第3个舵向电机指针(本底盘未使用) - * @param task_stack_depth 控制线程栈深度 - * @param chassis_param 麦轮底盘参数 - * @param pid_follow 跟随控制PID参数 - * @param pid_velocity_x X方向速度PID参数 - * @param pid_velocity_y Y方向速度PID参数 - * @param pid_omega 角速度PID参数 - * @param pid_wheel_omega_0 轮子0角速度PID参数 - * @param pid_wheel_omega_1 轮子1角速度PID参数 - * @param pid_wheel_omega_2 轮子2角速度PID参数 - * @param pid_wheel_omega_3 轮子3角速度PID参数 - * @param pid_steer_angle_0 舵机0角度PID参数(本底盘用作track_speed_pid) - * @param pid_steer_angle_1 舵机1角度PID参数(本底盘未使用) - * @param pid_steer_angle_2 舵机2角度PID参数(本底盘未使用) - * @param pid_steer_angle_3 舵机3角度PID参数(本底盘未使用) + * @brief 构造麦轮底盘控制对象,创建控制线程。 + * Construct the mecanum chassis controller and create the control thread. + * + * @param motor_wheel_0 第 0 个驱动轮电机。 + * Drive wheel motor 0. + * @param motor_wheel_1 第 1 个驱动轮电机。 + * Drive wheel motor 1. + * @param motor_wheel_2 第 2 个驱动轮电机。 + * Drive wheel motor 2. + * @param motor_wheel_3 第 3 个驱动轮电机。 + * Drive wheel motor 3. + * @param motor_steer_0 履带电机,可为 nullptr。 + * Track motor, may be nullptr. + * @param motor_steer_1 与其他底盘类型共用的构造参数。 + * Constructor parameter shared with the other chassis types. + * @param motor_steer_2 与其他底盘类型共用的构造参数。 + * Constructor parameter shared with the other chassis types. + * @param motor_steer_3 与其他底盘类型共用的构造参数。 + * Constructor parameter shared with the other chassis types. + * @param cmd 控制命令模块实例。 + * Control command Module instance. + * @param power_control 功率控制模块实例。 + * Power control Module instance. + * @param referee 裁判系统模块实例。 + * Referee Module instance. + * @param task_stack_depth 控制线程栈深。 + * Control thread stack depth. + * @param chassis_param 底盘几何与动力参数。 + * Chassis geometry and dynamics parameters. + * @param pid_follow 跟随云台的角度 PID 参数。 + * PID parameters of the gimbal-following angle loop. + * @param pid_velocity_x x 方向速度 PID 参数。 + * PID parameters of the x velocity loop. + * @param pid_velocity_y y 方向速度 PID 参数。 + * PID parameters of the y velocity loop. + * @param pid_omega 角速度 PID 参数。 + * PID parameters of the angular-velocity loop. + * @param pid_wheel_speed_0 轮 0 速度 PID 参数。 + * PID parameters of the speed loop of wheel 0. + * @param pid_wheel_speed_1 轮 1 速度 PID 参数。 + * PID parameters of the speed loop of wheel 1. + * @param pid_wheel_speed_2 轮 2 速度 PID 参数。 + * PID parameters of the speed loop of wheel 2. + * @param pid_wheel_speed_3 轮 3 速度 PID 参数。 + * PID parameters of the speed loop of wheel 3. + * @param pid_steer_angle_0 履带速度 PID 参数。 + * PID parameters of the track speed loop. + * @param pid_steer_angle_1 与其他底盘类型共用的构造参数。 + * Constructor parameter shared with the other chassis types. + * @param pid_steer_angle_2 与其他底盘类型共用的构造参数。 + * Constructor parameter shared with the other chassis types. + * @param pid_steer_angle_3 与其他底盘类型共用的构造参数。 + * Constructor parameter shared with the other chassis types. + * @param pid_steer_speed_0 与其他底盘类型共用的构造参数。 + * Constructor parameter shared with the other chassis types. + * @param pid_steer_speed_1 与其他底盘类型共用的构造参数。 + * Constructor parameter shared with the other chassis types. + * @param pid_steer_speed_2 与其他底盘类型共用的构造参数。 + * Constructor parameter shared with the other chassis types. + * @param pid_steer_speed_3 与其他底盘类型共用的构造参数。 + * Constructor parameter shared with the other chassis types. + * @param thread_priority 控制线程优先级。 + * Control thread priority. + * @param topic_names 订阅的 Topic 名称。 + * Names of the subscribed Topics. */ Mecanum( - LibXR::HardwareContainer& hw, LibXR::ApplicationManager& app, + Motor* motor_wheel_0, Motor* motor_wheel_1, Motor* motor_wheel_2, Motor* motor_wheel_3, Motor* motor_steer_0, Motor* motor_steer_1, - Motor* motor_steer_2, Motor* motor_steer_3, CMD* cmd, - PowerControl* power_control, Referee* referee, uint32_t task_stack_depth, - ChassisParam chassis_param, LibXR::PID::Param pid_follow, - LibXR::PID::Param pid_velocity_x, - LibXR::PID::Param pid_velocity_y, - LibXR::PID::Param pid_omega, + Motor* motor_steer_2, Motor* motor_steer_3, CMD* cmd, PowerControl* power_control, + Referee* referee, uint32_t task_stack_depth, ChassisParam chassis_param, + LibXR::PID::Param pid_follow, LibXR::PID::Param pid_velocity_x, + LibXR::PID::Param pid_velocity_y, LibXR::PID::Param pid_omega, LibXR::PID::Param pid_wheel_speed_0, LibXR::PID::Param pid_wheel_speed_1, LibXR::PID::Param pid_wheel_speed_2, @@ -118,7 +183,8 @@ class Mecanum { LibXR::PID::Param pid_steer_speed_1, LibXR::PID::Param pid_steer_speed_2, LibXR::PID::Param pid_steer_speed_3, - LibXR::Thread::Priority thread_priority = LibXR::Thread::Priority::MEDIUM) + LibXR::Thread::Priority thread_priority = LibXR::Thread::Priority::MEDIUM, + ChassisTopicNames topic_names = {}) /* * 麦轮编号,箭头为轮子正方向 * @@ -141,14 +207,14 @@ class Mecanum { pid_velocity_x_(pid_velocity_x), pid_velocity_y_(pid_velocity_y), pid_omega_(pid_omega), - pid_wheel_speed_{pid_wheel_speed_0, pid_wheel_speed_1, - pid_wheel_speed_2, pid_wheel_speed_3}, + pid_wheel_speed_{pid_wheel_speed_0, pid_wheel_speed_1, pid_wheel_speed_2, + pid_wheel_speed_3}, pid_track_speed_(pid_steer_angle_0), cmd_(cmd), power_control_(power_control), - referee_(referee) { - UNUSED(hw); - UNUSED(app); + referee_(referee) + { + topic_names_ = topic_names; UNUSED(motor_steer_1); UNUSED(motor_steer_2); UNUSED(motor_steer_3); @@ -160,7 +226,8 @@ class Mecanum { UNUSED(pid_steer_angle_2); UNUSED(pid_steer_angle_3); - for (int i = 0; i < 4; i++) { + for (int i = 0; i < 4; i++) + { motor_cmd_[i].mode = Motor::ControlMode::MODE_TORQUE; motor_cmd_[i].reduction_ratio = chassis_param.reduction_ratio; motor_cmd_[i].torque = 0.0f; @@ -177,10 +244,11 @@ class Mecanum { track_motor_cmd_.kp = 0.0f; track_motor_cmd_.kd = 0.0f; - thread_.Create(this, ThreadFunction, "MecanumChassisThread", - task_stack_depth, thread_priority); + thread_.Create(this, ThreadFunction, "MecanumChassisThread", task_stack_depth, + thread_priority); auto start_ctrl_callback = LibXR::Callback::Create( - [](bool in_isr, Mecanum* mecanum, uint32_t event_id) { + [](bool in_isr, Mecanum* mecanum, uint32_t event_id) + { UNUSED(in_isr); UNUSED(event_id); mecanum->mutex_.Lock(); @@ -190,7 +258,8 @@ class Mecanum { this); auto lost_ctrl_callback = LibXR::Callback::Create( - [](bool in_isr, Mecanum* mecanum, uint32_t event_id) { + [](bool in_isr, Mecanum* mecanum, uint32_t event_id) + { UNUSED(in_isr); UNUSED(event_id); mecanum->mutex_.Lock(); @@ -209,16 +278,23 @@ class Mecanum { } /** - * @brief 麦轮底盘控制线程函数 - * @param mecanum Mecanum对象指针 - * @details 控制线程主循环,负责接收控制指令、执行运动学解算和动力学控制输出 + * @brief 麦轮底盘控制线程函数。 + * Control thread function of the mecanum chassis. + * + * @details 线程主循环:接收控制命令,执行运动学解算并输出控制量。 + * Main loop: receives the control command, solves the kinematics and sends the + * outputs. + * + * @param mecanum Mecanum 对象指针。 + * Pointer to the Mecanum object. */ - static void ThreadFunction(Mecanum* mecanum) { + static void ThreadFunction(Mecanum* mecanum) + { mecanum->mutex_.Lock(); - LibXR::Topic::ASyncSubscriber cmd_suber("chassis_cmd"); + LibXR::Topic::ASyncSubscriber cmd_suber(mecanum->topic_names_.chassis_cmd); LibXR::Topic::ASyncSubscriber referee_suber( - "chassis_ref"); + mecanum->topic_names_.chassis_ref); LibXR::Topic::ASyncSubscriber yawmotor_angle_suber("yawmotor_angle"); cmd_suber.StartWaiting(); @@ -230,21 +306,25 @@ class Mecanum { mecanum->mutex_.Unlock(); - while (true) { - if (cmd_suber.Available()) { + while (true) + { + if (cmd_suber.Available()) + { mecanum->cmd_data_ = cmd_suber.GetData(); cmd_suber.StartWaiting(); } - if (referee_suber.Available()) { + if (referee_suber.Available()) + { mecanum->referee_chassis_pack_ = referee_suber.GetData(); mecanum->referee_last_rx_time_ = LibXR::Timebase::GetMilliseconds(); referee_suber.StartWaiting(); } - if (yawmotor_angle_suber.Available()) { - mecanum->current_yaw_ = LibXR::CycleValue( - yawmotor_angle_suber.GetData() - mecanum->yawmotor_zero_); + if (yawmotor_angle_suber.Available()) + { + mecanum->current_yaw_ = LibXR::CycleValue(yawmotor_angle_suber.GetData() - + mecanum->yawmotor_zero_); yawmotor_angle_suber.StartWaiting(); } @@ -267,56 +347,74 @@ class Mecanum { } /** - * @brief 更新电机状态 - * @details 获取当前时间戳并更新所有驱动轮电机的状态 + * @brief 更新电机状态。 + * Update the motor states. + * + * @details 记录采样间隔,更新四个驱动轮电机并读取反馈。 + * Records the sampling interval, updates the four wheel motors and reads their + * feedback. */ - void Update() { + void Update() + { auto now = LibXR::Timebase::GetMicroseconds(); dt_ = (now - last_online_time_).ToSecondf(); last_online_time_ = now; - for (int i = 0; i < 4; i++) { + for (int i = 0; i < 4; i++) + { motor_wheel_[i]->Update(); motor_feedback_[i] = motor_wheel_[i]->GetFeedback(); } } - void UpdateTrack() { - if (track_motor_ == nullptr) { + void UpdateTrack() + { + if (track_motor_ == nullptr) + { track_linear_speed_ = 0.0f; return; } track_motor_->Update(); track_motor_feedback_ = track_motor_->GetFeedback(); /* 转子角速度换算为履带线速度 */ - track_linear_speed_ = track_motor_feedback_.omega / PARAM.reduction_ratio * - TRACK_WHEEL_RADIUS_M; + track_linear_speed_ = + track_motor_feedback_.omega / PARAM.reduction_ratio * TRACK_WHEEL_RADIUS_M; } /** - * @brief 设置底盘模式 + * @brief 设置底盘模式并复位速度、履带与轮速 PID,由 Chassis 外壳调用。 + * Set the chassis mode and reset the velocity, track and wheel-speed PIDs; + * called by the Chassis shell. + * + * @param mode 新模式,取 ChassisMode 的值。 + * New mode, a value of ChassisMode. */ - void SetMode(uint32_t mode) { + void SetMode(uint32_t mode) + { mutex_.Lock(); chassis_event_ = static_cast(mode); pid_omega_.Reset(); pid_velocity_x_.Reset(); pid_velocity_y_.Reset(); pid_track_speed_.Reset(); - for (int i = 0; i < 4; i++) { + for (int i = 0; i < 4; i++) + { pid_wheel_speed_[i].Reset(); } mutex_.Unlock(); } /** - * @brief 更新底盘控制指令状态 - * @details 从CMD获取底盘控制指令,并转换为目标速度 + * @brief 从 CMD 命令更新目标角速度与目标平移速度。 + * Update the target angular velocity and the target translation velocity from + * the CMD command. */ - void UpdateCMD() { + void UpdateCMD() + { float max_v = PARAM.wheel_radius * MECANUM_MOTOR_MAX_OMEGA; /* 先生成目标角速度 */ - switch (chassis_event_) { + switch (chassis_event_) + { case (ChassisMode::RELAX): target_omega_ = 0.0f; break; @@ -333,36 +431,40 @@ class Mecanum { target_omega_ = -pid_follow_.Calculate(0.0f, -current_yaw_, dt_); break; - case (ChassisMode::TRACK_START): { + case (ChassisMode::TRACK_START): + { float max_omega = max_v / PARAM.wheel_to_center; /* 履带模式只保留小幅 FOLLOW 纠偏 */ target_omega_ = -pid_follow_.Calculate(0.0f, -current_yaw_, dt_); - target_omega_ = std::clamp(target_omega_, - -max_omega * TRACK_FOLLOW_OMEGA_LIMIT_SCALE, - max_omega * TRACK_FOLLOW_OMEGA_LIMIT_SCALE); - } break; + target_omega_ = + std::clamp(target_omega_, -max_omega * TRACK_FOLLOW_OMEGA_LIMIT_SCALE, + max_omega * TRACK_FOLLOW_OMEGA_LIMIT_SCALE); + } + break; default: break; } /* 再生成目标平移速度 */ - switch (chassis_event_) { + switch (chassis_event_) + { case (ChassisMode::RELAX): target_vx_ = 0.0f; target_vy_ = 0.0f; break; case (ChassisMode::ROTOR): - case (ChassisMode::FOLLOW): { + case (ChassisMode::FOLLOW): + { float beta = -current_yaw_; float cos_beta = cosf(beta); float sin_beta = sinf(beta); - target_vx_ = - (cos_beta * cmd_data_.x * max_v + sin_beta * cmd_data_.y * max_v); - target_vy_ = - (-sin_beta * cmd_data_.x * max_v + cos_beta * cmd_data_.y * max_v); - } break; - case (ChassisMode::TRACK_START): { + target_vx_ = (cos_beta * cmd_data_.x * max_v + sin_beta * cmd_data_.y * max_v); + target_vy_ = (-sin_beta * cmd_data_.x * max_v + cos_beta * cmd_data_.y * max_v); + } + break; + case (ChassisMode::TRACK_START): + { float beta = -current_yaw_; float cos_beta = cosf(beta); float sin_beta = sinf(beta); @@ -371,24 +473,28 @@ class Mecanum { float assist_vy = GetTrackWheelAssistSpeed(); target_vx_ = cos_beta * assist_vx + sin_beta * assist_vy; target_vy_ = -sin_beta * assist_vx + cos_beta * assist_vy; - } break; - case (ChassisMode::INDEPENDENT): { + } + break; + case (ChassisMode::INDEPENDENT): + { target_vx_ = cmd_data_.x * max_v; target_vy_ = cmd_data_.y * max_v; - } break; + } + break; default: break; } /* 小陀螺模式下根据平移输入与功率状态动态调整旋转速度 */ float rotor_translation_scale = 1.0f; - if (chassis_event_ == ChassisMode::ROTOR) { + if (chassis_event_ == ChassisMode::ROTOR) + { float translation_magnitude = sqrtf(target_vx_ * target_vx_ + target_vy_ * target_vy_); float translation_ratio = 0.0f; - if (max_v > 1e-3f) { - translation_ratio = - std::clamp(translation_magnitude / max_v, 0.0f, 1.0f); + if (max_v > 1e-3f) + { + translation_ratio = std::clamp(translation_magnitude / max_v, 0.0f, 1.0f); } rotor_translation_scale = 1.0f - (1.0f - PARAM.rotor_speed_scale) * translation_ratio; @@ -397,10 +503,14 @@ class Mecanum { } /** - * @brief 麦轮底盘正运动学解算 - * @details 根据四个麦轮的角速度,解算出底盘当前的运动状态 + * @brief 麦轮底盘正运动学解算。 + * Forward kinematics of the mecanum chassis. + * + * @details 由四个麦轮的角速度解算底盘的运动状态。 + * Solves the chassis motion from the angular velocities of the four wheels. */ - void SelfResolution() { + void SelfResolution() + { now_vx_ = (motor_feedback_[0].omega / PARAM.reduction_ratio - motor_feedback_[1].omega / PARAM.reduction_ratio - motor_feedback_[2].omega / PARAM.reduction_ratio + @@ -421,10 +531,15 @@ class Mecanum { } /** - * @brief 麦轮底盘逆运动学解算 - * @details 根据目标底盘速度(vx, vy, ω),计算四个麦轮的目标角速度 + * @brief 麦轮底盘逆运动学解算。 + * Inverse kinematics of the mecanum chassis. + * + * @details 由目标底盘速度 (vx, vy, ω) 计算四个麦轮的目标角速度。 + * Computes the target angular velocity of the four wheels from the target + * chassis velocity (vx, vy, ω). */ - void InverseKinematicsSolution() { + void InverseKinematicsSolution() + { target_motor_omega_[0] = (target_vx_ + target_vy_ + target_omega_ * PARAM.wheel_to_center) / PARAM.wheel_radius; @@ -440,31 +555,46 @@ class Mecanum { } /** - * @brief 计算 PID 输出电流 + * @brief 计算轮速 PID 输出并与前馈力矩合成。 + * Compute the wheel-speed PID outputs and combine them with the feedforward + * torque. */ - void CalculateMotorCurrent() { - if (chassis_event_ == ChassisMode::RELAX) { + void CalculateMotorCurrent() + { + if (chassis_event_ == ChassisMode::RELAX) + { LostCtrl(); - } else { - for (int i = 0; i < 4; i++) { + } + else + { + for (int i = 0; i < 4; i++) + { target_motor_current_[i] = pid_wheel_speed_[i].Calculate( - target_motor_omega_[i], - motor_feedback_[i].omega / PARAM.reduction_ratio, dt_); + target_motor_omega_[i], motor_feedback_[i].omega / PARAM.reduction_ratio, + dt_); } /* 计算输出 */ - for (int i = 0; i < 4; i++) { - output_[i] = target_motor_force_[i] * PARAM.wheel_radius + - target_motor_current_[i]; + for (int i = 0; i < 4; i++) + { + output_[i] = + target_motor_force_[i] * PARAM.wheel_radius + target_motor_current_[i]; } } } /** - * @brief 功率控制更新 + * @brief 更新功率控制与小陀螺动态缩放。 + * Update power control and the dynamic spin scale. + * + * @details 提交反馈与期望输出,计算功率上限并读取限幅结果。 + * Submits the feedback and the requested outputs, computes the power limit and + * reads the limited result. */ - void PowerControlUpdate() { + void PowerControlUpdate() + { /* 采样当前反馈电流和转速供功率模型参数估计使用 */ - for (int i = 0; i < 4; i++) { + for (int i = 0; i < 4; i++) + { motor_data_.rotorspeed_rpm_3508[i] = motor_feedback_[i].velocity; motor_data_.output_current_3508[i] = motor_feedback_[i].torque * M3508_NM_TO_LSB_RATIO; @@ -473,9 +603,8 @@ class Mecanum { motor_data_.rotorspeed_rpm_3508[4] = track_motor_ == nullptr ? 0.0f : track_motor_feedback_.velocity; motor_data_.output_current_3508[4] = - track_motor_ == nullptr - ? 0.0f - : track_motor_feedback_.torque * M3508_NM_TO_LSB_RATIO; + track_motor_ == nullptr ? 0.0f + : track_motor_feedback_.torque * M3508_NM_TO_LSB_RATIO; power_control_->SetMotorData3508(motor_data_.output_current_3508, motor_data_.rotorspeed_rpm_3508); @@ -485,9 +614,10 @@ class Mecanum { float speed_error[5] = {}; /* 写入五路期望电流供限功率使用 */ - for (int i = 0; i < 4; i++) { - speed_error[i] = target_motor_omega_[i] - - motor_feedback_[i].omega / PARAM.reduction_ratio; + for (int i = 0; i < 4; i++) + { + speed_error[i] = + target_motor_omega_[i] - motor_feedback_[i].omega / PARAM.reduction_ratio; motor_data_.output_current_3508[i] = std::clamp(output_[i] * M3508_NM_TO_LSB_RATIO / PARAM.reduction_ratio, -16384.0f, 16384.0f); @@ -496,14 +626,13 @@ class Mecanum { speed_error[4] = track_speed_error_ / TRACK_WHEEL_RADIUS_M; motor_data_.output_current_3508[4] = std::clamp( track_output_current_ * static_cast(M3508_MAX_ABS_LSB), - -static_cast(M3508_MAX_ABS_LSB), - static_cast(M3508_MAX_ABS_LSB)); + -static_cast(M3508_MAX_ABS_LSB), static_cast(M3508_MAX_ABS_LSB)); power_control_->SetMotorData3508(motor_data_.output_current_3508, - motor_data_.rotorspeed_rpm_3508, - speed_error); + motor_data_.rotorspeed_rpm_3508, speed_error); PowerControl::AllocationBias3508 allocation_bias{}; - if (track_motor_ != nullptr && chassis_event_ == ChassisMode::TRACK_START) { + if (track_motor_ != nullptr && chassis_event_ == ChassisMode::TRACK_START) + { const float TRACK_CMD_MAG = GetTrackCommandMagnitude(); const bool TRACK_ACTIVE = TRACK_CMD_MAG > TRACK_ACTIVE_SPEED_EPS_MPS; const bool TRACK_STALLED = @@ -511,22 +640,22 @@ class Mecanum { fabsf(track_linear_speed_) < fabsf(track_target_speed_) * TRACK_STALL_SPEED_RATIO; float wheel_omega_abs_sum = 0.0f; - for (int i = 0; i < 4; i++) { + for (int i = 0; i < 4; i++) + { wheel_omega_abs_sum += fabsf(motor_feedback_[i].omega); } const bool WHEEL_FREE_SPIN = - wheel_omega_abs_sum > - TRACK_WHEEL_FREE_SPIN_OMEGA_RADPS * WHEEL_COUNT_FLOAT; + wheel_omega_abs_sum > TRACK_WHEEL_FREE_SPIN_OMEGA_RADPS * WHEEL_COUNT_FLOAT; const bool TRACK_NEEDS_PRIORITY = - TRACK_ACTIVE && - (fabsf(track_speed_error_) > TRACK_PRIORITY_ERROR_EPS_MPS || - (TRACK_STALLED && WHEEL_FREE_SPIN)); + TRACK_ACTIVE && (fabsf(track_speed_error_) > TRACK_PRIORITY_ERROR_EPS_MPS || + (TRACK_STALLED && WHEEL_FREE_SPIN)); allocation_bias.enabled = true; allocation_bias.reserve_fraction = TRACK_NEEDS_PRIORITY ? TRACK_PRIORITY_RESERVE_FRACTION : TRACK_CRUISE_RESERVE_FRACTION; - for (int i = 0; i < 4; i++) { + for (int i = 0; i < 4; i++) + { allocation_bias.reserve_weight[i] = 0.0f; allocation_bias.allocation_weight_scale[i] = TRACK_NEEDS_PRIORITY ? TRACK_WHEEL_DEPRIORITY_SCALE @@ -534,8 +663,7 @@ class Mecanum { } allocation_bias.reserve_weight[4] = 1.0f; allocation_bias.allocation_weight_scale[4] = - TRACK_NEEDS_PRIORITY ? TRACK_PRIORITY_WEIGHT_SCALE - : TRACK_CRUISE_WEIGHT_SCALE; + TRACK_NEEDS_PRIORITY ? TRACK_PRIORITY_WEIGHT_SCALE : TRACK_CRUISE_WEIGHT_SCALE; } power_control_->SetAllocationBias3508(allocation_bias); @@ -545,20 +673,26 @@ class Mecanum { bool boost_mode = (cmd_data_.self_define == CMD::ChasStat::BOOST); /* 裁判系统离线或上限异常时回退到本地默认功率上限 */ - float max_power = - static_cast(referee_chassis_pack_.rs.chassis_power_limit); - if (!referee_online || max_power <= 1.0f) { + float max_power = static_cast(referee_chassis_pack_.rs.chassis_power_limit); + if (!referee_online || max_power <= 1.0f) + { max_power = MECANUM_CHASSIS_MAX_POWER; } /* BOOST 模式按电容能量分档提升可用功率上限 */ - if (power_control_online && boost_mode) { + if (power_control_online && boost_mode) + { float cap_energy = power_control_->GetCapEnergy(); - if (cap_energy > 0.8f) { + if (cap_energy > 0.8f) + { max_power += 300.0f; - } else if (cap_energy > 0.5f) { + } + else if (cap_energy > 0.5f) + { max_power += 200.0f; - } else if (cap_energy > 0.25f) { + } + else if (cap_energy > 0.25f) + { max_power += 100.0f; } } @@ -569,32 +703,32 @@ class Mecanum { /* 受限程度估计为限幅后电流总量除以请求电流总量 */ float req_current_abs_sum = 0.0f; float lim_current_abs_sum = 0.0f; - for (int i = 0; i < 5; i++) { + for (int i = 0; i < 5; i++) + { float req_current_abs = fabsf(motor_data_.output_current_3508[i]); - float lim_current_abs = - power_control_data_.is_power_limited - ? fabsf(power_control_data_.new_output_current_3508[i]) - : req_current_abs; + float lim_current_abs = power_control_data_.is_power_limited + ? fabsf(power_control_data_.new_output_current_3508[i]) + : req_current_abs; req_current_abs_sum += req_current_abs; lim_current_abs_sum += lim_current_abs; } float power_limit_ratio = 1.0f; - if (req_current_abs_sum > 1e-3f) { + if (req_current_abs_sum > 1e-3f) + { power_limit_ratio = std::clamp(lim_current_abs_sum / req_current_abs_sum, 0.0f, 1.0f); } - /* 将裁判缓冲能量映射为缩放因子离线时保持 1.0 */ + /* 将裁判缓冲能量映射为缩放因子,裁判系统离线时保持 1.0 */ float buffer_scale = 1.0f; - if (referee_online) { + if (referee_online) + { float buffer_range = std::max(PARAM.rotor_buffer_high_j - PARAM.rotor_buffer_low_j, 1.0f); - float referee_buffer_j = - static_cast(referee_chassis_pack_.power_buffer); + float referee_buffer_j = static_cast(referee_chassis_pack_.power_buffer); float buffer_norm = std::clamp( - (referee_buffer_j - PARAM.rotor_buffer_low_j) / buffer_range, 0.0f, - 1.0f); + (referee_buffer_j - PARAM.rotor_buffer_low_j) / buffer_range, 0.0f, 1.0f); buffer_scale = PARAM.rotor_omega_min_scale + (1.0f - PARAM.rotor_omega_min_scale) * buffer_norm; } @@ -604,26 +738,30 @@ class Mecanum { power_control_data_.is_power_limited ? std::clamp(power_limit_ratio, PARAM.rotor_omega_min_scale, 1.0f) : 1.0f; - float target_dynamic_scale = std::clamp(buffer_scale * limit_scale, - PARAM.rotor_omega_min_scale, 1.0f); + float target_dynamic_scale = + std::clamp(buffer_scale * limit_scale, PARAM.rotor_omega_min_scale, 1.0f); float lpf_alpha = std::clamp(PARAM.rotor_scale_lpf_alpha, 0.0f, 1.0f); - rotor_dynamic_scale_ += - (target_dynamic_scale - rotor_dynamic_scale_) * lpf_alpha; + rotor_dynamic_scale_ += (target_dynamic_scale - rotor_dynamic_scale_) * lpf_alpha; rotor_dynamic_scale_ = std::clamp(rotor_dynamic_scale_, PARAM.rotor_omega_min_scale, 1.0f); - /* 仅在小陀螺模式保留缩放其他模式统一复位 */ - if (chassis_event_ != ChassisMode::ROTOR) { + /* 仅在小陀螺模式保留缩放,其他模式复位为 1.0 */ + if (chassis_event_ != ChassisMode::ROTOR) + { rotor_dynamic_scale_ = 1.0f; } } /** - * @brief 麦轮底盘逆动力学解算 - * @details - * 通过运动学正解算出底盘现在的运动状态,并与目标状态进行PID控制,获得目标前馈力矩 + * @brief 麦轮底盘逆动力学解算。 + * Inverse dynamics of the mecanum chassis. + * + * @details 以运动学正解得到的当前状态与目标状态做 PID,得到各轮的前馈力。 + * Runs PIDs between the state from the forward kinematics and the target state + * to obtain the feedforward force of each wheel. */ - void DynamicInverseSolution() { + void DynamicInverseSolution() + { float force_x = pid_velocity_x_.Calculate(target_vx_, now_vx_, dt_); float force_y = pid_velocity_y_.Calculate(target_vy_, now_vy_, dt_); float force_z = pid_omega_.Calculate(target_omega_, now_omega_, dt_); @@ -636,49 +774,64 @@ class Mecanum { } /** - * @brief 麦轮底盘动力学输出 - * @details 限幅并输出四个麦轮的电流控制指令 + * @brief 麦轮底盘动力学输出。 + * Dynamics output of the mecanum chassis. + * + * @details 限幅后向四个麦轮下发力矩指令。 + * Limits the torques and sends them to the four wheels. */ - void OutputToDynamics() { - if (power_control_data_.is_power_limited) { - for (int i = 0; i < 4; i++) { - output_[i] = - std::clamp(power_control_data_.new_output_current_3508[i] / - M3508_NM_TO_LSB_RATIO * PARAM.reduction_ratio, - -6.0f, 6.0f); + void OutputToDynamics() + { + if (power_control_data_.is_power_limited) + { + for (int i = 0; i < 4; i++) + { + output_[i] = std::clamp(power_control_data_.new_output_current_3508[i] / + M3508_NM_TO_LSB_RATIO * PARAM.reduction_ratio, + -6.0f, 6.0f); } } - if (chassis_event_ == ChassisMode::RELAX) { + if (chassis_event_ == ChassisMode::RELAX) + { LostCtrl(); return; - } else { - for (int i = 0; i < 4; i++) { + } + else + { + for (int i = 0; i < 4; i++) + { motor_cmd_[i].torque = std::clamp(output_[i], -6.0f, 6.0f); } - for (int i = 0; i < 4; i++) { + for (int i = 0; i < 4; i++) + { motor_wheel_[i]->Control(motor_cmd_[i]); } } } - float GetTrackCommandMagnitude() const { - /* 遥控 y 先缩放再开方让低速段更细 */ + float GetTrackCommandMagnitude() const + { + /* 遥控 y 先缩放再开方,提高低速段的输入分辨率 */ const float INPUT = cmd_data_.y * TRACK_INPUT_SCALE; return std::sqrt(std::abs(INPUT)) * TRACK_MAX_LINEAR_SPEED_MPS; } - float GetTrackSetpointSpeed() const { + float GetTrackSetpointSpeed() const + { /* 履带电机方向和底盘前进方向相反 */ const float INPUT = cmd_data_.y * TRACK_INPUT_SCALE; const float SIGN = (INPUT < 0.0f) ? 1.0f : -1.0f; return SIGN * GetTrackCommandMagnitude(); } - float GetTrackWheelAssistSpeed() const { + float GetTrackWheelAssistSpeed() const + { /* 麦轮辅助方向和底盘前进方向一致 */ const float INPUT = cmd_data_.y * TRACK_INPUT_SCALE; const float SIGN = (INPUT < 0.0f) ? -1.0f : 1.0f; return SIGN * GetTrackCommandMagnitude() * TRACK_WHEEL_ASSIST_SCALE; } - void CalculateTrackCurrent() { - if (track_motor_ == nullptr || chassis_event_ != ChassisMode::TRACK_START) { + void CalculateTrackCurrent() + { + if (track_motor_ == nullptr || chassis_event_ != ChassisMode::TRACK_START) + { track_target_speed_ = 0.0f; track_output_current_ = 0.0f; track_speed_error_ = 0.0f; @@ -687,15 +840,17 @@ class Mecanum { const float DESIRED_TRACK_SPEED = GetTrackSetpointSpeed(); const float MAX_DELTA = TRACK_SPEED_RAMP_MPS2 * dt_; - /* 目标速度加斜坡避免履带突然打满 */ - track_target_speed_ += std::clamp(DESIRED_TRACK_SPEED - track_target_speed_, - -MAX_DELTA, MAX_DELTA); + /* 目标速度加斜坡,限制履带目标速度的变化率 */ + track_target_speed_ += + std::clamp(DESIRED_TRACK_SPEED - track_target_speed_, -MAX_DELTA, MAX_DELTA); track_speed_error_ = track_target_speed_ - track_linear_speed_; - track_output_current_ = pid_track_speed_.Calculate( - track_target_speed_, track_linear_speed_, dt_); + track_output_current_ = + pid_track_speed_.Calculate(track_target_speed_, track_linear_speed_, dt_); } - void ControlTrack() { - if (track_motor_ == nullptr) { + void ControlTrack() + { + if (track_motor_ == nullptr) + { return; } @@ -705,63 +860,70 @@ class Mecanum { const PowerControlData POWER_CONTROL_DATA = power_control_data_; mutex_.Unlock(); - if (RELAX) { + if (RELAX) + { track_motor_->Relax(); return; } - if (POWER_CONTROL_DATA.is_power_limited) { + if (POWER_CONTROL_DATA.is_power_limited) + { /* 限功率时使用第五路重新分配后的电流 */ - track_output_current = - std::clamp(POWER_CONTROL_DATA.new_output_current_3508[4] / - static_cast(M3508_MAX_ABS_LSB), - -1.0f, 1.0f); + track_output_current = std::clamp(POWER_CONTROL_DATA.new_output_current_3508[4] / + static_cast(M3508_MAX_ABS_LSB), + -1.0f, 1.0f); } /* 按麦轮相同的电流到输出轴扭矩关系下发 */ - track_motor_cmd_.torque = std::clamp( - track_output_current * static_cast(M3508_MAX_ABS_LSB) / - M3508_NM_TO_LSB_RATIO * PARAM.reduction_ratio, - -6.0f, 6.0f); + track_motor_cmd_.torque = + std::clamp(track_output_current * static_cast(M3508_MAX_ABS_LSB) / + M3508_NM_TO_LSB_RATIO * PARAM.reduction_ratio, + -6.0f, 6.0f); track_motor_cmd_.velocity = 0.0f; track_motor_->Control(track_motor_cmd_); } /** - * @brief 失去控制处理 - * + * @brief 失去控制时使全部电机放松。 + * Relax all motors when control is lost. */ - void LostCtrl() { - for (int i = 0; i < 4; i++) { + void LostCtrl() + { + for (int i = 0; i < 4; i++) + { motor_wheel_[i]->Relax(); } - if (track_motor_ != nullptr) { + if (track_motor_ != nullptr) + { track_motor_->Relax(); } } - void InitUi() { + void InitUi() + { const uint16_t id = referee_->GetRobotID(); const uint16_t client = referee_->GetClientID(id); Referee::UIFigureOp ADD_OP = Referee::UIFigureOp::UI_OP_MODIFY; - if (this->ui_dyn_step_ % 4 == 0) { + if (this->ui_dyn_step_ % 4 == 0) + { ADD_OP = Referee::UIFigureOp::UI_OP_ADD; ui_dyn_step_ = 0; } - switch (ui_step_) { - case 0: { - // 根据麦轮底盘的模式映射来显示UI + switch (ui_step_) + { + case 0: + { const char* mode_str = "RELX"; - // 麦轮底盘模式: RELAX, INDEPENDENT, ROTOR, FOLLOW - switch (chassis_event_) { + switch (chassis_event_) + { case ChassisMode::RELAX: - mode_str = "RELX"; // 对应麦轮的 RELAX + mode_str = "RELX"; current_mode_ = ChassisMode::RELAX; break; case ChassisMode::FOLLOW: - mode_str = "FOLW"; // 对应麦轮的 FOLLOW + mode_str = "FOLW"; current_mode_ = ChassisMode::FOLLOW; break; case ChassisMode::ROTOR: - mode_str = "ROTO"; // 对应麦轮的 ROTOR + mode_str = "ROTO"; current_mode_ = ChassisMode::ROTOR; break; case ChassisMode::TRACK_START: @@ -772,46 +934,52 @@ class Mecanum { current_mode_ = ChassisMode::RELAX; break; } - if (current_mode_ != last_ui_mode_ or - ADD_OP == Referee::UIFigureOp::UI_OP_ADD) { + if (current_mode_ != last_ui_mode_ or ADD_OP == Referee::UIFigureOp::UI_OP_ADD) + { Referee::UICharacter char_fig{}; referee_->FillCharacter(char_fig, "WM", ADD_OP, 1, - Referee::UIColor::UI_COLOR_CYAN, 27, 2, 1345, - 764, mode_str); + Referee::UIColor::UI_COLOR_CYAN, 27, 2, 1345, 764, + mode_str); referee_->SendUICharacter(id, client, char_fig); last_ui_mode_ = current_mode_; } break; } - case 1: { - if (ADD_OP == Referee::UIFigureOp::UI_OP_ADD) { + case 1: + { + if (ADD_OP == Referee::UIFigureOp::UI_OP_ADD) + { Referee::UIFigure line_fig{}; - referee_->FillLine(line_fig, "WSL", ADD_OP, 1, - Referee::UIColor::UI_COLOR_CYAN, 2, 0, 0, 400, - 1000); + referee_->FillLine(line_fig, "WSL", ADD_OP, 1, Referee::UIColor::UI_COLOR_CYAN, + 2, 0, 0, 400, 1000); referee_->SendUIFigure(id, client, line_fig); } break; } - case 3: { - if (ADD_OP == Referee::UIFigureOp::UI_OP_ADD) { + case 3: + { + if (ADD_OP == Referee::UIFigureOp::UI_OP_ADD) + { Referee::UIFigure line_fig{}; - referee_->FillLine(line_fig, "WSR", ADD_OP, 1, - Referee::UIColor::UI_COLOR_CYAN, 2, 1920, 0, 1520, - 1000); + referee_->FillLine(line_fig, "WSR", ADD_OP, 1, Referee::UIColor::UI_COLOR_CYAN, + 2, 1920, 0, 1520, 1000); referee_->SendUIFigure(id, client, line_fig); } break; } - case 2: { + case 2: + { Referee::UIFigure spfig{}; Referee::UIColor cap_state; float cap_energy = power_control_->GetCapEnergy(); - if (cap_energy < 0.35f) { + if (cap_energy < 0.35f) + { cap_state = Referee::UIColor::UI_COLOR_ORANGE; this->cnt_low++; - } else { + } + else + { cap_state = Referee::UIColor::UI_COLOR_CYAN; this->cnt_high++; } @@ -829,11 +997,11 @@ class Mecanum { } private: - /* 履带模式使用完整遥控行程,避免目标速度被额外压低 */ + /* 履带模式使用完整遥控行程 */ static constexpr float TRACK_INPUT_SCALE = 1.0f; /* 麦轮辅助速度按履带目标速度 1:1 跟随 */ static constexpr float TRACK_WHEEL_ASSIST_SCALE = 1.0f; - /* 履带 FOLLOW 纠偏只允许使用普通最大角速度的 35% */ + /* 履带模式的 FOLLOW 纠偏角速度上限为普通最大角速度的 35% */ static constexpr float TRACK_FOLLOW_OMEGA_LIMIT_SCALE = 0.35f; /* 履带目标线速度最大变化率, 1.2 表示每秒最多变化 1.2 m/s */ static constexpr float TRACK_SPEED_RAMP_MPS2 = 1.2f; @@ -881,8 +1049,7 @@ class Mecanum { Motor* motor_wheel_3_; Motor* track_motor_; - Motor* motor_wheel_[4]{motor_wheel_0_, motor_wheel_1_, motor_wheel_2_, - motor_wheel_3_}; + Motor* motor_wheel_[4]{motor_wheel_0_, motor_wheel_1_, motor_wheel_2_, motor_wheel_3_}; Motor::Feedback motor_feedback_[4]{}; Motor::MotorCmd motor_cmd_[4]{}; MotorData motor_data_{}; @@ -892,25 +1059,22 @@ class Mecanum { LibXR::PID pid_velocity_y_; LibXR::PID pid_omega_; - LibXR::PID pid_wheel_speed_[4] = { - LibXR::PID(LibXR::PID::Param()), - LibXR::PID(LibXR::PID::Param()), - LibXR::PID(LibXR::PID::Param()), - LibXR::PID(LibXR::PID::Param())}; + LibXR::PID pid_wheel_speed_[4] = {LibXR::PID(LibXR::PID::Param()), + LibXR::PID(LibXR::PID::Param()), + LibXR::PID(LibXR::PID::Param()), + LibXR::PID(LibXR::PID::Param())}; LibXR::PID pid_track_speed_; - LibXR::PID pid_steer_angle_[4] = { - LibXR::PID(LibXR::PID::Param()), - LibXR::PID(LibXR::PID::Param()), - LibXR::PID(LibXR::PID::Param()), - LibXR::PID(LibXR::PID::Param())}; + LibXR::PID pid_steer_angle_[4] = {LibXR::PID(LibXR::PID::Param()), + LibXR::PID(LibXR::PID::Param()), + LibXR::PID(LibXR::PID::Param()), + LibXR::PID(LibXR::PID::Param())}; - LibXR::PID pid_steer_speed_[4] = { - LibXR::PID(LibXR::PID::Param()), - LibXR::PID(LibXR::PID::Param()), - LibXR::PID(LibXR::PID::Param()), - LibXR::PID(LibXR::PID::Param())}; + LibXR::PID pid_steer_speed_[4] = {LibXR::PID(LibXR::PID::Param()), + LibXR::PID(LibXR::PID::Param()), + LibXR::PID(LibXR::PID::Param()), + LibXR::PID(LibXR::PID::Param())}; float track_linear_speed_ = 0.0f; float track_target_speed_ = 0.0f; @@ -928,6 +1092,7 @@ class Mecanum { LibXR::MillisecondTimestamp referee_last_rx_time_ = 0; Referee::ChassisPack referee_chassis_pack_{}; + ChassisTopicNames topic_names_{}; LibXR::Thread thread_; LibXR::Mutex mutex_; diff --git a/Omni.hpp b/Omni.hpp index ba87bb3..6578839 100644 --- a/Omni.hpp +++ b/Omni.hpp @@ -1,11 +1,8 @@ -#pragma once +#pragma once // clang-format off /* === MODULE MANIFEST V2 === -module_description: No description provided -constructor_args: [] -template_args: [] -required_hardware: [] +module_description: 全向轮底盘控制实现,由 Chassis 模板使用 / Omnidirectional-wheel chassis controller used by the Chassis template depends: [] === END MANIFEST === */ // clang-format on @@ -19,11 +16,11 @@ depends: [] #include "Motor.hpp" #include "PowerControl.hpp" #include "Referee.hpp" -#include "app_framework.hpp" #include "cycle_value.hpp" #include "libxr_def.hpp" #include "libxr_time.hpp" #include "pid.hpp" +#include "thread.hpp" #include "timebase.hpp" #include "timer.hpp" @@ -36,69 +33,138 @@ depends: [] template class Chassis; -class Omni { +/** + * @brief 全向轮底盘控制实现,由 Chassis 模板使用。 + * Omnidirectional-wheel chassis controller used by the Chassis template. + */ +class Omni +{ public: - struct ChassisParam { - float wheel_radius = 0.0f; - float wheel_to_center = 0.0f; - float gravity_height = 0.0f; - float reduction_ratio = 0.0f; - float wheel_resistance = 0.0f; - float error_compensation = 0.0f; - float gravity = 0.0f; - float length = 0.0f; - float width = 0.0f; - float rotor_speed_scale = - 1.0f; /* 小陀螺转速缩放比例,降低可给平移留出更多功率 */ - float rotor_omega_min_scale = 0.55f; /* 功率受限时小陀螺最小转速比例 */ - float rotor_buffer_low_j = 35.0f; /* 缓冲能量低阈值 J */ - float rotor_buffer_high_j = 70.0f; /* 缓冲能量高阈值 J */ - float rotor_scale_lpf_alpha = 0.2f; /* 动态缩放一阶低通系数 */ + /** + * @brief 底盘几何、动力与小陀螺缩放参数。 + * Chassis geometry, dynamics and spin-scaling parameters. + */ + struct ChassisParam + { + float wheel_radius = 0.0f; ///< 轮半径 (m) + ///< Wheel radius (m) + float wheel_to_center = 0.0f; ///< 轮心到底盘中心的距离 (m) + ///< Distance from the wheel center to the chassis center (m) + float gravity_height = 0.0f; ///< 质心高度 (m),用于全向轮姿态前馈 + ///< Center-of-mass height (m), used by the Omni attitude feedforward + float reduction_ratio = 0.0f; ///< 轮电机减速比 + ///< Reduction ratio of the wheel motors + float wheel_resistance = 0.0f; ///< 轮阻 + ///< Wheel resistance + float error_compensation = 0.0f; ///< 误差补偿 + ///< Error compensation + float gravity = 0.0f; ///< 底盘重力 (N),用于全向轮姿态前馈 + ///< Chassis weight (N), used by the Omni attitude feedforward + float length = 0.0f; ///< 轮距的长 (m),用于全向轮姿态前馈 + ///< Length of the wheel layout (m), used by the Omni attitude feedforward + float width = 0.0f; ///< 轮距的宽 (m),用于全向轮姿态前馈 + ///< Width of the wheel layout (m), used by the Omni attitude feedforward + float rotor_speed_scale = 1.0f; ///< 平移输入下的小陀螺转速缩放比例 + ///< Spin speed scale under translation input + float rotor_omega_min_scale = 0.55f; ///< 小陀螺动态缩放的下限 + ///< Lower bound of the dynamic spin scale + float rotor_buffer_low_j = 35.0f; ///< 缓冲能量低阈值 (J) + ///< Low buffer energy threshold (J) + float rotor_buffer_high_j = 70.0f; ///< 缓冲能量高阈值 (J) + ///< High buffer energy threshold (J) + float rotor_scale_lpf_alpha = 0.2f; ///< 动态缩放的一阶低通系数 + ///< First-order low-pass coefficient of the dynamic scale }; - enum class ChassisMode : uint8_t { - RELAX, - INDEPENDENT, - ROTOR, - FOLLOW, + /** + * @brief 底盘模式。 + * Chassis modes. + */ + enum class ChassisMode : uint8_t + { + RELAX, ///< 放松:全部电机放松 + ///< Relax: all motors relaxed + INDEPENDENT, ///< 独立:平移相对底盘坐标系 + ///< Independent: translation in the chassis frame + ROTOR, ///< 小陀螺:底盘旋转,平移相对云台坐标系 + ///< Spin: the chassis rotates, translation in the gimbal frame + FOLLOW ///< 跟随:底盘跟随云台 yaw,平移相对云台坐标系 + ///< Follow: the chassis follows the gimbal yaw, translation in the gimbal frame }; /** - * @brief 构造函数,初始化全向轮底盘控制对象 - * @param hw 硬件容器引用 - * @param app 应用管理器引用 - * @param cmd 控制命令引用 - * @param motor_wheel_0 第0个驱动轮电机指针 - * @param motor_wheel_1 第1个驱动轮电机指针 - * @param motor_wheel_2 第2个驱动轮电机指针 - * @param motor_wheel_3 第3个驱动轮电机指针 - * @param motor_steer_0 第0个舵向电机指针(本底盘未使用) - * @param motor_steer_1 第1个舵向电机指针(本底盘未使用) - * @param motor_steer_2 第2个舵向电机指针(本底盘未使用) - * @param motor_steer_3 第3个舵向电机指针(本底盘未使用) - * @param task_stack_depth 控制线程栈深度 - * @param chassis_param 全向轮底盘参数 - * @param pid_follow 跟随控制PID参数 - * @param pid_velocity_x X方向速度PID参数 - * @param pid_velocity_y Y方向速度PID参数 - * @param pid_omega 角速度PID参数 - * @param pid_wheel_omega_0 轮子0角速度PID参数 - * @param pid_wheel_omega_1 轮子1角速度PID参数 - * @param pid_wheel_omega_2 轮子2角速度PID参数 - * @param pid_wheel_omega_3 轮子3角速度PID参数 - * @param pid_steer_angle_0 舵机0角度PID参数(本底盘未使用) - * @param pid_steer_angle_1 舵机1角度PID参数(本底盘未使用) - * @param pid_steer_angle_2 舵机2角度PID参数(本底盘未使用) - * @param pid_steer_angle_3 舵机3角度PID参数(本底盘未使用) + * @brief 构造全向轮底盘控制对象,创建控制线程。 + * Construct the omnidirectional-wheel chassis controller and create the control + * thread. + * + * @param motor_wheel_0 第 0 个驱动轮电机。 + * Drive wheel motor 0. + * @param motor_wheel_1 第 1 个驱动轮电机。 + * Drive wheel motor 1. + * @param motor_wheel_2 第 2 个驱动轮电机。 + * Drive wheel motor 2. + * @param motor_wheel_3 第 3 个驱动轮电机。 + * Drive wheel motor 3. + * @param motor_steer_0 与其他底盘类型共用的构造参数。 + * Constructor parameter shared with the other chassis types. + * @param motor_steer_1 与其他底盘类型共用的构造参数。 + * Constructor parameter shared with the other chassis types. + * @param motor_steer_2 与其他底盘类型共用的构造参数。 + * Constructor parameter shared with the other chassis types. + * @param motor_steer_3 与其他底盘类型共用的构造参数。 + * Constructor parameter shared with the other chassis types. + * @param cmd 控制命令模块实例。 + * Control command Module instance. + * @param power_control 功率控制模块实例。 + * Power control Module instance. + * @param referee 裁判系统模块实例。 + * Referee Module instance. + * @param task_stack_depth 控制线程栈深。 + * Control thread stack depth. + * @param chassis_param 底盘几何与动力参数。 + * Chassis geometry and dynamics parameters. + * @param pid_follow 跟随云台的角度 PID 参数。 + * PID parameters of the gimbal-following angle loop. + * @param pid_velocity_x x 方向速度 PID 参数。 + * PID parameters of the x velocity loop. + * @param pid_velocity_y y 方向速度 PID 参数。 + * PID parameters of the y velocity loop. + * @param pid_omega 角速度 PID 参数。 + * PID parameters of the angular-velocity loop. + * @param pid_wheel_speed_0 轮 0 速度 PID 参数。 + * PID parameters of the speed loop of wheel 0. + * @param pid_wheel_speed_1 轮 1 速度 PID 参数。 + * PID parameters of the speed loop of wheel 1. + * @param pid_wheel_speed_2 轮 2 速度 PID 参数。 + * PID parameters of the speed loop of wheel 2. + * @param pid_wheel_speed_3 轮 3 速度 PID 参数。 + * PID parameters of the speed loop of wheel 3. + * @param pid_steer_angle_0 与其他底盘类型共用的构造参数。 + * Constructor parameter shared with the other chassis types. + * @param pid_steer_angle_1 与其他底盘类型共用的构造参数。 + * Constructor parameter shared with the other chassis types. + * @param pid_steer_angle_2 与其他底盘类型共用的构造参数。 + * Constructor parameter shared with the other chassis types. + * @param pid_steer_angle_3 与其他底盘类型共用的构造参数。 + * Constructor parameter shared with the other chassis types. + * @param pid_steer_speed_0 与其他底盘类型共用的构造参数。 + * Constructor parameter shared with the other chassis types. + * @param pid_steer_speed_1 与其他底盘类型共用的构造参数。 + * Constructor parameter shared with the other chassis types. + * @param pid_steer_speed_2 与其他底盘类型共用的构造参数。 + * Constructor parameter shared with the other chassis types. + * @param pid_steer_speed_3 与其他底盘类型共用的构造参数。 + * Constructor parameter shared with the other chassis types. + * @param thread_priority 控制线程优先级。 + * Control thread priority. + * @param topic_names 订阅的 Topic 名称。 + * Names of the subscribed Topics. */ - Omni(LibXR::HardwareContainer& hw, LibXR::ApplicationManager& app, - Motor* motor_wheel_0, Motor* motor_wheel_1, Motor* motor_wheel_2, + Omni(Motor* motor_wheel_0, Motor* motor_wheel_1, Motor* motor_wheel_2, Motor* motor_wheel_3, Motor* motor_steer_0, Motor* motor_steer_1, - Motor* motor_steer_2, Motor* motor_steer_3, CMD* cmd, - PowerControl* power_control, Referee* referee, uint32_t task_stack_depth, - ChassisParam chassis_param, LibXR::PID::Param pid_follow, - LibXR::PID::Param pid_velocity_x, - LibXR::PID::Param pid_velocity_y, - LibXR::PID::Param pid_omega, + Motor* motor_steer_2, Motor* motor_steer_3, CMD* cmd, PowerControl* power_control, + Referee* referee, uint32_t task_stack_depth, ChassisParam chassis_param, + LibXR::PID::Param pid_follow, LibXR::PID::Param pid_velocity_x, + LibXR::PID::Param pid_velocity_y, LibXR::PID::Param pid_omega, LibXR::PID::Param pid_wheel_speed_0, LibXR::PID::Param pid_wheel_speed_1, LibXR::PID::Param pid_wheel_speed_2, @@ -111,7 +177,8 @@ class Omni { LibXR::PID::Param pid_steer_speed_1, LibXR::PID::Param pid_steer_speed_2, LibXR::PID::Param pid_steer_speed_3, - LibXR::Thread::Priority thread_priority = LibXR::Thread::Priority::HIGH) + LibXR::Thread::Priority thread_priority = LibXR::Thread::Priority::HIGH, + ChassisTopicNames topic_names = {}) : PARAM(chassis_param), motor_wheel_0_(motor_wheel_0), /* wheel0 ▲ y wheel3 */ motor_wheel_1_(motor_wheel_1), /* ↙ │ ↖ */ @@ -121,17 +188,17 @@ class Omni { pid_velocity_x_(pid_velocity_x), /* ↘ │ ↗ */ pid_velocity_y_(pid_velocity_y), /* wheel1 │ wheel2 */ pid_omega_(pid_omega), - pid_wheel_speed_{pid_wheel_speed_0, pid_wheel_speed_1, - pid_wheel_speed_2, pid_wheel_speed_3}, - pid_steer_angle_{pid_steer_angle_0, pid_steer_angle_1, - pid_steer_angle_2, pid_steer_angle_3}, - pid_steer_speed_{pid_steer_speed_0, pid_steer_speed_1, - pid_steer_speed_2, pid_steer_speed_3}, + pid_wheel_speed_{pid_wheel_speed_0, pid_wheel_speed_1, pid_wheel_speed_2, + pid_wheel_speed_3}, + pid_steer_angle_{pid_steer_angle_0, pid_steer_angle_1, pid_steer_angle_2, + pid_steer_angle_3}, + pid_steer_speed_{pid_steer_speed_0, pid_steer_speed_1, pid_steer_speed_2, + pid_steer_speed_3}, cmd_(cmd), power_control_(power_control), - referee_(referee) { - UNUSED(hw); - UNUSED(app); + referee_(referee) + { + topic_names_ = topic_names; UNUSED(motor_steer_0); UNUSED(motor_steer_1); UNUSED(motor_steer_2); @@ -145,7 +212,8 @@ class Omni { UNUSED(pid_steer_angle_2); UNUSED(pid_steer_angle_3); - for (int i = 0; i < 4; i++) { + for (int i = 0; i < 4; i++) + { motor_cmd_[i].mode = Motor::ControlMode::MODE_TORQUE; motor_cmd_[i].reduction_ratio = chassis_param.reduction_ratio; motor_cmd_[i].torque = 0.0f; @@ -157,15 +225,17 @@ class Omni { thread_.Create(this, ThreadFunction, "OmniChassisThread", task_stack_depth, thread_priority); - if (referee_ != nullptr) { - /* 底盘裁判系统 UI 由 Omni 自己的定时器任务周期刷新 */ + if (referee_ != nullptr) + { + /* 底盘裁判系统 UI 由定时器任务周期刷新 */ timer_ui_ = LibXR::Timer::CreateTask(DrawUI, this, UI_REFRESH_PERIOD_MS); LibXR::Timer::Add(timer_ui_); LibXR::Timer::Start(timer_ui_); } auto start_ctrl_callback = LibXR::Callback::Create( - [](bool in_isr, Omni* omni, uint32_t event_id) { + [](bool in_isr, Omni* omni, uint32_t event_id) + { UNUSED(in_isr); UNUSED(event_id); omni->mutex_.Lock(); @@ -176,7 +246,8 @@ class Omni { this); auto lost_ctrl_callback = LibXR::Callback::Create( - [](bool in_isr, Omni* omni, uint32_t event_id) { + [](bool in_isr, Omni* omni, uint32_t event_id) + { UNUSED(in_isr); UNUSED(event_id); omni->mutex_.Lock(); @@ -191,21 +262,27 @@ class Omni { } /** - * @brief 全向轮底盘控制线程函数 - * @param omni Omni对象指针 - * @details 控制线程主循环,负责接收控制指令、执行运动学解算和动力学控制输出 + * @brief 全向轮底盘控制线程函数。 + * Control thread function of the omnidirectional-wheel chassis. + * + * @details 线程主循环:接收控制命令,执行运动学解算并输出控制量。 + * Main loop: receives the control command, solves the kinematics and sends the + * outputs. + * + * @param omni Omni 对象指针。 + * Pointer to the Omni object. */ - static void ThreadFunction(Omni* omni) { + static void ThreadFunction(Omni* omni) + { omni->mutex_.Lock(); - LibXR::Topic::ASyncSubscriber cmd_suber("chassis_cmd"); + LibXR::Topic::ASyncSubscriber cmd_suber(omni->topic_names_.chassis_cmd); LibXR::Topic::ASyncSubscriber referee_suber( - "chassis_ref"); + omni->topic_names_.chassis_ref); LibXR::Topic::ASyncSubscriber> euler_suber( - "gimbal_euler"); + omni->topic_names_.gimbal_euler); LibXR::Topic::ASyncSubscriber yawmotor_angle_suber("yawmotor_angle"); - LibXR::Topic::ASyncSubscriber pitchmotor_angle_suber( - "pitchmotor_angle"); + LibXR::Topic::ASyncSubscriber pitchmotor_angle_suber("pitchmotor_angle"); cmd_suber.StartWaiting(); referee_suber.StartWaiting(); @@ -216,19 +293,23 @@ class Omni { auto last_time = LibXR::Timebase::GetMilliseconds(); omni->mutex_.Unlock(); - while (true) { - if (cmd_suber.Available()) { + while (true) + { + if (cmd_suber.Available()) + { omni->cmd_data_ = cmd_suber.GetData(); cmd_suber.StartWaiting(); } - if (referee_suber.Available()) { + if (referee_suber.Available()) + { omni->referee_chassis_pack_ = referee_suber.GetData(); omni->referee_last_rx_time_ = LibXR::Timebase::GetMilliseconds(); referee_suber.StartWaiting(); } - if (euler_suber.Available()) { + if (euler_suber.Available()) + { omni->euler_ = euler_suber.GetData(); euler_suber.StartWaiting(); omni->imu_pitch_ = -(omni->euler_.Pitch()); @@ -236,11 +317,13 @@ class Omni { omni->imu_yaw_ = (omni->euler_.Yaw()); } - if (yawmotor_angle_suber.Available()) { + if (yawmotor_angle_suber.Available()) + { omni->yawmotor_angle_ = yawmotor_angle_suber.GetData(); yawmotor_angle_suber.StartWaiting(); } - if (pitchmotor_angle_suber.Available()) { + if (pitchmotor_angle_suber.Available()) + { omni->pitchmotor_angle_ = pitchmotor_angle_suber.GetData(); pitchmotor_angle_suber.StartWaiting(); } @@ -261,25 +344,36 @@ class Omni { } /** - * @brief 更新电机状态 - * @details 获取当前时间戳并更新所有驱动轮电机的状态 + * @brief 更新电机状态。 + * Update the motor states. + * + * @details 记录采样间隔,更新四个驱动轮电机并读取反馈。 + * Records the sampling interval, updates the four wheel motors and reads their + * feedback. */ - void Update() { + void Update() + { auto now = LibXR::Timebase::GetMicroseconds(); dt_ = (now - last_online_time_).ToSecondf(); last_online_time_ = now; - for (int i = 0; i < 4; i++) { + for (int i = 0; i < 4; i++) + { motor_wheel_[i]->Update(); motor_feedback_[i] = motor_wheel_[i]->GetFeedback(); } } /** - * @brief 设置底盘模式 (由 Chassis 外壳调用) - * @param mode 要设置的新模式 + * @brief 设置底盘模式并复位速度与轮速 PID,由 Chassis 外壳调用。 + * Set the chassis mode and reset the velocity and wheel-speed PIDs; called by + * the Chassis shell. + * + * @param mode 新模式,取 ChassisMode 的值。 + * New mode, a value of ChassisMode. */ - void SetMode(uint32_t mode) { + void SetMode(uint32_t mode) + { mutex_.Lock(); const ChassisMode NEXT_MODE = static_cast(mode); chassis_event_ = NEXT_MODE; @@ -288,21 +382,25 @@ class Omni { pid_omega_.Reset(); pid_velocity_x_.Reset(); pid_velocity_y_.Reset(); - for (int i = 0; i < 4; i++) { + for (int i = 0; i < 4; i++) + { pid_wheel_speed_[i].Reset(); } mutex_.Unlock(); } /** - * @brief 更新底盘控制指令状态 - * @details 从CMD获取底盘控制指令,并转换为目标速度 + * @brief 从 CMD 命令更新目标角速度与目标平移速度。 + * Update the target angular velocity and the target translation velocity from + * the CMD command. */ - void UpdateCMD() { + void UpdateCMD() + { float max_v = PARAM.wheel_radius * OMNI_MOTOR_MAX_OMEGA; /* 先生成目标角速度 */ - switch (chassis_event_) { + switch (chassis_event_) + { case ChassisMode::RELAX: target_omega_ = 0.0f; break; @@ -315,7 +413,6 @@ class Omni { target_omega_ = -static_cast(max_v / PARAM.wheel_to_center); break; - /* 正方向跟随云台 */ case ChassisMode::FOLLOW: target_omega_ = -pid_follow_.Calculate(0.0f, yawmotor_angle_, dt_); break; @@ -325,42 +422,46 @@ class Omni { } /* 再生成目标平移速度 */ - switch (chassis_event_) { + switch (chassis_event_) + { case ChassisMode::RELAX: target_vx_ = 0.0f; target_vy_ = 0.0f; break; case ChassisMode::ROTOR: - case ChassisMode::FOLLOW: { + case ChassisMode::FOLLOW: + { float beta = yawmotor_angle_; float cos_beta = cosf(beta); float sin_beta = sinf(beta); - target_vx_ = - (cos_beta * cmd_data_.x * max_v - sin_beta * cmd_data_.y * max_v); - target_vy_ = - (sin_beta * cmd_data_.x * max_v + cos_beta * cmd_data_.y * max_v); - } break; - case ChassisMode::INDEPENDENT: { + target_vx_ = (cos_beta * cmd_data_.x * max_v - sin_beta * cmd_data_.y * max_v); + target_vy_ = (sin_beta * cmd_data_.x * max_v + cos_beta * cmd_data_.y * max_v); + } + break; + case ChassisMode::INDEPENDENT: + { const float SQRT2 = 1.41421356237f; /* 独立模式用菱形限幅适配摇杆边界 */ float s = fabsf(cmd_data_.x) + fabsf(cmd_data_.y); float k = (s <= 1.0f) ? max_v : (max_v / s); target_vx_ = SQRT2 * k * cmd_data_.x; target_vy_ = SQRT2 * k * cmd_data_.y; - } break; + } + break; default: break; } /* 小陀螺模式按平移输入和功率状态动态压低转速 */ float rotor_translation_scale = 1.0f; - if (chassis_event_ == ChassisMode::ROTOR) { + if (chassis_event_ == ChassisMode::ROTOR) + { float translation_magnitude = sqrtf(target_vx_ * target_vx_ + target_vy_ * target_vy_); float translation_ratio = 0.0f; - if (max_v > 1e-3f) { - translation_ratio = - std::clamp(translation_magnitude / max_v, 0.0f, 1.0f); + if (max_v > 1e-3f) + { + translation_ratio = std::clamp(translation_magnitude / max_v, 0.0f, 1.0f); } rotor_translation_scale = 1.0f - (1.0f - PARAM.rotor_speed_scale) * translation_ratio; @@ -369,25 +470,38 @@ class Omni { } /** - * @brief 前馈死区软限幅 - * @param x 输入值 - * @param dz 死区范围 - * @return 软限幅后的输出 + * @brief 带死区的软限幅,用于姿态前馈。 + * Soft dead-zone used by the attitude feedforward. + * + * @param x 输入值。 + * Input value. + * @param dz 死区范围。 + * Dead-zone width. + * @return 死区外的输出值。 + * Output outside the dead zone. */ - float SoftDeadzone(float x, float dz) { - if (fabs(x) < dz) { + float SoftDeadzone(float x, float dz) + { + if (fabs(x) < dz) + { return 0.0f; - } else { + } + else + { return (fabs(x) - dz) * (x > 0.0f ? 1.0f : -1.0f); } } /** - * @brief 计算姿态前馈 + * @brief 由云台姿态与云台电机角度计算底盘姿态前馈力矩。 + * Compute the attitude feedforward torques from the gimbal attitude and the + * gimbal motor angles. */ - void FeedForward() { + void FeedForward() + { /* 角度先包到正负 pi */ - auto WrapToPi = [](float a) { + auto WrapToPi = [](float a) + { while (a > M_PI) a -= 2.0f * M_PI; while (a < -M_PI) a += 2.0f * M_PI; return a; @@ -450,8 +564,10 @@ class Omni { /* 转置得到云台系到底盘系的旋转矩阵 */ float R_gc[3][3]; - for (int i = 0; i < 3; i++) { - for (int j = 0; j < 3; j++) { + for (int i = 0; i < 3; i++) + { + for (int j = 0; j < 3; j++) + { R_gc[i][j] = R_cg[j][i]; } } @@ -459,10 +575,12 @@ class Omni { /* 合成世界系到底盘系的旋转矩阵 */ float R_wc[3][3] = {{0}}; - for (int i = 0; i < 3; i++) { - for (int j = 0; j < 3; j++) { - R_wc[i][j] = R_wg[i][0] * R_gc[0][j] + R_wg[i][1] * R_gc[1][j] + - R_wg[i][2] * R_gc[2][j]; + for (int i = 0; i < 3; i++) + { + for (int j = 0; j < 3; j++) + { + R_wc[i][j] = + R_wg[i][0] * R_gc[0][j] + R_wg[i][1] * R_gc[1][j] + R_wg[i][2] * R_gc[2][j]; } } @@ -484,7 +602,8 @@ class Omni { py = -PARAM.gravity_height * SoftDeadzone(sinf(chassis_pitch_), sin(k)); px = -PARAM.gravity_height * SoftDeadzone(sinf(chassis_roll_), sin(k)); - for (int i = 0; i < 4; i++) { + for (int i = 0; i < 4; i++) + { float dx = post_x_[i] - px; float dy = post_y_[i] - py; length_[i] = sqrtf(dx * dx + dy * dy); @@ -496,23 +615,29 @@ class Omni { baseff_[2] = (-gx_ff_ - gy_ff_) * SQRT2; baseff_[3] = (gx_ff_ - gy_ff_) * SQRT2; - for (int i = 0; i < 4; i++) { + for (int i = 0; i < 4; i++) + { baseff_l_[i] = 1.0f / length_[i]; - torque_n_[i] = baseff_l_[i] / (baseff_l_[0] + baseff_l_[1] + - baseff_l_[2] + baseff_l_[3]); + torque_n_[i] = + baseff_l_[i] / (baseff_l_[0] + baseff_l_[1] + baseff_l_[2] + baseff_l_[3]); baseff_[i] = baseff_[i] * torque_n_[i]; } - for (int i = 0; i < 4; i++) { + for (int i = 0; i < 4; i++) + { torque_ff_[i] = baseff_[i] * PARAM.wheel_radius; } } /** - * @brief 全向轮底盘正运动学解算 - * @details 根据四个全向轮的角速度,解算出底盘当前的运动状态 + * @brief 全向轮底盘正运动学解算。 + * Forward kinematics of the omnidirectional-wheel chassis. + * + * @details 由四个全向轮的角速度解算底盘的运动状态。 + * Solves the chassis motion from the angular velocities of the four wheels. */ - void SelfResolution() { + void SelfResolution() + { const float SQRT2 = 1.41421356237f; now_vx_ = -(motor_feedback_[0].omega / PARAM.reduction_ratio - @@ -535,10 +660,15 @@ class Omni { } /** - * @brief 全向轮底盘逆运动学解算 - * @details 根据目标底盘速度(vx, vy, ω),计算四个全向轮的目标角速度 + * @brief 全向轮底盘逆运动学解算。 + * Inverse kinematics of the omnidirectional-wheel chassis. + * + * @details 由目标底盘速度 (vx, vy, ω) 计算四个全向轮的目标角速度。 + * Computes the target angular velocity of the four wheels from the target + * chassis velocity (vx, vy, ω). */ - void InverseKinematicsSolution() { + void InverseKinematicsSolution() + { const float SQRT1 = 0.70710678118f; target_motor_omega_[0] = (-SQRT1 * target_vx_ - SQRT1 * target_vy_ + @@ -556,21 +686,29 @@ class Omni { } /** - * @brief 计算 PID 输出电流 + * @brief 计算轮速 PID 输出,并与前馈力矩及姿态前馈合成。 + * Compute the wheel-speed PID outputs and combine them with the feedforward + * torque and the attitude feedforward. */ - void CalculateMotorCurrent() { - if (chassis_event_ == ChassisMode::RELAX) { + void CalculateMotorCurrent() + { + if (chassis_event_ == ChassisMode::RELAX) + { LostCtrl(); - } else { + } + else + { /* 轮速 PID 输出 */ - for (int i = 0; i < 4; i++) { + for (int i = 0; i < 4; i++) + { target_motor_current_[i] = pid_wheel_speed_[i].Calculate( - target_motor_omega_[i], - motor_feedback_[i].omega / PARAM.reduction_ratio, dt_); + target_motor_omega_[i], motor_feedback_[i].omega / PARAM.reduction_ratio, + dt_); } /* 合成动力学输出和上坡前馈 */ - for (int i = 0; i < 4; i++) { + for (int i = 0; i < 4; i++) + { output_[i] = target_motor_force_[i] * PARAM.wheel_radius + target_motor_current_[i] + torque_ff_[i]; } @@ -578,10 +716,17 @@ class Omni { } /** - * @brief 功率控制更新 + * @brief 更新功率控制与小陀螺动态缩放。 + * Update power control and the dynamic spin scale. + * + * @details 提交反馈与期望输出,计算功率上限并读取限幅结果。 + * Submits the feedback and the requested outputs, computes the power limit and + * reads the limited result. */ - void PowerControlUpdate() { - for (int i = 0; i < 4; i++) { + void PowerControlUpdate() + { + for (int i = 0; i < 4; i++) + { motor_data_.rotorspeed_rpm_3508[i] = motor_feedback_[i].velocity; motor_data_.output_current_3508[i] = motor_feedback_[i].torque * M3508_NM_TO_LSB_RATIO; @@ -592,36 +737,42 @@ class Omni { power_control_->CalculatePowerControlParam(); float speed_error[4]; - for (int i = 0; i < 4; i++) { - speed_error[i] = target_motor_omega_[i] - - motor_feedback_[i].omega / PARAM.reduction_ratio; + for (int i = 0; i < 4; i++) + { + speed_error[i] = + target_motor_omega_[i] - motor_feedback_[i].omega / PARAM.reduction_ratio; motor_data_.output_current_3508[i] = std::clamp(output_[i] * M3508_NM_TO_LSB_RATIO / PARAM.reduction_ratio, -16384.0f, 16384.0f); } power_control_->SetMotorData3508(motor_data_.output_current_3508, - motor_data_.rotorspeed_rpm_3508, - speed_error); + motor_data_.rotorspeed_rpm_3508, speed_error); auto now_ms = LibXR::Timebase::GetMilliseconds(); bool referee_online = (now_ms - referee_last_rx_time_).ToSecondf() <= 1.0f; bool power_control_online = power_control_->IsOnline(); bool boost_mode = (cmd_data_.self_define == CMD::ChasStat::BOOST); - float max_power = - static_cast(referee_chassis_pack_.rs.chassis_power_limit); - if (!referee_online || max_power <= 1.0f) { + float max_power = static_cast(referee_chassis_pack_.rs.chassis_power_limit); + if (!referee_online || max_power <= 1.0f) + { max_power = OMNI_CHASSIS_MAX_POWER; } - if (power_control_online && boost_mode) { + if (power_control_online && boost_mode) + { float cap_energy = power_control_->GetCapEnergy(); - if (cap_energy > 0.8f) { + if (cap_energy > 0.8f) + { max_power += 300.0f; - } else if (cap_energy > 0.5f) { + } + else if (cap_energy > 0.5f) + { max_power += 200.0f; - } else if (cap_energy > 0.25f) { + } + else if (cap_energy > 0.25f) + { max_power += 100.0f; } } @@ -631,31 +782,31 @@ class Omni { float req_current_abs_sum = 0.0f; float lim_current_abs_sum = 0.0f; - for (int i = 0; i < 4; i++) { + for (int i = 0; i < 4; i++) + { float req_current_abs = fabsf(motor_data_.output_current_3508[i]); - float lim_current_abs = - power_control_data_.is_power_limited - ? fabsf(power_control_data_.new_output_current_3508[i]) - : req_current_abs; + float lim_current_abs = power_control_data_.is_power_limited + ? fabsf(power_control_data_.new_output_current_3508[i]) + : req_current_abs; req_current_abs_sum += req_current_abs; lim_current_abs_sum += lim_current_abs; } float power_limit_ratio = 1.0f; - if (req_current_abs_sum > 1e-3f) { + if (req_current_abs_sum > 1e-3f) + { power_limit_ratio = std::clamp(lim_current_abs_sum / req_current_abs_sum, 0.0f, 1.0f); } float buffer_scale = 1.0f; - if (referee_online) { + if (referee_online) + { float buffer_range = std::max(PARAM.rotor_buffer_high_j - PARAM.rotor_buffer_low_j, 1.0f); - float referee_buffer_j = - static_cast(referee_chassis_pack_.power_buffer); + float referee_buffer_j = static_cast(referee_chassis_pack_.power_buffer); float buffer_norm = std::clamp( - (referee_buffer_j - PARAM.rotor_buffer_low_j) / buffer_range, 0.0f, - 1.0f); + (referee_buffer_j - PARAM.rotor_buffer_low_j) / buffer_range, 0.0f, 1.0f); buffer_scale = PARAM.rotor_omega_min_scale + (1.0f - PARAM.rotor_omega_min_scale) * buffer_norm; } @@ -664,25 +815,29 @@ class Omni { power_control_data_.is_power_limited ? std::clamp(power_limit_ratio, PARAM.rotor_omega_min_scale, 1.0f) : 1.0f; - float target_dynamic_scale = std::clamp(buffer_scale * limit_scale, - PARAM.rotor_omega_min_scale, 1.0f); + float target_dynamic_scale = + std::clamp(buffer_scale * limit_scale, PARAM.rotor_omega_min_scale, 1.0f); float lpf_alpha = std::clamp(PARAM.rotor_scale_lpf_alpha, 0.0f, 1.0f); - rotor_dynamic_scale_ += - (target_dynamic_scale - rotor_dynamic_scale_) * lpf_alpha; + rotor_dynamic_scale_ += (target_dynamic_scale - rotor_dynamic_scale_) * lpf_alpha; rotor_dynamic_scale_ = std::clamp(rotor_dynamic_scale_, PARAM.rotor_omega_min_scale, 1.0f); - if (chassis_event_ != ChassisMode::ROTOR) { + if (chassis_event_ != ChassisMode::ROTOR) + { rotor_dynamic_scale_ = 1.0f; } } /** - * @brief 全向轮底盘逆动力学解算 - * @details - * 通过运动学正解算出底盘现在的运动状态,并与目标状态进行PID控制,获得目标前馈力矩 + * @brief 全向轮底盘逆动力学解算。 + * Inverse dynamics of the omnidirectional-wheel chassis. + * + * @details 以运动学正解得到的当前状态与目标状态做 PID,得到各轮的前馈力。 + * Runs PIDs between the state from the forward kinematics and the target state + * to obtain the feedforward force of each wheel. */ - void DynamicInverseSolution() { + void DynamicInverseSolution() + { const float SQRT2 = 1.41421356237f; float force_x = pid_velocity_x_.Calculate(target_vx_, now_vx_, dt_); @@ -697,37 +852,49 @@ class Omni { } /** - * @brief 全向轮底盘动力学输出 - * @details 限幅并输出四个全向轮的电流控制指令 + * @brief 全向轮底盘动力学输出。 + * Dynamics output of the omnidirectional-wheel chassis. + * + * @details 限幅后向四个全向轮下发力矩指令。 + * Limits the torques and sends them to the four wheels. */ - void OutputToDynamics() { + void OutputToDynamics() + { /* 功率受限时使用限幅后的电流反算输出扭矩 */ - if (power_control_data_.is_power_limited) { - for (int i = 0; i < 4; i++) { - output_[i] = - std::clamp(power_control_data_.new_output_current_3508[i] / - M3508_NM_TO_LSB_RATIO * PARAM.reduction_ratio, - -6.0f, 6.0f); + if (power_control_data_.is_power_limited) + { + for (int i = 0; i < 4; i++) + { + output_[i] = std::clamp(power_control_data_.new_output_current_3508[i] / + M3508_NM_TO_LSB_RATIO * PARAM.reduction_ratio, + -6.0f, 6.0f); } } - if (chassis_event_ == ChassisMode::RELAX) { + if (chassis_event_ == ChassisMode::RELAX) + { LostCtrl(); return; - } else { - for (int i = 0; i < 4; i++) { + } + else + { + for (int i = 0; i < 4; i++) + { motor_cmd_[i].torque = std::clamp(output_[i], -6.0f, 6.0f); } - for (int i = 0; i < 4; i++) { + for (int i = 0; i < 4; i++) + { motor_wheel_[i]->Control(motor_cmd_[i]); } } } /** - * @brief 失去控制时的处理 - * + * @brief 失去控制时使全部电机放松。 + * Relax all motors when control is lost. */ - void LostCtrl() { - for (int i = 0; i < 4; i++) { + void LostCtrl() + { + for (int i = 0; i < 4; i++) + { motor_wheel_[i]->Relax(); } } @@ -753,7 +920,7 @@ class Omni { /* 电容能量填充条内边距和线宽 */ static constexpr uint16_t UI_CAP_FILL_MARGIN = 4; static constexpr uint16_t UI_CAP_FILL_WIDTH = 8; - /* 中间外框位置和尺寸, 当前只绘制外框 */ + /* 中间外框的位置和尺寸 */ static constexpr uint16_t UI_STATUS_BOX_X1 = 780; static constexpr uint16_t UI_STATUS_BOX_Y1 = 360; static constexpr uint16_t UI_STATUS_BOX_X2 = 1200; @@ -780,10 +947,11 @@ class Omni { static constexpr uint32_t UI_AI_TEXT_READD_DIV = 37; static constexpr uint32_t UI_CAP_FILL_REFRESH_DIV = 3; static constexpr uint32_t UI_CAP_FILL_REFRESH_OFFSET = 2; - /* 电容条低频重建周期, 客户端丢图后靠 ADD 补回来 */ + /* 电容条的低频重建周期,周期性发送 ADD 使客户端丢失图形后恢复 */ static constexpr uint32_t UI_CAP_FILL_READD_DIV = 50; - static void ResetModeUILocked(Omni* omni) { + static void ResetModeUILocked(Omni* omni) + { omni->ui_text_initialized_ = false; omni->ui_frame_initialized_ = false; omni->ui_guide_initialized_ = false; @@ -793,8 +961,10 @@ class Omni { omni->ui_refresh_tick_ = 0; } - static const char* GetModeText(ChassisMode mode) { - switch (mode) { + static const char* GetModeText(ChassisMode mode) + { + switch (mode) + { case ChassisMode::RELAX: return "RELX"; case ChassisMode::INDEPENDENT: @@ -808,25 +978,28 @@ class Omni { } } - static void FormatModeText(char (&text)[16], ChassisMode mode) { + static void FormatModeText(char (&text)[16], ChassisMode mode) + { std::snprintf(text, sizeof(text), "%s", GetModeText(mode)); } - static const char* GetAIModeText(bool enabled) { - return enabled ? "AI ON" : "AI OFF"; - } + static const char* GetAIModeText(bool enabled) { return enabled ? "AI ON" : "AI OFF"; } - static Referee::UIColor GetAIModeColor(bool enabled) { + static Referee::UIColor GetAIModeColor(bool enabled) + { return enabled ? Referee::UIColor::UI_COLOR_YELLOW : Referee::UIColor::UI_COLOR_ORANGE; } - static void DrawUI(Omni* omni) { - if (omni->referee_ == nullptr) { + static void DrawUI(Omni* omni) + { + if (omni->referee_ == nullptr) + { return; } const uint16_t ROBOT_ID = omni->referee_->GetRobotID(); - if (ROBOT_ID == 0) { + if (ROBOT_ID == 0) + { return; } const uint16_t CLIENT_ID = omni->referee_->GetClientID(ROBOT_ID); @@ -841,21 +1014,20 @@ class Omni { const bool UI_LAYER_CLEARED = omni->ui_layer_cleared_; const uint32_t UI_TICK = omni->ui_refresh_tick_++; const bool AI_MODE_ENABLED = - omni->cmd_ != nullptr && - omni->cmd_->GetCtrlMode() == CMD::Mode::CMD_AUTO_CTRL; + omni->cmd_ != nullptr && omni->cmd_->GetCtrlMode() == CMD::Mode::CMD_AUTO_CTRL; const bool CAP_ONLINE = omni->power_control_ != nullptr && omni->power_control_->IsOnline(); - const float CAP_ENERGY = omni->power_control_ != nullptr - ? omni->power_control_->GetCapEnergy() - : 0.0f; + const float CAP_ENERGY = + omni->power_control_ != nullptr ? omni->power_control_->GetCapEnergy() : 0.0f; omni->mutex_.Unlock(); Referee::UILayerDelete ui_del{}; - ui_del.delete_type = - static_cast(Referee::UIDeleteType::UI_DELETE_LAYER); + ui_del.delete_type = static_cast(Referee::UIDeleteType::UI_DELETE_LAYER); ui_del.layer = UI_LAYER_CHASSIS; - if (!UI_LAYER_CLEARED) { + if (!UI_LAYER_CLEARED) + { if (omni->referee_->SendUILayerDelete(ROBOT_ID, CLIENT_ID, ui_del) != - LibXR::ErrorCode::OK) { + LibXR::ErrorCode::OK) + { return; } omni->mutex_.Lock(); @@ -864,24 +1036,25 @@ class Omni { return; } - if (!UI_FRAME_INITIALIZED || (UI_TICK % UI_BOX_RESEND_DIV) == 0) { + if (!UI_FRAME_INITIALIZED || (UI_TICK % UI_BOX_RESEND_DIV) == 0) + { Referee::UIFigure2 box_figs{}; /* - * 这里绘制两个外框 + * 绘制两个外框 * CBX 是中间自瞄外框 * CPF 是左侧电容外框 */ omni->referee_->FillRect( box_figs.interaction_figure[0], "CBX", Referee::UIFigureOp::UI_OP_ADD, - UI_LAYER_CHASSIS, Referee::UIColor::UI_COLOR_CYAN, - UI_STATUS_BOX_WIDTH, UI_STATUS_BOX_X1, UI_STATUS_BOX_Y1, - UI_STATUS_BOX_X2, UI_STATUS_BOX_Y2); + UI_LAYER_CHASSIS, Referee::UIColor::UI_COLOR_CYAN, UI_STATUS_BOX_WIDTH, + UI_STATUS_BOX_X1, UI_STATUS_BOX_Y1, UI_STATUS_BOX_X2, UI_STATUS_BOX_Y2); omni->referee_->FillRect( box_figs.interaction_figure[1], "CPF", Referee::UIFigureOp::UI_OP_ADD, UI_LAYER_CHASSIS, Referee::UIColor::UI_COLOR_WHITE, UI_CAP_BOX_WIDTH, UI_CAP_BOX_X1, UI_CAP_BOX_Y1, UI_CAP_BOX_X2, UI_CAP_BOX_Y2); if (omni->referee_->SendUIFigure2(ROBOT_ID, CLIENT_ID, box_figs) == - LibXR::ErrorCode::OK) { + LibXR::ErrorCode::OK) + { omni->mutex_.Lock(); omni->ui_frame_initialized_ = true; omni->ui_cap_fill_initialized_ = false; @@ -890,7 +1063,8 @@ class Omni { return; } - if ((UI_TICK % UI_CAP_FILL_REFRESH_DIV) == UI_CAP_FILL_REFRESH_OFFSET) { + if ((UI_TICK % UI_CAP_FILL_REFRESH_DIV) == UI_CAP_FILL_REFRESH_OFFSET) + { Referee::UIFigure cap_fill_fig{}; const bool REBUILD_CAP_FILL = !UI_CAP_FILL_INITIALIZED || (UI_TICK % UI_CAP_FILL_READD_DIV) == 2; @@ -899,26 +1073,26 @@ class Omni { const uint16_t INNER_Y1 = UI_CAP_BOX_Y1 + UI_CAP_FILL_MARGIN; const uint16_t INNER_Y2 = UI_CAP_BOX_Y2 - UI_CAP_FILL_MARGIN; uint16_t inner_x2 = INNER_X1; - if (CAP_ONLINE) { + if (CAP_ONLINE) + { const float CLAMPED_CAP_ENERGY = std::clamp(CAP_ENERGY, 0.0f, 1.0f); - const float INNER_WIDTH = static_cast( - (UI_CAP_BOX_X2 - UI_CAP_BOX_X1) - (UI_CAP_FILL_MARGIN * 2)); - inner_x2 = static_cast( - INNER_X1 + std::lround(INNER_WIDTH * CLAMPED_CAP_ENERGY)); - inner_x2 = std::clamp( - inner_x2, INNER_X1, - static_cast(UI_CAP_BOX_X2 - UI_CAP_FILL_MARGIN)); + const float INNER_WIDTH = static_cast((UI_CAP_BOX_X2 - UI_CAP_BOX_X1) - + (UI_CAP_FILL_MARGIN * 2)); + inner_x2 = static_cast(INNER_X1 + + std::lround(INNER_WIDTH * CLAMPED_CAP_ENERGY)); + inner_x2 = std::clamp(inner_x2, INNER_X1, + static_cast(UI_CAP_BOX_X2 - UI_CAP_FILL_MARGIN)); } - omni->referee_->FillRect( - cap_fill_fig, "CPI", - REBUILD_CAP_FILL ? Referee::UIFigureOp::UI_OP_ADD - : Referee::UIFigureOp::UI_OP_MODIFY, - UI_LAYER_CHASSIS, - CAP_ONLINE ? Referee::UIColor::UI_COLOR_WHITE - : Referee::UIColor::UI_COLOR_BLACK, - UI_CAP_FILL_WIDTH, INNER_X1, INNER_Y1, inner_x2, INNER_Y2); + omni->referee_->FillRect(cap_fill_fig, "CPI", + REBUILD_CAP_FILL ? Referee::UIFigureOp::UI_OP_ADD + : Referee::UIFigureOp::UI_OP_MODIFY, + UI_LAYER_CHASSIS, + CAP_ONLINE ? Referee::UIColor::UI_COLOR_WHITE + : Referee::UIColor::UI_COLOR_BLACK, + UI_CAP_FILL_WIDTH, INNER_X1, INNER_Y1, inner_x2, INNER_Y2); if (omni->referee_->SendUIFigure(ROBOT_ID, CLIENT_ID, cap_fill_fig) == - LibXR::ErrorCode::OK) { + LibXR::ErrorCode::OK) + { omni->mutex_.Lock(); omni->ui_cap_fill_initialized_ = true; omni->mutex_.Unlock(); @@ -926,24 +1100,23 @@ class Omni { return; } - if (!UI_GUIDE_INITIALIZED || - (UI_TICK % UI_BOX_RESEND_DIV) == UI_GUIDE_RESEND_OFFSET) { + if (!UI_GUIDE_INITIALIZED || (UI_TICK % UI_BOX_RESEND_DIV) == UI_GUIDE_RESEND_OFFSET) + { Referee::UIFigure2 guide_figs{}; /* 左右两侧的引导斜线 */ omni->referee_->FillLine(guide_figs.interaction_figure[0], "GLF", Referee::UIFigureOp::UI_OP_ADD, UI_LAYER_CHASSIS, - Referee::UIColor::UI_COLOR_GREEN, - UI_STATUS_GUIDE_WIDTH, UI_STATUS_GUIDE_LEFT_X1, - UI_STATUS_GUIDE_LEFT_Y1, UI_STATUS_GUIDE_LEFT_X2, - UI_STATUS_GUIDE_LEFT_Y2); - omni->referee_->FillLine( - guide_figs.interaction_figure[1], "GRI", - Referee::UIFigureOp::UI_OP_ADD, UI_LAYER_CHASSIS, - Referee::UIColor::UI_COLOR_GREEN, UI_STATUS_GUIDE_WIDTH, - UI_STATUS_GUIDE_RIGHT_X1, UI_STATUS_GUIDE_RIGHT_Y1, - UI_STATUS_GUIDE_RIGHT_X2, UI_STATUS_GUIDE_RIGHT_Y2); + Referee::UIColor::UI_COLOR_GREEN, UI_STATUS_GUIDE_WIDTH, + UI_STATUS_GUIDE_LEFT_X1, UI_STATUS_GUIDE_LEFT_Y1, + UI_STATUS_GUIDE_LEFT_X2, UI_STATUS_GUIDE_LEFT_Y2); + omni->referee_->FillLine(guide_figs.interaction_figure[1], "GRI", + Referee::UIFigureOp::UI_OP_ADD, UI_LAYER_CHASSIS, + Referee::UIColor::UI_COLOR_GREEN, UI_STATUS_GUIDE_WIDTH, + UI_STATUS_GUIDE_RIGHT_X1, UI_STATUS_GUIDE_RIGHT_Y1, + UI_STATUS_GUIDE_RIGHT_X2, UI_STATUS_GUIDE_RIGHT_Y2); if (omni->referee_->SendUIFigure2(ROBOT_ID, CLIENT_ID, guide_figs) == - LibXR::ErrorCode::OK) { + LibXR::ErrorCode::OK) + { omni->mutex_.Lock(); omni->ui_guide_initialized_ = true; omni->mutex_.Unlock(); @@ -956,20 +1129,20 @@ class Omni { (UI_AI_TEXT_INITIALIZED && UI_TICK >= UI_AI_TEXT_READD_DIV && (UI_TICK % UI_AI_TEXT_READD_DIV) == UI_AI_TEXT_TICK); const bool UPDATE_AI_TEXT = - REBUILD_AI_TEXT || - (UI_TICK % UI_AI_TEXT_REFRESH_DIV) == UI_AI_TEXT_TICK; - if (UPDATE_AI_TEXT) { + REBUILD_AI_TEXT || (UI_TICK % UI_AI_TEXT_REFRESH_DIV) == UI_AI_TEXT_TICK; + if (UPDATE_AI_TEXT) + { Referee::UICharacter ai_fig{}; /* AI 模式开关状态文字 */ - omni->referee_->FillCharacter( - ai_fig, "AIM", - REBUILD_AI_TEXT ? Referee::UIFigureOp::UI_OP_ADD - : Referee::UIFigureOp::UI_OP_MODIFY, - UI_LAYER_CHASSIS, GetAIModeColor(AI_MODE_ENABLED), UI_FONT_SIZE, - UI_CHAR_WIDTH, UI_AI_TEXT_X, UI_AI_TEXT_Y, - GetAIModeText(AI_MODE_ENABLED)); + omni->referee_->FillCharacter(ai_fig, "AIM", + REBUILD_AI_TEXT ? Referee::UIFigureOp::UI_OP_ADD + : Referee::UIFigureOp::UI_OP_MODIFY, + UI_LAYER_CHASSIS, GetAIModeColor(AI_MODE_ENABLED), + UI_FONT_SIZE, UI_CHAR_WIDTH, UI_AI_TEXT_X, + UI_AI_TEXT_Y, GetAIModeText(AI_MODE_ENABLED)); if (omni->referee_->SendUICharacter(ROBOT_ID, CLIENT_ID, ai_fig) == - LibXR::ErrorCode::OK) { + LibXR::ErrorCode::OK) + { omni->mutex_.Lock(); omni->ui_ai_text_initialized_ = true; omni->mutex_.Unlock(); @@ -983,14 +1156,15 @@ class Omni { const bool REBUILD_MODE_TEXT = !UI_TEXT_INITIALIZED || (UI_TICK % UI_TEXT_READD_DIV) == 4; /* 底盘模式文字本体 */ - omni->referee_->FillCharacter( - mode_fig, "CMT", - REBUILD_MODE_TEXT ? Referee::UIFigureOp::UI_OP_ADD - : Referee::UIFigureOp::UI_OP_MODIFY, - UI_LAYER_CHASSIS, Referee::UIColor::UI_COLOR_PINK, UI_FONT_SIZE, - UI_CHAR_WIDTH, UI_MODE_TEXT_X, UI_MODE_TEXT_Y, mode_text); + omni->referee_->FillCharacter(mode_fig, "CMT", + REBUILD_MODE_TEXT ? Referee::UIFigureOp::UI_OP_ADD + : Referee::UIFigureOp::UI_OP_MODIFY, + UI_LAYER_CHASSIS, Referee::UIColor::UI_COLOR_PINK, + UI_FONT_SIZE, UI_CHAR_WIDTH, UI_MODE_TEXT_X, + UI_MODE_TEXT_Y, mode_text); if (omni->referee_->SendUICharacter(ROBOT_ID, CLIENT_ID, mode_fig) == - LibXR::ErrorCode::OK) { + LibXR::ErrorCode::OK) + { omni->mutex_.Lock(); omni->ui_text_initialized_ = true; omni->mutex_.Unlock(); @@ -1043,8 +1217,7 @@ class Omni { Motor* motor_wheel_2_; Motor* motor_wheel_3_; - Motor* motor_wheel_[4]{motor_wheel_0_, motor_wheel_1_, motor_wheel_2_, - motor_wheel_3_}; + Motor* motor_wheel_[4]{motor_wheel_0_, motor_wheel_1_, motor_wheel_2_, motor_wheel_3_}; Motor::Feedback motor_feedback_[4]{}; Motor::MotorCmd motor_cmd_[4]{}; MotorData motor_data_{}; @@ -1054,23 +1227,21 @@ class Omni { LibXR::PID pid_velocity_y_; LibXR::PID pid_omega_; - LibXR::PID pid_wheel_speed_[4] = { - LibXR::PID(LibXR::PID::Param()), - LibXR::PID(LibXR::PID::Param()), - LibXR::PID(LibXR::PID::Param()), - LibXR::PID(LibXR::PID::Param())}; - LibXR::PID pid_steer_angle_[4] = { - LibXR::PID(LibXR::PID::Param()), - LibXR::PID(LibXR::PID::Param()), - LibXR::PID(LibXR::PID::Param()), - LibXR::PID(LibXR::PID::Param())}; - - LibXR::PID pid_steer_speed_[4] = { - LibXR::PID(LibXR::PID::Param()), - LibXR::PID(LibXR::PID::Param()), - LibXR::PID(LibXR::PID::Param()), - LibXR::PID(LibXR::PID::Param())}; - + LibXR::PID pid_wheel_speed_[4] = {LibXR::PID(LibXR::PID::Param()), + LibXR::PID(LibXR::PID::Param()), + LibXR::PID(LibXR::PID::Param()), + LibXR::PID(LibXR::PID::Param())}; + LibXR::PID pid_steer_angle_[4] = {LibXR::PID(LibXR::PID::Param()), + LibXR::PID(LibXR::PID::Param()), + LibXR::PID(LibXR::PID::Param()), + LibXR::PID(LibXR::PID::Param())}; + + LibXR::PID pid_steer_speed_[4] = {LibXR::PID(LibXR::PID::Param()), + LibXR::PID(LibXR::PID::Param()), + LibXR::PID(LibXR::PID::Param()), + LibXR::PID(LibXR::PID::Param())}; + + ChassisTopicNames topic_names_{}; LibXR::Thread thread_; LibXR::Mutex mutex_; diff --git a/README.md b/README.md index a3365d3..6c0fd88 100644 --- a/README.md +++ b/README.md @@ -1,165 +1,253 @@ # Chassis -## 1. 模块作用 -底盘总控模块。通过模板参数选择 Omni、Mecanum 或 Helm 实现,并统一提供事件接口。 - -## 2. 主要函数说明 -1. Chassis::GetEvent / EventHandler: 对外事件入口与模式切换代理。 -2. Omni/Mecanum/Helm::ThreadFunction: 控制线程主循环。 -3. Update / UpdateCMD / SetMode: 状态更新、命令解析与模式管理。 -4. InverseKinematicsSolution / Helmcontrol / Output: 运动学解算与输出下发。 -5. PowerControlUpdate / LostCtrl: 功率限制与失控保护。 - -## 3. 接入步骤 -1. 添加模块并在 template_args 里选择 ChassisType。 -2. 绑定轮电机、舵向电机、CMD、PowerControl。 -3. 先验证 RELAX/FOLLOW/ROTOR 等模式切换,再联调整车。 - -底盘内部订阅的公共 topic: -- `chassis_cmd`:CMD 发布的底盘控制命令。 -- `chassis_ref`:裁判系统底盘功率数据。 -- `gimbal_euler`:云台 IMU 融合姿态,Omni 底盘用于跟随云台 yaw。 -- `yawmotor_angle`:云台 yaw 电机角度反馈。 - -标准命令流程: - xrobot_add_mod Chassis --instance-id chassis - xrobot_gen_main - cube-cmake --build /home/leo/Documents/bsp-dev-c/build/debug -- - -## 4. 配置示例(YAML) -module: Chassis -entry_header: Modules/Chassis/Chassis.hpp -constructor_args: - - motor_wheel_0: '@&motor_wheel_0' - - motor_wheel_1: '@&motor_wheel_1' - - motor_wheel_2: '@&motor_wheel_2' - - motor_wheel_3: '@&motor_wheel_3' - - motor_steer_0: '@&motor_steer_0' - - motor_steer_1: '@&motor_steer_1' - - motor_steer_2: '@&motor_steer_2' - - motor_steer_3: '@&motor_steer_3' - - cmd: '@&cmd' - - power_control: '@&power_control' - - task_stack_depth: 4096 - - ChassisParam: - wheel_radius: 0.065 - wheel_to_center: 0.26 - gravity_height: 0.0 - reductionratio: 15.7647 - wheel_resistance: 0.0 - error_compensation: 0.0 - gravity: 83.692 - - pid_follow_: - k: 1.0 - p: 20.0 - i: 0.0 - d: 0.0 - i_limit: 0.0 - out_limit: 30.0 - cycle: true - - pid_velocity_x_: - k: 1.0 - p: 20.0 - i: 0.0 - d: 0.0 - i_limit: 0.0 - out_limit: 30.0 - cycle: false - - pid_velocity_y_: - k: 1.0 - p: 20.0 - i: 0.0 - d: 0.0 - i_limit: 0.0 - out_limit: 30.0 - cycle: false - - pid_omega_: - k: 1.0 - p: 20.0 - i: 0.0 - d: 0.0 - i_limit: 0.0 - out_limit: 30.0 - cycle: false - - pid_wheel_speed_0_: - k: 1.0 - p: 0.3 - i: 0.0 - d: 0.0 - i_limit: 0.0 - out_limit: 5.0 - cycle: false - - pid_wheel_speed_1_: - k: 1.0 - p: 0.3 - i: 0.0 - d: 0.0 - i_limit: 0.0 - out_limit: 5.0 - cycle: false - - pid_wheel_speed_2_: - k: 1.0 - p: 0.3 - i: 0.0 - d: 0.0 - i_limit: 0.0 - out_limit: 5.0 - cycle: false - - pid_wheel_speed_3_: - k: 1.0 - p: 0.3 - i: 0.0 - d: 0.0 - i_limit: 0.0 - out_limit: 5.0 - cycle: false - - pid_steer_angle_0_: - k: 1.0 - p: 0.0 - i: 0.0 - d: 0.0 - i_limit: 0.0 - out_limit: 0.0 - cycle: false - - pid_steer_angle_1_: - k: 1.0 - p: 0.0 - i: 0.0 - d: 0.0 - i_limit: 0.0 - out_limit: 0.0 - cycle: false - - pid_steer_angle_2_: - k: 1.0 - p: 0.0 - i: 0.0 - d: 0.0 - i_limit: 0.0 - out_limit: 0.0 - cycle: false - - pid_steer_angle_3_: - k: 1.0 - p: 0.0 - i: 0.0 - d: 0.0 - i_limit: 0.0 - out_limit: 0.0 - cycle: false -template_args: - - ChassisType: Omni - -## 5. 依赖与硬件 -Required Hardware: - - dr16 - - motor - - can - - bmi088 - -Depends: - - qdu-future/BMI088 - - qdu-future/RMMotor - - qdu-future/CMD - - xrobot-org/MadgwickAHRS - -## 6. 代码入口 -Modules/Chassis/Chassis.hpp +底盘控制模块:麦轮、全向轮与舵轮底盘的速度闭环、功率限制与模式切换 / Chassis control Module with velocity loops, power limiting and mode switching for mecanum, omnidirectional and swerve (helm) chassis + +## 1. 模块作用 / Purpose + +`Chassis` 是一个模板外壳:模板参数 `ChassisType` 选择具体的底盘实现(`Mecanum` 麦轮、`Omni` 全向轮、`Helm` 舵轮,分别位于 `Mecanum.hpp`、`Omni.hpp`、`Helm.hpp`),外壳转发构造参数并通过 `GetEvent()` 提供模式切换事件。 + +构造时,所选底盘创建控制线程(`MecanumChassisThread`、`OmniChassisThread` 或 `HelmChassisThread`),栈深为 `task_stack_depth`,优先级为 `param.thread_priority`,周期 2 ms。 + +模式切换:`GetEvent()` 返回的 `LibXR::Event` 注册了 `ChassisType::ChassisMode` 的各个值,激活对应的事件 ID 即切换模式并复位速度相关的 PID,通常由 `EventBinder` 把遥控器事件绑定到这些 ID。`Mecanum` 与 `Omni` 在 CMD 的 `CMD_EVENT_LOST_CTRL` 与 `CMD_EVENT_START_CTRL` 时切换到 `RELAX`。`RELAX` 模式下所有电机执行 `Relax()`。 + +控制链(`Mecanum`、`Omni`):由 `chassis_cmd` 的 x、y、z 得到目标速度,经逆运动学得到各轮目标角速度;轮速 PID 的输出加上速度环与角速度环 PID 得到的前馈力,经 PowerControl 限幅后,以 `MODE_TORQUE` 下发给轮电机,力矩限幅 ±6 N·m。`Helm` 由目标速度解算各轮的舵角与轮速,舵向为角度环加速度环,轮为速度环,同样以 `MODE_TORQUE` 下发,轮力矩限幅 ±4.4 N·m,舵力矩限幅 ±2.5 N·m。 + +功率上限:裁判系统 `chassis_ref` 在 1 s 内有数据且 `chassis_power_limit` 大于 1 W 时使用该值,否则使用默认值(`Mecanum` 70 W,`Omni` 与 `Helm` 100 W)。`chassis_cmd` 的 `self_define` 为 `CMD::ChasStat::BOOST` 且超级电容在线时,按电容能量大于 0.25、0.5、0.8 分别额外增加功率上限:`Mecanum` 与 `Omni` 为 100、200、300 W,`Helm` 为 40、70、100 W。限幅由 `PowerControl` 完成。 + +小陀螺(`ROTOR`)转速缩放(`Mecanum`、`Omni`):平移输入越大,按 `rotor_speed_scale` 降低的转速越多;另按裁判系统缓冲能量(在 `rotor_buffer_low_j` 与 `rotor_buffer_high_j` 之间线性映射)和功率受限程度动态缩放,下限为 `rotor_omega_min_scale`,经系数为 `rotor_scale_lpf_alpha` 的一阶低通。 + +`Chassis` is a template shell: the template parameter `ChassisType` selects the chassis implementation (`Mecanum`, `Omni` or `Helm`, located in `Mecanum.hpp`, `Omni.hpp` and `Helm.hpp`). The shell forwards the constructor parameters and provides the mode-switching events through `GetEvent()`. + +Upon construction, the selected chassis creates its control thread (`MecanumChassisThread`, `OmniChassisThread` or `HelmChassisThread`) with stack depth `task_stack_depth`, priority `param.thread_priority` and a period of 2 ms. + +Mode switching: the `LibXR::Event` returned by `GetEvent()` has every value of `ChassisType::ChassisMode` registered. Activating the corresponding event ID switches the mode and resets the velocity-related PIDs; an `EventBinder` usually binds remote-controller events to these IDs. `Mecanum` and `Omni` switch to `RELAX` on the CMD events `CMD_EVENT_LOST_CTRL` and `CMD_EVENT_START_CTRL`. In `RELAX` mode all motors execute `Relax()`. + +Control chain (`Mecanum`, `Omni`): the x, y and z of `chassis_cmd` give the target velocity, and inverse kinematics gives the target angular velocity of each wheel. The wheel-speed PID output plus the feedforward force from the velocity and angular-velocity PIDs is limited by PowerControl and sent to the wheel motors in `MODE_TORQUE`, with a torque limit of ±6 N·m. `Helm` solves the steering angle and wheel speed of each wheel from the target velocity: steering uses an angle loop plus a speed loop and the wheels use a speed loop. The outputs are also sent in `MODE_TORQUE`, with a torque limit of ±4.4 N·m for the wheels and ±2.5 N·m for the steering motors. + +Power limit: when the referee `chassis_ref` has delivered data within 1 s and `chassis_power_limit` is greater than 1 W, that value is used; otherwise the default applies (`Mecanum` 70 W, `Omni` and `Helm` 100 W). When `self_define` of `chassis_cmd` is `CMD::ChasStat::BOOST` and the supercapacitor is online, the limit is raised for capacitor energy above 0.25, 0.5 and 0.8: by 100, 200 and 300 W for `Mecanum` and `Omni`, and by 40, 70 and 100 W for `Helm`. `PowerControl` performs the limiting. + +Spin (`ROTOR`) speed scaling (`Mecanum`, `Omni`): the larger the translation input, the more the speed is reduced according to `rotor_speed_scale`. The speed is also scaled dynamically by the referee buffer energy (mapped linearly between `rotor_buffer_low_j` and `rotor_buffer_high_j`) and by how strongly the power is limited, with a lower bound of `rotor_omega_min_scale`, and passes a first-order low-pass filter with coefficient `rotor_scale_lpf_alpha`. + +## 2. 底盘类型 / Chassis Types + +| | `Mecanum` | `Omni` | `Helm` | +| --- | --- | --- | --- | +| 模式 `ChassisMode` | `RELAX`、`INDEPENDENT`、`ROTOR`、`FOLLOW`、`TRACK_START` | `RELAX`、`INDEPENDENT`、`ROTOR`、`FOLLOW` | `RELAX`、`INDEPENDENT`、`ROTOR`、`FOLLOW` | +| `motor_steer_0` | 履带电机,可为 `nullptr` | `nullptr` | 舵向电机 0,非空 | +| `motor_steer_1..3` | `nullptr` | `nullptr` | 舵向电机 1..3,非空 | +| 使用的 PID | `pid_follow`、`pid_velocity_x/y`、`pid_omega`、`pid_wheel_speed_0..3`;`pid_steer_angle_0` 为履带速度 PID | `pid_follow`、`pid_velocity_x/y`、`pid_omega`、`pid_wheel_speed_0..3` | `pid_follow`、`pid_wheel_speed_0..3`、`pid_steer_angle_0..3`、`pid_steer_speed_0..3` | +| 使用的 `chassis_param` | `wheel_radius`、`wheel_to_center`、`reduction_ratio`、`rotor_*` | 与 `Mecanum` 相同,另有 `length`、`width`、`gravity_height`、`gravity`(姿态前馈) | `reduction_ratio` | +| 裁判系统 UI | 定时任务每 52 ms 绘制模式字符、辅助线与电容能量条 | 定时任务每 100 ms 绘制模式、AI 状态与电容能量 | 无 | + +`Mecanum` 与 `Omni` 的轮序(x 向右,y 向前):wheel0 左前,wheel1 左后,wheel2 右后,wheel3 右前。`Mecanum` 的 `TRACK_START` 模式由履带(`motor_steer_0`)负责前后,麦轮提供横移与辅助,履带目标速度带斜坡,功率分配时优先分配给履带。`Helm` 的舵向零点取自 `Helm.hpp` 中的 `zero_` 数组。 + +| | `Mecanum` | `Omni` | `Helm` | +| --- | --- | --- | --- | +| Modes `ChassisMode` | `RELAX`, `INDEPENDENT`, `ROTOR`, `FOLLOW`, `TRACK_START` | `RELAX`, `INDEPENDENT`, `ROTOR`, `FOLLOW` | `RELAX`, `INDEPENDENT`, `ROTOR`, `FOLLOW` | +| `motor_steer_0` | Track motor, may be `nullptr` | `nullptr` | Steering motor 0, non-null | +| `motor_steer_1..3` | `nullptr` | `nullptr` | Steering motors 1..3, non-null | +| PIDs used | `pid_follow`, `pid_velocity_x/y`, `pid_omega`, `pid_wheel_speed_0..3`; `pid_steer_angle_0` is the track speed PID | `pid_follow`, `pid_velocity_x/y`, `pid_omega`, `pid_wheel_speed_0..3` | `pid_follow`, `pid_wheel_speed_0..3`, `pid_steer_angle_0..3`, `pid_steer_speed_0..3` | +| `chassis_param` used | `wheel_radius`, `wheel_to_center`, `reduction_ratio`, `rotor_*` | Same as `Mecanum`, plus `length`, `width`, `gravity_height`, `gravity` (attitude feedforward) | `reduction_ratio` | +| Referee UI | Timer task every 52 ms draws the mode text, guide lines and capacitor energy bar | Timer task every 100 ms draws the mode, AI status and capacitor energy | None | + +Wheel numbering of `Mecanum` and `Omni` (x to the right, y forward): wheel0 front left, wheel1 rear left, wheel2 rear right, wheel3 front right. In the `TRACK_START` mode of `Mecanum` the track (`motor_steer_0`) drives forward and backward while the mecanum wheels provide lateral motion and assistance; the track target speed has a ramp, and the power allocation favors the track. The steering zero points of `Helm` come from the `zero_` array in `Helm.hpp`. + +## 3. 构造接口 / Constructor + +```cpp +template +class Chassis; + +Chassis(Motor& motor_wheel_0, Motor& motor_wheel_1, Motor& motor_wheel_2, + Motor& motor_wheel_3, Motor* motor_steer_0, Motor* motor_steer_1, + Motor* motor_steer_2, Motor* motor_steer_3, CMD& cmd, + PowerControl& power_control, Referee& referee, uint32_t task_stack_depth = 1536, + const Param& param = {...}); // 节选 / excerpt +``` + +模板参数: + +- `ChassisType`:`Mecanum`、`Omni` 或 `Helm`,见第 2 节。 + +依赖: + +- `motor_wheel_0..3`:`Motor`,四个驱动轮电机,例如 `RMMotor` 实例。 +- `motor_steer_0..3`:`Motor*`,舵向电机或履带电机,按底盘类型填写,不使用的填 `nullptr`。 +- `cmd`:`CMD` 实例,提供控制命令、控制模式与 CMD 事件。 +- `power_control`:`PowerControl` 实例,提供功率限制与电容能量。 +- `referee`:`Referee` 实例,提供裁判系统数据与 UI 绘制。 + +配置参数: + +- `task_stack_depth`:控制线程栈深,默认 1536。 +- `param.chassis_param`:底盘几何与动力参数,默认值见下,默认为 0 的字段取实车数值: + - `wheel_radius`:轮半径,单位 m; + - `wheel_to_center`:轮心到底盘中心的距离,单位 m; + - `reduction_ratio`:轮电机减速比; + - `wheel_resistance`:轮阻; + - `error_compensation`:误差补偿; + - `gravity_height`:质心高度,单位 m,用于 `Omni` 姿态前馈; + - `gravity`:底盘重力,单位 N,用于 `Omni` 姿态前馈; + - `length`、`width`:底盘轮距的长与宽,单位 m,用于 `Omni` 姿态前馈; + - `rotor_speed_scale`:平移输入下的小陀螺转速缩放,默认 1.0; + - `rotor_omega_min_scale`:小陀螺动态缩放的下限,默认 0.55; + - `rotor_buffer_low_j`、`rotor_buffer_high_j`:缓冲能量的低、高阈值,单位 J,默认 35 与 70; + - `rotor_scale_lpf_alpha`:动态缩放的一阶低通系数,默认 0.2。 +- `param.pid_follow`、`pid_velocity_x`、`pid_velocity_y`、`pid_omega`、`pid_wheel_speed_0..3`、`pid_steer_angle_0..3`、`pid_steer_speed_0..3`:`LibXR::PID::Param`,字段为 `k`、`p`、`i`、`d`、`i_limit`、`out_limit`、`cycle`,默认 `k = 1.0`,其余为 0,`cycle = false`。 +- `param.thread_priority`:控制线程优先级,默认 `LibXR::Thread::Priority::HIGH`。 +- `param.chassis_cmd_topic_name`:订阅的底盘控制命令 Topic 名称,默认 `"chassis_cmd"`。 +- `param.chassis_ref_topic_name`:订阅的裁判系统底盘数据 Topic 名称,默认 `"chassis_ref"`。 +- `param.gimbal_euler_topic_name`:订阅的云台欧拉角 Topic 名称,默认 `"gimbal_euler"`,由 `Omni` 使用。 + +Template parameter: + +- `ChassisType`: `Mecanum`, `Omni` or `Helm`, see section 2. + +Dependencies: + +- `motor_wheel_0..3`: `Motor` objects for the four drive wheels, for example `RMMotor` instances. +- `motor_steer_0..3`: `Motor*`, steering or track motors; filled according to the chassis type, with `nullptr` for the unused ones. +- `cmd`: the `CMD` instance, providing the control command, the control mode and the CMD events. +- `power_control`: the `PowerControl` instance, providing power limiting and capacitor energy. +- `referee`: the `Referee` instance, providing the referee data and the UI drawing. + +Configuration parameters: + +- `task_stack_depth`: control thread stack depth, default 1536. +- `param.chassis_param`: chassis geometry and dynamics parameters, defaults below; fields defaulting to 0 take the values of the machine: + - `wheel_radius`: wheel radius in m; + - `wheel_to_center`: distance from the wheel center to the chassis center in m; + - `reduction_ratio`: reduction ratio of the wheel motors; + - `wheel_resistance`: wheel resistance; + - `error_compensation`: error compensation; + - `gravity_height`: center-of-mass height in m, used by the `Omni` attitude feedforward; + - `gravity`: chassis weight in N, used by the `Omni` attitude feedforward; + - `length`, `width`: length and width of the wheel layout in m, used by the `Omni` attitude feedforward; + - `rotor_speed_scale`: spin speed scale under translation input, default 1.0; + - `rotor_omega_min_scale`: lower bound of the dynamic spin scale, default 0.55; + - `rotor_buffer_low_j`, `rotor_buffer_high_j`: low and high buffer energy thresholds in J, default 35 and 70; + - `rotor_scale_lpf_alpha`: first-order low-pass coefficient of the dynamic scale, default 0.2. +- `param.pid_follow`, `pid_velocity_x`, `pid_velocity_y`, `pid_omega`, `pid_wheel_speed_0..3`, `pid_steer_angle_0..3`, `pid_steer_speed_0..3`: `LibXR::PID::Param` with fields `k`, `p`, `i`, `d`, `i_limit`, `out_limit`, `cycle`; default `k = 1.0`, the others 0 and `cycle = false`. +- `param.thread_priority`: control thread priority, default `LibXR::Thread::Priority::HIGH`. +- `param.chassis_cmd_topic_name`: name of the subscribed chassis command Topic, default `"chassis_cmd"`. +- `param.chassis_ref_topic_name`: name of the subscribed referee chassis data Topic, default `"chassis_ref"`. +- `param.gimbal_euler_topic_name`: name of the subscribed gimbal Euler angle Topic, default `"gimbal_euler"`, used by `Omni`. + +## 4. Topic + +| Topic | 方向 | 类型 | 说明 | +| --- | --- | --- | --- | +| `param.chassis_cmd_topic_name`(默认 `chassis_cmd`) | 订阅 | `CMD::ChassisCMD` | CMD 发布的底盘控制命令 | +| `param.chassis_ref_topic_name`(默认 `chassis_ref`) | 订阅 | `Referee::ChassisPack` | 裁判系统的功率上限与缓冲能量 | +| `yawmotor_angle` | 订阅 | `float` | Gimbal 发布的 yaw 电机相对零点的角度,用于 `FOLLOW` 与坐标变换 | +| `param.gimbal_euler_topic_name`(默认 `gimbal_euler`) | 订阅 | `LibXR::EulerAngle` | `Omni`:云台姿态,用于姿态前馈 | +| `pitchmotor_angle` | 订阅 | `float` | `Omni`:Gimbal 发布的 pitch 电机角度,用于姿态前馈 | +| `delta_yaw` | 发布 | `float` | `Helm`:每个控制周期发布,值为 `yawmotor_angle` 归一化后取反 | + +订阅的 Topic 名称与发布方实例使用的名称相同:`yawmotor_angle` 与 `pitchmotor_angle` 由 Gimbal 以固定名称发布。 + +| Topic | Direction | Type | Meaning | +| --- | --- | --- | --- | +| `param.chassis_cmd_topic_name` (default `chassis_cmd`) | Subscribe | `CMD::ChassisCMD` | Chassis control command published by CMD | +| `param.chassis_ref_topic_name` (default `chassis_ref`) | Subscribe | `Referee::ChassisPack` | Power limit and buffer energy from the referee system | +| `yawmotor_angle` | Subscribe | `float` | Yaw motor angle relative to its zero point, published by Gimbal; used by `FOLLOW` and the coordinate transform | +| `param.gimbal_euler_topic_name` (default `gimbal_euler`) | Subscribe | `LibXR::EulerAngle` | `Omni`: gimbal attitude, used for the attitude feedforward | +| `pitchmotor_angle` | Subscribe | `float` | `Omni`: pitch motor angle published by Gimbal, used for the attitude feedforward | +| `delta_yaw` | Publish | `float` | `Helm`: published every control cycle, the negated, normalized `yawmotor_angle` | + +The subscribed Topic names match the names used by the publishing instances: `yawmotor_angle` and `pitchmotor_angle` are published by Gimbal under these fixed names. + +## 5. 配置示例 / Configuration Example + +`xrobot instance add QDU-Robomaster/Chassis --template-arg Omni` 写入的实例,`template_args` 为底盘类型(本例为 `Omni`),依赖填写为其他模块实例的 id,几何参数与 PID 取自一份全向轮步兵的实机配置: + +An instance written by `xrobot instance add QDU-Robomaster/Chassis --template-arg Omni`, with `template_args` holding the chassis type (`Omni` in this example), the dependencies set to the ids of other Module instances, and the geometry and PID values taken from an omnidirectional-wheel infantry configuration: + +```yaml +modules: + - module: QDU-Robomaster/Chassis + id: chassis + template_args: + - Omni + args: + - motor_wheel_0: motor_wheel_0 + - motor_wheel_1: motor_wheel_1 + - motor_wheel_2: motor_wheel_2 + - motor_wheel_3: motor_wheel_3 + - motor_steer_0: nullptr + - motor_steer_1: nullptr + - motor_steer_2: nullptr + - motor_steer_3: nullptr + - cmd: cmd + - power_control: power_control + - referee: ref + - task_stack_depth: 1536 + - param: + chassis_param: + wheel_radius: 0.065f + wheel_to_center: 0.26f + gravity_height: 0.2f + reduction_ratio: 15.7647f + wheel_resistance: 0.0f + error_compensation: 0.0f + gravity: 230.0f + length: 0.3f + width: 0.3f + rotor_speed_scale: 1.0f + rotor_omega_min_scale: 0.55f + rotor_buffer_low_j: 35.0f + rotor_buffer_high_j: 70.0f + rotor_scale_lpf_alpha: 0.3f + pid_follow: '{.p = 10.0, .cycle = true}' + pid_velocity_x: '{.p = 300.0}' + pid_velocity_y: '{.p = 300.0}' + pid_omega: '{.p = 300.0}' + pid_wheel_speed_0: '{.p = 0.05, .out_limit = 5.0}' + pid_wheel_speed_1: '{.p = 0.05, .out_limit = 5.0}' + pid_wheel_speed_2: '{.p = 0.05, .out_limit = 5.0}' + pid_wheel_speed_3: '{.p = 0.05, .out_limit = 5.0}' + pid_steer_angle_0: '{}' + pid_steer_angle_1: '{}' + pid_steer_angle_2: '{}' + pid_steer_angle_3: '{}' + pid_steer_speed_0: '{}' + pid_steer_speed_1: '{}' + pid_steer_speed_2: '{}' + pid_steer_speed_3: '{}' + thread_priority: LibXR::Thread::Priority::HIGH + chassis_cmd_topic_name: "chassis_cmd" + chassis_ref_topic_name: "chassis_ref" + gimbal_euler_topic_name: "gimbal_euler" +``` + +`motor_wheel_0..3` 取自 `QDU-Robomaster/RMMotor` 实例,`cmd` 取自 `QDU-Robomaster/CMD` 实例,`power_control` 取自 `QDU-Robomaster/PowerControl` 实例,`referee` 取自 `QDU-Robomaster/Referee` 实例,它们须在本实例之前列出。`Omni` 的 `motor_steer_0..3` 为 `nullptr`;带履带的 `Mecanum` 把 `motor_steer_0` 填为履带电机的实例或 `XR_REGISTER` 注册的对象;`Helm` 把 `motor_steer_0..3` 都填为舵向电机。`motor_steer_0..3` 是指针依赖,写成 `'&id'`。PID 为 C++ 表达式,使用指定初始化器,省略的字段取默认值。 + +`motor_wheel_0..3` are taken from `QDU-Robomaster/RMMotor` instances, `cmd` from a `QDU-Robomaster/CMD` instance, `power_control` from a `QDU-Robomaster/PowerControl` instance and `referee` from a `QDU-Robomaster/Referee` instance; they are listed before this instance. For `Omni`, `motor_steer_0..3` are `nullptr`; a `Mecanum` with a track sets `motor_steer_0` to the instance or the `XR_REGISTER` object of the track motor; `Helm` sets all of `motor_steer_0..3` to the steering motors. `motor_steer_0..3` are pointer dependencies written as `'&id'`. The PIDs are C++ expressions written as designated initializers, and omitted fields take their defaults. + +## 6. 依赖与硬件 / Dependencies and Hardware + +依赖: + +- `QDU-Robomaster/Motor`:轮电机与舵向电机的接口。 +- `QDU-Robomaster/RMMotor`:RoboMaster 电机的量程常量。 +- `QDU-Robomaster/CMD`:控制命令类型与 CMD 事件。 +- `QDU-Robomaster/PowerControl`:功率限制与电容能量。 +- `QDU-Robomaster/Referee`:裁判系统数据类型与 UI 绘制。 +- `QDU-Robomaster/SuperPower`:超级电容电源模块,`Mecanum.hpp` 包含其头文件。 +- `xrobot-org/BMI088`、`xrobot-org/MadgwickAHRS`:云台 IMU 与姿态解算,`Omni` 订阅的云台姿态 Topic 由该类实例发布。 +- LibXR。 + +硬件:四个驱动轮电机(功率模型按 M3508 的电流与扭矩比例计算);`Helm` 另有四个舵向电机(按 GM6020 的比例计算),`Mecanum` 可选一个履带电机;超级电容(PowerControl 提供电容能量)与裁判系统;云台提供 `yawmotor_angle`,`Omni` 另需云台姿态与 `pitchmotor_angle`。 + +Dependencies: + +- `QDU-Robomaster/Motor`: interface of the wheel and steering motors. +- `QDU-Robomaster/RMMotor`: range constants of the RoboMaster motors. +- `QDU-Robomaster/CMD`: control command types and CMD events. +- `QDU-Robomaster/PowerControl`: power limiting and capacitor energy. +- `QDU-Robomaster/Referee`: referee data types and UI drawing. +- `QDU-Robomaster/SuperPower`: supercapacitor power Module, whose header is included by `Mecanum.hpp`. +- `xrobot-org/BMI088`, `xrobot-org/MadgwickAHRS`: gimbal IMU and attitude estimation; the gimbal attitude Topic subscribed by `Omni` is published by instances of these Modules. +- LibXR. + +Hardware: four drive-wheel motors (the power model uses the M3508 current-to-torque ratio); `Helm` adds four steering motors (using the GM6020 ratio) and `Mecanum` may add one track motor; a supercapacitor (PowerControl provides the capacitor energy) and the referee system; the gimbal provides `yawmotor_angle`, and `Omni` also needs the gimbal attitude and `pitchmotor_angle`.