139 lines
6.3 KiB
C
139 lines
6.3 KiB
C
/*
|
||
* 电容模组
|
||
*/
|
||
|
||
/* Includes ----------------------------------------------------------------- */
|
||
#include "cap.h"
|
||
#include "component/limiter.h"
|
||
#include "device/referee.h"
|
||
|
||
/* Private typedef ---------------------------------------------------------- */
|
||
/* Private define ----------------------------------------------------------- */
|
||
#define CAP_CUTOFF_VOLT 12.0f
|
||
#define WARRING_ENERGY_BUFFER 50
|
||
/* Private macro ------------------------------------------------------------ */
|
||
/* Private variables -------------------------------------------------------- */
|
||
/* Private function -------------------------------------------------------- */
|
||
float ChassisSetPower = 0;
|
||
float ChassisMaxPower = 0; // 底盘最大允许功率
|
||
extern Chassis_t chassis;
|
||
/**
|
||
* @brief 运行电容控制逻辑
|
||
*
|
||
* @param cap 电容数据结构体
|
||
* @param referee 裁判系统数据
|
||
* @param cap_out 电容输出结构体
|
||
*/
|
||
void Cap_Control(CAN_SuperCapRXDataTypeDef *cap, const Referee_ForCap_t *referee)
|
||
{
|
||
|
||
if (CAN_SuperCapRXData.SuperCapEnergy<=35)chassis.power_limit = referee->chassis_power_limit ;
|
||
else chassis.power_limit = -1;
|
||
/*
|
||
if (referee->ref_status != REF_STATUS_RUNNING) {
|
||
//当裁判系统离线时，依然使用裁判系统进程传来的数据
|
||
ChassisSetPower = referee->chassis_power_limit;
|
||
} else {
|
||
//当裁判系统在线时，使用算法控制裁判系统输出（即电容输入）
|
||
ChassisSetPower =
|
||
PowerLimit_CapInput(referee->chassis_watt, referee->chassis_power_limit,
|
||
referee->chassis_pwr_buff);
|
||
}
|
||
// 获取裁判系统功率限制和底盘能量缓冲referee->chassis_power_limit
|
||
//referee->chassis_power_limit
|
||
//referee->chassis_pwr_buff
|
||
|
||
static float power_scale = 0; // 功率缩放比例
|
||
static float energy_scale = 0; // 能量缩放比例
|
||
static uint8_t PowerOffset = 0; // 功率补偿值
|
||
// 超级电容在线状态判断
|
||
if(get_supercap_online_state()) {
|
||
// 超级电容在线时的功率控制逻辑
|
||
|
||
if(CAN_SuperCapRXData.SuperCapReady == SUPERCAP_STATUS_RUNNING) {
|
||
|
||
//超电正常运行时，超电可以随时补偿底盘功率，故底盘的功率可以超出功率限
|
||
//制。如果超电检测的底盘功率超出了设定值，则说明功率模型有误差，需要对
|
||
//模型进行一定的修正这是为了确保底盘功率能够按照设定的功率运行。
|
||
|
||
// 超级电容就绪状态：可以补偿底盘功率，允许超出裁判系统限制
|
||
if(CAN_SuperCapRXData.ChassisPower > ChassisSetPower) {
|
||
// 超电检测的底盘功率超出设定值，说明功率模型有误差，需要进行修正
|
||
power_scale = ChassisSetPower / (float)CAN_SuperCapRXData.ChassisPower;
|
||
ChassisMaxPower = ChassisSetPower * power_scale; // 按比例缩放最大功率
|
||
} else {
|
||
ChassisMaxPower = ChassisSetPower; // 直接使用设定功率
|
||
}
|
||
} else {
|
||
// 超级电容保护状态：无法补偿功率，只能使用裁判系统功率限制
|
||
if(CAN_SuperCapRXData.ChassisPower > referee->chassis_power_limit) {
|
||
// 超电检测功率超出裁判系统限制，进行功率修正
|
||
power_scale = (float)referee->chassis_power_limit / (float)CAN_SuperCapRXData.ChassisPower;
|
||
ChassisMaxPower = (float)referee->chassis_power_limit * power_scale;
|
||
} else {
|
||
ChassisMaxPower = referee->chassis_power_limit; // 直接使用裁判系统限制
|
||
}
|
||
}
|
||
|
||
// 能量缓冲不足时的功率补偿
|
||
if(referee->chassis_pwr_buff < WARRING_ENERGY_BUFFER) {
|
||
|
||
//超电在线的时候，经过测试，拟合校准过的数据，经过50CM的18AWG线带载5A，与裁判系
|
||
//统的功率误差小于5W另外，PLUS版本拥有远端补偿的设计，经过补偿，与裁判系统的功率
|
||
//误差< 1W所以，如果缓冲能量被意外消耗，说明相对误差仍然存在，需要小幅度修正，如
|
||
//果补偿5W之后仍超功率，则说明拟合参数有问题。
|
||
|
||
// 能量缓冲低于警告阈值，根据剩余缓冲能量进行功率补偿
|
||
// 补偿原理：超电功率测量存在误差，缓冲能量消耗说明存在累积误差
|
||
PowerOffset = (WARRING_ENERGY_BUFFER - referee->chassis_pwr_buff) * 0.2f; // 最大补偿10W
|
||
} else {
|
||
PowerOffset = 0; // 能量缓冲充足，不需要补偿
|
||
}
|
||
|
||
// 设置功率补偿值并调整最大功率
|
||
set_supercap_power_offset(PowerOffset);
|
||
ChassisMaxPower = ChassisMaxPower - PowerOffset; // 减去补偿值得到最终最大功率
|
||
|
||
} else {
|
||
// 超级电容离线时的功率控制逻辑
|
||
// 使用缓冲能量对电机功率模型进行修正，避免模型误差导致超功率
|
||
|
||
if(referee->chassis_pwr_buff < WARRING_ENERGY_BUFFER) {
|
||
// 能量缓冲不足时，按比例缩放最大功率
|
||
energy_scale = referee->chassis_pwr_buff * 0.02f; // 0.02 = 1/50，转换为乘法优化计算速度
|
||
ChassisMaxPower = (float)referee->chassis_power_limit * energy_scale;
|
||
} else {
|
||
// 能量缓冲充足，直接使用裁判系统功率限制
|
||
ChassisMaxPower = referee->chassis_power_limit;
|
||
}
|
||
}
|
||
|
||
// 第一步：计算总功率//CAN_SuperCapRXData.ChassisPower
|
||
|
||
// 第二步：如果总功率超过最大允许功率，进行功率缩放
|
||
if (CAN_SuperCapRXData.ChassisPower > ChassisMaxPower) {
|
||
// 计算功率缩放比例
|
||
power_scale = ChassisMaxPower / CAN_SuperCapRXData.ChassisPower;
|
||
|
||
// 计算缩放后的各电机功率
|
||
|
||
// 根据缩放后的功率反算新的转矩输出（PID输出）
|
||
|
||
}
|
||
// 如果总功率未超过限制，保持原有的PID输出不变
|
||
//*/
|
||
}
|
||
|
||
/**
|
||
* @brief 导出电容数据
|
||
*
|
||
* @param cap 电容数据
|
||
* @param ui 结构体
|
||
*/
|
||
void Cap_DumpUI(CAN_SuperCapRXDataTypeDef *cap, Cap_RefereeUI_t *ui) {
|
||
ui->percentage = cap->SuperCapEnergy;
|
||
ui->status = cap->SuperCapState;
|
||
ui->ready = cap->SuperCapReady;
|
||
ui->online = get_supercap_online_state();
|
||
}
|