From 8330656915b5512fbd7d31b832639a868bec42d7 Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Wed, 14 Jan 2026 10:50:31 +0800 Subject: [PATCH] =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E8=B7=B3=E8=B7=83=E6=A0=87?= =?UTF-8?q?=E5=BF=97?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- CMakeLists.txt | 1 + User/component/component_config.yaml | 3 + User/component/limiter.c | 167 +++++++++++++++++++++++++++ User/component/limiter.h | 93 +++++++++++++++ User/module/balance_chassis.h | 1 + User/module/gimbal.c | 78 ++++--------- User/task/ctrl_chassis.c | 6 + User/task/rc.c | 14 +++ 8 files changed, 305 insertions(+), 58 deletions(-) create mode 100644 User/component/limiter.c create mode 100644 User/component/limiter.h diff --git a/CMakeLists.txt b/CMakeLists.txt index f3b594a..0902d21 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -62,6 +62,7 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE User/component/error_detect.c User/component/filter.c User/component/freertos_cli.c + User/component/limiter.c User/component/lqr.c User/component/pid.c User/component/user_math.c diff --git a/User/component/component_config.yaml b/User/component/component_config.yaml index 0ee3384..1eadd3d 100644 --- a/User/component/component_config.yaml +++ b/User/component/component_config.yaml @@ -19,6 +19,9 @@ filter: freertos_cli: dependencies: [] enabled: true +limiter: + dependencies: [] + enabled: true pid: dependencies: - component/filter diff --git a/User/component/limiter.c b/User/component/limiter.c new file mode 100644 index 0000000..52412e2 --- /dev/null +++ b/User/component/limiter.c @@ -0,0 +1,167 @@ +/* + 限制器 +*/ + +#include "limiter.h" +#include "user_math.h" + +#include +#include + +#define POWER_BUFF_THRESHOLD 20 +#define CHASSIS_POWER_CHECK_FREQ 10 +#define CHASSIS_POWER_FACTOR_PASS 0.9f +#define CHASSIS_POWER_FACTOR_NO_PASS 1.5f + +#define CHASSIS_MOTOR_CIRCUMFERENCE 0.12f + +/** + * @brief 限制底盘功率不超过power_limit + * + * @param power_limit 最大功率 + * @param motor_out 电机输出值 + * @param speed 电机转速 + * @param len 电机数量 + * @return int8_t 0对应没有错误 + */ +int8_t PowerLimit_ChassicOutput(float power_limit, float *motor_out, + float *speed, uint32_t len) { + /* power_limit小于0时不进行限制 */ + if (motor_out == NULL || speed == NULL || power_limit < 0) return -1; + + float sum_motor_out = 0.0f; + for (uint32_t i = 0; i < len; i++) { + /* 总功率计算 P=F(由转矩电流表示)*V(由转速表示) */ + sum_motor_out += + fabsf(motor_out[i]) * fabsf(speed[i]) * CHASSIS_MOTOR_CIRCUMFERENCE; + } + + /* 保持每个电机输出值缩小时比例不变 */ + if (sum_motor_out > power_limit) { + for (uint32_t i = 0; i < len; i++) { + motor_out[i] *= power_limit / sum_motor_out; + } + } + + return 0; +} + +/** + * @brief 电容输入功率计算 + * + * @param power_in 底盘当前功率 + * @param power_limit 裁判系统功率限制值 + * @param power_buffer 缓冲能量 + * @return float 裁判系统输出最大值 + */ +float PowerLimit_CapInput(float power_in, float power_limit, + float power_buffer) { + float target_power = 0.0f; + + /* 计算下一个检测周期的剩余缓冲能量 */ + float heat_buff = power_buffer - (float)(power_in - power_limit) / + (float)CHASSIS_POWER_CHECK_FREQ; + if (heat_buff < POWER_BUFF_THRESHOLD) { /* 功率限制 */ + target_power = power_limit * CHASSIS_POWER_FACTOR_PASS; + } else { + target_power = power_limit * CHASSIS_POWER_FACTOR_NO_PASS; + } + + return target_power; +} + +/** + * @brief 使用缓冲能量计算底盘最大功率 + * + * @param power_limit 裁判系统功率限制值 + * @param power_buffer 缓冲能量 + * @return float 底盘输出最大值 + */ +float PowerLimit_TargetPower(float power_limit, float power_buffer) { + float target_power = 0.0f; + + /* 根据剩余缓冲能量计算输出功率 */ + target_power = power_limit * (power_buffer - 10.0f) / 20.0f; + if (target_power < 0.0f) target_power = 0.0f; + + return target_power; +} + +/** + * @brief 射击频率控制 + * + * @param heat 当前热量 + * @param heat_limit 热量上限 + * @param cooling_rate 冷却速率 + * @param heat_increase 冷却增加 + * @param shoot_freq 经过热量限制后的射击频率 + * @return float 射击频率 + */ +float HeatLimit_ShootFreq(float heat, float heat_limit, float cooling_rate, + float heat_increase, bool is_big) { + float heat_percent = heat / heat_limit; + float stable_freq = cooling_rate / heat_increase; + if (is_big) + return stable_freq; + else + return (heat_percent > 0.7f) ? stable_freq : 3.0f * stable_freq; +} + +/** + * @brief 计算电机与IMU坐标系之间的偏差（处理跨越±π的情况） + * @param motor_angle 电机角度 + * @param imu_angle IMU角度 + * @return 偏差值（已归一化到 -π ~ π） + */ +static float CalcMotorImuOffset(float motor_angle, float imu_angle) { + float offset = motor_angle - imu_angle; + if (offset > M_PI) offset -= M_2PI; + if (offset < -M_PI) offset += M_2PI; + return offset; +} + +/** + * @brief 圆周角度限位器 - 考虑电机与IMU坐标系偏差 + */ +void CircleAngleLimit(float *setpoint, float motor_angle, float imu_angle, + float limit_max, float limit_min, float range) { + if (setpoint == NULL) return; + + /* 计算电机与IMU坐标系偏差 */ + float motor_imu_offset = CalcMotorImuOffset(motor_angle, imu_angle); + + /* 将IMU setpoint转换为电机角度后进行限位检查 */ + float motor_target = *setpoint + motor_imu_offset; + + /* 检查是否超过最大限位 */ + if (CircleError(motor_target, limit_max, range) > 0) { + *setpoint = limit_max - motor_imu_offset; + } + /* 检查是否低于最小限位 */ + if (CircleError(motor_target, limit_min, range) < 0) { + *setpoint = limit_min - motor_imu_offset; + } +} + +/** + * @brief 圆周角度增量限位器 - 考虑电机与IMU坐标系偏差 + */ +void CircleAngleDeltaLimit(float *delta, float setpoint, float motor_angle, + float imu_angle, float limit_max, float limit_min, + float range) { + if (delta == NULL) return; + + /* 计算电机与IMU坐标系偏差 */ + float motor_imu_offset = CalcMotorImuOffset(motor_angle, imu_angle); + + /* 计算应用增量后在电机坐标系下的目标角度 */ + float motor_target_after_delta = setpoint + motor_imu_offset + *delta; + + /* 计算到限位边界的距离 */ + float delta_max = CircleError(limit_max, motor_target_after_delta, range); + float delta_min = CircleError(limit_min, motor_target_after_delta, range); + + /* 限制增量 */ + if (*delta > delta_max) *delta = delta_max; + if (*delta < delta_min) *delta = delta_min; +} diff --git a/User/component/limiter.h b/User/component/limiter.h new file mode 100644 index 0000000..cb14527 --- /dev/null +++ b/User/component/limiter.h @@ -0,0 +1,93 @@ +/* + 限制器 +*/ + +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/** + * @brief 限制底盘功率不超过power_limit + * + * @param power_limit 最大功率 + * @param motor_out 电机输出值 + * @param speed 电机转速 + * @param len 电机数量 + * @return int8_t 0对应没有错误 + */ +int8_t PowerLimit_ChassicOutput(float power_limit, float *motor_out, + float *speed, uint32_t len); +/** + * @brief 电容输入功率计算 + * + * @param power_in 底盘当前功率 + * @param power_limit 裁判系统功率限制值 + * @param power_buffer 缓冲能量 + * @return float 裁判系统输出最大值 + */ +float PowerLimit_CapInput(float power_in, float power_limit, + float power_buffer); +/** + * @brief 使用缓冲能量计算底盘最大功率 + * + * @param power_limit 裁判系统功率限制值 + * @param power_buffer 缓冲能量 + * @return float 底盘输出最大值 + */ +float PowerLimit_TargetPower(float power_limit, float power_buffer); + +/** + * @brief 射击频率控制 + * + * @param heat 当前热量 + * @param heat_limit 热量上限 + * @param cooling_rate 冷却速率 + * @param heat_increase 冷却增加 + * @param shoot_freq 经过热量限制后的射击频率 + * @return float 射击频率 + */ +float HeatLimit_ShootFreq(float heat, float heat_limit, float cooling_rate, + float heat_increase, bool is_big); + +/** + * @brief 圆周角度限位器 - 考虑电机与IMU坐标系偏差 + * @note 用于将 IMU 坐标系下的目标角度限制在电机坐标系的限位范围内 + * + * @param setpoint 目标角度（IMU坐标系），会被直接修改 + * @param motor_angle 电机当前角度 + * @param imu_angle IMU当前角度 + * @param limit_max 电机坐标系下的最大限位值 + * @param limit_min 电机坐标系下的最小限位值 + * @param range 角度循环范围（通常为 M_2PI） + */ +void CircleAngleLimit(float *setpoint, float motor_angle, float imu_angle, + float limit_max, float limit_min, float range); + +/** + * @brief 圆周角度增量限位器 - 考虑电机与IMU坐标系偏差 + * @note 用于限制增量控制时不超过电机限位范围 + * + * @param delta 增量值，会被直接修改 + * @param setpoint 当前目标角度（IMU坐标系） + * @param motor_angle 电机当前角度 + * @param imu_angle IMU当前角度 + * @param limit_max 电机坐标系下的最大限位值 + * @param limit_min 电机坐标系下的最小限位值 + * @param range 角度循环范围（通常为 M_2PI） + */ +void CircleAngleDeltaLimit(float *delta, float setpoint, float motor_angle, + float imu_angle, float limit_max, float limit_min, + float range); diff --git a/User/module/balance_chassis.h b/User/module/balance_chassis.h index 0c883c5..c41c369 100644 --- a/User/module/balance_chassis.h +++ b/User/module/balance_chassis.h @@ -56,6 +56,7 @@ typedef struct { Chassis_Mode_t mode; /* 底盘模式 */ MoveVector_t move_vec; /* 底盘运动向量 */ float height; /* 目标高度 */ + bool jump_trigger; /* 跳跃触发标志 */ } Chassis_CMD_t; typedef struct { diff --git a/User/module/gimbal.c b/User/module/gimbal.c index 0d6c39e..6a45d28 100644 --- a/User/module/gimbal.c +++ b/User/module/gimbal.c @@ -8,6 +8,7 @@ #include "bsp/time.h" #include #include "component/filter.h" +#include "component/limiter.h" #include "component/pid.h" #include "device/gimbal_imu.h" #include "device/motor_dm.h" @@ -173,45 +174,23 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd, Gimbal_AI_t *g_ai) { Gimbal_SetMode(g, g_cmd->mode); - /* 处理yaw控制命令，软件限位 - 使用电机绝对角度 */ + /* 处理yaw控制命令，软件限位 */ float delta_yaw = g_cmd->delta_yaw * g->dt * 1.5f; if (g->param->travel.yaw > 0) { - /* 计算当前电机角度与IMU角度的偏差 */ - float motor_imu_offset = g->feedback.motor.yaw.rotor_abs_angle - g->feedback.imu.eulr.yaw; - /* 处理跨越±π的情况 */ - if (motor_imu_offset > M_PI) motor_imu_offset -= M_2PI; - if (motor_imu_offset < -M_PI) motor_imu_offset += M_2PI; - - /* 计算到限位边界的距离 */ - const float delta_max = CircleError(g->limit.yaw.max, - (g->setpoint.eulr.yaw + motor_imu_offset + delta_yaw), M_2PI); - const float delta_min = CircleError(g->limit.yaw.min, - (g->setpoint.eulr.yaw + motor_imu_offset + delta_yaw), M_2PI); - - /* 限制控制命令 */ - if (delta_yaw > delta_max) delta_yaw = delta_max; - if (delta_yaw < delta_min) delta_yaw = delta_min; + CircleAngleDeltaLimit(&delta_yaw, g->setpoint.eulr.yaw, + g->feedback.motor.yaw.rotor_abs_angle, + g->feedback.imu.eulr.yaw, + g->limit.yaw.max, g->limit.yaw.min, M_2PI); } CircleAdd(&(g->setpoint.eulr.yaw), delta_yaw, M_2PI); - /* 处理pitch控制命令，软件限位 - 使用电机绝对角度 */ + /* 处理pitch控制命令，软件限位 */ float delta_pit = g_cmd->delta_pit * g->dt; if (g->param->travel.pit > 0) { - /* 计算当前电机角度与IMU角度的偏差 */ - float motor_imu_offset = g->feedback.motor.pit.rotor_abs_angle - g->feedback.imu.eulr.rol; - /* 处理跨越±π的情况 */ - if (motor_imu_offset > M_PI) motor_imu_offset -= M_2PI; - if (motor_imu_offset < -M_PI) motor_imu_offset += M_2PI; - - /* 计算到限位边界的距离 */ - const float delta_max = CircleError(g->limit.pit.max, - (g->setpoint.eulr.pit + motor_imu_offset + delta_pit), M_2PI); - const float delta_min = CircleError(g->limit.pit.min, - (g->setpoint.eulr.pit + motor_imu_offset + delta_pit), M_2PI); - - /* 限制控制命令 */ - if (delta_pit > delta_max) delta_pit = delta_max; - if (delta_pit < delta_min) delta_pit = delta_min; + CircleAngleDeltaLimit(&delta_pit, g->setpoint.eulr.pit, + g->feedback.motor.pit.rotor_abs_angle, + g->feedback.imu.eulr.rol, + g->limit.pit.max, g->limit.pit.min, M_2PI); } CircleAdd(&(g->setpoint.eulr.pit), delta_pit, M_2PI); @@ -227,35 +206,18 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd, Gimbal_AI_t *g_ai) { g->setpoint.eulr.yaw = g_ai->yaw; g->setpoint.eulr.pit = -g_ai->pit; - /* 限位处理 - 需要考虑电机和IMU之间的偏差 */ + /* 限位处理 */ if (g->param->travel.yaw > 0) { - float yaw_motor_imu_offset = g->feedback.motor.yaw.rotor_abs_angle - g->feedback.imu.eulr.yaw; - if (yaw_motor_imu_offset > M_PI) yaw_motor_imu_offset -= M_2PI; - if (yaw_motor_imu_offset < -M_PI) yaw_motor_imu_offset += M_2PI; - - /* 将IMU setpoint转换为电机角度后进行限位检查 */ - float yaw_motor_target = g->setpoint.eulr.yaw + yaw_motor_imu_offset; - if (CircleError(yaw_motor_target, g->limit.yaw.max, M_2PI) > 0) { - g->setpoint.eulr.yaw = g->limit.yaw.max - yaw_motor_imu_offset; - } - if (CircleError(yaw_motor_target, g->limit.yaw.min, M_2PI) < 0) { - g->setpoint.eulr.yaw = g->limit.yaw.min - yaw_motor_imu_offset; - } + CircleAngleLimit(&g->setpoint.eulr.yaw, + g->feedback.motor.yaw.rotor_abs_angle, + g->feedback.imu.eulr.yaw, + g->limit.yaw.max, g->limit.yaw.min, M_2PI); } - if (g->param->travel.pit > 0) { - float pit_motor_imu_offset = g->feedback.motor.pit.rotor_abs_angle - g->feedback.imu.eulr.rol; - if (pit_motor_imu_offset > M_PI) pit_motor_imu_offset -= M_2PI; - if (pit_motor_imu_offset < -M_PI) pit_motor_imu_offset += M_2PI; - - /* 将IMU setpoint转换为电机角度后进行限位检查 */ - float pit_motor_target = g->setpoint.eulr.pit + pit_motor_imu_offset; - if (CircleError(pit_motor_target, g->limit.pit.max, M_2PI) > 0) { - g->setpoint.eulr.pit = g->limit.pit.max - pit_motor_imu_offset; - } - if (CircleError(pit_motor_target, g->limit.pit.min, M_2PI) < 0) { - g->setpoint.eulr.pit = g->limit.pit.min - pit_motor_imu_offset; - } + CircleAngleLimit(&g->setpoint.eulr.pit, + g->feedback.motor.pit.rotor_abs_angle, + g->feedback.imu.eulr.rol, + g->limit.pit.max, g->limit.pit.min, M_2PI); } } /* fallthrough - AI控制模式也需要执行PID计算 */ diff --git a/User/task/ctrl_chassis.c b/User/task/ctrl_chassis.c index 398e444..5bc32d6 100644 --- a/User/task/ctrl_chassis.c +++ b/User/task/ctrl_chassis.c @@ -62,6 +62,12 @@ void Task_ctrl_chassis(void *argument) { Chassis_UpdateFeedback(&chassis); + /* 检查跳跃触发 */ + if (chassis_cmd.jump_trigger) { + Chassis_TriggerJump(&chassis); + chassis_cmd.jump_trigger = false; /* 清除触发标志 */ + } + Chassis_Control(&chassis, &chassis_cmd); /* USER CODE END */ osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */ diff --git a/User/task/rc.c b/User/task/rc.c index 46b76b0..95830a2 100644 --- a/User/task/rc.c +++ b/User/task/rc.c @@ -113,6 +113,20 @@ void Task_rc(void *argument) { osMessageQueuePut(task_runtime.msgq.gimbal.cmd, &cmd_for_gimbal, 0, 0); /************************* 底盘命令 **************************************/ + /* 跳跃触发检测：ch_res 从 -1.0f 松开回 0 时触发 */ + static bool ch_res_was_min = false; /* 记录上次是否在最低位置 */ + const float CH_RES_MIN_THRESHOLD = -0.9f; /* 判定为最低位置的阈值 */ + const float CH_RES_RELEASE_THRESHOLD = -0.3f; /* 判定为松开的阈值 */ + + cmd_for_chassis.jump_trigger = false; /* 默认不触发 */ + if (dr16.data.ch_res < CH_RES_MIN_THRESHOLD) { + ch_res_was_min = true; /* 记录已到达最低位置 */ + } else if (ch_res_was_min && dr16.data.ch_res > CH_RES_RELEASE_THRESHOLD) { + /* 从最低位置松开，触发跳跃 */ + cmd_for_chassis.jump_trigger = true; + ch_res_was_min = false; /* 重置状态 */ + } + switch (dr16.data.sw_l) { case DR16_SW_UP: cmd_for_chassis.mode = CHASSIS_MODE_RELAX;