From f2813bc1a5388c08bb720e8c610c3e4939636aea Mon Sep 17 00:00:00 2001 From: TuxMonkey Date: Wed, 15 Apr 2026 16:10:26 +0800 Subject: [PATCH] supercap&PowerCtrl --- application/chassis/chassis.c | 122 +++++++++++++++++++++++++++++++--- modules/super_cap/super_cap.c | 20 +++++- modules/super_cap/super_cap.h | 8 ++- 3 files changed, 137 insertions(+), 13 deletions(-) diff --git a/application/chassis/chassis.c b/application/chassis/chassis.c index 455c08c..1265f2e 100644 --- a/application/chassis/chassis.c +++ b/application/chassis/chassis.c @@ -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); diff --git a/modules/super_cap/super_cap.c b/modules/super_cap/super_cap.c index 0b981d1..f5e771f 100644 --- a/modules/super_cap/super_cap.c +++ b/modules/super_cap/super_cap.c @@ -5,6 +5,7 @@ #include "super_cap.h" #include "memory.h" #include "stdlib.h" +#include "bsp_dwt.h" // 【新增】引入获取时间的头文件，或者是 "main.h" 用 HAL_GetTick() static SuperCapInstance *super_cap_instance = NULL; @@ -15,6 +16,9 @@ static void SuperCapRxCallback(CANInstance *_instance) rxbuff = _instance->rx_buff; Msg = &super_cap_instance->cap_msg; + // 【新增】更新最后一次收到消息的时间戳 + super_cap_instance->last_update_tick = DWT_GetTimeline_ms(); // 如果没有DWT，用 HAL_GetTick() 替换 + // 按照新协议解析接收数据 Msg->ready = (SuperCapReadyEnum)rxbuff[0]; Msg->state = (SuperCapStateEnum)rxbuff[1]; @@ -24,6 +28,18 @@ static void SuperCapRxCallback(CANInstance *_instance) Msg->bat_power = rxbuff[5]; } +// 【新增】在线判断函数 +uint8_t SuperCapIsOnline(SuperCapInstance *instance) +{ + if (instance == NULL) return 0; + + // 如果当前时间减去上次接收时间超过 1000ms (1秒)，则认为离线 + if (DWT_GetTimeline_ms() - instance->last_update_tick > 2000) { + return 0; // 离线 + } + return 1; // 在线 +} + SuperCapInstance *SuperCapInit(SuperCap_Init_Config_s *supercap_config) { super_cap_instance = (SuperCapInstance *)malloc(sizeof(SuperCapInstance)); @@ -40,9 +56,9 @@ void SuperCapSend(SuperCapInstance *instance, SuperCap_TxMsg_s *tx_data) // 按照新协议装载发送数据 txbuff[0] = tx_data->Enable; - txbuff[1] = tx_data->Charge; + txbuff[1] = tx_data->Reserved1; txbuff[2] = tx_data->Powerlimit - instance->power_offset; // 计算功率补偿 - txbuff[3] = tx_data->ChargePower; + txbuff[3] = tx_data->Reserved2; CANTransmit(instance->can_ins, 1); } diff --git a/modules/super_cap/super_cap.h b/modules/super_cap/super_cap.h index 732d6a7..124c4de 100644 --- a/modules/super_cap/super_cap.h +++ b/modules/super_cap/super_cap.h @@ -44,9 +44,9 @@ typedef struct /* 需要发送给超级电容的控制信息结构体 */ typedef struct { uint8_t Enable; // 超级电容使能。1使能，0失能 - uint8_t Charge; // 超级电容充电。1充电，0放电 + uint8_t Reserved1; // Reserved 超级电容充电。1充电，0放电 uint8_t Powerlimit; // 裁判系统功率限制 - uint8_t ChargePower; // 超级电容充电功率 + uint8_t Reserved2; // Reserved 超级电容充电功率 } SuperCap_TxMsg_s; #pragma pack() @@ -56,6 +56,7 @@ typedef struct CANInstance *can_ins; // CAN实例 SuperCap_Msg_s cap_msg; // 超级电容接收信息 uint8_t power_offset; // 功率补偿值 + uint32_t last_update_tick; // 【新增】上次收到数据的时间戳 } SuperCapInstance; /* 超级电容初始化配置 */ @@ -69,6 +70,9 @@ typedef struct */ SuperCapInstance *SuperCapInit(SuperCap_Init_Config_s *supercap_config); +// 【新增】函数声明 +uint8_t SuperCapIsOnline(SuperCapInstance *instance); + /** * @brief 发送超级电容控制信息 */