From b8aa3f52e30e0fca4fe8ad77d84f7e3a5c2753b9 Mon Sep 17 00:00:00 2001 From: kaMi-0932 <1580654962@qq.com> Date: Tue, 27 Oct 2026 18:26:55 +0800 Subject: [PATCH 1/6] Port projected-torque swerve control --- Chassis.hpp | 67 ++-- Helm.hpp | 843 +++++++++++++++++++++++++++++------------ HelmMigrationRecord.md | 68 ++++ 3 files changed, 713 insertions(+), 265 deletions(-) create mode 100644 HelmMigrationRecord.md diff --git a/Chassis.hpp b/Chassis.hpp index c11f9b5..2890111 100644 --- a/Chassis.hpp +++ b/Chassis.hpp @@ -127,6 +127,8 @@ class Chassis ///< High buffer energy threshold (J) float rotor_scale_lpf_alpha = 0.2f; ///< 动态缩放的一阶低通系数 ///< First-order low-pass coefficient of the dynamic scale + float moment_of_inertia = + 0.0f; ///< Helm yaw inertia (kg*m^2); zero disables yaw feedforward. }; /** @@ -239,34 +241,19 @@ class Chassis .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}), + : 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, MakeChassisParam(param.chassis_param), 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( @@ -313,6 +300,30 @@ class Chassis void EventHandler(uint32_t event_id) { chassis_.SetMode(event_id); } private: + static typename ChassisType::ChassisParam MakeChassisParam(const ChassisParam& param) + { + typename ChassisType::ChassisParam result{ + .wheel_radius = param.wheel_radius, + .wheel_to_center = param.wheel_to_center, + .gravity_height = param.gravity_height, + .reduction_ratio = param.reduction_ratio, + .wheel_resistance = param.wheel_resistance, + .error_compensation = param.error_compensation, + .gravity = param.gravity, + .length = param.length, + .width = param.width, + .rotor_speed_scale = param.rotor_speed_scale, + .rotor_omega_min_scale = param.rotor_omega_min_scale, + .rotor_buffer_low_j = param.rotor_buffer_low_j, + .rotor_buffer_high_j = param.rotor_buffer_high_j, + .rotor_scale_lpf_alpha = param.rotor_scale_lpf_alpha}; + if constexpr (std::is_same_v) + { + result.moment_of_inertia = param.moment_of_inertia; + } + return result; + } + ChassisType chassis_; LibXR::Event chassis_event_; Referee* referee_; diff --git a/Helm.hpp b/Helm.hpp index 93492e0..05f9959 100644 --- a/Helm.hpp +++ b/Helm.hpp @@ -32,133 +32,117 @@ depends: [] #define HELM_CHASSIS_MAX_POWER 100 +/* 固定 RAM 符号供 Ozone Data Sampling 使用,不参与控制反馈。 */ +extern "C" +{ + inline volatile float helm_plot_cmd_x = 0.0f; + inline volatile float helm_plot_cmd_y = 0.0f; + inline volatile float helm_plot_cmd_z = 0.0f; + inline volatile float helm_plot_chassis_mode = 0.0f; + inline volatile float helm_plot_target_vy_mps = 0.0f; + inline volatile float helm_plot_measured_vy_mps = 0.0f; + inline volatile float helm_plot_target_vx_mps = 0.0f; + inline volatile float helm_plot_measured_vx_mps = 0.0f; + inline volatile float helm_plot_omega_reference_radps = 0.0f; + inline volatile float helm_plot_omega_feedback_radps = 0.0f; + inline volatile float helm_plot_omega_control_z = 0.0f; + inline volatile float helm_plot_feedback_valid = 0.0f; + inline volatile float helm_plot_wheel_target_rpm[4] = {}; + inline volatile float helm_plot_wheel_feedback_rpm[4] = {}; + inline volatile float helm_plot_wheel_current_cmd_lsb[4] = {}; + inline volatile float helm_plot_wheel_reverse[4] = {}; + inline volatile float helm_plot_power_limited = 0.0f; + /* 功率模型估算,单位 W;只统计驱动轮,舵向功率显示保持为零。 */ + inline volatile float helm_plot_power_estimated_w = 0.0f; + inline volatile float helm_plot_wheel_power_estimated_w = 0.0f; + inline volatile float helm_plot_steer_power_estimated_w = 0.0f; + inline volatile float helm_plot_power_requested_w = 0.0f; + inline volatile float helm_plot_power_output_estimated_w = 0.0f; + inline volatile float helm_plot_power_estimate_valid = 0.0f; + inline volatile float helm_plot_target_ax_mps2 = 0.0f; + inline volatile float helm_plot_target_ay_mps2 = 0.0f; + inline volatile float helm_plot_target_alpha_radps2 = 0.0f; + inline volatile float helm_plot_wheel_torque_ff_nm[4] = {}; + /* 力控版专用标量,便于排除数组采样地址和两份 ELF 混用的问题。 */ + inline volatile float helm_force_plot_wheel0_rpm = 0.0f; + inline volatile float helm_force_plot_wheel1_rpm = 0.0f; + inline volatile float helm_force_plot_wheel2_rpm = 0.0f; + inline volatile float helm_force_plot_wheel3_rpm = 0.0f; + inline volatile float helm_force_plot_wheel1_angle_rad = 0.0f; + inline volatile float helm_force_plot_wheel1_update_ok = 0.0f; + inline volatile float helm_force_plot_wheel1_torque_ff_nm = 0.0f; + /* pid_nm 是 PID 计算结果;仅前馈输出时它不会被加入最终力矩。 */ + inline volatile float helm_force_plot_wheel1_pid_nm = 0.0f; + inline volatile float helm_force_plot_wheel1_torque_cmd_nm = 0.0f; + inline volatile float helm_force_plot_wheel1_steer_error_rad = 0.0f; + inline volatile uint32_t helm_force_plot_cycle_count = 0U; +} +struct ChassisVelocity +{ + float x = 0.0f; // m/s, +x right + float y = 0.0f; // m/s, +y forward + float omega = 0.0f; // rad/s, counterclockwise positive + bool valid = false; +}; +static ChassisVelocity* HOOK_; + template class Chassis; - -/** - * @brief 舵轮底盘控制实现,由 Chassis 模板使用。 - * Helm (swerve) chassis controller used by the Chassis template. - */ class Helm { public: - /** - * @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 + 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; /* 动态缩放低通系数 */ + /* 绕底盘中心竖直轴的转动惯量 kg*m^2;0 表示停用旋转力矩前馈。 */ + float moment_of_inertia = 0.0f; }; - /** - * @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 + RELAX, + INDEPENDENT, + ROTOR, + FOLLOW, }; /** - * @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. + * @brief 构造函数,初始化舵轮底盘控制对象 + * @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 舵轮底盘参数,包含重力 N 和转动惯量 kg*m^2 + * @param pid_follow 跟随云台角度PID参数 + * @param pid_velocity_x X方向速度PID参数 + * @param pid_velocity_y Y方向速度PID参数 + * @param pid_omega 角速度PID参数 + * @param pid_wheel_speed_0 轮子0速度PID参数 + * @param pid_wheel_speed_1 轮子1速度PID参数 + * @param pid_wheel_speed_2 轮子2速度PID参数 + * @param pid_wheel_speed_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参数 */ 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, @@ -207,13 +191,11 @@ class Helm power_control_(power_control) { topic_names_ = topic_names; - UNUSED(referee); - - /* 舵轮底盘控制线程无条件访问四个舵向电机 */ ASSERT(motor_steer_0 != nullptr); ASSERT(motor_steer_1 != nullptr); ASSERT(motor_steer_2 != nullptr); ASSERT(motor_steer_3 != nullptr); + UNUSED(referee); for (int i = 0; i < 4; i++) { @@ -250,21 +232,16 @@ class Helm } /** - * @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. + * @brief 舵轮底盘控制线程函数 + * @param helm Helm对象指针 + * @details 控制线程主循环,负责接收控制指令、执行运动学解算和动力学控制输出 */ static void ThreadFunction(Helm* helm) { helm->mutex_.Lock(); - LibXR::Topic::ASyncSubscriber cmd_suber(helm->topic_names_.chassis_cmd); + LibXR::Topic::ASyncSubscriber cmd_suber( + helm->topic_names_.chassis_cmd); LibXR::Topic::ASyncSubscriber referee_suber( helm->topic_names_.chassis_ref); LibXR::Topic::ASyncSubscriber yawmotor_angle_suber("yawmotor_angle"); @@ -303,7 +280,12 @@ class Helm helm->Update(); helm->UpdateCMD(); helm->topic_delta_yaw_.Publish(helm->delta_yaw_); - helm->Helmcontrol(); + helm->SelfResolution(); + helm->InverseKinematicsSolution(); + helm->OptimizeWheelAngle(); + helm->CompensateWheelSpeed(); + helm->DynamicInverseSolution(); + helm->CalculateMotorCurrent(); helm->PowerControlUpdate(); helm->mutex_.Unlock(); helm->Output(); @@ -311,12 +293,8 @@ class Helm } } /** - * @brief 更新电机状态。 - * Update the motor states. - * - * @details 记录采样间隔,更新四个驱动轮电机与四个舵向电机并读取反馈。 - * Records the sampling interval, updates the four wheel motors and the four - * steering motors and reads their feedback. + * @brief 更新电机状态 + * @details 获取当前时间戳并更新所有驱动轮电机的状态 */ void Update() { @@ -324,18 +302,28 @@ class Helm dt_ = (now - last_online_time_).ToSecondf(); last_online_time_ = now; + feedback_valid_ = true; for (int i = 0; i < 4; i++) { - motor_wheel_[i]->Update(); - motor_steer_[i]->Update(); + const auto WHEEL_STATUS = motor_wheel_[i]->Update(); + const auto STEER_STATUS = motor_steer_[i]->Update(); motor_wheel_feedback_[i] = motor_wheel_[i]->GetFeedback(); motor_steer_feedback_[i] = motor_steer_[i]->GetFeedback(); + if (i == 1) + { + helm_force_plot_wheel1_update_ok = + WHEEL_STATUS == LibXR::ErrorCode::OK ? 1.0f : 0.0f; + } + feedback_valid_ = feedback_valid_ && WHEEL_STATUS == LibXR::ErrorCode::OK && + STEER_STATUS == LibXR::ErrorCode::OK && + motor_wheel_feedback_[i].state != 0 && + motor_steer_feedback_[i].state != 0; } } /** - * @brief 失去控制时使全部电机放松。 - * Relax all motors when control is lost. + * @brief 失去控制处理 + * */ void LostCtrl() { @@ -347,12 +335,8 @@ class Helm } /** - * @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. + * @brief 设置底盘模式 (由 Chassis 外壳调用) + * @param mode 要设置的新模式 */ void SetMode(uint32_t mode) { @@ -371,28 +355,77 @@ class Helm } /** - * @brief 从 CMD 命令更新目标速度。 - * Update the target velocities from the CMD command. + * @brief 更新底盘控制指令状态 + * @details 将归一化 CMD 输入转换为底盘目标速度,单位为 m/s 和 rad/s */ void UpdateCMD() { - target_vx_ = cmd_data_.x; - target_vy_ = cmd_data_.y; - target_omega_ = cmd_data_.z; + const float MAX_WHEEL_LINEAR_SPEED = GetMaxWheelLinearSpeed(); + const float MAX_CHASSIS_OMEGA = GetMaxChassisOmega(); + + /* 先生成目标角速度。 */ + switch (chassis_event_) + { + case ChassisMode::RELAX: + target_omega_ = 0.0f; + break; + case ChassisMode::INDEPENDENT: + case ChassisMode::ROTOR: + target_omega_ = cmd_data_.z * MAX_CHASSIS_OMEGA; + break; + case ChassisMode::FOLLOW: + target_omega_ = + -pid_follow_.Calculate(0.0f, current_yaw_, dt_) * MAX_CHASSIS_OMEGA; + break; + default: + target_omega_ = 0.0f; + break; + } + /* 记录角速度期望,便于与正解算反馈对比。 */ + helm_plot_omega_reference_radps = target_omega_; + + /* 再生成目标平移速度,保持原有云台坐标变换。 */ + switch (chassis_event_) + { + case ChassisMode::RELAX: + target_vx_ = 0.0f; + target_vy_ = 0.0f; + break; + case ChassisMode::INDEPENDENT: + target_vx_ = cmd_data_.x * MAX_WHEEL_LINEAR_SPEED; + target_vy_ = cmd_data_.y * MAX_WHEEL_LINEAR_SPEED; + break; + case ChassisMode::FOLLOW: + case ChassisMode::ROTOR: + { + const float BETA = current_yaw_; + const float COS_BETA = cosf(BETA); + const float SIN_BETA = sinf(BETA); + target_vx_ = + (COS_BETA * cmd_data_.x - SIN_BETA * cmd_data_.y) * MAX_WHEEL_LINEAR_SPEED; + target_vy_ = + (SIN_BETA * cmd_data_.x + COS_BETA * cmd_data_.y) * MAX_WHEEL_LINEAR_SPEED; + } + break; + default: + target_vx_ = 0.0f; + target_vy_ = 0.0f; + break; + } } /** - * @brief 更新功率控制:提交反馈与期望输出,计算功率上限并读取限幅结果。 - * Update power control: submit the feedback and the requested outputs, compute - * the power limit and read the limited result. + * @brief 功率控制更新 + * */ 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; + motor_data_.output_current_3508[i] = motor_wheel_feedback_[i].torque * + M3508_NM_TO_LSB_RATIO * + motor_wheel_[i]->GetFeedbackTorquePolarity(); motor_data_.rotorspeed_rpm_6020[i] = motor_steer_feedback_[i].velocity; motor_data_.output_current_6020[i] = @@ -408,13 +441,23 @@ class Helm power_control_->CalculatePowerControlParam(); + /* 估算功率时将反馈力矩转换到反馈转速的正方向。 */ + const auto FEEDBACK_POWER = + feedback_valid_ ? power_control_->EstimatePower(motor_data_.output_current_3508, + motor_data_.rotorspeed_rpm_3508) + : PowerControl::PowerEstimate{}; + helm_plot_power_estimated_w = FEEDBACK_POWER.wheel_power_w; + helm_plot_wheel_power_estimated_w = FEEDBACK_POWER.wheel_power_w; + helm_plot_steer_power_estimated_w = FEEDBACK_POWER.steer_power_w; + helm_plot_power_estimate_valid = FEEDBACK_POWER.valid ? 1.0f : 0.0f; + /* 计算 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; - speed_error_3508[i] = target_speed_[i] - actual_speed; + speed_error_3508[i] = target_motor_rpm_[i] - actual_speed; motor_data_.output_current_3508[i] = std::clamp(wheel_out_[i] * M3508_NM_TO_LSB_RATIO / PARAM.reduction_ratio, @@ -465,99 +508,187 @@ class Helm } } + const auto REQUESTED_POWER = + feedback_valid_ ? power_control_->EstimatePower(motor_data_.output_current_3508, + motor_data_.rotorspeed_rpm_3508) + : PowerControl::PowerEstimate{}; + helm_plot_power_requested_w = REQUESTED_POWER.wheel_power_w; + power_control_->OutputLimit(max_power); power_control_data_ = power_control_->GetPowerControlData(); } /** - * @brief 按模式计算目标速度、舵角,以及轮与舵向电机的输出。 - * Compute the target velocity and steering angles for the current mode, and the - * outputs of the wheel and steering motors. + * @brief 舵轮底盘逆动力学解算 + * @details 车体速度 PID 输出加速度,分配轮端前馈力矩,单位 N*m */ - void Helmcontrol() + void DynamicInverseSolution() { - /* 计算目标平移速度 */ - switch (chassis_event_) + target_ax_ = 0.0f; + target_ay_ = 0.0f; + target_alpha_ = 0.0f; + for (int i = 0; i < 4; i++) { - case (ChassisMode::RELAX): - target_vx_ = 0.0f; - target_vy_ = 0.0f; - break; - case (ChassisMode::INDEPENDENT): + target_motor_torque_ff_[i] = 0.0f; + } + + const float MAX_WHEEL_LINEAR_SPEED = GetMaxWheelLinearSpeed(); + const float MAX_CHASSIS_OMEGA = GetMaxChassisOmega(); + if (chassis_event_ == ChassisMode::RELAX || + + !measured_chassis_velocity_.valid || !std::isfinite(dt_) || + !std::isfinite(target_vx_) || !std::isfinite(target_vy_) || + !std::isfinite(target_omega_) || dt_ <= 0.0f || + !std::isfinite(MAX_WHEEL_LINEAR_SPEED) || MAX_WHEEL_LINEAR_SPEED <= 0.0f || + !std::isfinite(MAX_CHASSIS_OMEGA) || MAX_CHASSIS_OMEGA <= 0.0f || + !std::isfinite(PARAM.gravity) || PARAM.gravity <= 0.0f || + !std::isfinite(PARAM.moment_of_inertia) || PARAM.moment_of_inertia < 0.0f) + { + pid_velocity_x_.Reset(); + pid_velocity_y_.Reset(); + pid_omega_.Reset(); + return; + } + + /* 零速目标时仍闭环底盘速度,以模型力矩制动残余运动。 */ + target_ax_ = pid_velocity_x_.Calculate(target_vx_, measured_chassis_velocity_.x, dt_); + target_ay_ = pid_velocity_y_.Calculate(target_vy_, measured_chassis_velocity_.y, dt_); + if (PARAM.moment_of_inertia > 0.0f) + { + target_alpha_ = + pid_omega_.Calculate(target_omega_, measured_chassis_velocity_.omega, dt_); + } + else + { + pid_omega_.Reset(); + } + if (!std::isfinite(target_ax_) || !std::isfinite(target_ay_) || + !std::isfinite(target_alpha_)) + { + target_ax_ = 0.0f; + target_ay_ = 0.0f; + target_alpha_ = 0.0f; + pid_velocity_x_.Reset(); + pid_velocity_y_.Reset(); + pid_omega_.Reset(); + return; + } + + constexpr float GRAVITY_ACCELERATION = 9.80665f; + const float MASS = PARAM.gravity / GRAVITY_ACCELERATION; + float sum_radius_squared = 0.0f; + for (int i = 0; i < 4; i++) + { + const WheelPosition WHEEL_POSITION = GetWheelPosition(static_cast(i)); + sum_radius_squared += + WHEEL_POSITION.x * WHEEL_POSITION.x + WHEEL_POSITION.y * WHEEL_POSITION.y; + } + if (!std::isfinite(sum_radius_squared) || sum_radius_squared <= 1.0e-12f) + { + return; + } + + /* 对称四轮的最小范数分配: + * Fx_i = m*ax/4 - J*alpha*y_i/S + * Fy_i = m*ay/4 + J*alpha*x_i/S,S = sum(x_i^2+y_i^2)。 */ + const float TRANSLATION_FORCE_X = MASS * target_ax_ / 4.0f; + const float TRANSLATION_FORCE_Y = MASS * target_ay_ / 4.0f; + const float ROTATION_FORCE_SCALE = + PARAM.moment_of_inertia * target_alpha_ / sum_radius_squared; + const bool ZERO_CHASSIS_COMMAND = fabsf(target_vx_) <= 1.0e-6f && + fabsf(target_vy_) <= 1.0e-6f && + fabsf(target_omega_) <= 1.0e-6f; + for (int i = 0; i < 4; i++) + { + /* 停车时舵角虽锁存,仍需在该实际方向施加制动力矩。 */ + if (steer_angle_held_[i] && !ZERO_CHASSIS_COMMAND) { - target_vx_ = cmd_data_.x; - target_vy_ = cmd_data_.y; + continue; } - break; - case (ChassisMode::FOLLOW): - case (ChassisMode::ROTOR): + const WheelPosition WHEEL_POSITION = GetWheelPosition(static_cast(i)); + const float STEER_ANGLE = + LibXR::CycleValue(motor_steer_feedback_[i].abs_angle - zero_[i]); + const float FORCE_X = TRANSLATION_FORCE_X - ROTATION_FORCE_SCALE * WHEEL_POSITION.y; + const float FORCE_Y = TRANSLATION_FORCE_Y + ROTATION_FORCE_SCALE * WHEEL_POSITION.x; + /* 实际舵向的驱动正方向为 (sin(a), cos(a))。 + * 投影已包含方向,不再按 motor_reverse 取反或重复乘 cos^3。 */ + const float TORQUE_FF = PARAM.wheel_radius * + (FORCE_X * sinf(STEER_ANGLE) + FORCE_Y * cosf(STEER_ANGLE)); + if (std::isfinite(TORQUE_FF)) { - 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; + target_motor_torque_ff_[i] = TORQUE_FF; } - break; - - default: - target_vx_ = 0.0f; - target_vy_ = 0.0f; - break; } + } - const float SQRT2 = 1.41421356237f; - /* 计算 wz */ - switch (chassis_event_) + /** + * @brief 舵轮底盘逆运动学解算 + * @details 根据底盘目标速度计算各轮目标舵角与电机转子转速 + */ + void InverseKinematicsSolution() + { + const float MAX_WHEEL_LINEAR_SPEED = GetMaxWheelLinearSpeed(); + if (MAX_WHEEL_LINEAR_SPEED <= 1.0e-6f) { - case (ChassisMode::RELAX): - target_omega_ = 0.0f; - break; - case (ChassisMode::INDEPENDENT): - /* 独立模式每个轮子的方向相同,wz当作轮子转向角速度 */ - target_omega_ = -cmd_data_.z; - break; - case (ChassisMode::FOLLOW): - target_omega_ = pid_follow_.Calculate(0.0f, current_yaw_, dt_); - break; - case (ChassisMode::ROTOR): - /* 陀螺模式底盘以一定速度旋转 */ - target_omega_ = -cmd_data_.z; - break; - default: - target_omega_ = 0.0f; - break; + for (int i = 0; i < 4; i++) + { + target_motor_rpm_[i] = 0.0f; + target_steer_angle_[i] = 0.0f; + steer_angle_held_[i] = false; + } + return; } + const float MOTOR_RPM_PER_MPS = motor_max_speed_ / MAX_WHEEL_LINEAR_SPEED; switch (chassis_event_) { case (ChassisMode::RELAX): for (int i = 0; i < 4; i++) { - target_speed_[i] = 0.0f; - target_angle_[i] = 0.0; + target_motor_rpm_[i] = 0.0f; + target_steer_angle_[i] = 0.0f; + steer_angle_held_[i] = false; } break; case (ChassisMode::INDEPENDENT): case (ChassisMode::FOLLOW): case (ChassisMode::ROTOR): { - float x = 0, y = 0, wheel_pos = 0; + float max_target_speed = 0.0f; 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) + const WheelPosition WHEEL_POSITION = GetWheelPosition(static_cast(i)); + const float WHEEL_VX = target_vx_ - target_omega_ * WHEEL_POSITION.y; + const float WHEEL_VY = target_vy_ + target_omega_ * WHEEL_POSITION.x; + const float WHEEL_VECTOR_SPEED = + sqrtf(WHEEL_VX * WHEEL_VX + WHEEL_VY * WHEEL_VY); + if (WHEEL_VECTOR_SPEED <= 1.0e-6f) { - target_angle_[i] = wheel_pos; - target_speed_[i] = 0.0f; + /* 仅轮速度近零时锁存舵角,避免零向量的目标舵角无定义。 */ + if (!steer_angle_held_[i]) + { + target_steer_angle_[i] = + LibXR::CycleValue(motor_steer_feedback_[i].abs_angle - zero_[i]); + steer_angle_held_[i] = true; + } + target_motor_rpm_[i] = 0.0f; } else { - target_angle_[i] = M_PI_2 - atan2f(y, x); - target_speed_[i] = motor_max_speed_ * (fmaxf(fabsf(x), fabsf(y))); + steer_angle_held_[i] = false; + /* 沿用原有舵角零位:+y 为 0,正角朝 +x。 */ + target_steer_angle_[i] = + static_cast(M_PI_2) - atan2f(WHEEL_VY, WHEEL_VX); + target_motor_rpm_[i] = WHEEL_VECTOR_SPEED * MOTOR_RPM_PER_MPS; + max_target_speed = fmaxf(max_target_speed, target_motor_rpm_[i]); + } + } + /* 按最大轮速同比例缩放,保留四轮速度比例并限制至 RPM 上限。 */ + if (max_target_speed > motor_max_speed_) + { + const float SPEED_SCALE = motor_max_speed_ / max_target_speed; + for (int i = 0; i < 4; i++) + { + target_motor_rpm_[i] *= SPEED_SCALE; } } } @@ -565,17 +696,30 @@ class Helm default: for (int i = 0; i < 4; i++) { - target_speed_[i] = 0.0f; - target_angle_[i] = 0.0; + target_motor_rpm_[i] = 0.0f; + target_steer_angle_[i] = 0.0f; + steer_angle_held_[i] = false; } break; } + } - /* 舵向差超过 90 度时反转轮电机, 减少舵向转动 */ + /** + * @brief 最优舵角选择 + * @details 舵向差超过 90 度时反转驱动轮,减少舵向转动 + */ + void OptimizeWheelAngle() + { for (int i = 0; i < 4; i++) { + if (steer_angle_held_[i]) + { + /* 静止时保持锁存的物理舵角,不再做 180 度等价翻转。 */ + motor_reverse_[i] = false; + continue; + } if (fabs(LibXR::CycleValue(motor_steer_feedback_[i].abs_angle - zero_[i]) - - target_angle_[i]) > M_PI_2) + target_steer_angle_[i]) > M_PI_2) { motor_reverse_[i] = true; } @@ -584,17 +728,69 @@ class Helm motor_reverse_[i] = false; } } + } - /* 根据反转状态计算轮电机和舵向电机输出 */ + /** + * @brief 舵向误差对应的驱动轮速度补偿 + */ + void CompensateWheelSpeed() + { + /* 最优角确定后按舵向误差抑制轮速,减少转舵时的侧向拖拽。 */ for (int i = 0; i < 4; i++) { + const LibXR::CycleValue CURRENT_ANGLE(motor_steer_feedback_[i].abs_angle - + zero_[i]); + const LibXR::CycleValue OPTIMAL_ANGLE( + target_steer_angle_[i] + (motor_reverse_[i] ? static_cast(M_PI) : 0.0f)); + const float STEER_ERROR = OPTIMAL_ANGLE - CURRENT_ANGLE; + const float ALIGNMENT = fmaxf(0.0f, cosf(STEER_ERROR)); + target_motor_rpm_[i] *= ALIGNMENT * ALIGNMENT * ALIGNMENT; + } + } + + /** + * @brief 计算舵向和驱动轮 PID 输出 + */ + void CalculateMotorCurrent() + { + /* 轮速 PID 跟踪观测底盘速度在当前实际舵向上的投影。 */ + const float MAX_WHEEL_LINEAR_SPEED = GetMaxWheelLinearSpeed(); + const float MOTOR_RPM_PER_MPS = measured_chassis_velocity_.valid && + std::isfinite(MAX_WHEEL_LINEAR_SPEED) && + MAX_WHEEL_LINEAR_SPEED > 1.0e-6f + ? motor_max_speed_ / MAX_WHEEL_LINEAR_SPEED + : 0.0f; + /* 轮速目标的符号由实际舵向决定,反转状态只影响舵向控制。 */ + for (int i = 0; i < 4; i++) + { + float observed_motor_rpm = 0.0f; + if (MOTOR_RPM_PER_MPS > 0.0f) + { + const WheelPosition WHEEL_POSITION = GetWheelPosition(static_cast(i)); + const float STEER_ANGLE = + LibXR::CycleValue(motor_steer_feedback_[i].abs_angle - zero_[i]); + const float WHEEL_VX = measured_chassis_velocity_.x - + measured_chassis_velocity_.omega * WHEEL_POSITION.y; + const float WHEEL_VY = measured_chassis_velocity_.y + + measured_chassis_velocity_.omega * WHEEL_POSITION.x; + observed_motor_rpm = + (WHEEL_VX * sinf(STEER_ANGLE) + WHEEL_VY * cosf(STEER_ANGLE)) * + MOTOR_RPM_PER_MPS; + if (!std::isfinite(observed_motor_rpm)) + { + observed_motor_rpm = 0.0f; + } + } + wheel_out_[i] = pid_wheel_speed_[i].Calculate( + observed_motor_rpm, motor_wheel_feedback_[i].velocity, dt_); + if (i == 1) + { + helm_force_plot_wheel1_pid_nm = wheel_out_[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) + + LibXR::CycleValue(target_steer_angle_[i] + static_cast(M_PI) + zero_[i]), motor_steer_feedback_[i].abs_angle, dt_); steer_out_[i] = pid_steer_speed_[i].Calculate( @@ -604,34 +800,32 @@ class Helm } 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_steer_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, dt_); } + /* 与轮速 PID 同为减速器输出轴扭矩 N*m,交给现有功率控制限幅。 */ + wheel_out_[i] += target_motor_torque_ff_[i]; } } /** - * @brief 向电机下发控制命令;RELAX 模式下使全部电机放松。 - * Send the control commands to the motors; all motors are relaxed in RELAX mode. + * @brief 输出控制命令到电机 + * */ void Output() { if (power_control_data_.is_power_limited) { + /* 只替换驱动轮输出,舵向力矩仍使用舵向 PID 的结果。 */ 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)); } } @@ -652,25 +846,198 @@ class Helm motor_steer_[i]->Control(motor_steer_cmd_[i]); } } + + /* 按最终力矩限幅和 RELAX 状态,估算下发电流对应的功率。 */ + MotorData output_motor_data{}; + for (int i = 0; i < 4; i++) + { + output_motor_data.rotorspeed_rpm_3508[i] = motor_wheel_feedback_[i].velocity; + output_motor_data.rotorspeed_rpm_6020[i] = motor_steer_feedback_[i].velocity; + if (chassis_event_ != ChassisMode::RELAX && PARAM.reduction_ratio > 1.0e-6f) + { + output_motor_data.output_current_3508[i] = std::clamp( + motor_wheel_cmd_[i].torque * M3508_NM_TO_LSB_RATIO / PARAM.reduction_ratio, + -16384.0f, 16384.0f); + output_motor_data.output_current_6020[i] = std::clamp( + motor_steer_cmd_[i].torque * GM6020_NM_TO_LSB_RATIO, -16384.0f, 16384.0f); + } + } + const auto OUTPUT_POWER = feedback_valid_ ? power_control_->EstimatePower( + output_motor_data.output_current_3508, + output_motor_data.rotorspeed_rpm_3508) + : PowerControl::PowerEstimate{}; + helm_plot_power_output_estimated_w = OUTPUT_POWER.wheel_power_w; + + helm_plot_cmd_x = cmd_data_.x; + helm_plot_cmd_y = cmd_data_.y; + helm_plot_cmd_z = cmd_data_.z; + helm_plot_chassis_mode = static_cast(static_cast(chassis_event_)); + helm_plot_target_vy_mps = target_vy_; + helm_plot_measured_vy_mps = measured_chassis_velocity_.y; + helm_plot_target_vx_mps = target_vx_; + helm_plot_measured_vx_mps = measured_chassis_velocity_.x; + helm_plot_omega_feedback_radps = measured_chassis_velocity_.omega; + const float MAX_CHASSIS_OMEGA = GetMaxChassisOmega(); + helm_plot_omega_control_z = + MAX_CHASSIS_OMEGA > 1.0e-6f ? target_omega_ / MAX_CHASSIS_OMEGA : 0.0f; + helm_plot_feedback_valid = measured_chassis_velocity_.valid ? 1.0f : 0.0f; + helm_plot_target_ax_mps2 = target_ax_; + helm_plot_target_ay_mps2 = target_ay_; + helm_plot_target_alpha_radps2 = target_alpha_; + helm_plot_power_limited = power_control_data_.is_power_limited ? 1.0f : 0.0f; + for (int i = 0; i < 4; i++) + { + helm_plot_wheel_target_rpm[i] = + motor_reverse_[i] ? -target_motor_rpm_[i] : target_motor_rpm_[i]; + helm_plot_wheel_feedback_rpm[i] = motor_wheel_feedback_[i].velocity; + helm_plot_wheel_reverse[i] = motor_reverse_[i] ? 1.0f : 0.0f; + helm_plot_wheel_torque_ff_nm[i] = target_motor_torque_ff_[i]; + helm_plot_wheel_current_cmd_lsb[i] = + chassis_event_ == ChassisMode::RELAX || PARAM.reduction_ratio <= 1.0e-6f + ? 0.0f + : std::clamp(motor_wheel_cmd_[i].torque * M3508_NM_TO_LSB_RATIO / + PARAM.reduction_ratio, + -16384.0f, 16384.0f); + } + helm_force_plot_wheel0_rpm = motor_wheel_feedback_[0].velocity; + helm_force_plot_wheel1_rpm = motor_wheel_feedback_[1].velocity; + helm_force_plot_wheel2_rpm = motor_wheel_feedback_[2].velocity; + helm_force_plot_wheel3_rpm = motor_wheel_feedback_[3].velocity; + helm_force_plot_wheel1_angle_rad = motor_wheel_feedback_[1].position; + helm_force_plot_wheel1_torque_ff_nm = target_motor_torque_ff_[1]; + helm_force_plot_wheel1_torque_cmd_nm = + chassis_event_ == ChassisMode::RELAX ? 0.0f : motor_wheel_cmd_[1].torque; + const LibXR::CycleValue WHEEL1_TARGET_ANGLE( + target_steer_angle_[1] + (motor_reverse_[1] ? static_cast(M_PI) : 0.0f)); + const LibXR::CycleValue WHEEL1_CURRENT_ANGLE( + motor_steer_feedback_[1].abs_angle - zero_[1]); + helm_force_plot_wheel1_steer_error_rad = WHEEL1_TARGET_ANGLE - WHEEL1_CURRENT_ANGLE; + helm_force_plot_cycle_count = helm_force_plot_cycle_count + 1U; } private: const ChassisParam PARAM; + struct WheelPosition + { + float x; + float y; + }; + + WheelPosition GetWheelPosition(uint8_t wheel_index) const + { + const float COMPONENT = PARAM.wheel_to_center * 0.70710678118f; + switch (wheel_index) + { + case 0: + return {-COMPONENT, -COMPONENT}; // Left rear, quadrant III + case 1: + return {COMPONENT, -COMPONENT}; // Right rear, quadrant IV + case 2: + return {COMPONENT, COMPONENT}; // Right front, quadrant I + case 3: + return {-COMPONENT, COMPONENT}; // Left front, quadrant II + default: + return {0.0f, 0.0f}; + } + } + + float GetMaxWheelLinearSpeed() const + { + if (PARAM.wheel_radius <= 1.0e-6f || PARAM.reduction_ratio <= 1.0e-6f || + motor_max_speed_ <= 1.0e-6f) + { + return 0.0f; + } + return motor_max_speed_ * static_cast(LibXR::TWO_PI) * PARAM.wheel_radius / + (60.0f * PARAM.reduction_ratio); + } + + float GetMaxChassisOmega() const + { + /* 纯旋转时轮心线速度为 |omega|*R,R 已是轮心到车体中心的距离。 */ + return PARAM.wheel_to_center > 1.0e-6f + ? GetMaxWheelLinearSpeed() / PARAM.wheel_to_center + : 0.0f; + } + + /** + * @brief 舵轮底盘正运动学解算 + * @details 根据四轮转速和舵角解算当前底盘速度 + */ + void SelfResolution() + { + measured_chassis_velocity_ = {}; + if (!feedback_valid_ || PARAM.wheel_to_center <= 0.0f || PARAM.wheel_radius <= 0.0f || + PARAM.reduction_ratio <= 0.0f) + { + return; + } + + const float RPM_TO_LINEAR_SPEED = static_cast(LibXR::TWO_PI) * + PARAM.wheel_radius / + (60.0f * PARAM.reduction_ratio); + float sum_vx = 0.0f; + float sum_vy = 0.0f; + float sum_rotation = 0.0f; + float sum_radius_squared = 0.0f; + for (int i = 0; i < 4; i++) + { + const WheelPosition WHEEL_POSITION = GetWheelPosition(static_cast(i)); + /* 舵向零位沿 +y,正角朝 +x;电机 reverse 已在反馈解码中处理。 */ + const float STEER_ANGLE = + LibXR::CycleValue(motor_steer_feedback_[i].abs_angle - zero_[i]); + const float WHEEL_SPEED = motor_wheel_feedback_[i].velocity * RPM_TO_LINEAR_SPEED; + if (!std::isfinite(STEER_ANGLE) || !std::isfinite(WHEEL_SPEED)) + { + return; + } + const float WHEEL_VX = WHEEL_SPEED * sinf(STEER_ANGLE); + const float WHEEL_VY = WHEEL_SPEED * cosf(STEER_ANGLE); + sum_vx += WHEEL_VX; + sum_vy += WHEEL_VY; + sum_rotation += WHEEL_POSITION.x * WHEEL_VY - WHEEL_POSITION.y * WHEEL_VX; + sum_radius_squared += + WHEEL_POSITION.x * WHEEL_POSITION.x + WHEEL_POSITION.y * WHEEL_POSITION.y; + } + if (sum_radius_squared <= 1.0e-12f) + { + return; + } + measured_chassis_velocity_.x = sum_vx / 4.0f; + measured_chassis_velocity_.y = sum_vy / 4.0f; + measured_chassis_velocity_.omega = sum_rotation / sum_radius_squared; + measured_chassis_velocity_.valid = std::isfinite(measured_chassis_velocity_.x) && + std::isfinite(measured_chassis_velocity_.y) && + std::isfinite(measured_chassis_velocity_.omega); + if (!measured_chassis_velocity_.valid) + { + measured_chassis_velocity_ = {}; + } + HOOK_ = &measured_chassis_velocity_; + } + + ChassisVelocity measured_chassis_velocity_{}; + bool feedback_valid_ = false; float target_vx_ = 0.0f; float target_vy_ = 0.0f; float target_omega_ = 0.0f; + float target_ax_ = 0.0f; /* m/s^2 */ + float target_ay_ = 0.0f; /* m/s^2 */ + float target_alpha_ = 0.0f; /* rad/s^2 */ + float target_motor_torque_ff_[4] = {0.0f, 0.0f, 0.0f, 0.0f}; bool motor_reverse_[4]{false, false, false, false}; 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}; + /* 驱动电机转子目标转速 RPM,舵角以 +y 为零位、单位 rad。 */ + float target_motor_rpm_[4] = {0.0f, 0.0f, 0.0f, 0.0f}; + LibXR::CycleValue target_steer_angle_[4] = {0.0f, 0.0f, 0.0f, 0.0f}; + bool steer_angle_held_[4] = {false, false, false, false}; - /* 轮与舵向电机的输出 */ + /* 输出的电流值 */ 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}; @@ -691,6 +1058,8 @@ class Helm Motor* motor_steer_2_; Motor* motor_steer_3_; + // Wheel order: 0=LB (QIII), 1=RB (QIV), 2=RF (QI), 3=LF (QII). + // Coordinates use +x right and +y forward; indices increase counterclockwise. Motor* motor_wheel_[4] = {motor_wheel_0_, motor_wheel_1_, motor_wheel_2_, motor_wheel_3_}; Motor* motor_steer_[4] = {motor_steer_0_, motor_steer_1_, motor_steer_2_, @@ -717,6 +1086,7 @@ class Helm LibXR::PID(LibXR::PID::Param()), LibXR::PID(LibXR::PID::Param())}; + ChassisTopicNames topic_names_{}; CMD* cmd_; CMD::ChassisCMD cmd_data_; @@ -729,7 +1099,6 @@ 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/HelmMigrationRecord.md b/HelmMigrationRecord.md new file mode 100644 index 0000000..7dc547f --- /dev/null +++ b/HelmMigrationRecord.md @@ -0,0 +1,68 @@ +# Helm migration from luyan2 + +The source is the working tree of `D:/Documents/luyan2`, including its local module +changes. `User/xrobot.yaml` is converted to the current XRobot constructor/YAML +contracts. The older `User/RobotConfig/helm_infantry.yaml` in the source contains +only BlinkLED and is not the product configuration used for this migration. + +## Control and configuration + +- The control thread runs every 2 ms. Commands are normalized inputs; the Helm + controller converts them to m/s and rad/s using wheel radius, reduction ratio, + the 9000 RPM motor limit, and wheel-to-center distance. +- Coordinates are +x right, +y forward, and positive angular velocity + counterclockwise. Wheel indices are 0 left rear, 1 right rear, 2 right front, + 3 left front. Geometry assumes four symmetric 45-degree wheel positions. +- Steering zero is along +y, with positive steering angle toward +x. The existing + four zero offsets are retained. The wheel radius is 0.06 m, wheel-to-center + distance 0.233 m, reduction ratio 14, chassis weight 182.692 N, and yaw inertia + 0.49436791247 kg*m^2, matching the source product. +- The source forward/inverse kinematics, zero-speed steering hold, optimal-angle + reversal, cos-cubed speed compensation, chassis-velocity feedback, and wheel + torque feedforward are retained. Wheel-speed PID uses observed chassis velocity + projected along each actual steering direction, matching the source. +- The left switch selects RELAX / FOLLOW / INDEPENDENT from top to bottom. The + right switch selects gimbal RELAX / COMMON / COMMON, matching the source. +- Existing hardware registrations, CAN receive/transport implementation, other + module constructor interfaces, and configurable Topic names are retained. + +## Drive-only power policy + +Only the four M3508 drive motors enter the model estimate and Helm power budget. +The default product sets `motor_count_6020` to zero. The shared Helm limiter also +passes steering requests through for configurations that still register 6020s; +steering retains its local PID and +/-2.5 N*m output clamp. + +The drive budget is the referee/boost limit minus the 4 W control margin. Steering +and static board losses are not charged against this budget. RLS uses the measured +power minus configured static loss to identify drive losses; this assumes that +the measured power supplied by SuperPower covers the drive branch. Check the +actual power-meter wiring before relying on online identification. + +Raw RMMotor feedback torque is converted to the direction used by feedback RPM +before power estimation and RLS. The limiter keeps current within the original +request magnitude and sign. When a budget is physically infeasible under the +model, it chooses the minimum-power permitted currents rather than claiming the +budget can always be met. Telemetry powers are model estimates, not measurements; +the steering estimate remains zero. + +`moment_of_inertia` defaults to zero in the other product configurations. It is +forwarded only to Helm; Omni and Mecanum keep their current parameter contracts. +Module edits live in the modules' own Git repositories and are not captured by +the parent BSP's lock file until the module commits are published and relocked. + +## Hardware validation still required + +1. With no projectiles and the wheels unloaded, check CAN IDs, wheel ordering, + reverse flags, steering zeros, reduction ratio and feedback units. Confirm + RELAX and loss of remote control remove motor torque. +2. Apply small +y, +x and pure rotation commands. Check wheel angle, drive sign, + and measured chassis velocity; verify FOLLOW decreases the yaw error. +3. Check steering hold at zero input and reduced wheel targets during steering + alignment. Record estimated drive power, actual metered power and limited + current without including steering in the requested budget. +4. Only after those checks, test on the floor at low speed. Observe braking, + mode transitions, feedback validity, current and power before raising limits. + +Compilation and host power-limiter tests do not validate physical direction, +power-meter coverage, PID tuning, or closed-loop stability on the robot. From 5462dc9b56f7620a6662218c54015fa5311fa7f6 Mon Sep 17 00:00:00 2001 From: kaMi-0932 <1580654962@qq.com> Date: Fri, 30 Oct 2026 23:43:30 +0800 Subject: [PATCH 2/6] Simplify Helm power integration and remove sampling globals --- Helm.hpp | 388 ++++++++++++++++------------------------- HelmMigrationRecord.md | 48 ++--- 2 files changed, 181 insertions(+), 255 deletions(-) diff --git a/Helm.hpp b/Helm.hpp index 05f9959..c5f4267 100644 --- a/Helm.hpp +++ b/Helm.hpp @@ -32,51 +32,6 @@ depends: [] #define HELM_CHASSIS_MAX_POWER 100 -/* 固定 RAM 符号供 Ozone Data Sampling 使用,不参与控制反馈。 */ -extern "C" -{ - inline volatile float helm_plot_cmd_x = 0.0f; - inline volatile float helm_plot_cmd_y = 0.0f; - inline volatile float helm_plot_cmd_z = 0.0f; - inline volatile float helm_plot_chassis_mode = 0.0f; - inline volatile float helm_plot_target_vy_mps = 0.0f; - inline volatile float helm_plot_measured_vy_mps = 0.0f; - inline volatile float helm_plot_target_vx_mps = 0.0f; - inline volatile float helm_plot_measured_vx_mps = 0.0f; - inline volatile float helm_plot_omega_reference_radps = 0.0f; - inline volatile float helm_plot_omega_feedback_radps = 0.0f; - inline volatile float helm_plot_omega_control_z = 0.0f; - inline volatile float helm_plot_feedback_valid = 0.0f; - inline volatile float helm_plot_wheel_target_rpm[4] = {}; - inline volatile float helm_plot_wheel_feedback_rpm[4] = {}; - inline volatile float helm_plot_wheel_current_cmd_lsb[4] = {}; - inline volatile float helm_plot_wheel_reverse[4] = {}; - inline volatile float helm_plot_power_limited = 0.0f; - /* 功率模型估算,单位 W;只统计驱动轮,舵向功率显示保持为零。 */ - inline volatile float helm_plot_power_estimated_w = 0.0f; - inline volatile float helm_plot_wheel_power_estimated_w = 0.0f; - inline volatile float helm_plot_steer_power_estimated_w = 0.0f; - inline volatile float helm_plot_power_requested_w = 0.0f; - inline volatile float helm_plot_power_output_estimated_w = 0.0f; - inline volatile float helm_plot_power_estimate_valid = 0.0f; - inline volatile float helm_plot_target_ax_mps2 = 0.0f; - inline volatile float helm_plot_target_ay_mps2 = 0.0f; - inline volatile float helm_plot_target_alpha_radps2 = 0.0f; - inline volatile float helm_plot_wheel_torque_ff_nm[4] = {}; - /* 力控版专用标量,便于排除数组采样地址和两份 ELF 混用的问题。 */ - inline volatile float helm_force_plot_wheel0_rpm = 0.0f; - inline volatile float helm_force_plot_wheel1_rpm = 0.0f; - inline volatile float helm_force_plot_wheel2_rpm = 0.0f; - inline volatile float helm_force_plot_wheel3_rpm = 0.0f; - inline volatile float helm_force_plot_wheel1_angle_rad = 0.0f; - inline volatile float helm_force_plot_wheel1_update_ok = 0.0f; - inline volatile float helm_force_plot_wheel1_torque_ff_nm = 0.0f; - /* pid_nm 是 PID 计算结果;仅前馈输出时它不会被加入最终力矩。 */ - inline volatile float helm_force_plot_wheel1_pid_nm = 0.0f; - inline volatile float helm_force_plot_wheel1_torque_cmd_nm = 0.0f; - inline volatile float helm_force_plot_wheel1_steer_error_rad = 0.0f; - inline volatile uint32_t helm_force_plot_cycle_count = 0U; -} struct ChassisVelocity { float x = 0.0f; // m/s, +x right @@ -84,65 +39,135 @@ struct ChassisVelocity float omega = 0.0f; // rad/s, counterclockwise positive bool valid = false; }; -static ChassisVelocity* HOOK_; template class Chassis; + +/** + * @brief 舵轮底盘控制实现,由 Chassis 模板使用。 + * Helm (swerve) chassis controller used by the Chassis template. + */ class Helm { public: + /** + * @brief 底盘几何、动力与小陀螺缩放参数。 + * Chassis geometry, dynamics and spin-scaling parameters. + */ 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; /* 动态缩放低通系数 */ - /* 绕底盘中心竖直轴的转动惯量 kg*m^2;0 表示停用旋转力矩前馈。 */ - float moment_of_inertia = 0.0f; + 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 + float moment_of_inertia = 0.0f; ///< Yaw inertia (kg*m^2), zero disables feedforward }; + /** + * @brief 底盘模式。 + * Chassis modes. + */ enum class ChassisMode : uint8_t { - RELAX, - INDEPENDENT, - ROTOR, - FOLLOW, + 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 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 舵轮底盘参数,包含重力 N 和转动惯量 kg*m^2 - * @param pid_follow 跟随云台角度PID参数 - * @param pid_velocity_x X方向速度PID参数 - * @param pid_velocity_y Y方向速度PID参数 - * @param pid_omega 角速度PID参数 - * @param pid_wheel_speed_0 轮子0速度PID参数 - * @param pid_wheel_speed_1 轮子1速度PID参数 - * @param pid_wheel_speed_2 轮子2速度PID参数 - * @param pid_wheel_speed_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(Motor* motor_wheel_0, Motor* motor_wheel_1, Motor* motor_wheel_2, Motor* motor_wheel_3, Motor* motor_steer_0, Motor* motor_steer_1, @@ -220,14 +245,27 @@ class Helm thread_.Create(this, ThreadFunction, "HelmChassisThread", task_stack_depth, thread_priority); + auto start_ctrl_callback = LibXR::Callback::Create( + [](bool in_isr, Helm* helm, uint32_t event_id) + { + UNUSED(in_isr); + UNUSED(event_id); + helm->mutex_.Lock(); + helm->chassis_event_ = ChassisMode::RELAX; + helm->mutex_.Unlock(); + }, + this); auto lost_ctrl_callback = LibXR::Callback::Create( [](bool in_isr, Helm* helm, uint32_t event_id) { UNUSED(in_isr); UNUSED(event_id); - helm->LostCtrl(); + helm->mutex_.Lock(); + helm->chassis_event_ = ChassisMode::RELAX; + helm->mutex_.Unlock(); }, this); + cmd_->GetEvent().Register(CMD::CMD_EVENT_START_CTRL, start_ctrl_callback); cmd_->GetEvent().Register(CMD::CMD_EVENT_LOST_CTRL, lost_ctrl_callback); } @@ -309,11 +347,6 @@ class Helm const auto STEER_STATUS = motor_steer_[i]->Update(); motor_wheel_feedback_[i] = motor_wheel_[i]->GetFeedback(); motor_steer_feedback_[i] = motor_steer_[i]->GetFeedback(); - if (i == 1) - { - helm_force_plot_wheel1_update_ok = - WHEEL_STATUS == LibXR::ErrorCode::OK ? 1.0f : 0.0f; - } feedback_valid_ = feedback_valid_ && WHEEL_STATUS == LibXR::ErrorCode::OK && STEER_STATUS == LibXR::ErrorCode::OK && motor_wheel_feedback_[i].state != 0 && @@ -371,7 +404,7 @@ class Helm break; case ChassisMode::INDEPENDENT: case ChassisMode::ROTOR: - target_omega_ = cmd_data_.z * MAX_CHASSIS_OMEGA; + target_omega_ = 0.5 * MAX_CHASSIS_OMEGA; break; case ChassisMode::FOLLOW: target_omega_ = @@ -381,8 +414,6 @@ class Helm target_omega_ = 0.0f; break; } - /* 记录角速度期望,便于与正解算反馈对比。 */ - helm_plot_omega_reference_radps = target_omega_; /* 再生成目标平移速度,保持原有云台坐标变换。 */ switch (chassis_event_) @@ -420,65 +451,35 @@ class Helm */ void PowerControlUpdate() { + /* 仅提交驱动轮数据;PowerControl 的 motor_count_6020 配置为 0。 */ 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 * - motor_wheel_[i]->GetFeedbackTorquePolarity(); - - motor_data_.rotorspeed_rpm_6020[i] = motor_steer_feedback_[i].velocity; - motor_data_.output_current_6020[i] = - motor_steer_feedback_[i].torque * GM6020_NM_TO_LSB_RATIO; + motor_data_.output_current_3508[i] = + motor_wheel_feedback_[i].torque * M3508_NM_TO_LSB_RATIO; } /* 第一次设置: 反馈数据用于 RLS 参数估计 */ power_control_->SetMotorData3508(motor_data_.output_current_3508, motor_data_.rotorspeed_rpm_3508); - power_control_->SetMotorData6020(motor_data_.output_current_6020, - motor_data_.rotorspeed_rpm_6020); - power_control_->CalculatePowerControlParam(); - /* 估算功率时将反馈力矩转换到反馈转速的正方向。 */ - const auto FEEDBACK_POWER = - feedback_valid_ ? power_control_->EstimatePower(motor_data_.output_current_3508, - motor_data_.rotorspeed_rpm_3508) - : PowerControl::PowerEstimate{}; - helm_plot_power_estimated_w = FEEDBACK_POWER.wheel_power_w; - helm_plot_wheel_power_estimated_w = FEEDBACK_POWER.wheel_power_w; - helm_plot_steer_power_estimated_w = FEEDBACK_POWER.steer_power_w; - helm_plot_power_estimate_valid = FEEDBACK_POWER.valid ? 1.0f : 0.0f; - /* 计算 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; - speed_error_3508[i] = target_motor_rpm_[i] - actual_speed; + 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); - } - - /* 计算 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; } power_control_->SetMotorData3508(motor_data_.output_current_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); auto now_ms = LibXR::Timebase::GetMilliseconds(); bool referee_online = (now_ms - referee_last_rx_time_).ToSecondf() <= 1.0f; @@ -508,12 +509,6 @@ class Helm } } - const auto REQUESTED_POWER = - feedback_valid_ ? power_control_->EstimatePower(motor_data_.output_current_3508, - motor_data_.rotorspeed_rpm_3508) - : PowerControl::PowerEstimate{}; - helm_plot_power_requested_w = REQUESTED_POWER.wheel_power_w; - power_control_->OutputLimit(max_power); power_control_data_ = power_control_->GetPowerControlData(); } @@ -632,8 +627,8 @@ class Helm { for (int i = 0; i < 4; i++) { - target_motor_rpm_[i] = 0.0f; - target_steer_angle_[i] = 0.0f; + target_speed_[i] = 0.0f; + target_angle_[i] = 0.0f; steer_angle_held_[i] = false; } return; @@ -644,8 +639,8 @@ class Helm case (ChassisMode::RELAX): for (int i = 0; i < 4; i++) { - target_motor_rpm_[i] = 0.0f; - target_steer_angle_[i] = 0.0f; + target_speed_[i] = 0.0f; + target_angle_[i] = 0.0f; steer_angle_held_[i] = false; } break; @@ -666,20 +661,19 @@ class Helm /* 仅轮速度近零时锁存舵角,避免零向量的目标舵角无定义。 */ if (!steer_angle_held_[i]) { - target_steer_angle_[i] = + target_angle_[i] = LibXR::CycleValue(motor_steer_feedback_[i].abs_angle - zero_[i]); steer_angle_held_[i] = true; } - target_motor_rpm_[i] = 0.0f; + target_speed_[i] = 0.0f; } else { steer_angle_held_[i] = false; /* 沿用原有舵角零位:+y 为 0,正角朝 +x。 */ - target_steer_angle_[i] = - static_cast(M_PI_2) - atan2f(WHEEL_VY, WHEEL_VX); - target_motor_rpm_[i] = WHEEL_VECTOR_SPEED * MOTOR_RPM_PER_MPS; - max_target_speed = fmaxf(max_target_speed, target_motor_rpm_[i]); + target_angle_[i] = static_cast(M_PI_2) - atan2f(WHEEL_VY, WHEEL_VX); + target_speed_[i] = WHEEL_VECTOR_SPEED * MOTOR_RPM_PER_MPS; + max_target_speed = fmaxf(max_target_speed, target_speed_[i]); } } /* 按最大轮速同比例缩放,保留四轮速度比例并限制至 RPM 上限。 */ @@ -688,7 +682,7 @@ class Helm const float SPEED_SCALE = motor_max_speed_ / max_target_speed; for (int i = 0; i < 4; i++) { - target_motor_rpm_[i] *= SPEED_SCALE; + target_speed_[i] *= SPEED_SCALE; } } } @@ -696,8 +690,8 @@ class Helm default: for (int i = 0; i < 4; i++) { - target_motor_rpm_[i] = 0.0f; - target_steer_angle_[i] = 0.0f; + target_speed_[i] = 0.0f; + target_angle_[i] = 0.0f; steer_angle_held_[i] = false; } break; @@ -719,7 +713,7 @@ class Helm continue; } if (fabs(LibXR::CycleValue(motor_steer_feedback_[i].abs_angle - zero_[i]) - - target_steer_angle_[i]) > M_PI_2) + target_angle_[i]) > M_PI_2) { motor_reverse_[i] = true; } @@ -741,10 +735,10 @@ class Helm const LibXR::CycleValue CURRENT_ANGLE(motor_steer_feedback_[i].abs_angle - zero_[i]); const LibXR::CycleValue OPTIMAL_ANGLE( - target_steer_angle_[i] + (motor_reverse_[i] ? static_cast(M_PI) : 0.0f)); + target_angle_[i] + (motor_reverse_[i] ? static_cast(M_PI) : 0.0f)); const float STEER_ERROR = OPTIMAL_ANGLE - CURRENT_ANGLE; const float ALIGNMENT = fmaxf(0.0f, cosf(STEER_ERROR)); - target_motor_rpm_[i] *= ALIGNMENT * ALIGNMENT * ALIGNMENT; + target_speed_[i] *= ALIGNMENT * ALIGNMENT * ALIGNMENT; } } @@ -783,14 +777,10 @@ class Helm } wheel_out_[i] = pid_wheel_speed_[i].Calculate( observed_motor_rpm, motor_wheel_feedback_[i].velocity, dt_); - if (i == 1) - { - helm_force_plot_wheel1_pid_nm = wheel_out_[i]; - } if (motor_reverse_[i]) { steer_angle_[i] = pid_steer_angle_[i].Calculate( - LibXR::CycleValue(target_steer_angle_[i] + static_cast(M_PI) + + 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( @@ -801,7 +791,7 @@ class Helm else { steer_angle_[i] = pid_steer_angle_[i].Calculate( - target_steer_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, @@ -820,7 +810,7 @@ class Helm { if (power_control_data_.is_power_limited) { - /* 只替换驱动轮输出,舵向力矩仍使用舵向 PID 的结果。 */ + /* 功率限制只替换驱动轮输出,舵向保留 PID 输出和自身扭矩限幅。 */ for (int i = 0; i < 4; i++) { wheel_out_[i] = std::clamp(power_control_data_.new_output_current_3508[i] / @@ -846,73 +836,6 @@ class Helm motor_steer_[i]->Control(motor_steer_cmd_[i]); } } - - /* 按最终力矩限幅和 RELAX 状态,估算下发电流对应的功率。 */ - MotorData output_motor_data{}; - for (int i = 0; i < 4; i++) - { - output_motor_data.rotorspeed_rpm_3508[i] = motor_wheel_feedback_[i].velocity; - output_motor_data.rotorspeed_rpm_6020[i] = motor_steer_feedback_[i].velocity; - if (chassis_event_ != ChassisMode::RELAX && PARAM.reduction_ratio > 1.0e-6f) - { - output_motor_data.output_current_3508[i] = std::clamp( - motor_wheel_cmd_[i].torque * M3508_NM_TO_LSB_RATIO / PARAM.reduction_ratio, - -16384.0f, 16384.0f); - output_motor_data.output_current_6020[i] = std::clamp( - motor_steer_cmd_[i].torque * GM6020_NM_TO_LSB_RATIO, -16384.0f, 16384.0f); - } - } - const auto OUTPUT_POWER = feedback_valid_ ? power_control_->EstimatePower( - output_motor_data.output_current_3508, - output_motor_data.rotorspeed_rpm_3508) - : PowerControl::PowerEstimate{}; - helm_plot_power_output_estimated_w = OUTPUT_POWER.wheel_power_w; - - helm_plot_cmd_x = cmd_data_.x; - helm_plot_cmd_y = cmd_data_.y; - helm_plot_cmd_z = cmd_data_.z; - helm_plot_chassis_mode = static_cast(static_cast(chassis_event_)); - helm_plot_target_vy_mps = target_vy_; - helm_plot_measured_vy_mps = measured_chassis_velocity_.y; - helm_plot_target_vx_mps = target_vx_; - helm_plot_measured_vx_mps = measured_chassis_velocity_.x; - helm_plot_omega_feedback_radps = measured_chassis_velocity_.omega; - const float MAX_CHASSIS_OMEGA = GetMaxChassisOmega(); - helm_plot_omega_control_z = - MAX_CHASSIS_OMEGA > 1.0e-6f ? target_omega_ / MAX_CHASSIS_OMEGA : 0.0f; - helm_plot_feedback_valid = measured_chassis_velocity_.valid ? 1.0f : 0.0f; - helm_plot_target_ax_mps2 = target_ax_; - helm_plot_target_ay_mps2 = target_ay_; - helm_plot_target_alpha_radps2 = target_alpha_; - helm_plot_power_limited = power_control_data_.is_power_limited ? 1.0f : 0.0f; - for (int i = 0; i < 4; i++) - { - helm_plot_wheel_target_rpm[i] = - motor_reverse_[i] ? -target_motor_rpm_[i] : target_motor_rpm_[i]; - helm_plot_wheel_feedback_rpm[i] = motor_wheel_feedback_[i].velocity; - helm_plot_wheel_reverse[i] = motor_reverse_[i] ? 1.0f : 0.0f; - helm_plot_wheel_torque_ff_nm[i] = target_motor_torque_ff_[i]; - helm_plot_wheel_current_cmd_lsb[i] = - chassis_event_ == ChassisMode::RELAX || PARAM.reduction_ratio <= 1.0e-6f - ? 0.0f - : std::clamp(motor_wheel_cmd_[i].torque * M3508_NM_TO_LSB_RATIO / - PARAM.reduction_ratio, - -16384.0f, 16384.0f); - } - helm_force_plot_wheel0_rpm = motor_wheel_feedback_[0].velocity; - helm_force_plot_wheel1_rpm = motor_wheel_feedback_[1].velocity; - helm_force_plot_wheel2_rpm = motor_wheel_feedback_[2].velocity; - helm_force_plot_wheel3_rpm = motor_wheel_feedback_[3].velocity; - helm_force_plot_wheel1_angle_rad = motor_wheel_feedback_[1].position; - helm_force_plot_wheel1_torque_ff_nm = target_motor_torque_ff_[1]; - helm_force_plot_wheel1_torque_cmd_nm = - chassis_event_ == ChassisMode::RELAX ? 0.0f : motor_wheel_cmd_[1].torque; - const LibXR::CycleValue WHEEL1_TARGET_ANGLE( - target_steer_angle_[1] + (motor_reverse_[1] ? static_cast(M_PI) : 0.0f)); - const LibXR::CycleValue WHEEL1_CURRENT_ANGLE( - motor_steer_feedback_[1].abs_angle - zero_[1]); - helm_force_plot_wheel1_steer_error_rad = WHEEL1_TARGET_ANGLE - WHEEL1_CURRENT_ANGLE; - helm_force_plot_cycle_count = helm_force_plot_cycle_count + 1U; } private: @@ -1014,7 +937,6 @@ class Helm { measured_chassis_velocity_ = {}; } - HOOK_ = &measured_chassis_velocity_; } ChassisVelocity measured_chassis_velocity_{}; @@ -1033,8 +955,8 @@ class Helm float delta_yaw_ = 0.0f; /* 驱动电机转子目标转速 RPM,舵角以 +y 为零位、单位 rad。 */ - float target_motor_rpm_[4] = {0.0f, 0.0f, 0.0f, 0.0f}; - LibXR::CycleValue target_steer_angle_[4] = {0.0f, 0.0f, 0.0f, 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}; bool steer_angle_held_[4] = {false, false, false, false}; /* 输出的电流值 */ @@ -1044,8 +966,6 @@ class Helm float motor_max_speed_ = 9000.0; - LibXR::CycleValue main_direct_ = 0.0f; - float dt_ = 0; LibXR::MicrosecondTimestamp last_online_time_ = 0; diff --git a/HelmMigrationRecord.md b/HelmMigrationRecord.md index 7dc547f..044f077 100644 --- a/HelmMigrationRecord.md +++ b/HelmMigrationRecord.md @@ -21,30 +21,35 @@ only BlinkLED and is not the product configuration used for this migration. reversal, cos-cubed speed compensation, chassis-velocity feedback, and wheel torque feedforward are retained. Wheel-speed PID uses observed chassis velocity projected along each actual steering direction, matching the source. -- The left switch selects RELAX / FOLLOW / INDEPENDENT from top to bottom. The - right switch selects gimbal RELAX / COMMON / COMMON, matching the source. +- The current default configuration selects RELAX / FOLLOW / ROTOR from top to + bottom on the left switch. The right switch selects gimbal RELAX / COMMON / COMMON. - Existing hardware registrations, CAN receive/transport implementation, other module constructor interfaces, and configurable Topic names are retained. -## Drive-only power policy +## Upstream power policy -Only the four M3508 drive motors enter the model estimate and Helm power budget. -The default product sets `motor_count_6020` to zero. The shared Helm limiter also -passes steering requests through for configurations that still register 6020s; -steering retains its local PID and +/-2.5 N*m output clamp. +PowerControl is restored to QDU-Robomaster/PowerControl at upstream commit +`4097e1f6a3fad89126e82b920217961286966d84`. The Helm power-data submission, RLS +invocation and referee/boost budget selection follow +QDU-Robomaster/Chassis commit `32761d015294031f3d5ac8326b1a394c796302a9`. -The drive budget is the referee/boost limit minus the 4 W control margin. Steering -and static board losses are not charged against this budget. RLS uses the measured -power minus configured static loss to identify drive losses; this assumes that -the measured power supplied by SuperPower covers the drive branch. Check the -actual power-meter wiring before relying on online identification. +Only the four M3508 drive motors enter power limiting. The Helm configurations +set `motor_count_6020` to zero, and Helm submits only M3508 data to PowerControl. +Limited output currents replace only drive-wheel torque requests; steering keeps +its PID output and local +/-2.5 N*m torque clamp. The upstream limiter sums positive +drive model powers, with static chassis loss and the 4 W margin deducted from the +budget. This drive-only policy is the intentional exception to upstream Helm +power integration; the PowerControl module itself is unmodified. -Raw RMMotor feedback torque is converted to the direction used by feedback RPM -before power estimation and RLS. The limiter keeps current within the original -request magnitude and sign. When a budget is physically infeasible under the -model, it chooses the minimum-power permitted currents rather than claiming the -budget can always be met. Telemetry powers are model estimates, not measurements; -the steering estimate remains zero. +Additional Helm power estimates, Ozone sampling globals, and the velocity debug +pointer are removed. Physical chassis-velocity feedback remains part of the +migrated motion controller. The motor target names at the power-control boundary +match upstream: `target_speed_` is motor rotor RPM and `target_angle_` is radians. +RMMotor decodes torque using the same configured direction as velocity, so power +identification consumes aligned feedback directly; the extra torque-polarity +interface is no longer needed. +Confirm the SuperPower protocol and actual power-meter coverage before relying +on online power identification. `moment_of_inertia` defaults to zero in the other product configurations. It is forwarded only to Helm; Omni and Mecanum keep their current parameter contracts. @@ -59,10 +64,11 @@ the parent BSP's lock file until the module commits are published and relocked. 2. Apply small +y, +x and pure rotation commands. Check wheel angle, drive sign, and measured chassis velocity; verify FOLLOW decreases the yaw error. 3. Check steering hold at zero input and reduced wheel targets during steering - alignment. Record estimated drive power, actual metered power and limited - current without including steering in the requested budget. + alignment. Check actual metered drive power and limited drive currents against + the drive budget. Steering power is outside that budget; verify the measurement + branch before using online identification. 4. Only after those checks, test on the floor at low speed. Observe braking, mode transitions, feedback validity, current and power before raising limits. -Compilation and host power-limiter tests do not validate physical direction, +Compilation does not validate physical direction, power-meter coverage, PID tuning, or closed-loop stability on the robot. From 6e34abc92bd01f611dde61216e41f8f5c4e646cd Mon Sep 17 00:00:00 2001 From: kaMi-0932 <1580654962@qq.com> Date: Sun, 1 Nov 2026 22:19:41 +0800 Subject: [PATCH 3/6] Add follow yaw deadband and restore joystick rotation --- Helm.hpp | 19 ++++++++++++++++--- HelmMigrationRecord.md | 11 +++++++++++ 2 files changed, 27 insertions(+), 3 deletions(-) diff --git a/Helm.hpp b/Helm.hpp index c5f4267..edbcab4 100644 --- a/Helm.hpp +++ b/Helm.hpp @@ -404,11 +404,24 @@ class Helm break; case ChassisMode::INDEPENDENT: case ChassisMode::ROTOR: - target_omega_ = 0.5 * MAX_CHASSIS_OMEGA; + target_omega_ = cmd_data_.z * MAX_CHASSIS_OMEGA; break; case ChassisMode::FOLLOW: - target_omega_ = - -pid_follow_.Calculate(0.0f, current_yaw_, dt_) * MAX_CHASSIS_OMEGA; + /* 增加FOLLOW死区,避免不期望抖动 */ + { + const float BETA = LibXR::CycleValue(current_yaw_) - 0.0f; + constexpr float FOLLOW_DEADZONE = static_cast(M_PI) / 90.0f; + if (std::fabs(BETA) < FOLLOW_DEADZONE) + { + target_omega_ = 0.0f; + pid_follow_.Reset(); + } + else + { + target_omega_ = + -pid_follow_.Calculate(0.0f, current_yaw_, dt_) * MAX_CHASSIS_OMEGA; + } + } break; default: target_omega_ = 0.0f; diff --git a/HelmMigrationRecord.md b/HelmMigrationRecord.md index 044f077..eca1d2d 100644 --- a/HelmMigrationRecord.md +++ b/HelmMigrationRecord.md @@ -21,6 +21,11 @@ only BlinkLED and is not the product configuration used for this migration. reversal, cos-cubed speed compensation, chassis-velocity feedback, and wheel torque feedforward are retained. Wheel-speed PID uses observed chassis velocity projected along each actual steering direction, matching the source. +- INDEPENDENT and ROTOR use the joystick z command for target angular velocity. + FOLLOW uses a 2-degree deadband on the signed shortest yaw error: inside the + deadband, target angular velocity is zero and the follow PID is reset. Outside + it, normal following resumes. Kinematics and inverse dynamics share that target; + the deadband has no hysteresis. - The current default configuration selects RELAX / FOLLOW / ROTOR from top to bottom on the left switch. The right switch selects gimbal RELAX / COMMON / COMMON. - Existing hardware registrations, CAN receive/transport implementation, other @@ -56,6 +61,12 @@ forwarded only to Helm; Omni and Mecanum keep their current parameter contracts. Module edits live in the modules' own Git repositories and are not captured by the parent BSP's lock file until the module commits are published and relocked. +## Friction measurement + +The user reports a measured wheel-ground friction coefficient of approximately +0.4. This is a recorded bench observation; no grip or acceleration constraint +has been added to the controller. + ## Hardware validation still required 1. With no projectiles and the wheels unloaded, check CAN IDs, wheel ordering, From afd44c1d44f4d3c0767c230e179ed90b8dce1127 Mon Sep 17 00:00:00 2001 From: kaMi-0932 <1580654962@qq.com> Date: Tue, 3 Nov 2026 19:27:54 +0800 Subject: [PATCH 4/6] Constrain swerve acceleration by signed power and traction --- AccelerationConstraint.hpp | 423 +++++++++++++++++++++++++ Chassis.hpp | 8 + Helm.hpp | 172 +++++++--- HelmMigrationRecord.md | 95 ++++-- tests/acceleration_constraint_test.cpp | 244 ++++++++++++++ tests/helm_constraint_model_test.cpp | 383 ++++++++++++++++++++++ 6 files changed, 1242 insertions(+), 83 deletions(-) create mode 100644 AccelerationConstraint.hpp create mode 100644 tests/acceleration_constraint_test.cpp create mode 100644 tests/helm_constraint_model_test.cpp diff --git a/AccelerationConstraint.hpp b/AccelerationConstraint.hpp new file mode 100644 index 0000000..8498d3a --- /dev/null +++ b/AccelerationConstraint.hpp @@ -0,0 +1,423 @@ +#pragma once + +#include +#include +#include + +/** + * Two-dimensional acceleration allocation, following the document's polygon, + * ellipse-support and edge-intersection construction. Motor speeds and wheel PID + * outputs are frozen in Input. Powers are signed; braking power is not clipped. + * Decision coordinates are normalized acceleration fractions in [0, 1]. + */ +namespace AccelerationConstraint +{ +struct Vec2 +{ + float x = 0.0f; + float y = 0.0f; +}; + +struct MotorModel +{ + float u = 0.0f; ///< Current = u*x + v*y + c, in motor command units. + float v = 0.0f; + float c = 0.0f; + float rpm = 0.0f; + float kt = 0.0f; + float k1 = 0.0f; + float k2 = 0.0f; +}; + +struct HalfPlane +{ + float x = 0.0f; + float y = 0.0f; + float limit = 0.0f; ///< x*decision.x + y*decision.y <= limit. +}; + +struct Input +{ + MotorModel motors[4]{}; + HalfPlane halfplanes[12]{}; + int halfplane_count = 0; + Vec2 weights{1.0f, 1.0f}; + float power_budget = 0.0f; ///< Already excludes static chassis loss and margin. +}; + +enum class Status +{ + OPTIMAL, + INFEASIBLE, + INVALID_INPUT +}; + +struct Result +{ + bool feasible = false; + bool minimum_available = false; + Vec2 point{}; + float power = 0.0f; + Status status = Status::INVALID_INPUT; +}; + +namespace Detail +{ +struct Point +{ + double x; + double y; +}; + +struct Polygon +{ + Point points[20]{}; + int count = 0; +}; + +struct Quadratic +{ + double a = 0.0; + double b = 0.0; // f = a*x*x + 2*b*x*y + c*y*y + d*x + e*y + f. + double c = 0.0; + double d = 0.0; + double e = 0.0; + double f = 0.0; + + double Value(Point p) const + { + return (a * p.x + 2.0 * b * p.y + d) * p.x + (c * p.y + e) * p.y + f; + } +}; + +inline bool AddPoint(Polygon& polygon, Point point) +{ + if (polygon.count > 0) + { + const Point LAST = polygon.points[polygon.count - 1]; + if (std::abs(point.x - LAST.x) + std::abs(point.y - LAST.y) < 1e-12) + { + return true; + } + } + if (polygon.count == 20) + { + return false; + } + polygon.points[polygon.count++] = point; + return true; +} + +inline bool Clip(Polygon& polygon, HalfPlane plane) +{ + Polygon output; + if (polygon.count == 0) + { + return true; + } + Point previous = polygon.points[polygon.count - 1]; + double previous_distance = plane.x * previous.x + plane.y * previous.y - plane.limit; + for (int i = 0; i < polygon.count; ++i) + { + const Point CURRENT = polygon.points[i]; + const double DISTANCE = plane.x * CURRENT.x + plane.y * CURRENT.y - plane.limit; + if ((DISTANCE <= 0.0) != (previous_distance <= 0.0)) + { + const double T = previous_distance / (previous_distance - DISTANCE); + if (!AddPoint(output, {previous.x + T * (CURRENT.x - previous.x), + previous.y + T * (CURRENT.y - previous.y)})) + { + return false; + } + } + if (DISTANCE <= 0.0 && !AddPoint(output, CURRENT)) + { + return false; + } + previous = CURRENT; + previous_distance = DISTANCE; + } + if (output.count > 1) + { + const Point FIRST = output.points[0]; + const Point LAST = output.points[output.count - 1]; + if (std::abs(FIRST.x - LAST.x) + std::abs(FIRST.y - LAST.y) < 1e-12) + { + --output.count; + } + } + polygon = output; + return true; +} + +inline bool Inside(const Input& input, Point point) +{ + constexpr double TOLERANCE = 2e-7; + if (!std::isfinite(point.x) || !std::isfinite(point.y) || point.x < -TOLERANCE || + point.x > 1.0 + TOLERANCE || point.y < -TOLERANCE || point.y > 1.0 + TOLERANCE) + { + return false; + } + for (int i = 0; i < input.halfplane_count; ++i) + { + const auto& PLANE = input.halfplanes[i]; + const double SCALE = + 1.0 + std::abs(PLANE.x) + std::abs(PLANE.y) + std::abs(PLANE.limit); + if (PLANE.x * point.x + PLANE.y * point.y - PLANE.limit > TOLERANCE * SCALE) + { + return false; + } + } + return true; +} + +inline double MotorPower(const Input& input, Point point) +{ + double total = 0.0; + for (const auto& MOTOR : input.motors) + { + const double CURRENT = MOTOR.u * point.x + MOTOR.v * point.y + MOTOR.c; + total += MOTOR.k1 * CURRENT * CURRENT + + static_cast(MOTOR.kt) * MOTOR.rpm * CURRENT + + static_cast(MOTOR.k2) * MOTOR.rpm * MOTOR.rpm; + } + return total; +} + +inline Quadratic MakeQuadratic(const Input& input) +{ + Quadratic result; + for (const auto& MOTOR : input.motors) + { + const double U = MOTOR.u; + const double V = MOTOR.v; + const double C = MOTOR.c; + const double LINEAR = static_cast(MOTOR.kt) * MOTOR.rpm; + result.a += MOTOR.k1 * U * U; + result.b += MOTOR.k1 * U * V; + result.c += MOTOR.k1 * V * V; + result.d += (2.0 * MOTOR.k1 * C + LINEAR) * U; + result.e += (2.0 * MOTOR.k1 * C + LINEAR) * V; + result.f += MOTOR.k1 * C * C + LINEAR * C + + static_cast(MOTOR.k2) * MOTOR.rpm * MOTOR.rpm; + } + return result; +} + +struct Search +{ + const Input& input; + const Quadratic& quadratic; + Point minimum{}; + double minimum_power = std::numeric_limits::infinity(); + Point best{}; + double best_value = -std::numeric_limits::infinity(); + bool found = false; + + void Minimum(Point point) + { + if (!Inside(input, point)) + { + return; + } + const double POWER = MotorPower(input, point); + if (POWER < minimum_power) + { + minimum_power = POWER; + minimum = point; + } + } + + void Candidate(Point point) + { + // Validate the actual float coordinates which the firmware will consume. + point = {static_cast(std::clamp(point.x, 0.0, 1.0)), + static_cast(std::clamp(point.y, 0.0, 1.0))}; + if (!Inside(input, point)) + { + return; + } + const double POWER = MotorPower(input, point); + const double POWER_TOLERANCE = 2e-5 + 2e-7 * std::abs(input.power_budget); + if (!std::isfinite(POWER) || POWER > input.power_budget + POWER_TOLERANCE) + { + return; + } + const double VALUE = input.weights.x * point.x + input.weights.y * point.y; + if (!found || VALUE > best_value + 1e-12 || + (std::abs(VALUE - best_value) <= 1e-12 && point.x + point.y > best.x + best.y)) + { + best = point; + best_value = VALUE; + found = true; + } + } + + void Edge(Point start, Point end) + { + const Point STEP{end.x - start.x, end.y - start.y}; + const double A = quadratic.a * STEP.x * STEP.x + 2.0 * quadratic.b * STEP.x * STEP.y + + quadratic.c * STEP.y * STEP.y; + const double B = 2.0 * quadratic.a * start.x * STEP.x + + 2.0 * quadratic.b * (start.x * STEP.y + start.y * STEP.x) + + 2.0 * quadratic.c * start.y * STEP.y + quadratic.d * STEP.x + + quadratic.e * STEP.y; + const double C = quadratic.Value(start) - input.power_budget; + if (A > 0.0) + { + const double T = std::clamp(-B / (2.0 * A), 0.0, 1.0); + Minimum({start.x + T * STEP.x, start.y + T * STEP.y}); + } + if (std::abs(A) <= 1e-14 * (1.0 + std::abs(B) + std::abs(C))) + { + if (B != 0.0) + { + const double T = -C / B; + if (T >= -1e-10 && T <= 1.0 + 1e-10) + { + Candidate({start.x + std::clamp(T, 0.0, 1.0) * STEP.x, + start.y + std::clamp(T, 0.0, 1.0) * STEP.y}); + } + } + return; + } + double discriminant = B * B - 4.0 * A * C; + const double DISCRIMINANT_TOLERANCE = 1e-12 * (B * B + std::abs(4.0 * A * C) + 1e-30); + if (discriminant < -DISCRIMINANT_TOLERANCE) + { + return; + } + discriminant = std::max(0.0, discriminant); + const double ROOT = std::sqrt(discriminant); + // Stable quadratic roots, avoiding cancellation when B and sqrt(delta) are close. + const double Q = -0.5 * (B + std::copysign(ROOT, B)); + const double ROOTS[2] = {Q / A, Q != 0.0 ? C / Q : -B / (2.0 * A)}; + for (double t : ROOTS) + { + if (t >= -1e-10 && t <= 1.0 + 1e-10) + { + t = std::clamp(t, 0.0, 1.0); + Candidate({start.x + t * STEP.x, start.y + t * STEP.y}); + } + } + } + + void Interior() + { + const double A = quadratic.a; + const double B = quadratic.b; + const double C = quadratic.c; + const double D = quadratic.d; + const double E = quadratic.e; + const double DETERMINANT = A * C - B * B; + const double SCALE = std::max(A, C); + if (DETERMINANT > 1e-14 * SCALE * SCALE && SCALE > 0.0) + { + const Point CENTER{(B * E - C * D) / (2.0 * DETERMINANT), + (B * D - A * E) / (2.0 * DETERMINANT)}; + Minimum(CENTER); + Candidate(CENTER); + const double RADIUS_SQUARED = input.power_budget - quadratic.Value(CENTER); + const Point DIRECTION{(C * input.weights.x - B * input.weights.y) / DETERMINANT, + (A * input.weights.y - B * input.weights.x) / DETERMINANT}; + const double NORM = input.weights.x * DIRECTION.x + input.weights.y * DIRECTION.y; + if (RADIUS_SQUARED >= 0.0 && NORM > 0.0) + { + const double STEP = std::sqrt(RADIUS_SQUARED / NORM); + Candidate({CENTER.x + STEP * DIRECTION.x, CENTER.y + STEP * DIRECTION.y}); + } + return; + } + const double EIGENVALUE = A + C; + if (EIGENVALUE <= 0.0) + { + return; // Affine or constant power; polygon edges cover the optimum. + } + // Rank-one PSD: a strip/line, or a parabola when the linear null-space term is + // nonzero. + Point normal = A >= C ? Point{A, B} : Point{B, C}; + const double LENGTH = std::hypot(normal.x, normal.y); + normal.x /= LENGTH; + normal.y /= LENGTH; + const Point TANGENT{-normal.y, normal.x}; + const double DN = D * normal.x + E * normal.y; + const double DT = D * TANGENT.x + E * TANGENT.y; + const double WN = input.weights.x * normal.x + input.weights.y * normal.y; + const double WT = input.weights.x * TANGENT.x + input.weights.y * TANGENT.y; + if (std::abs(DT) > 1e-12 * (1.0 + std::abs(D) + std::abs(E)) && WT / DT > 0.0) + { + const double MULTIPLIER = WT / DT; + const double N = (WN / MULTIPLIER - DN) / (2.0 * EIGENVALUE); + const double T = + (input.power_budget - quadratic.f - EIGENVALUE * N * N - DN * N) / DT; + Candidate({normal.x * N + TANGENT.x * T, normal.y * N + TANGENT.y * T}); + } + } +}; +} // namespace Detail + +inline Result Solve(const Input& input) +{ + Result result; + if (input.halfplane_count < 0 || input.halfplane_count > 12 || + !std::isfinite(input.power_budget) || !std::isfinite(input.weights.x) || + !std::isfinite(input.weights.y) || input.weights.x < 0.0f || input.weights.y < 0.0f) + { + return result; + } + for (const auto& MOTOR : input.motors) + { + if (!std::isfinite(MOTOR.u) || !std::isfinite(MOTOR.v) || !std::isfinite(MOTOR.c) || + !std::isfinite(MOTOR.rpm) || !std::isfinite(MOTOR.kt) || + !std::isfinite(MOTOR.k1) || !std::isfinite(MOTOR.k2) || MOTOR.kt < 0.0f || + MOTOR.k1 < 0.0f || MOTOR.k2 < 0.0f) + { + return result; + } + } + Detail::Polygon polygon{{{0.0, 0.0}, {1.0, 0.0}, {1.0, 1.0}, {0.0, 1.0}}, 4}; + for (int i = 0; i < input.halfplane_count; ++i) + { + const auto& PLANE = input.halfplanes[i]; + if (!std::isfinite(PLANE.x) || !std::isfinite(PLANE.y) || + !std::isfinite(PLANE.limit) || !Detail::Clip(polygon, PLANE)) + { + return result; + } + } + result.status = Status::INFEASIBLE; + if (polygon.count == 0) + { + return result; + } + const auto QUADRATIC = Detail::MakeQuadratic(input); + Detail::Search search{input, QUADRATIC}; + // Monotone objective: the fully requested acceleration is globally optimal if feasible. + if (Detail::Inside(input, {1.0, 1.0}) && + Detail::MotorPower(input, {1.0, 1.0}) <= input.power_budget) + { + result.feasible = true; + result.minimum_available = true; + result.point = {1.0f, 1.0f}; + result.power = static_cast(Detail::MotorPower(input, {1.0, 1.0})); + result.status = Status::OPTIMAL; + return result; + } + for (int i = 0; i < polygon.count; ++i) + { + search.Minimum(polygon.points[i]); + search.Candidate(polygon.points[i]); + search.Edge(polygon.points[i], polygon.points[(i + 1) % polygon.count]); + } + search.Interior(); + search.Candidate(search.minimum); + result.minimum_available = std::isfinite(search.minimum_power); + result.feasible = search.found; + const auto POINT = search.found ? search.best : search.minimum; + result.point = {static_cast(POINT.x), static_cast(POINT.y)}; + result.power = + static_cast(Detail::MotorPower(input, {result.point.x, result.point.y})); + result.status = search.found ? Status::OPTIMAL : Status::INFEASIBLE; + return result; +} +} // namespace AccelerationConstraint diff --git a/Chassis.hpp b/Chassis.hpp index 2890111..0185262 100644 --- a/Chassis.hpp +++ b/Chassis.hpp @@ -129,6 +129,10 @@ class Chassis ///< First-order low-pass coefficient of the dynamic scale float moment_of_inertia = 0.0f; ///< Helm yaw inertia (kg*m^2); zero disables yaw feedforward. + float friction_coefficient = 0.35f; ///< Helm equal-load traction model. + float translation_acceleration_weight = 1.0f; + float angular_acceleration_weight = 1.0f; + float rotor_angular_acceleration_weight = 4.0f; }; /** @@ -320,6 +324,10 @@ class Chassis if constexpr (std::is_same_v) { result.moment_of_inertia = param.moment_of_inertia; + result.friction_coefficient = param.friction_coefficient; + result.translation_acceleration_weight = param.translation_acceleration_weight; + result.angular_acceleration_weight = param.angular_acceleration_weight; + result.rotor_angular_acceleration_weight = param.rotor_angular_acceleration_weight; } return result; } diff --git a/Helm.hpp b/Helm.hpp index edbcab4..16eabee 100644 --- a/Helm.hpp +++ b/Helm.hpp @@ -12,6 +12,7 @@ depends: [] #include #include +#include "AccelerationConstraint.hpp" #include "CMD.hpp" #include "Chassis.hpp" #include "Motor.hpp" @@ -85,6 +86,10 @@ class Helm float rotor_scale_lpf_alpha = 0.2f; ///< 动态缩放的一阶低通系数 ///< First-order low-pass coefficient of the dynamic scale float moment_of_inertia = 0.0f; ///< Yaw inertia (kg*m^2), zero disables feedforward + float friction_coefficient = 0.35f; + float translation_acceleration_weight = 1.0f; + float angular_acceleration_weight = 1.0f; + float rotor_angular_acceleration_weight = 4.0f; }; /** * @brief 底盘模式。 @@ -464,66 +469,137 @@ class Helm */ void PowerControlUpdate() { - /* 仅提交驱动轮数据;PowerControl 的 motor_count_6020 配置为 0。 */ + constraint_output_valid_ = false; + constraint_pid_scale_ = 0.0f; + acceleration_constraint_result_ = {}; + if (chassis_event_ == ChassisMode::RELAX || !feedback_valid_ || + !measured_chassis_velocity_.valid || !std::isfinite(dt_) || dt_ <= 0.0f || + !std::isfinite(target_vx_) || !std::isfinite(target_vy_) || + !std::isfinite(target_omega_)) + { + return; + } + + // Feedback and command currents use the same configured motor direction. 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; } - - /* 第一次设置: 反馈数据用于 RLS 参数估计 */ power_control_->SetMotorData3508(motor_data_.output_current_3508, motor_data_.rotorspeed_rpm_3508); - power_control_->CalculatePowerControlParam(); - - /* 计算 3508 速度跟踪误差 */ - float speed_error_3508[4]; - for (int i = 0; i < 4; i++) + const auto MODEL = power_control_->GetDrivePowerModel(); + if (!MODEL.valid || !std::isfinite(PARAM.friction_coefficient) || + PARAM.friction_coefficient <= 0.0f || !std::isfinite(PARAM.gravity) || + PARAM.gravity <= 0.0f || !std::isfinite(PARAM.wheel_radius) || + PARAM.wheel_radius <= 0.0f || !std::isfinite(PARAM.wheel_to_center) || + PARAM.wheel_to_center <= 0.0f || !std::isfinite(PARAM.moment_of_inertia) || + PARAM.moment_of_inertia < 0.0f || !std::isfinite(PARAM.reduction_ratio) || + PARAM.reduction_ratio <= 0.0f) { - 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); + return; } - power_control_->SetMotorData3508(motor_data_.output_current_3508, - motor_data_.rotorspeed_rpm_3508, speed_error_3508); + const float TRANSLATION_REQUEST = hypotf(target_ax_, target_ay_); + constexpr float GRAVITY_ACCELERATION = 9.80665f; + const float CURRENT_PER_TORQUE = M3508_NM_TO_LSB_RATIO / PARAM.reduction_ratio; + // In addition to the document's diamond, include actual PID+FF drive traction. + const float TORQUE_LIMIT = std::min( + {4.4f, 16384.0f / CURRENT_PER_TORQUE, + PARAM.friction_coefficient * PARAM.gravity * PARAM.wheel_radius / 4.0f}); + const float CURRENT_LIMIT = TORQUE_LIMIT * CURRENT_PER_TORQUE; + AccelerationConstraint::Input input{}; + input.power_budget = GetChassisPowerBudget() - MODEL.static_loss - MODEL.margin; + input.weights = { + TRANSLATION_REQUEST > 1.0e-6f ? PARAM.translation_acceleration_weight : 0.0f, + fabsf(target_alpha_) > 1.0e-6f ? (chassis_event_ == ChassisMode::ROTOR + ? PARAM.rotor_angular_acceleration_weight + : PARAM.angular_acceleration_weight) + : 0.0f}; + input.halfplanes[0] = { + TRANSLATION_REQUEST / (PARAM.friction_coefficient * GRAVITY_ACCELERATION), + fabsf(target_alpha_) * PARAM.moment_of_inertia / + (PARAM.friction_coefficient * PARAM.gravity * PARAM.wheel_to_center), + 1.0f}; + + // PID state is advanced only once by CalculateMotorCurrent. If its fixed + // offset makes the request box infeasible, retry with less PID contribution. + constexpr float PID_SCALES[] = {1.0f, 0.5f, 0.0f}; + for (float pid_scale : PID_SCALES) + { + input.halfplane_count = 1; + for (int i = 0; i < 4; i++) + { + auto& motor = input.motors[i]; + motor = {wheel_translation_ff_[i] * CURRENT_PER_TORQUE, + wheel_rotation_ff_[i] * CURRENT_PER_TORQUE, + pid_scale * wheel_pid_out_[i] * CURRENT_PER_TORQUE, + motor_data_.rotorspeed_rpm_3508[i], + MODEL.kt, + MODEL.k1, + MODEL.k2}; + input.halfplanes[input.halfplane_count++] = {motor.u, motor.v, + CURRENT_LIMIT - motor.c}; + input.halfplanes[input.halfplane_count++] = {-motor.u, -motor.v, + CURRENT_LIMIT + motor.c}; + } + acceleration_constraint_result_ = AccelerationConstraint::Solve(input); + if (acceleration_constraint_result_.feasible) + { + constraint_pid_scale_ = pid_scale; + constraint_output_valid_ = true; + break; + } + } + if (!constraint_output_valid_) + { + // Do not claim an impossible electrical budget was met. Output() relaxes + // the motors; zero acceleration alone may still exceed the loss model. + return; + } - 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); + const auto SCALE = acceleration_constraint_result_.point; + target_ax_ *= SCALE.x; + target_ay_ *= SCALE.x; + target_alpha_ *= SCALE.y; + for (int i = 0; i < 4; i++) + { + target_motor_torque_ff_[i] = + SCALE.x * wheel_translation_ff_[i] + SCALE.y * wheel_rotation_ff_[i]; + wheel_out_[i] = + constraint_pid_scale_ * wheel_pid_out_[i] + target_motor_torque_ff_[i]; + } + } + float GetChassisPowerBudget() + { + const auto NOW_MS = LibXR::Timebase::GetMilliseconds(); + const bool REFEREE_ONLINE = (NOW_MS - referee_last_rx_time_).ToSecondf() <= 1.0f; float max_power = static_cast(referee_chassis_pack_.rs.chassis_power_limit); - if (!referee_online || max_power <= 1.0f) + if (!REFEREE_ONLINE || max_power <= 1.0f) { max_power = HELM_CHASSIS_MAX_POWER; } - - if (power_control_online && boost_mode) + if (power_control_->IsOnline() && cmd_data_.self_define == CMD::ChasStat::BOOST) { - float cap_energy = power_control_->GetCapEnergy(); - if (cap_energy > 0.8f) + // Preserve the existing board feedback interpretation and boost tiers. + const float CAP_LEVEL = power_control_->GetCapEnergy(); + if (CAP_LEVEL > 0.8f) { max_power += 100.0f; } - else if (cap_energy > 0.5f) + else if (CAP_LEVEL > 0.5f) { max_power += 70.0f; } - else if (cap_energy > 0.25f) + else if (CAP_LEVEL > 0.25f) { max_power += 40.0f; } } - - power_control_->OutputLimit(max_power); - power_control_data_ = power_control_->GetPowerControlData(); + return max_power; } /** @@ -538,6 +614,8 @@ class Helm for (int i = 0; i < 4; i++) { target_motor_torque_ff_[i] = 0.0f; + wheel_translation_ff_[i] = 0.0f; + wheel_rotation_ff_[i] = 0.0f; } const float MAX_WHEEL_LINEAR_SPEED = GetMaxWheelLinearSpeed(); @@ -616,12 +694,15 @@ class Helm const WheelPosition WHEEL_POSITION = GetWheelPosition(static_cast(i)); const float STEER_ANGLE = LibXR::CycleValue(motor_steer_feedback_[i].abs_angle - zero_[i]); - const float FORCE_X = TRANSLATION_FORCE_X - ROTATION_FORCE_SCALE * WHEEL_POSITION.y; - const float FORCE_Y = TRANSLATION_FORCE_Y + ROTATION_FORCE_SCALE * WHEEL_POSITION.x; /* 实际舵向的驱动正方向为 (sin(a), cos(a))。 * 投影已包含方向,不再按 motor_reverse 取反或重复乘 cos^3。 */ - const float TORQUE_FF = PARAM.wheel_radius * - (FORCE_X * sinf(STEER_ANGLE) + FORCE_Y * cosf(STEER_ANGLE)); + wheel_translation_ff_[i] = + PARAM.wheel_radius * (TRANSLATION_FORCE_X * sinf(STEER_ANGLE) + + TRANSLATION_FORCE_Y * cosf(STEER_ANGLE)); + wheel_rotation_ff_[i] = + PARAM.wheel_radius * ROTATION_FORCE_SCALE * + (-WHEEL_POSITION.y * sinf(STEER_ANGLE) + WHEEL_POSITION.x * cosf(STEER_ANGLE)); + const float TORQUE_FF = wheel_translation_ff_[i] + wheel_rotation_ff_[i]; if (std::isfinite(TORQUE_FF)) { target_motor_torque_ff_[i] = TORQUE_FF; @@ -790,6 +871,7 @@ class Helm } wheel_out_[i] = pid_wheel_speed_[i].Calculate( observed_motor_rpm, motor_wheel_feedback_[i].velocity, dt_); + wheel_pid_out_[i] = wheel_out_[i]; if (motor_reverse_[i]) { steer_angle_[i] = pid_steer_angle_[i].Calculate( @@ -821,18 +903,7 @@ class Helm */ void Output() { - if (power_control_data_.is_power_limited) - { - /* 功率限制只替换驱动轮输出,舵向保留 PID 输出和自身扭矩限幅。 */ - 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); - } - } - - if (chassis_event_ == (ChassisMode::RELAX)) + if (chassis_event_ == ChassisMode::RELAX || !constraint_output_valid_) { LostCtrl(); } @@ -961,6 +1032,9 @@ class Helm float target_ay_ = 0.0f; /* m/s^2 */ float target_alpha_ = 0.0f; /* rad/s^2 */ float target_motor_torque_ff_[4] = {0.0f, 0.0f, 0.0f, 0.0f}; + float wheel_translation_ff_[4]{}; + float wheel_rotation_ff_[4]{}; + float wheel_pid_out_[4]{}; bool motor_reverse_[4]{false, false, false, false}; LibXR::CycleValue zero_[4] = {1.10063124, 2.05323339, 3.2244277, 0.979446769}; @@ -1028,7 +1102,9 @@ class Helm MotorData motor_data_ = {}; PowerControl* power_control_; - PowerControlData power_control_data_; + bool constraint_output_valid_ = false; + float constraint_pid_scale_ = 0.0f; + AccelerationConstraint::Result acceleration_constraint_result_{}; Referee::ChassisPack referee_chassis_pack_{}; LibXR::MillisecondTimestamp referee_last_rx_time_ = 0; diff --git a/HelmMigrationRecord.md b/HelmMigrationRecord.md index eca1d2d..8ba7b0f 100644 --- a/HelmMigrationRecord.md +++ b/HelmMigrationRecord.md @@ -31,41 +31,62 @@ only BlinkLED and is not the product configuration used for this migration. - Existing hardware registrations, CAN receive/transport implementation, other module constructor interfaces, and configurable Topic names are retained. -## Upstream power policy - -PowerControl is restored to QDU-Robomaster/PowerControl at upstream commit -`4097e1f6a3fad89126e82b920217961286966d84`. The Helm power-data submission, RLS -invocation and referee/boost budget selection follow -QDU-Robomaster/Chassis commit `32761d015294031f3d5ac8326b1a394c796302a9`. - -Only the four M3508 drive motors enter power limiting. The Helm configurations -set `motor_count_6020` to zero, and Helm submits only M3508 data to PowerControl. -Limited output currents replace only drive-wheel torque requests; steering keeps -its PID output and local +/-2.5 N*m torque clamp. The upstream limiter sums positive -drive model powers, with static chassis loss and the 4 W margin deducted from the -budget. This drive-only policy is the intentional exception to upstream Helm -power integration; the PowerControl module itself is unmodified. - -Additional Helm power estimates, Ozone sampling globals, and the velocity debug -pointer are removed. Physical chassis-velocity feedback remains part of the -migrated motion controller. The motor target names at the power-control boundary -match upstream: `target_speed_` is motor rotor RPM and `target_angle_` is radians. -RMMotor decodes torque using the same configured direction as velocity, so power -identification consumes aligned feedback directly; the extra torque-polarity -interface is no longer needed. -Confirm the SuperPower protocol and actual power-meter coverage before relying -on online power identification. - -`moment_of_inertia` defaults to zero in the other product configurations. It is -forwarded only to Helm; Omni and Mecanum keep their current parameter contracts. -Module edits live in the modules' own Git repositories and are not captured by -the parent BSP's lock file until the module commits are published and relocked. - -## Friction measurement - -The user reports a measured wheel-ground friction coefficient of approximately -0.4. This is a recorded bench observation; no grip or acceleration constraint -has been added to the controller. +## Acceleration, traction and signed power constraints + +`AccelerationConstraint.hpp` follows sections 1.1.5 and 1.1.6 of the supplied +swerve-controller report: Sutherland-Hodgman half-plane clipping, feasible polygon +vertices, quadratic/segment intersections, and the power ellipse's objective +support point. Rank-zero/rank-one quadratics, point/line polygons, negative budgets, +and infeasible requests are handled without changing the physical quadratic. +The solver uses fixed arrays and no dynamic allocation; intermediate double +arithmetic improves numerical accuracy but requires software arithmetic on M4F. + +Decision coordinates are retained fractions x,y in [0,1] of requested translation +and angular acceleration. Translation direction and angular acceleration sign are +preserved. The objective is translation_weight*x + angular_weight*y. Defaults are +1:1 in normal modes and 1:4 in ROTOR, a weighted preference rather than strict +lexicographic priority. YAML fields `translation_acceleration_weight`, +`angular_acceleration_weight` and `rotor_angular_acceleration_weight` are forwarded +only to Helm. Zero requested axes have zero objective weight. + +The document's equal-load traction diamond is +|a|/(mu*g) + |alpha|*J/(mu*weight*R) <= 1. The user measured mu approximately 0.4; +the requested conservative configured `friction_coefficient` is 0.35. Extra affine +half-planes bound each actual PID+FF wheel torque by the minimum of 4.4 N*m, +the 16384-LSB hardware limit, and mu*weight*wheel_radius/4. Thus the PID offset +cannot bypass the longitudinal drive traction bound. Normal load is assumed equal; +load transfer, lateral tire force and transient tire dynamics are not modeled. +The constraints are model-based and do not guarantee absence of real wheel slip. + +Motor speeds and wheel PID outputs are frozen once per cycle. Per-wheel current +is affine in x,y: I_i=u_i*x+v_i*y+c_i. Power is the SIGNED sum of +kt*I_i*rpm_i + k1*I_i^2 + k2*rpm_i^2. Regenerative negative power is not clipped or +counted twice. The constant PID loss term is k1*c_i^2. RMMotor already aligns +feedback torque with feedback RPM, so no extra polarity interface is used. + +PowerControl retains its existing RLS/legacy limiters, adding only a locked +GetDrivePowerModel() coefficient snapshot. Helm no longer invokes the legacy +positive-power limiter or replaces outputs with its result. Only four drive motors +are constrained; motor_count_6020 remains zero and steering keeps its PID output +and +/-2.5 N*m clamp. Static chassis loss and the existing 4 W control margin are +deducted from the available chassis budget before solving. + +Referee data older than one second, or a missing/invalid limit, selects +HELM_CHASSIS_MAX_POWER. Existing supercap policy is preserved: only online BOOST +adds 100/70/40 W when feedback exceeds 0.8/0.5/0.25 respectively. GetCapEnergy() +currently decodes an output-capability ratio, not a separately measured energy +in joules; this board-protocol interpretation still needs hardware confirmation. + +If full frozen PID makes the constrained request infeasible, solve again at PID +scales 0.5 and 0 without advancing PID state. If still infeasible, or feedback, +commands, timing or model are invalid, the output path relaxes the motors. This +is an explicit failure fallback, not a claim that impossible modeled power can be +met. `acceleration_constraint_result_`, `constraint_pid_scale_` and +`constraint_output_valid_` retain the result/status inside Helm for diagnosis. + +Other product configurations retain their constructor contract after xrobot sync; +only Helm receives the new fields. No vendor code is changed. Module changes remain +local to their own repositories until explicitly committed, published and relocked. ## Hardware validation still required @@ -81,5 +102,9 @@ has been added to the controller. 4. Only after those checks, test on the floor at low speed. Observe braking, mode transitions, feedback validity, current and power before raising limits. +Before enabling higher limits, measure DWT cycle count and task stack high-water +mark on the MCU, including the worst-case three solver attempts, against the +2 ms control period. Compilation and host solver tests do not establish timing. + Compilation does not validate physical direction, power-meter coverage, PID tuning, or closed-loop stability on the robot. diff --git a/tests/acceleration_constraint_test.cpp b/tests/acceleration_constraint_test.cpp new file mode 100644 index 0000000..edbd657 --- /dev/null +++ b/tests/acceleration_constraint_test.cpp @@ -0,0 +1,244 @@ +#include +#include +#include +#include +#include +#include + +#include "AccelerationConstraint.hpp" + +using AccelerationConstraint::Input; +using AccelerationConstraint::Result; +using AccelerationConstraint::Solve; +using AccelerationConstraint::Status; + +namespace +{ +void near(float actual, float expected, float tolerance = 1e-4f) +{ + assert(std::isfinite(actual)); + if (std::abs(actual - expected) > tolerance) + { + std::fprintf(stderr, "Expected %.9g, got %.9g\n", expected, actual); + assert(false); + } +} + +double power(const Input& input, double x, double y) +{ + double result = 0.0; + for (const auto& MOTOR : input.motors) + { + const double CURRENT = MOTOR.u * x + MOTOR.v * y + MOTOR.c; + result += MOTOR.k1 * CURRENT * CURRENT + + static_cast(MOTOR.kt) * MOTOR.rpm * CURRENT + + static_cast(MOTOR.k2) * MOTOR.rpm * MOTOR.rpm; + } + return result; +} + +void check_feasible(const Input& input, const Result& result) +{ + assert(result.feasible && result.status == Status::OPTIMAL); + assert(result.point.x >= 0.0f && result.point.x <= 1.0f); + assert(result.point.y >= 0.0f && result.point.y <= 1.0f); + assert(power(input, result.point.x, result.point.y) <= input.power_budget + 1e-4); + for (int i = 0; i < input.halfplane_count; ++i) + { + const auto& PLANE = input.halfplanes[i]; + assert(PLANE.x * result.point.x + PLANE.y * result.point.y <= PLANE.limit + 1e-5f); + } +} + +void deterministic() +{ + Input input; + input.motors[0].u = 1.0f; + input.motors[0].k1 = 1.0f; + input.motors[1].v = 1.0f; + input.motors[1].k1 = 1.0f; + input.power_budget = 0.5f; + Result result = Solve(input); + check_feasible(input, result); + near(result.point.x, 0.5f); + near(result.point.y, 0.5f); + + // A shifted ellipse has an interior support point rather than a polygon-edge optimum. + input.motors[0].c = -0.4f; + input.motors[1].c = -0.6f; + input.motors[1].v = 2.0f; + input.power_budget = 0.1f; + result = Solve(input); + check_feasible(input, result); + near(result.point.x, 0.4f + std::sqrt(0.1f / 1.25f)); + near(result.point.y, 0.3f + 0.25f * std::sqrt(0.1f / 1.25f)); + + // Zero budget may leave exactly one interior feasible point. + input.power_budget = 0.0f; + result = Solve(input); + check_feasible(input, result); + near(result.point.x, 0.4f); + near(result.point.y, 0.3f); + + // Rank-one strip, followed by the zero-width line degeneracy. + input.motors[1] = {}; + input.power_budget = 0.04f; + result = Solve(input); + check_feasible(input, result); + near(result.point.x, 0.6f); + near(result.point.y, 1.0f); + input.power_budget = 0.0f; + result = Solve(input); + check_feasible(input, result); + near(result.point.x, 0.4f); + near(result.point.y, 1.0f); + + // Rank-one parabola x^2 + y <= 0.5: its support is not on a polygon edge. + input = {}; + input.motors[0].u = 1.0f; + input.motors[0].k1 = 1.0f; + input.motors[1].v = 1.0f; + input.motors[1].rpm = 1.0f; + input.motors[1].kt = 1.0f; + input.power_budget = 0.5f; + result = Solve(input); + check_feasible(input, result); + near(result.point.x, 0.5f); + near(result.point.y, 0.25f); + + // Signed braking power increases usable positive motor power, without abs/max0. + input = {}; + input.motors[0].u = 140.0f; + input.motors[0].rpm = 1.0f; + input.motors[0].kt = 1.0f; + input.motors[1].v = -40.0f; + input.motors[1].rpm = 1.0f; + input.motors[1].kt = 1.0f; + input.power_budget = 70.0f; + result = Solve(input); + check_feasible(input, result); + near(result.point.x, 110.0f / 140.0f); + near(result.point.y, 1.0f); + + // Linear clipping, angular preference and a disabled translation axis. + input = {}; + input.power_budget = 100.0f; + input.halfplanes[0] = {1.0f, 1.0f, 0.6f}; + input.halfplane_count = 1; + input.weights = {1.0f, 10.0f}; + result = Solve(input); + check_feasible(input, result); + near(result.point.x, 0.0f); + near(result.point.y, 0.6f); + input.halfplanes[1] = {1.0f, 0.0f, 0.0f}; + input.halfplane_count = 2; + result = Solve(input); + check_feasible(input, result); + near(result.point.x, 0.0f); + + // Infeasible power still returns the actual minimum-power point on the polygon. + input = {}; + input.motors[0].u = 1.0f; + input.motors[0].c = -0.3f; + input.motors[0].k1 = 1.0f; + input.motors[1].v = 1.0f; + input.motors[1].rpm = 1.0f; + input.motors[1].kt = 1.0f; + input.motors[2].rpm = 1.0f; + input.motors[2].k2 = 10.0f; + input.power_budget = 9.0f; + result = Solve(input); + assert(!result.feasible && result.minimum_available && + result.status == Status::INFEASIBLE); + near(result.point.x, 0.3f); + near(result.point.y, 0.0f); + near(result.power, 10.0f); + + // Empty geometric polygon must not pretend that the origin is a valid fallback. + input.halfplanes[0] = {0.0f, 0.0f, -1.0f}; + input.halfplane_count = 1; + result = Solve(input); + assert(!result.feasible && !result.minimum_available && + result.status == Status::INFEASIBLE); + + input = {}; + input.power_budget = std::numeric_limits::quiet_NaN(); + assert(Solve(input).status == Status::INVALID_INPUT); + input.power_budget = 1.0f; + input.motors[0].k1 = -1.0f; + assert(Solve(input).status == Status::INVALID_INPUT); +} + +void randomized() +{ + std::mt19937 random(0xacc35U); + std::uniform_real_distribution signed_unit(-1.0f, 1.0f); + std::uniform_real_distribution unit(0.0f, 1.0f); + for (int trial = 0; trial < 1500; ++trial) + { + Input input; + for (auto& motor : input.motors) + { + motor.u = 5000.0f * signed_unit(random); + motor.v = 5000.0f * signed_unit(random); + motor.c = 1000.0f * signed_unit(random); + motor.rpm = 4000.0f * signed_unit(random); + motor.kt = 1.99688994e-6f; + motor.k1 = 2e-7f; + motor.k2 = 3e-7f; + } + input.power_budget = 60.0f * unit(random); + input.weights = {0.1f + unit(random), 0.1f + 5.0f * unit(random)}; + input.halfplanes[0] = {1.0f, 0.7f, 0.2f + 1.5f * unit(random)}; + input.halfplane_count = 1; + const Result RESULT = Solve(input); + if (RESULT.feasible) + { + check_feasible(input, RESULT); + } + double reference = -std::numeric_limits::infinity(); + constexpr int GRID = 80; + for (int ix = 0; ix <= GRID; ++ix) + { + for (int iy = 0; iy <= GRID; ++iy) + { + const double X = static_cast(ix) / GRID; + const double Y = static_cast(iy) / GRID; + if (X + 0.7 * Y <= input.halfplanes[0].limit && + power(input, X, Y) <= input.power_budget) + { + reference = std::max(reference, input.weights.x * X + input.weights.y * Y); + } + } + } + if (std::isfinite(reference)) + { + if (!RESULT.feasible) + { + std::fprintf(stderr, "False infeasible at trial %d\n", trial); + assert(false); + } + const double OBJECTIVE = + input.weights.x * RESULT.point.x + input.weights.y * RESULT.point.y; + if (OBJECTIVE + 2e-4 < reference) + { + std::fprintf(stderr, "Suboptimal trial %d: %.9g vs grid %.9g\n", trial, OBJECTIVE, + reference); + assert(false); + } + } + } +} +} // namespace + +int main() +{ + deterministic(); + const auto START = std::chrono::steady_clock::now(); + randomized(); + const auto STOP = std::chrono::steady_clock::now(); + std::printf( + "PASS: polygon/ellipse geometry, signed braking, degenerate quadratics, " + "infeasible fallback and 1500 dense-reference randomized cases (%.3f s).\n", + std::chrono::duration(STOP - START).count()); +} diff --git a/tests/helm_constraint_model_test.cpp b/tests/helm_constraint_model_test.cpp new file mode 100644 index 0000000..df7a20d --- /dev/null +++ b/tests/helm_constraint_model_test.cpp @@ -0,0 +1,383 @@ +#include +#include +#include +#include +#include +#include +#include + +#include "AccelerationConstraint.hpp" + +namespace +{ +constexpr float GRAVITY = 9.80665f; +constexpr float WEIGHT = 182.692f; +constexpr float MASS = WEIGHT / GRAVITY; +constexpr float RADIUS = 0.06f; +constexpr float CENTER_RADIUS = 0.233f; +constexpr float INERTIA = 0.49436791247f; +constexpr float MU = 0.35f; +constexpr float REDUCTION = 14.0f; +constexpr float CURRENT_PER_TORQUE = 52437.5f / REDUCTION; +constexpr float KT = 1.99688994e-6f; +constexpr float K1 = 2.0e-7f; +constexpr float K2 = 3.0e-7f; +constexpr float PI = 3.14159265358979323846f; + +struct Scenario +{ + float ax = 0.0f; + float ay = 0.0f; + float alpha = 0.0f; + float inertia = INERTIA; + float power_budget = 100.0f - 5.5f - 4.0f; + float angular_weight = 1.0f; + std::array steer{}; + std::array pid{}; + std::array rpm{}; +}; + +struct Model +{ + AccelerationConstraint::Input input{}; + std::array translation{}; + std::array rotation{}; + float pid_scale = 1.0f; +}; + +void near(double actual, double expected, double tolerance = 1e-5) +{ + assert(std::isfinite(actual)); + if (std::abs(actual - expected) > tolerance) + { + std::fprintf(stderr, "Expected %.12g, got %.12g\n", expected, actual); + assert(false); + } +} + +float wheel_x(int index) +{ + return CENTER_RADIUS * (index == 0 || index == 3 ? -1.0f : 1.0f) / std::sqrt(2.0f); +} + +float wheel_y(int index) +{ + return CENTER_RADIUS * (index < 2 ? -1.0f : 1.0f) / std::sqrt(2.0f); +} + +float effective_alpha(const Scenario& scenario) +{ + return scenario.inertia > 0.0f ? scenario.alpha : 0.0f; +} + +float torque_limit() +{ + return std::min({4.4f, 16384.0f / CURRENT_PER_TORQUE, MU * WEIGHT * RADIUS / 4.0f}); +} + +// Physical scenario adapter independent of LibXR, motor drivers and RTOS objects. +Model make_model(const Scenario& scenario, float pid_scale) +{ + Model model; + model.pid_scale = pid_scale; + auto& input = model.input; + const float ALPHA = effective_alpha(scenario); + const float ACCELERATION = std::hypot(scenario.ax, scenario.ay); + input.power_budget = scenario.power_budget; + input.weights = {ACCELERATION > 1e-6f ? 1.0f : 0.0f, + std::abs(ALPHA) > 1e-6f ? scenario.angular_weight : 0.0f}; + input.halfplanes[input.halfplane_count++] = { + ACCELERATION / (MU * GRAVITY), + std::abs(ALPHA) * scenario.inertia / (MU * WEIGHT * CENTER_RADIUS), 1.0f}; + for (int i = 0; i < 4; ++i) + { + const float SIN = std::sin(scenario.steer[i]); + const float COS = std::cos(scenario.steer[i]); + model.translation[i] = RADIUS * MASS * (scenario.ax * SIN + scenario.ay * COS) / 4.0f; + model.rotation[i] = RADIUS * scenario.inertia * ALPHA * + (-wheel_y(i) * SIN + wheel_x(i) * COS) / + (4.0f * CENTER_RADIUS * CENTER_RADIUS); + auto& motor = input.motors[i]; + motor = {model.translation[i] * CURRENT_PER_TORQUE, + model.rotation[i] * CURRENT_PER_TORQUE, + pid_scale * scenario.pid[i] * CURRENT_PER_TORQUE, + scenario.rpm[i], + KT, + K1, + K2}; + const float CURRENT_LIMIT = torque_limit() * CURRENT_PER_TORQUE; + input.halfplanes[input.halfplane_count++] = {motor.u, motor.v, + CURRENT_LIMIT - motor.c}; + input.halfplanes[input.halfplane_count++] = {-motor.u, -motor.v, + CURRENT_LIMIT + motor.c}; + } + assert(input.halfplane_count == 9); + return model; +} + +// Reconstruct exactly the float torque/current path sent to Motor::Control. +float output_torque(const Scenario& scenario, const Model& model, int index, + float translation_fraction, float angular_fraction) +{ + const float FF = translation_fraction * model.translation[index] + + angular_fraction * model.rotation[index]; + return model.pid_scale * scenario.pid[index] + FF; +} + +double output_power(const Scenario& scenario, const Model& model, float x, float y, + bool clip_negative = false) +{ + double sum = 0.0; + for (int i = 0; i < 4; ++i) + { + const float CURRENT = output_torque(scenario, model, i, x, y) * CURRENT_PER_TORQUE; + const double POWER = static_cast(KT) * CURRENT * scenario.rpm[i] + + static_cast(K1) * CURRENT * CURRENT + + static_cast(K2) * scenario.rpm[i] * scenario.rpm[i]; + sum += clip_negative ? std::max(0.0, POWER) : POWER; + } + return sum; +} + +bool physically_feasible(const Scenario& scenario, const Model& model, float x, float y) +{ + const double TRANSLATION = std::hypot(scenario.ax, scenario.ay) * x; + const double ROTATION = std::abs(effective_alpha(scenario)) * y; + if (TRANSLATION / (MU * GRAVITY) + + ROTATION * scenario.inertia / (MU * WEIGHT * CENTER_RADIUS) > + 1.0) + { + return false; + } + for (int i = 0; i < 4; ++i) + { + if (std::abs(output_torque(scenario, model, i, x, y)) > torque_limit()) + { + return false; + } + } + return output_power(scenario, model, x, y) <= scenario.power_budget; +} + +void check_result(const Scenario& scenario, const Model& model, + const AccelerationConstraint::Result& result) +{ + assert(result.feasible); + const float X = result.point.x; + const float Y = result.point.y; + assert(X >= 0.0f && X <= 1.0f && Y >= 0.0f && Y <= 1.0f); + const double DIAMOND = std::hypot(scenario.ax, scenario.ay) * X / (MU * GRAVITY) + + std::abs(effective_alpha(scenario)) * Y * scenario.inertia / + (MU * WEIGHT * CENTER_RADIUS); + assert(DIAMOND <= 1.0 + 3e-6); + for (int i = 0; i < 4; ++i) + { + const float TORQUE = output_torque(scenario, model, i, X, Y); + assert(std::abs(TORQUE) <= torque_limit() + 5e-6f); + assert(std::abs(TORQUE * CURRENT_PER_TORQUE) <= 16384.0f + 0.02f); + const auto& MOTOR = model.input.motors[i]; + const float AFFINE_CURRENT = MOTOR.u * X + MOTOR.v * Y + MOTOR.c; + near(TORQUE * CURRENT_PER_TORQUE, AFFINE_CURRENT, 0.005); + } + // Separate float reconstruction may differ slightly from the solver's double + // evaluation. + assert(output_power(scenario, model, X, Y) <= scenario.power_budget + 3e-4); +} + +void feedforward_equivalence() +{ + std::mt19937 random(0xff3508U); + std::uniform_real_distribution unit(-1.0f, 1.0f); + for (int trial = 0; trial < 5000; ++trial) + { + Scenario scenario; + scenario.ax = 8.0f * unit(random); + scenario.ay = 8.0f * unit(random); + scenario.alpha = 80.0f * unit(random); + for (float& angle : scenario.steer) + { + angle = PI * unit(random); + } + const Model MODEL = make_model(scenario, 1.0f); + for (int i = 0; i < 4; ++i) + { + // Original force allocation, projected only after forming its 2D force vector. + const double SCALE = + scenario.inertia * scenario.alpha / (4.0 * CENTER_RADIUS * CENTER_RADIUS); + const double FX = MASS * scenario.ax / 4.0 - SCALE * wheel_y(i); + const double FY = MASS * scenario.ay / 4.0 + SCALE * wheel_x(i); + const double OLD_TORQUE = + RADIUS * (FX * std::sin(scenario.steer[i]) + FY * std::cos(scenario.steer[i])); + near(MODEL.translation[i] + MODEL.rotation[i], OLD_TORQUE, 3e-6); + } + } +} + +void deterministic() +{ + Scenario scenario; + scenario.ay = 1.5f; + scenario.rpm = {5000.0f, -3000.0f, -3000.0f, -3000.0f}; + scenario.power_budget = 10.0f; + Model model = make_model(scenario, 1.0f); + auto result = AccelerationConstraint::Solve(model.input); + check_result(scenario, model, result); + near(result.point.x, 1.0); + assert(output_power(scenario, model, result.point.x, result.point.y, true) > + scenario.power_budget); + + // A shared physical acceleration budget must favor rotation more under ROTOR weight. + scenario = {}; + scenario.ay = 3.0f; + scenario.alpha = 30.0f; + scenario.power_budget = 500.0f; + model = make_model(scenario, 1.0f); + const auto BALANCED = AccelerationConstraint::Solve(model.input); + check_result(scenario, model, BALANCED); + scenario.angular_weight = 4.0f; + model = make_model(scenario, 1.0f); + const auto ROTOR = AccelerationConstraint::Solve(model.input); + check_result(scenario, model, ROTOR); + assert(ROTOR.point.y > BALANCED.point.y + 0.5f); + assert(ROTOR.point.x < BALANCED.point.x); + + // Opposite angular requests must give opposite torques with symmetric budgets. + scenario = {}; + scenario.alpha = 40.0f; + model = make_model(scenario, 1.0f); + const auto POSITIVE = AccelerationConstraint::Solve(model.input); + check_result(scenario, model, POSITIVE); + scenario.alpha = -40.0f; + const Model NEGATIVE_MODEL = make_model(scenario, 1.0f); + const auto NEGATIVE = AccelerationConstraint::Solve(NEGATIVE_MODEL.input); + check_result(scenario, NEGATIVE_MODEL, NEGATIVE); + near(POSITIVE.point.y, NEGATIVE.point.y); + for (int i = 0; i < 4; ++i) + { + near(output_torque(scenario, model, i, POSITIVE.point.x, POSITIVE.point.y), + -output_torque(scenario, NEGATIVE_MODEL, i, NEGATIVE.point.x, NEGATIVE.point.y)); + } + + // Zero axes and disabled yaw inertia leave no spurious rotational constraint/output. + scenario = {}; + model = make_model(scenario, 1.0f); + result = AccelerationConstraint::Solve(model.input); + check_result(scenario, model, result); + assert(model.input.weights.x == 0.0f && model.input.weights.y == 0.0f); + scenario.ay = 2.0f; + scenario.alpha = 100.0f; + scenario.inertia = 0.0f; + model = make_model(scenario, 1.0f); + result = AccelerationConstraint::Solve(model.input); + check_result(scenario, model, result); + assert(model.input.weights.y == 0.0f && model.input.halfplanes[0].y == 0.0f); + for (float rotation : model.rotation) + { + near(rotation, 0.0); + } +} + +void pid_fallbacks() +{ + for (const float REQUESTED_PID : {0.2f, 1.2f, 2.5f}) + { + Scenario scenario; + scenario.pid.fill(REQUESTED_PID); + const auto FROZEN_PID = scenario.pid; + float accepted = -1.0f; + for (const float SCALE : {1.0f, 0.5f, 0.0f}) + { + const Model MODEL = make_model(scenario, SCALE); + const auto RESULT = AccelerationConstraint::Solve(MODEL.input); + assert(scenario.pid == FROZEN_PID); + if (RESULT.feasible) + { + check_result(scenario, MODEL, RESULT); + accepted = SCALE; + break; + } + } + near(accepted, REQUESTED_PID == 0.2f ? 1.0 : REQUESTED_PID == 1.2f ? 0.5 : 0.0); + } + Scenario impossible; + impossible.rpm.fill(9000.0f); + impossible.power_budget = 20.0f; + for (const float SCALE : {1.0f, 0.5f, 0.0f}) + { + const auto RESULT = + AccelerationConstraint::Solve(make_model(impossible, SCALE).input); + assert(!RESULT.feasible && + RESULT.status == AccelerationConstraint::Status::INFEASIBLE); + } +} + +void random_physical_scenarios() +{ + std::mt19937 random(0xc0ffeeU); + std::uniform_real_distribution unit(-1.0f, 1.0f); + std::uniform_real_distribution positive(0.0f, 1.0f); + int feasible_count = 0; + for (int trial = 0; trial < 600; ++trial) + { + Scenario scenario; + scenario.ax = 8.0f * unit(random); + scenario.ay = 8.0f * unit(random); + scenario.alpha = 80.0f * unit(random); + scenario.angular_weight = trial % 2 ? 4.0f : 1.0f; + scenario.power_budget = 5.0f + 90.0f * positive(random); + for (int i = 0; i < 4; ++i) + { + scenario.steer[i] = PI * unit(random); + scenario.rpm[i] = 5000.0f * unit(random); + scenario.pid[i] = 2.0f * unit(random); + } + for (const float SCALE : {1.0f, 0.5f, 0.0f}) + { + const Model MODEL = make_model(scenario, SCALE); + const auto RESULT = AccelerationConstraint::Solve(MODEL.input); + double reference = -std::numeric_limits::infinity(); + constexpr int GRID = 40; + for (int ix = 0; ix <= GRID; ++ix) + { + for (int iy = 0; iy <= GRID; ++iy) + { + const float X = static_cast(ix) / GRID; + const float Y = static_cast(iy) / GRID; + if (physically_feasible(scenario, MODEL, X, Y)) + { + reference = + std::max(reference, static_cast(MODEL.input.weights.x) * X + + static_cast(MODEL.input.weights.y) * Y); + } + } + } + if (std::isfinite(reference)) + { + assert(RESULT.feasible); + const double OBJECTIVE = MODEL.input.weights.x * RESULT.point.x + + MODEL.input.weights.y * RESULT.point.y; + assert(OBJECTIVE >= reference - 3e-4); + } + if (RESULT.feasible) + { + check_result(scenario, MODEL, RESULT); + ++feasible_count; + break; + } + } + } + assert(feasible_count > 400); +} +} // namespace + +int main() +{ + feedforward_equivalence(); + deterministic(); + pid_fallbacks(); + random_physical_scenarios(); + std::puts( + "PASS: 5000 FF equivalence cases, physical torque/current/grip/power bounds, " + "signed braking, +/- yaw, zero axes, J=0, ROTOR weighting, frozen PID " + "fallbacks and 600 dense-reference physical scenarios."); +} From 2d1a0cd1f36975d98531298d6ac18dc95b831970 Mon Sep 17 00:00:00 2001 From: kaMi-0932 <1580654962@qq.com> Date: Tue, 3 Nov 2026 22:11:36 +0800 Subject: [PATCH 5/6] Optimize swerve solver and stabilize control scheduling --- AccelerationConstraint.hpp | 311 +++++++++++++++---------- Helm.hpp | 135 ++++++++++- HelmMigrationRecord.md | 52 ++++- tests/acceleration_constraint_test.cpp | 15 ++ tests/helm_constraint_model_test.cpp | 32 ++- 5 files changed, 406 insertions(+), 139 deletions(-) diff --git a/AccelerationConstraint.hpp b/AccelerationConstraint.hpp index 8498d3a..dbf9373 100644 --- a/AccelerationConstraint.hpp +++ b/AccelerationConstraint.hpp @@ -8,7 +8,8 @@ * Two-dimensional acceleration allocation, following the document's polygon, * ellipse-support and edge-intersection construction. Motor speeds and wheel PID * outputs are frozen in Input. Powers are signed; braking power is not clipped. - * Decision coordinates are normalized acceleration fractions in [0, 1]. + * Decision coordinates are normalized acceleration fractions in [0, 1]. All + * geometry uses single precision so Cortex-M4F runs it on the hardware FPU. */ namespace AccelerationConstraint { @@ -65,8 +66,8 @@ namespace Detail { struct Point { - double x; - double y; + float x; + float y; }; struct Polygon @@ -77,16 +78,16 @@ struct Polygon struct Quadratic { - double a = 0.0; - double b = 0.0; // f = a*x*x + 2*b*x*y + c*y*y + d*x + e*y + f. - double c = 0.0; - double d = 0.0; - double e = 0.0; - double f = 0.0; + float a = 0.0f; + float b = 0.0f; // f = a*x*x + 2*b*x*y + c*y*y + d*x + e*y + f. + float c = 0.0f; + float d = 0.0f; + float e = 0.0f; + float f = 0.0f; - double Value(Point p) const + float Value(Point p) const { - return (a * p.x + 2.0 * b * p.y + d) * p.x + (c * p.y + e) * p.y + f; + return (a * p.x + 2.0f * b * p.y + d) * p.x + (c * p.y + e) * p.y + f; } }; @@ -95,7 +96,7 @@ inline bool AddPoint(Polygon& polygon, Point point) if (polygon.count > 0) { const Point LAST = polygon.points[polygon.count - 1]; - if (std::abs(point.x - LAST.x) + std::abs(point.y - LAST.y) < 1e-12) + if (std::abs(point.x - LAST.x) + std::abs(point.y - LAST.y) < 1e-12f) { return true; } @@ -116,21 +117,21 @@ inline bool Clip(Polygon& polygon, HalfPlane plane) return true; } Point previous = polygon.points[polygon.count - 1]; - double previous_distance = plane.x * previous.x + plane.y * previous.y - plane.limit; + float previous_distance = plane.x * previous.x + plane.y * previous.y - plane.limit; for (int i = 0; i < polygon.count; ++i) { const Point CURRENT = polygon.points[i]; - const double DISTANCE = plane.x * CURRENT.x + plane.y * CURRENT.y - plane.limit; - if ((DISTANCE <= 0.0) != (previous_distance <= 0.0)) + const float DISTANCE = plane.x * CURRENT.x + plane.y * CURRENT.y - plane.limit; + if ((DISTANCE <= 0.0f) != (previous_distance <= 0.0f)) { - const double T = previous_distance / (previous_distance - DISTANCE); + const float T = previous_distance / (previous_distance - DISTANCE); if (!AddPoint(output, {previous.x + T * (CURRENT.x - previous.x), previous.y + T * (CURRENT.y - previous.y)})) { return false; } } - if (DISTANCE <= 0.0 && !AddPoint(output, CURRENT)) + if (DISTANCE <= 0.0f && !AddPoint(output, CURRENT)) { return false; } @@ -141,7 +142,7 @@ inline bool Clip(Polygon& polygon, HalfPlane plane) { const Point FIRST = output.points[0]; const Point LAST = output.points[output.count - 1]; - if (std::abs(FIRST.x - LAST.x) + std::abs(FIRST.y - LAST.y) < 1e-12) + if (std::abs(FIRST.x - LAST.x) + std::abs(FIRST.y - LAST.y) < 1e-12f) { --output.count; } @@ -152,17 +153,17 @@ inline bool Clip(Polygon& polygon, HalfPlane plane) inline bool Inside(const Input& input, Point point) { - constexpr double TOLERANCE = 2e-7; + constexpr float TOLERANCE = 2e-7f; if (!std::isfinite(point.x) || !std::isfinite(point.y) || point.x < -TOLERANCE || - point.x > 1.0 + TOLERANCE || point.y < -TOLERANCE || point.y > 1.0 + TOLERANCE) + point.x > 1.0f + TOLERANCE || point.y < -TOLERANCE || point.y > 1.0f + TOLERANCE) { return false; } for (int i = 0; i < input.halfplane_count; ++i) { const auto& PLANE = input.halfplanes[i]; - const double SCALE = - 1.0 + std::abs(PLANE.x) + std::abs(PLANE.y) + std::abs(PLANE.limit); + const float SCALE = + 1.0f + std::abs(PLANE.x) + std::abs(PLANE.y) + std::abs(PLANE.limit); if (PLANE.x * point.x + PLANE.y * point.y - PLANE.limit > TOLERANCE * SCALE) { return false; @@ -171,15 +172,14 @@ inline bool Inside(const Input& input, Point point) return true; } -inline double MotorPower(const Input& input, Point point) +inline float MotorPower(const Input& input, Point point) { - double total = 0.0; + float total = 0.0f; for (const auto& MOTOR : input.motors) { - const double CURRENT = MOTOR.u * point.x + MOTOR.v * point.y + MOTOR.c; - total += MOTOR.k1 * CURRENT * CURRENT + - static_cast(MOTOR.kt) * MOTOR.rpm * CURRENT + - static_cast(MOTOR.k2) * MOTOR.rpm * MOTOR.rpm; + const float CURRENT = MOTOR.u * point.x + MOTOR.v * point.y + MOTOR.c; + total += MOTOR.k1 * CURRENT * CURRENT + MOTOR.kt * MOTOR.rpm * CURRENT + + MOTOR.k2 * MOTOR.rpm * MOTOR.rpm; } return total; } @@ -189,17 +189,16 @@ inline Quadratic MakeQuadratic(const Input& input) Quadratic result; for (const auto& MOTOR : input.motors) { - const double U = MOTOR.u; - const double V = MOTOR.v; - const double C = MOTOR.c; - const double LINEAR = static_cast(MOTOR.kt) * MOTOR.rpm; + const float U = MOTOR.u; + const float V = MOTOR.v; + const float C = MOTOR.c; + const float LINEAR = MOTOR.kt * MOTOR.rpm; result.a += MOTOR.k1 * U * U; result.b += MOTOR.k1 * U * V; result.c += MOTOR.k1 * V * V; - result.d += (2.0 * MOTOR.k1 * C + LINEAR) * U; - result.e += (2.0 * MOTOR.k1 * C + LINEAR) * V; - result.f += MOTOR.k1 * C * C + LINEAR * C + - static_cast(MOTOR.k2) * MOTOR.rpm * MOTOR.rpm; + result.d += (2.0f * MOTOR.k1 * C + LINEAR) * U; + result.e += (2.0f * MOTOR.k1 * C + LINEAR) * V; + result.f += MOTOR.k1 * C * C + LINEAR * C + MOTOR.k2 * MOTOR.rpm * MOTOR.rpm; } return result; } @@ -209,9 +208,9 @@ struct Search const Input& input; const Quadratic& quadratic; Point minimum{}; - double minimum_power = std::numeric_limits::infinity(); + float minimum_power = std::numeric_limits::infinity(); Point best{}; - double best_value = -std::numeric_limits::infinity(); + float best_value = -std::numeric_limits::infinity(); bool found = false; void Minimum(Point point) @@ -220,7 +219,7 @@ struct Search { return; } - const double POWER = MotorPower(input, point); + const float POWER = MotorPower(input, point); if (POWER < minimum_power) { minimum_power = POWER; @@ -231,21 +230,36 @@ struct Search void Candidate(Point point) { // Validate the actual float coordinates which the firmware will consume. - point = {static_cast(std::clamp(point.x, 0.0, 1.0)), - static_cast(std::clamp(point.y, 0.0, 1.0))}; + point = {std::clamp(point.x, 0.0f, 1.0f), std::clamp(point.y, 0.0f, 1.0f)}; if (!Inside(input, point)) { return; } - const double POWER = MotorPower(input, point); - const double POWER_TOLERANCE = 2e-5 + 2e-7 * std::abs(input.power_budget); - if (!std::isfinite(POWER) || POWER > input.power_budget + POWER_TOLERANCE) + float power = MotorPower(input, point); + const float POWER_TOLERANCE = 2e-5f + 2e-7f * std::abs(input.power_budget); + // Rounding an analytic boundary root can put it a few ulps outside. Move + // toward a known lower-power point, preserving every convex half-plane. + // Convexity bounds power on this segment without a root-search iteration. + if (std::isfinite(power) && power > input.power_budget + POWER_TOLERANCE && + minimum_power < input.power_budget) + { + const float FRACTION = std::min( + 1.0f, (power - input.power_budget + POWER_TOLERANCE) / (power - minimum_power)); + point = {point.x + FRACTION * (minimum.x - point.x), + point.y + FRACTION * (minimum.y - point.y)}; + power = MotorPower(input, point); + if (!Inside(input, point)) + { + return; + } + } + if (!std::isfinite(power) || power > input.power_budget + POWER_TOLERANCE) { return; } - const double VALUE = input.weights.x * point.x + input.weights.y * point.y; - if (!found || VALUE > best_value + 1e-12 || - (std::abs(VALUE - best_value) <= 1e-12 && point.x + point.y > best.x + best.y)) + const float VALUE = input.weights.x * point.x + input.weights.y * point.y; + if (!found || VALUE > best_value + 2e-7f || + (std::abs(VALUE - best_value) <= 2e-7f && point.x + point.y > best.x + best.y)) { best = point; best_value = VALUE; @@ -256,47 +270,48 @@ struct Search void Edge(Point start, Point end) { const Point STEP{end.x - start.x, end.y - start.y}; - const double A = quadratic.a * STEP.x * STEP.x + 2.0 * quadratic.b * STEP.x * STEP.y + - quadratic.c * STEP.y * STEP.y; - const double B = 2.0 * quadratic.a * start.x * STEP.x + - 2.0 * quadratic.b * (start.x * STEP.y + start.y * STEP.x) + - 2.0 * quadratic.c * start.y * STEP.y + quadratic.d * STEP.x + - quadratic.e * STEP.y; - const double C = quadratic.Value(start) - input.power_budget; - if (A > 0.0) + const float A = quadratic.a * STEP.x * STEP.x + 2.0f * quadratic.b * STEP.x * STEP.y + + quadratic.c * STEP.y * STEP.y; + const float B = 2.0f * quadratic.a * start.x * STEP.x + + 2.0f * quadratic.b * (start.x * STEP.y + start.y * STEP.x) + + 2.0f * quadratic.c * start.y * STEP.y + quadratic.d * STEP.x + + quadratic.e * STEP.y; + const float C = quadratic.Value(start) - input.power_budget; + if (A > 0.0f) { - const double T = std::clamp(-B / (2.0 * A), 0.0, 1.0); + const float T = std::clamp(-B / (2.0f * A), 0.0f, 1.0f); Minimum({start.x + T * STEP.x, start.y + T * STEP.y}); } - if (std::abs(A) <= 1e-14 * (1.0 + std::abs(B) + std::abs(C))) + if (std::abs(A) <= 2e-7f * (1.0f + std::abs(B) + std::abs(C))) { - if (B != 0.0) + if (B != 0.0f) { - const double T = -C / B; - if (T >= -1e-10 && T <= 1.0 + 1e-10) + const float T = -C / B; + if (T >= -2e-7f && T <= 1.0f + 2e-7f) { - Candidate({start.x + std::clamp(T, 0.0, 1.0) * STEP.x, - start.y + std::clamp(T, 0.0, 1.0) * STEP.y}); + Candidate({start.x + std::clamp(T, 0.0f, 1.0f) * STEP.x, + start.y + std::clamp(T, 0.0f, 1.0f) * STEP.y}); } } return; } - double discriminant = B * B - 4.0 * A * C; - const double DISCRIMINANT_TOLERANCE = 1e-12 * (B * B + std::abs(4.0 * A * C) + 1e-30); + float discriminant = B * B - 4.0f * A * C; + const float DISCRIMINANT_TOLERANCE = + 2e-7f * (B * B + std::abs(4.0f * A * C) + 1e-30f); if (discriminant < -DISCRIMINANT_TOLERANCE) { return; } - discriminant = std::max(0.0, discriminant); - const double ROOT = std::sqrt(discriminant); + discriminant = std::max(0.0f, discriminant); + const float ROOT = std::sqrt(discriminant); // Stable quadratic roots, avoiding cancellation when B and sqrt(delta) are close. - const double Q = -0.5 * (B + std::copysign(ROOT, B)); - const double ROOTS[2] = {Q / A, Q != 0.0 ? C / Q : -B / (2.0 * A)}; - for (double t : ROOTS) + const float Q = -0.5f * (B + std::copysign(ROOT, B)); + const float ROOTS[2] = {Q / A, Q != 0.0f ? C / Q : -B / (2.0f * A)}; + for (float t : ROOTS) { - if (t >= -1e-10 && t <= 1.0 + 1e-10) + if (t >= -2e-7f && t <= 1.0f + 2e-7f) { - t = std::clamp(t, 0.0, 1.0); + t = std::clamp(t, 0.0f, 1.0f); Candidate({start.x + t * STEP.x, start.y + t * STEP.y}); } } @@ -304,51 +319,69 @@ struct Search void Interior() { - const double A = quadratic.a; - const double B = quadratic.b; - const double C = quadratic.c; - const double D = quadratic.d; - const double E = quadratic.e; - const double DETERMINANT = A * C - B * B; - const double SCALE = std::max(A, C); - if (DETERMINANT > 1e-14 * SCALE * SCALE && SCALE > 0.0) + const float A = quadratic.a; + const float B = quadratic.b; + const float C = quadratic.c; + const float EIGENVALUE = A + C; + if (EIGENVALUE <= 0.0f) { - const Point CENTER{(B * E - C * D) / (2.0 * DETERMINANT), - (B * D - A * E) / (2.0 * DETERMINANT)}; + return; // Affine or constant power; polygon edges cover the optimum. + } + // Rotate before forming the Gram matrix. Directly subtracting a*c-b*b + // loses a thin ellipse's small curvature in single precision. Projecting + // the original motor-current vectors preserves it as a sum of squares. + const float RATIO = A >= C ? B / A : B / C; + const float INVERSE_LENGTH = 1.0f / std::sqrt(1.0f + RATIO * RATIO); + const Point normal = A >= C ? Point{INVERSE_LENGTH, RATIO * INVERSE_LENGTH} + : Point{RATIO * INVERSE_LENGTH, INVERSE_LENGTH}; + const Point TANGENT{-normal.y, normal.x}; + float qn = 0.0f; + float qt = 0.0f; + float qnt = 0.0f; + float dn = 0.0f; + float dt = 0.0f; + for (const auto& MOTOR : input.motors) + { + const float UN = MOTOR.u * normal.x + MOTOR.v * normal.y; + const float UT = MOTOR.u * TANGENT.x + MOTOR.v * TANGENT.y; + const float LINEAR = 2.0f * MOTOR.k1 * MOTOR.c + MOTOR.kt * MOTOR.rpm; + qn += MOTOR.k1 * UN * UN; + qt += MOTOR.k1 * UT * UT; + qnt += MOTOR.k1 * UN * UT; + dn += LINEAR * UN; + dt += LINEAR * UT; + } + const float DN = dn; + const float DT = dt; + const float WN = input.weights.x * normal.x + input.weights.y * normal.y; + const float WT = input.weights.x * TANGENT.x + input.weights.y * TANGENT.y; + const float DETERMINANT = qn * qt - qnt * qnt; + if (DETERMINANT > 1e-12f * qn * qn) + { + const float CN = (qnt * DT - qt * DN) / (2.0f * DETERMINANT); + const float CT = (qnt * DN - qn * DT) / (2.0f * DETERMINANT); + const Point CENTER{normal.x * CN + TANGENT.x * CT, normal.y * CN + TANGENT.y * CT}; Minimum(CENTER); Candidate(CENTER); - const double RADIUS_SQUARED = input.power_budget - quadratic.Value(CENTER); - const Point DIRECTION{(C * input.weights.x - B * input.weights.y) / DETERMINANT, - (A * input.weights.y - B * input.weights.x) / DETERMINANT}; - const double NORM = input.weights.x * DIRECTION.x + input.weights.y * DIRECTION.y; - if (RADIUS_SQUARED >= 0.0 && NORM > 0.0) + const float RADIUS_SQUARED = input.power_budget - MotorPower(input, CENTER); + const float DIRECTION_N = (qt * WN - qnt * WT) / DETERMINANT; + const float DIRECTION_T = (qn * WT - qnt * WN) / DETERMINANT; + const float NORM = WN * DIRECTION_N + WT * DIRECTION_T; + if (RADIUS_SQUARED >= 0.0f && NORM > 0.0f) { - const double STEP = std::sqrt(RADIUS_SQUARED / NORM); - Candidate({CENTER.x + STEP * DIRECTION.x, CENTER.y + STEP * DIRECTION.y}); + const float STEP = std::sqrt(RADIUS_SQUARED / NORM); + Candidate({CENTER.x + STEP * (normal.x * DIRECTION_N + TANGENT.x * DIRECTION_T), + CENTER.y + STEP * (normal.y * DIRECTION_N + TANGENT.y * DIRECTION_T)}); } return; } - const double EIGENVALUE = A + C; - if (EIGENVALUE <= 0.0) - { - return; // Affine or constant power; polygon edges cover the optimum. - } - // Rank-one PSD: a strip/line, or a parabola when the linear null-space term is - // nonzero. - Point normal = A >= C ? Point{A, B} : Point{B, C}; - const double LENGTH = std::hypot(normal.x, normal.y); - normal.x /= LENGTH; - normal.y /= LENGTH; - const Point TANGENT{-normal.y, normal.x}; - const double DN = D * normal.x + E * normal.y; - const double DT = D * TANGENT.x + E * TANGENT.y; - const double WN = input.weights.x * normal.x + input.weights.y * normal.y; - const double WT = input.weights.x * TANGENT.x + input.weights.y * TANGENT.y; - if (std::abs(DT) > 1e-12 * (1.0 + std::abs(D) + std::abs(E)) && WT / DT > 0.0) + // Rank-one PSD: a strip/line, or a parabola when the linear null-space + // term is nonzero. Edges cover a strip's optimum. + if (std::abs(DT) > 2e-7f * (1.0f + std::abs(DN) + std::abs(DT)) && WT / DT > 0.0f) { - const double MULTIPLIER = WT / DT; - const double N = (WN / MULTIPLIER - DN) / (2.0 * EIGENVALUE); - const double T = + const float MULTIPLIER = WT / DT; + const float N = (WN / MULTIPLIER - DN) / (2.0f * EIGENVALUE); + const float T = (input.power_budget - quadratic.f - EIGENVALUE * N * N - DN * N) / DT; Candidate({normal.x * N + TANGENT.x * T, normal.y * N + TANGENT.y * T}); } @@ -375,12 +408,28 @@ inline Result Solve(const Input& input) return result; } } - Detail::Polygon polygon{{{0.0, 0.0}, {1.0, 0.0}, {1.0, 1.0}, {0.0, 1.0}}, 4}; for (int i = 0; i < input.halfplane_count; ++i) { const auto& PLANE = input.halfplanes[i]; - if (!std::isfinite(PLANE.x) || !std::isfinite(PLANE.y) || - !std::isfinite(PLANE.limit) || !Detail::Clip(polygon, PLANE)) + if (!std::isfinite(PLANE.x) || !std::isfinite(PLANE.y) || !std::isfinite(PLANE.limit)) + { + return result; + } + } + // Do not build a polygon or quadratic for the unconstrained common case. + if (Detail::Inside(input, {1.0f, 1.0f})) + { + const float POWER = Detail::MotorPower(input, {1.0f, 1.0f}); + if (std::isfinite(POWER) && POWER <= input.power_budget) + { + return {true, true, {1.0f, 1.0f}, POWER, Status::OPTIMAL}; + } + } + + Detail::Polygon polygon{{{0.0f, 0.0f}, {1.0f, 0.0f}, {1.0f, 1.0f}, {0.0f, 1.0f}}, 4}; + for (int i = 0; i < input.halfplane_count; ++i) + { + if (!Detail::Clip(polygon, input.halfplanes[i])) { return result; } @@ -390,19 +439,32 @@ inline Result Solve(const Input& input) { return result; } - const auto QUADRATIC = Detail::MakeQuadratic(input); - Detail::Search search{input, QUADRATIC}; - // Monotone objective: the fully requested acceleration is globally optimal if feasible. - if (Detail::Inside(input, {1.0, 1.0}) && - Detail::MotorPower(input, {1.0, 1.0}) <= input.power_budget) + + // With inactive power constraints a linear objective attains its optimum at + // a polygon vertex. This also makes hard traction saturation a cheap path. + Detail::Point vertex = polygon.points[0]; + float vertex_value = input.weights.x * vertex.x + input.weights.y * vertex.y; + for (int i = 1; i < polygon.count; ++i) { - result.feasible = true; - result.minimum_available = true; - result.point = {1.0f, 1.0f}; - result.power = static_cast(Detail::MotorPower(input, {1.0, 1.0})); - result.status = Status::OPTIMAL; - return result; + const auto POINT = polygon.points[i]; + const float VALUE = input.weights.x * POINT.x + input.weights.y * POINT.y; + if (VALUE > vertex_value || + (VALUE == vertex_value && POINT.x + POINT.y > vertex.x + vertex.y)) + { + vertex = POINT; + vertex_value = VALUE; + } } + vertex = {std::clamp(vertex.x, 0.0f, 1.0f), std::clamp(vertex.y, 0.0f, 1.0f)}; + const float VERTEX_POWER = Detail::MotorPower(input, vertex); + if (Detail::Inside(input, vertex) && std::isfinite(VERTEX_POWER) && + VERTEX_POWER <= input.power_budget) + { + return {true, true, {vertex.x, vertex.y}, VERTEX_POWER, Status::OPTIMAL}; + } + + const auto QUADRATIC = Detail::MakeQuadratic(input); + Detail::Search search{input, QUADRATIC}; for (int i = 0; i < polygon.count; ++i) { search.Minimum(polygon.points[i]); @@ -414,9 +476,8 @@ inline Result Solve(const Input& input) result.minimum_available = std::isfinite(search.minimum_power); result.feasible = search.found; const auto POINT = search.found ? search.best : search.minimum; - result.point = {static_cast(POINT.x), static_cast(POINT.y)}; - result.power = - static_cast(Detail::MotorPower(input, {result.point.x, result.point.y})); + result.point = {POINT.x, POINT.y}; + result.power = Detail::MotorPower(input, {result.point.x, result.point.y}); result.status = search.found ? Status::OPTIMAL : Status::INFEASIBLE; return result; } diff --git a/Helm.hpp b/Helm.hpp index 16eabee..d7104a9 100644 --- a/Helm.hpp +++ b/Helm.hpp @@ -299,6 +299,7 @@ class Helm helm->mutex_.Unlock(); while (true) { + const auto LOOP_START = LibXR::Timebase::GetMicroseconds(); if (cmd_suber.Available()) { helm->cmd_data_ = cmd_suber.GetData(); @@ -329,10 +330,42 @@ class Helm helm->CompensateWheelSpeed(); helm->DynamicInverseSolution(); helm->CalculateMotorCurrent(); + const auto POWER_START = LibXR::Timebase::GetMicroseconds(); helm->PowerControlUpdate(); + helm->control_diagnostics_.power_control_time_us = ElapsedMicroseconds(POWER_START); + helm->control_diagnostics_.max_power_control_time_us = + std::max(helm->control_diagnostics_.max_power_control_time_us, + helm->control_diagnostics_.power_control_time_us); helm->mutex_.Unlock(); + const auto OUTPUT_START = LibXR::Timebase::GetMicroseconds(); helm->Output(); - helm->thread_.SleepUntil(last_time, 2); + auto& diagnostics = helm->control_diagnostics_; + diagnostics.output_time_us = ElapsedMicroseconds(OUTPUT_START); + diagnostics.max_output_time_us = + std::max(diagnostics.max_output_time_us, diagnostics.output_time_us); + ++diagnostics.loop_count; +#if defined(LIBXR_SYSTEM_freertos) && INCLUDE_uxTaskGetStackHighWaterMark == 1 + if ((diagnostics.loop_count & 255U) == 0U) + { + diagnostics.stack_min_free_bytes = + uxTaskGetStackHighWaterMark(nullptr) * sizeof(StackType_t); + } +#endif + diagnostics.loop_time_us = ElapsedMicroseconds(LOOP_START); + diagnostics.max_loop_time_us = + std::max(diagnostics.max_loop_time_us, diagnostics.loop_time_us); + if (diagnostics.loop_time_us >= CONTROL_PERIOD_US) + { + ++diagnostics.loop_overrun_count; + } + const auto NOW_MS = LibXR::Timebase::GetMilliseconds(); + if ((NOW_MS - last_time).ToMillisecond() >= CONTROL_PERIOD_MS) + { + // Skip missed releases instead of running back-to-back catch-up loops. + last_time = NOW_MS; + ++diagnostics.schedule_reset_count; + } + helm->thread_.SleepUntil(last_time, CONTROL_PERIOD_MS); } } /** @@ -343,6 +376,14 @@ class Helm { auto now = LibXR::Timebase::GetMicroseconds(); dt_ = (now - last_online_time_).ToSecondf(); + control_diagnostics_.period_us = + static_cast((now - last_online_time_).ToMicrosecond()); + control_diagnostics_.max_period_us = + std::max(control_diagnostics_.max_period_us, control_diagnostics_.period_us); + if (control_diagnostics_.period_us >= 3000U) + { + ++control_diagnostics_.period_over_3ms_count; + } last_online_time_ = now; feedback_valid_ = true; @@ -470,8 +511,16 @@ class Helm void PowerControlUpdate() { constraint_output_valid_ = false; + constraint_infeasible_ = false; constraint_pid_scale_ = 0.0f; acceleration_constraint_result_ = {}; + control_diagnostics_.solver_attempts = 0; + control_diagnostics_.solve_time_us = 0; + control_diagnostics_.total_solve_time_us = 0; + control_diagnostics_.model_update_time_us = 0; + control_diagnostics_.requested_ax = target_ax_; + control_diagnostics_.requested_ay = target_ay_; + control_diagnostics_.requested_alpha = target_alpha_; if (chassis_event_ == ChassisMode::RELAX || !feedback_valid_ || !measured_chassis_velocity_.valid || !std::isfinite(dt_) || dt_ <= 0.0f || !std::isfinite(target_vx_) || !std::isfinite(target_vy_) || @@ -480,6 +529,7 @@ class Helm return; } + const auto MODEL_START = LibXR::Timebase::GetMicroseconds(); // Feedback and command currents use the same configured motor direction. for (int i = 0; i < 4; i++) { @@ -491,6 +541,11 @@ class Helm motor_data_.rotorspeed_rpm_3508); power_control_->CalculatePowerControlParam(); const auto MODEL = power_control_->GetDrivePowerModel(); + control_diagnostics_.model_update_time_us = ElapsedMicroseconds(MODEL_START); + control_diagnostics_.max_model_update_time_us = + std::max(control_diagnostics_.max_model_update_time_us, + control_diagnostics_.model_update_time_us); + control_diagnostics_.power_model = MODEL; if (!MODEL.valid || !std::isfinite(PARAM.friction_coefficient) || PARAM.friction_coefficient <= 0.0f || !std::isfinite(PARAM.gravity) || PARAM.gravity <= 0.0f || !std::isfinite(PARAM.wheel_radius) || @@ -512,6 +567,7 @@ class Helm const float CURRENT_LIMIT = TORQUE_LIMIT * CURRENT_PER_TORQUE; AccelerationConstraint::Input input{}; input.power_budget = GetChassisPowerBudget() - MODEL.static_loss - MODEL.margin; + control_diagnostics_.drive_power_budget = input.power_budget; input.weights = { TRANSLATION_REQUEST > 1.0e-6f ? PARAM.translation_acceleration_weight : 0.0f, fabsf(target_alpha_) > 1.0e-6f ? (chassis_event_ == ChassisMode::ROTOR @@ -545,7 +601,13 @@ class Helm input.halfplanes[input.halfplane_count++] = {-motor.u, -motor.v, CURRENT_LIMIT + motor.c}; } + const auto SOLVE_START = LibXR::Timebase::GetMicroseconds(); acceleration_constraint_result_ = AccelerationConstraint::Solve(input); + control_diagnostics_.solve_time_us = ElapsedMicroseconds(SOLVE_START); + control_diagnostics_.max_solve_time_us = std::max( + control_diagnostics_.max_solve_time_us, control_diagnostics_.solve_time_us); + control_diagnostics_.total_solve_time_us += control_diagnostics_.solve_time_us; + ++control_diagnostics_.solver_attempts; if (acceleration_constraint_result_.feasible) { constraint_pid_scale_ = pid_scale; @@ -555,8 +617,18 @@ class Helm } if (!constraint_output_valid_) { - // Do not claim an impossible electrical budget was met. Output() relaxes - // the motors; zero acceleration alone may still exceed the loss model. + constraint_infeasible_ = acceleration_constraint_result_.status == + AccelerationConstraint::Status::INFEASIBLE; + if (constraint_infeasible_) + { + ++control_diagnostics_.infeasible_count; + } + else + { + ++control_diagnostics_.invalid_solver_input_count; + } + // Zero drive current is the fallback, not a feasible optimum. Speed loss + // alone can exceed the budget. Keep valid steering control independent. return; } @@ -903,7 +975,14 @@ class Helm */ void Output() { - if (chassis_event_ == ChassisMode::RELAX || !constraint_output_valid_) + bool finite_output = true; + for (int i = 0; i < 4; i++) + { + finite_output = finite_output && std::isfinite(steer_out_[i]) && + (!constraint_output_valid_ || std::isfinite(wheel_out_[i])); + } + if (chassis_event_ == ChassisMode::RELAX || !feedback_valid_ || !finite_output || + (!constraint_output_valid_ && !constraint_infeasible_)) { LostCtrl(); } @@ -911,7 +990,8 @@ class Helm { for (int i = 0; i < 4; i++) { - motor_wheel_cmd_[i].torque = std::clamp(wheel_out_[i], -4.4f, 4.4f); + motor_wheel_cmd_[i].torque = + constraint_output_valid_ ? std::clamp(wheel_out_[i], -4.4f, 4.4f) : 0.0f; motor_steer_cmd_[i].torque = std::clamp(steer_out_[i], -2.5f, 2.5f); } for (int i = 0; i < 4; i++) @@ -925,6 +1005,50 @@ class Helm private: const ChassisParam PARAM; + static constexpr uint32_t CONTROL_PERIOD_MS = 2; + static constexpr uint32_t CONTROL_PERIOD_US = CONTROL_PERIOD_MS * 1000; + + static uint32_t ElapsedMicroseconds(LibXR::MicrosecondTimestamp start) + { + return static_cast( + (LibXR::Timebase::GetMicroseconds() - start).ToMicrosecond()); + } + + // Instance diagnostics only: no logging or global sampling symbols in the loop. + // Work times include preemption and lock waits, but exclude scheduled sleep. + // period_us measures Update-to-Update, including sleep, just like dt_. + struct ControlDiagnostics + { + uint32_t loop_count = 0; + uint32_t loop_time_us = 0; + uint32_t max_loop_time_us = 0; + uint32_t period_us = 0; + uint32_t max_period_us = 0; + uint32_t period_over_3ms_count = 0; + uint32_t loop_overrun_count = 0; + uint32_t schedule_reset_count = 0; + uint32_t power_control_time_us = 0; + uint32_t max_power_control_time_us = 0; + uint32_t output_time_us = 0; + uint32_t max_output_time_us = 0; + uint32_t model_update_time_us = 0; + uint32_t max_model_update_time_us = 0; + uint32_t solve_time_us = 0; + uint32_t max_solve_time_us = 0; + uint32_t total_solve_time_us = 0; + uint32_t solver_attempts = 0; + uint32_t infeasible_count = 0; + uint32_t invalid_solver_input_count = 0; + uint32_t stack_min_free_bytes = 0; + float drive_power_budget = 0.0f; + float requested_ax = 0.0f; + float requested_ay = 0.0f; + float requested_alpha = 0.0f; + PowerControl::DrivePowerModel power_model{}; + }; + + ControlDiagnostics control_diagnostics_{}; + struct WheelPosition { float x; @@ -1103,6 +1227,7 @@ class Helm MotorData motor_data_ = {}; PowerControl* power_control_; bool constraint_output_valid_ = false; + bool constraint_infeasible_ = false; float constraint_pid_scale_ = 0.0f; AccelerationConstraint::Result acceleration_constraint_result_{}; diff --git a/HelmMigrationRecord.md b/HelmMigrationRecord.md index 8ba7b0f..bd6b394 100644 --- a/HelmMigrationRecord.md +++ b/HelmMigrationRecord.md @@ -38,8 +38,13 @@ swerve-controller report: Sutherland-Hodgman half-plane clipping, feasible polyg vertices, quadratic/segment intersections, and the power ellipse's objective support point. Rank-zero/rank-one quadratics, point/line polygons, negative budgets, and infeasible requests are handled without changing the physical quadratic. -The solver uses fixed arrays and no dynamic allocation; intermediate double -arithmetic improves numerical accuracy but requires software arithmetic on M4F. +The solver uses fixed arrays and no dynamic allocation. Its arithmetic is single +precision for the M4F hardware FPU. It first checks the full request, then the +polygon's objective-maximizing vertex, before enumerating power intersections and +support points. Thin ellipses use a rotated Gram matrix constructed from motor +current coefficients. Rounded boundary candidates are checked against the actual +float current/power expressions and, if needed, moved toward a known lower-power +feasible point without relaxing the power limit. Decision coordinates are retained fractions x,y in [0,1] of requested translation and angular acceleration. Translation direction and angular acceleration sign are @@ -78,11 +83,44 @@ currently decodes an output-capability ratio, not a separately measured energy in joules; this board-protocol interpretation still needs hardware confirmation. If full frozen PID makes the constrained request infeasible, solve again at PID -scales 0.5 and 0 without advancing PID state. If still infeasible, or feedback, -commands, timing or model are invalid, the output path relaxes the motors. This -is an explicit failure fallback, not a claim that impossible modeled power can be -met. `acceleration_constraint_result_`, `constraint_pid_scale_` and -`constraint_output_valid_` retain the result/status inside Helm for diagnosis. +scales 0.5 and 0 without advancing PID state. If the solver still reports +INFEASIBLE, drive torque is zero while valid steering control continues, avoiding +relax/re-enable cycles of the steering motors. Invalid feedback, commands, timing, +model or solver input still relax all motors, as does RELAX. Zero drive current +is an explicit fallback, not a claim that impossible modeled power can be met: +the speed-loss term alone can exceed the budget. `acceleration_constraint_result_`, +`constraint_pid_scale_`, `constraint_output_valid_` and `constraint_infeasible_` +retain the result/status inside Helm for diagnosis. + +The control thread uses a 2 ms scheduled period. A missed release is discarded +and the schedule is restarted, avoiding back-to-back catch-up iterations. This +does not make an overlong computation meet its deadline. `Sleep(1)` would instead +add a tick delay to variable computation time and does not enforce a 2 ms period. + +`control_diagnostics_` is an instance member, with no fixed RAM globals or printing +in the control loop. Work durations use the existing microsecond timebase and +include interrupts, preemption and mutex waits. Inspect these on the MCU: + +| Member | Meaning | +| --- | --- | +| `period_us`, `max_period_us` | Update-to-Update interval, including sleep, matching `dt_` | +| `loop_time_us`, `max_loop_time_us` | Complete iteration work, excluding scheduled sleep | +| `loop_overrun_count` | Iterations with work lasting at least 2000 us | +| `period_over_3ms_count` | Update intervals lasting at least 3000 us | +| `power_control_time_us`, `max_power_control_time_us` | Entire power/traction update, including all retries | +| `model_update_time_us`, `max_model_update_time_us` | Motor-data submission, online identification and model snapshot | +| `solve_time_us`, `max_solve_time_us` | Last individual solve and maximum individual solve | +| `total_solve_time_us`, `solver_attempts` | Sum of solve times and attempt count in the latest iteration | +| `output_time_us`, `max_output_time_us` | Motor command submission, including any driver logging | +| `infeasible_count`, `invalid_solver_input_count` | Failed solver cycles, excluding early feedback/model rejection | +| `stack_min_free_bytes` | FreeRTOS stack high-water mark in bytes, sampled every 256 iterations when enabled | +| `drive_power_budget`, `power_model` | Budget after static loss/margin, and live coefficients | +| `requested_ax`, `requested_ay`, `requested_alpha` | Acceleration requests before constraint scaling | + +Watch maxima over repeated constraint activation and all three retry paths; +average solve duration cannot establish a 2 ms worst-case bound. If solver time +is short while iteration time is long, investigate other tasks, driver output +and logging. This driver can log on every command when a motor exceeds 75 C. Other product configurations retain their constructor contract after xrobot sync; only Helm receives the new fields. No vendor code is changed. Module changes remain diff --git a/tests/acceleration_constraint_test.cpp b/tests/acceleration_constraint_test.cpp index edbd657..738f07c 100644 --- a/tests/acceleration_constraint_test.cpp +++ b/tests/acceleration_constraint_test.cpp @@ -161,6 +161,21 @@ void deterministic() assert(!result.feasible && !result.minimum_available && result.status == Status::INFEASIBLE); + // Near-collinear motor vectors produce a thin interior ellipse. Forming + // a*c-b*b in float would erase its small but physically significant axis. + input = {}; + input.motors[0] = {10000.0f, 10000.0f, -10000.0f, 0.0f, 0.0f, 1.0f, 0.0f}; + input.motors[1] = {1.0f, -1.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f}; + input.power_budget = 0.1f; + input.weights = {1.0f, 4.0f}; + result = Solve(input); + check_feasible(input, result); + const double SUPPORT = std::sqrt(0.1 / (6.25e-8 + 2.25)); + near(result.point.x, + static_cast((1.0 + 2.5e-8 * SUPPORT - 1.5 * SUPPORT) / 2.0)); + near(result.point.y, + static_cast((1.0 + 2.5e-8 * SUPPORT + 1.5 * SUPPORT) / 2.0)); + input = {}; input.power_budget = std::numeric_limits::quiet_NaN(); assert(Solve(input).status == Status::INVALID_INPUT); diff --git a/tests/helm_constraint_model_test.cpp b/tests/helm_constraint_model_test.cpp index df7a20d..665fb57 100644 --- a/tests/helm_constraint_model_test.cpp +++ b/tests/helm_constraint_model_test.cpp @@ -179,8 +179,8 @@ void check_result(const Scenario& scenario, const Model& model, const float AFFINE_CURRENT = MOTOR.u * X + MOTOR.v * Y + MOTOR.c; near(TORQUE * CURRENT_PER_TORQUE, AFFINE_CURRENT, 0.005); } - // Separate float reconstruction may differ slightly from the solver's double - // evaluation. + // Separate reconstruction may differ slightly from the solver's affine + // current evaluation. assert(output_power(scenario, model, X, Y) <= scenario.power_budget + 3e-4); } @@ -311,6 +311,33 @@ void pid_fallbacks() } } +void high_demand_saturation() +{ + // Large stick steps exercise the traction-limited vertex path and large + // affine coefficients, rather than only the small-command early return. + for (const float ACCELERATION : {20.0f, 80.0f, 200.0f}) + { + for (const float RPM : {0.0f, 3000.0f, -3000.0f}) + { + Scenario scenario; + scenario.ay = ACCELERATION; + scenario.rpm.fill(RPM); + scenario.power_budget = 90.5f; + const Model MODEL = make_model(scenario, 0.0f); + const auto RESULT = AccelerationConstraint::Solve(MODEL.input); + check_result(scenario, MODEL, RESULT); + assert(RESULT.point.x < 1.0f); + assert(RESULT.point.x > 0.0f); + // At rest traction alone sets the optimum; the retained acceleration + // must remain independent of the raw acceleration request's magnitude. + if (RPM == 0.0f) + { + near(ACCELERATION * RESULT.point.x, MU * GRAVITY, 3e-5); + } + } + } +} + void random_physical_scenarios() { std::mt19937 random(0xc0ffeeU); @@ -375,6 +402,7 @@ int main() feedforward_equivalence(); deterministic(); pid_fallbacks(); + high_demand_saturation(); random_physical_scenarios(); std::puts( "PASS: 5000 FF equivalence cases, physical torque/current/grip/power bounds, " From dfce7594a9c230614bc418dee7c83dab9d8c5810 Mon Sep 17 00:00:00 2001 From: kaMi-0932 <1580654962@qq.com> Date: Sun, 8 Nov 2026 17:19:51 +0800 Subject: [PATCH 6/6] Save current swerve spin and translation settings --- Helm.hpp | 39 +++++++++++++++++++++++++++++++++++++-- 1 file changed, 37 insertions(+), 2 deletions(-) diff --git a/Helm.hpp b/Helm.hpp index d7104a9..1d5d62f 100644 --- a/Helm.hpp +++ b/Helm.hpp @@ -441,6 +441,11 @@ class Helm { const float MAX_WHEEL_LINEAR_SPEED = GetMaxWheelLinearSpeed(); const float MAX_CHASSIS_OMEGA = GetMaxChassisOmega(); + if (chassis_event_ != ChassisMode::ROTOR) + { + rotor_ramped_vx_ = 0.0f; + rotor_ramped_vy_ = 0.0f; + } /* 先生成目标角速度。 */ switch (chassis_event_) @@ -450,9 +455,11 @@ class Helm break; case ChassisMode::INDEPENDENT: case ChassisMode::ROTOR: - target_omega_ = cmd_data_.z * MAX_CHASSIS_OMEGA; + target_omega_ = 0.7 * MAX_CHASSIS_OMEGA; break; case ChassisMode::FOLLOW: + target_omega_ = + -pid_follow_.Calculate(0.0f, current_yaw_, dt_) * MAX_CHASSIS_OMEGA; /* 增加FOLLOW死区,避免不期望抖动 */ { const float BETA = LibXR::CycleValue(current_yaw_) - 0.0f; @@ -488,13 +495,39 @@ class Helm case ChassisMode::FOLLOW: case ChassisMode::ROTOR: { - const float BETA = current_yaw_; + const float BETA = 2 * current_yaw_; const float COS_BETA = cosf(BETA); const float SIN_BETA = sinf(BETA); target_vx_ = (COS_BETA * cmd_data_.x - SIN_BETA * cmd_data_.y) * MAX_WHEEL_LINEAR_SPEED; target_vy_ = (SIN_BETA * cmd_data_.x + COS_BETA * cmd_data_.y) * MAX_WHEEL_LINEAR_SPEED; + /*限制平移速度来保持自旋*/ + if (chassis_event_ == ChassisMode::ROTOR) + { + constexpr float MAX_TRANSLATION_SPEED = 1.5f; + // constexpr float MAX_TRANSLATION_ACCELERATION = 1.0f; + const float REQUESTED_SPEED = hypotf(target_vx_, target_vy_); + if (REQUESTED_SPEED > MAX_TRANSLATION_SPEED) + { + const float SPEED_SCALE = MAX_TRANSLATION_SPEED / REQUESTED_SPEED; + target_vx_ *= SPEED_SCALE; + target_vy_ *= SPEED_SCALE; + } + + // const float DELTA_X = target_vx_ - rotor_ramped_vx_; + // const float DELTA_Y = target_vy_ - rotor_ramped_vy_; + // const float DELTA_SPEED = hypotf(DELTA_X, DELTA_Y); + // const float MAX_DELTA_SPEED = MAX_TRANSLATION_ACCELERATION * dt_; + // if (DELTA_SPEED > MAX_DELTA_SPEED && DELTA_SPEED > 0.0f) + // { + // const float RAMP_SCALE = MAX_DELTA_SPEED / DELTA_SPEED; + // target_vx_ = rotor_ramped_vx_ + DELTA_X * RAMP_SCALE; + // target_vy_ = rotor_ramped_vy_ + DELTA_Y * RAMP_SCALE; + // } + // rotor_ramped_vx_ = target_vx_; + // rotor_ramped_vy_ = target_vy_; + } } break; default: @@ -1149,6 +1182,8 @@ class Helm ChassisVelocity measured_chassis_velocity_{}; bool feedback_valid_ = false; + float rotor_ramped_vx_ = 0.0f; + float rotor_ramped_vy_ = 0.0f; float target_vx_ = 0.0f; float target_vy_ = 0.0f; float target_omega_ = 0.0f;