184 lines
7.9 KiB
C
184 lines
7.9 KiB
C
/**
|
||
****************************(C) COPYRIGHT 2025 群星战队****************************
|
||
* @file cap.c/h
|
||
* @brief 超级电容对底盘功率的控制，
|
||
* 代码使用西交利物浦的电机功率模型与功率控制开源改编而成
|
||
* @note
|
||
* @history
|
||
* 版本 日期 作者 变更
|
||
* V1.0.2516 2025-4-20 魔法电容使 1. 完成
|
||
* V1.1.2517 2025-4-25 魔法电容使 2. 添加功率计算，新增功率补偿
|
||
*
|
||
@verbatim
|
||
==============================================================================
|
||
|
||
==============================================================================
|
||
@endverbatim
|
||
****************************(C) COPYRIGHT 2025 群星战队****************************
|
||
*/
|
||
|
||
#include "cap.h"
|
||
|
||
#define WARRING_ENERGY_BUFFER 50
|
||
|
||
float ChassisSetPower = 0;
|
||
void set_chassis_power(float power)
|
||
{
|
||
ChassisSetPower = power;
|
||
}
|
||
|
||
void chassis_power_control_with_supercap(chassis_move_t *chassis_power_control)
|
||
{
|
||
uint8_t RefereePowerLimit = 0; // 裁判系统功率限制值
|
||
|
||
static float ChassisMaxPower = 0; // 底盘最大允许功率
|
||
static float InitialGivePower[4]; // 初始计算的各电机功率
|
||
static float InitialTotalPower = 0; // 初始总功率
|
||
static float ScaledGivePower[4]; // 缩放后的各电机功率
|
||
static float chassis_energy_buffer = 0.0f; // 底盘能量缓冲值
|
||
|
||
// 电机功率模型参数
|
||
static const float toque_coefficient = 1.99688994e-6f; // 转矩系数：(20/16384)*(0.3)*(187/3591)/9.55
|
||
static const float k1 = 1.23e-07; // 功率模型系数k1
|
||
static const float k2 = 1.453e-07; // 功率模型系数k2
|
||
static const float constant = 4.081f; // 功率模型常数项
|
||
|
||
static float power_scale = 0; // 功率缩放比例
|
||
static float energy_scale = 0; // 能量缩放比例
|
||
static uint8_t PowerOffset = 0; // 功率补偿值
|
||
|
||
// 获取裁判系统功率限制和底盘能量缓冲
|
||
// get_chassis_power_limit(&RefereePowerLimit);
|
||
// get_chassis_power_and_buffer(NULL, &chassis_energy_buffer);
|
||
|
||
// 超级电容在线状态判断
|
||
if(get_supercap_online_state()) {
|
||
// 超级电容在线时的功率控制逻辑
|
||
|
||
if(CAN_SuperCapRXData.SuperCapReady == READY) {
|
||
|
||
/* 超电正常运行时，超电可以随时补偿底盘功率，故底盘的功率可以超出功率限
|
||
制。如果超电检测的底盘功率超出了设定值，则说明功率模型有误差，需要对
|
||
模型进行一定的修正这是为了确保底盘功率能够按照设定的功率运行。*/
|
||
|
||
// 超级电容就绪状态：可以补偿底盘功率，允许超出裁判系统限制
|
||
if(CAN_SuperCapRXData.ChassisPower > ChassisSetPower) {
|
||
// 超电检测的底盘功率超出设定值，说明功率模型有误差，需要进行修正
|
||
power_scale = ChassisSetPower / (float)CAN_SuperCapRXData.ChassisPower;
|
||
ChassisMaxPower = ChassisSetPower * power_scale; // 按比例缩放最大功率
|
||
} else {
|
||
ChassisMaxPower = ChassisSetPower; // 直接使用设定功率
|
||
}
|
||
} else {
|
||
|
||
/* 超电处于保护状态时，超电无法补偿功率，故底盘功率只能使用裁判系
|
||
统的功率。这里使用超电反馈的功率进行功率限制，减小对裁判系统依
|
||
赖，保证没有裁判系统的时候仍然可以 以指定的功率去运行。*/
|
||
|
||
// 超级电容保护状态：无法补偿功率，只能使用裁判系统功率限制
|
||
if(CAN_SuperCapRXData.ChassisPower > RefereePowerLimit) {
|
||
// 超电检测功率超出裁判系统限制，进行功率修正
|
||
power_scale = (float)RefereePowerLimit / (float)CAN_SuperCapRXData.ChassisPower;
|
||
ChassisMaxPower = (float)RefereePowerLimit * power_scale;
|
||
} else {
|
||
ChassisMaxPower = RefereePowerLimit; // 直接使用裁判系统限制
|
||
}
|
||
}
|
||
|
||
// 能量缓冲不足时的功率补偿
|
||
if(chassis_energy_buffer < WARRING_ENERGY_BUFFER) {
|
||
|
||
/* 超电在线的时候，经过测试，拟合校准过的数据，经过50CM的18AWG线带载5A，与裁判系
|
||
统的功率误差小于5W另外，PLUS版本拥有远端补偿的设计，经过补偿，与裁判系统的功率
|
||
误差< 1W所以，如果缓冲能量被意外消耗，说明相对误差仍然存在，需要小幅度修正，如
|
||
果补偿5W之后仍超功率，则说明拟合参数有问题。 */
|
||
|
||
// 能量缓冲低于警告阈值，根据剩余缓冲能量进行功率补偿
|
||
// 补偿原理：超电功率测量存在误差，缓冲能量消耗说明存在累积误差
|
||
PowerOffset = (WARRING_ENERGY_BUFFER - chassis_energy_buffer) * 0.2f; // 最大补偿10W
|
||
} else {
|
||
PowerOffset = 0; // 能量缓冲充足，不需要补偿
|
||
}
|
||
|
||
// 设置功率补偿值并调整最大功率
|
||
set_supercap_power_offset(PowerOffset);
|
||
ChassisMaxPower = ChassisMaxPower - PowerOffset; // 减去补偿值得到最终最大功率
|
||
|
||
} else {
|
||
// 超级电容离线时的功率控制逻辑
|
||
// 使用缓冲能量对电机功率模型进行修正，避免模型误差导致超功率
|
||
|
||
if(chassis_energy_buffer < WARRING_ENERGY_BUFFER) {
|
||
// 能量缓冲不足时，按比例缩放最大功率
|
||
energy_scale = chassis_energy_buffer * 0.02f; // 0.02 = 1/50，转换为乘法优化计算速度
|
||
ChassisMaxPower = (float)RefereePowerLimit * energy_scale;
|
||
} else {
|
||
// 能量缓冲充足，直接使用裁判系统功率限制
|
||
ChassisMaxPower = RefereePowerLimit;
|
||
}
|
||
}
|
||
|
||
// 西交利物浦电机功率模型控制算法
|
||
|
||
// 第一步：计算各电机的初始功率和总功率
|
||
for (uint8_t i = 0; i < 4; i++) {
|
||
// 电机功率模型公式：P = τ*ω + k^2*ω^2 + k1*τ^2 + constant
|
||
InitialGivePower[i] = chassis_power_control->motor_speed_pid[i].last.out *
|
||
toque_coefficient *
|
||
chassis_power_control->motor_chassis[i].chassis_motor_measure->speed_rpm +
|
||
k2 * chassis_power_control->motor_chassis[i].chassis_motor_measure->speed_rpm *
|
||
chassis_power_control->motor_chassis[i].chassis_motor_measure->speed_rpm +
|
||
k1 * chassis_power_control->motor_speed_pid[i].last.out *
|
||
chassis_power_control->motor_speed_pid[i].last.out + constant;
|
||
|
||
// 负功率不计入总功率（瞬态过程）
|
||
if (InitialGivePower[i] < 0) {
|
||
continue;
|
||
}
|
||
InitialTotalPower += InitialGivePower[i]; // 累加各电机功率得到总功率
|
||
}
|
||
|
||
// 第二步：如果总功率超过最大允许功率，进行功率缩放
|
||
if (InitialTotalPower > ChassisMaxPower) {
|
||
// 计算功率缩放比例
|
||
power_scale = ChassisMaxPower / InitialTotalPower;
|
||
|
||
for (uint8_t i = 0; i < 4; i++) {
|
||
// 计算缩放后的各电机功率
|
||
ScaledGivePower[i] = InitialGivePower[i] * power_scale;
|
||
|
||
// 跳过负功率电机
|
||
if (ScaledGivePower[i] < 0) {
|
||
continue;
|
||
}
|
||
|
||
// 根据缩放后的功率反算新的转矩输出（PID输出）
|
||
float b = toque_coefficient * chassis_power_control->motor_chassis[i].chassis_motor_measure->speed_rpm;
|
||
float c = k2 * chassis_power_control->motor_chassis[i].chassis_motor_measure->speed_rpm *
|
||
chassis_power_control->motor_chassis[i].chassis_motor_measure->speed_rpm -
|
||
ScaledGivePower[i] + constant;
|
||
float inside = b * b - 4 * k1 * c; // 判别式
|
||
|
||
// 检查判别式有效性
|
||
if (inside < 0) {
|
||
continue; // 无实数解，保持原输出
|
||
}
|
||
else if (chassis_power_control->motor_speed_pid[i].last.out > 0) {
|
||
// 原转矩为正时，取正根解
|
||
float temp = (-b + sqrt(inside)) / (2 * k1);
|
||
|
||
// 使用三目运算限制输出范围在±16000以内
|
||
chassis_power_control->motor_speed_pid[i].last.out = (temp > 16000) ? 16000 : temp;
|
||
|
||
} else {
|
||
// 原转矩为负时，取负根解
|
||
float temp = (-b - sqrt(inside)) / (2 * k1);
|
||
|
||
// 使用三目运算限制输出范围在±16000以内
|
||
chassis_power_control->motor_speed_pid[i].last.out = (temp < -16000) ? -16000 : temp;
|
||
}
|
||
}
|
||
}
|
||
// 如果总功率未超过限制，保持原有的PID输出不变
|
||
}
|