mirror of
https://gitee.com/dlmu-cone/half-steering-basic-framework
synced 2026-10-24 01:52:20 +08:00
supercap&PowerCtrl
This commit is contained in:
@@ -26,6 +26,17 @@
|
||||
#include "referee_UI.h"
|
||||
#include "arm_math.h"
|
||||
|
||||
// =================【功率模型参数】=================
|
||||
// ⚠️警告：以下参数原本是群星战队为 DJI M3508 电机拟合的。
|
||||
// 你们的驱动电机是 LK9015，它的扭矩常数、内阻、空载损耗和 M3508 不同！
|
||||
// 建议后期针对 LK9015 重新拟合这四个参数以达到完美的控制效果。
|
||||
#define TOQUE_COEFF 1.99688994e-6f
|
||||
#define K1 1.23e-07f
|
||||
#define K2 1.453e-07f
|
||||
#define CONSTANT_POWER 4.081f
|
||||
#define WARRING_ENERGY_BUFFER 50.0f
|
||||
// ==================================================
|
||||
|
||||
/* 根据robot_def.h中的macro自动计算的参数 */
|
||||
#define HALF_WHEEL_BASE (WHEEL_BASE / 2.0f) // 半轴距
|
||||
#define HALF_TRACK_WIDTH (TRACK_WIDTH / 2.0f) // 半轮距
|
||||
@@ -179,7 +190,7 @@ void ChassisInit()
|
||||
cap = SuperCapInit(&cap_conf); // 超级电容初始化
|
||||
|
||||
// （新增）设置超级电容功率补偿，例如补偿5W，可根据实际需求调整
|
||||
SuperCapSetPowerOffset(cap, 5);
|
||||
// SuperCapSetPowerOffset(cap, 5);
|
||||
|
||||
// 发布订阅初始化,如果为双板,则需要can comm来传递消息
|
||||
#ifdef CHASSIS_BOARD
|
||||
@@ -337,19 +348,102 @@ static void SteeringWheelCalculate()
|
||||
* @brief 根据裁判系统和电容剩余容量对输出进行限制并设置电机参考值
|
||||
*
|
||||
*/
|
||||
/**
|
||||
* @brief 根据裁判系统和电容剩余容量对输出进行限制，并应用 WJTLU 功率模型
|
||||
*/
|
||||
static void LimitChassisOutput()
|
||||
{
|
||||
// 功率限制待添加
|
||||
// referee_data->PowerHeatData.chassis_power;
|
||||
// referee_data->PowerHeatData.chassis_power_buffer;
|
||||
float chassis_max_power = 0;
|
||||
float chassis_set_power = 90.0f; // 设定的期望功率，可根据需求修改
|
||||
|
||||
// 完成功率限制后进行电机参考输入设定
|
||||
// 获取裁判系统数据
|
||||
uint16_t referee_power_limit = referee_data->GameRobotState.chassis_power_limit;
|
||||
float energy_buffer = referee_data->PowerHeatData.chassis_power_buffer;
|
||||
|
||||
// LKMotorSetRef( motor_lf , vt_lf );
|
||||
// LKMotorSetRef( motor_rb , vt_rb );
|
||||
// 1. ============ 超级电容与裁判系统功率统筹 ============
|
||||
if (SuperCapIsOnline(cap)) {
|
||||
SuperCap_Msg_s cap_msg = SuperCapGet(cap);
|
||||
if (cap_msg.ready == READY) {
|
||||
// 超电正常可用，动态按比例缩放允许功率
|
||||
if (cap_msg.chassis_power > chassis_set_power) {
|
||||
chassis_max_power = chassis_set_power * (chassis_set_power / (float)cap_msg.chassis_power);
|
||||
} else {
|
||||
chassis_max_power = chassis_set_power;
|
||||
}
|
||||
} else {
|
||||
// 超电保护中，使用裁判系统限制
|
||||
if (cap_msg.chassis_power > referee_power_limit) {
|
||||
chassis_max_power = referee_power_limit * ((float)referee_power_limit / (float)cap_msg.chassis_power);
|
||||
} else {
|
||||
chassis_max_power = referee_power_limit;
|
||||
}
|
||||
}
|
||||
|
||||
LKMotorSetRef( motor_rf , vt_rf );
|
||||
LKMotorSetRef( motor_lb , vt_lb );
|
||||
// 缓冲能量补偿逻辑
|
||||
uint8_t power_offset = 0;
|
||||
if (energy_buffer < WARRING_ENERGY_BUFFER) {
|
||||
power_offset = (uint8_t)((WARRING_ENERGY_BUFFER - energy_buffer) * 0.2f);
|
||||
}
|
||||
SuperCapSetPowerOffset(cap, power_offset);
|
||||
chassis_max_power -= power_offset;
|
||||
} else {
|
||||
// 超电离线时逻辑
|
||||
chassis_max_power = (energy_buffer < WARRING_ENERGY_BUFFER) ?
|
||||
(referee_power_limit * energy_buffer * 0.02f) : referee_power_limit;
|
||||
}
|
||||
|
||||
// 2. ============ 基于 LK9015 的 WJTLU 二次功率模型限流 ============
|
||||
float initial_total_power = 0;
|
||||
float initial_motor_power[2];
|
||||
LKMotorInstance* motors[2] = {motor_rf, motor_lb}; // 注意舵轮底盘只有两个驱动轮
|
||||
float current_pid_out[2];
|
||||
|
||||
for (int i = 0; i < 2; i++) {
|
||||
// 修正 1：LK 电机驱动中，speed_PID 是实例的直接成员
|
||||
current_pid_out[i] = motors[i]->speed_PID.Output;
|
||||
|
||||
// 修正 2：LK 的速度单位是 rad/s，WJTLU模型要求 RPM，进行单位转换 (rad/s * 30/pi ≈ RPM)
|
||||
float rpm = motors[i]->measure.speed_rads * 9.549296f;
|
||||
|
||||
initial_motor_power[i] = current_pid_out[i] * TOQUE_COEFF * rpm +
|
||||
K1 * current_pid_out[i] * current_pid_out[i] +
|
||||
K2 * rpm * rpm + CONSTANT_POWER;
|
||||
|
||||
if (initial_motor_power[i] > 0) initial_total_power += initial_motor_power[i];
|
||||
}
|
||||
|
||||
// 3. 动态修改电机 PID 的 MaxOut 属性
|
||||
if (initial_total_power > chassis_max_power && initial_total_power > 0) {
|
||||
float scale = chassis_max_power / initial_total_power;
|
||||
for (int i = 0; i < 2; i++) {
|
||||
float scaled_p = initial_motor_power[i] * scale;
|
||||
if (scaled_p <= CONSTANT_POWER) {
|
||||
motors[i]->speed_PID.MaxOut = 0; // 该轮分配不到功率
|
||||
continue;
|
||||
}
|
||||
|
||||
// 同步修正单位
|
||||
float rpm = motors[i]->measure.speed_rads * 9.549296f;
|
||||
float b = TOQUE_COEFF * rpm;
|
||||
float c = K2 * rpm * rpm - scaled_p + CONSTANT_POWER;
|
||||
float delta = b * b - 4 * K1 * c;
|
||||
|
||||
if (delta >= 0) {
|
||||
// 解方程获得当前允许的最大电流
|
||||
float out_limit = (-b + sqrtf(delta)) / (2.0f * K1);
|
||||
// 修正 3：动态限制 LK 电机 speed_PID 的最大输出（LK9015 默认最大输出为 12000）
|
||||
motors[i]->speed_PID.MaxOut = fminf(out_limit, 12000.0f);
|
||||
}
|
||||
}
|
||||
} else {
|
||||
// 功率充裕时，恢复电机的默认最大输出
|
||||
motor_rf->speed_PID.MaxOut = 12000.0f;
|
||||
motor_lb->speed_PID.MaxOut = 12000.0f;
|
||||
}
|
||||
|
||||
// 4. 下发目标速度（限流操作已通过改变 MaxOut 动态生效）
|
||||
LKMotorSetRef(motor_rf, vt_rf);
|
||||
LKMotorSetRef(motor_lb, vt_lb);
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -445,6 +539,16 @@ void ChassisTask() {
|
||||
// chassis_feedback_data.bullet_speed = referee_data->GameRobotState.shooter_id1_17mm_speed_limit;
|
||||
// chassis_feedback_data.rest_heat = referee_data->PowerHeatData.shooter_heat0;
|
||||
|
||||
// ================== 发送超级电容控制指令 ==================
|
||||
// 每帧向超电控制板发送最新状态
|
||||
SuperCap_TxMsg_s cap_tx = {
|
||||
.Enable = 1,
|
||||
.Reserved1 = 0, // 可根据需求动态关断充电
|
||||
.Powerlimit = referee_data->GameRobotState.chassis_power_limit,
|
||||
.Reserved2 = 0 // PLUS版本忽略此位
|
||||
};
|
||||
SuperCapSend(cap, &cap_tx);
|
||||
|
||||
// 推送反馈消息
|
||||
#ifdef ONE_BOARD
|
||||
PubPushMessage(chassis_pub, (void *)&chassis_feedback_data);
|
||||
|
||||
Reference in New Issue
Block a user