robofish/rm_balance
2
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

ciallo电

Browse Source
This commit is contained in:
xxxxm 2026-03-15 10:51:08 +08:00
parent 96da4ad6dc
commit 0cf5df7194
11 changed files with 329 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
User/component/limiter.c
View File

@ -11,7 +11,7 @@
#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_POWER_FACTOR_NO_PASS 5.5f
#define CHASSIS_MOTOR_CIRCUMFERENCE 0.12f

57
User/device/supercap.c
View File

@ -1,6 +1,7 @@
#include "device/supercap.h"
#include "device/referee.h"//ref-add
/* 全局变量 */
extern Referee_t ref;//ref-add
CAN_SuperCapRXDataTypeDef CAN_SuperCapRXData = {0};
uint8_t PowerOffset =0;
@ -24,12 +25,12 @@ uint8_t get_supercap_online_state(void){
uint32_t DeltaCapTick = 0;
DeltaCapTick = NowCapTick - LastCapTick; //计算时间差
// if(get_game_progress() == 4){
// //比赛开始的时候，开始统计消耗的能量
if(ref.ref_status == REF_STATUS_RUNNING){//ref-add
//比赛开始的时候，开始统计消耗的能量
chassis_energy_in_gamming += CAN_SuperCapRXData.BatPower * DeltaCapTick *0.001f;
// 因为STM32的系统定时器是1ms的周期，所以*0.001，单位化为秒（S），能量单位才是焦耳（J）
// }
}
if(DeltaCapTick > 1000){
//如果时间差大于1s，说明超电信号丢失，返回超电离线的标志
@ -85,8 +86,8 @@ int8_t SuperCap_Update(void)
void transfer_SuperCap_measure(uint8_t *data)
{
LastCapTick = HAL_GetTick();
CAN_SuperCapRXData.SuperCapReady = (SuperCap_Status_t)data[0];
CAN_SuperCapRXData.SuperCapState = (SuperCap_Status_t)data[1];
CAN_SuperCapRXData.SuperCapReady = (SuperCapReadyEnum)data[0];
CAN_SuperCapRXData.SuperCapState = (SuperCapStateEnum)data[1];
CAN_SuperCapRXData.SuperCapEnergy = data[2];
CAN_SuperCapRXData.ChassisPower = data[3] << 1; //左移一位是为了扩大范围，超电发出来的的时候右移了一位
CAN_SuperCapRXData.BatVoltage = data[4];
@ -155,4 +156,48 @@ int8_t CAN_TX_SuperCapData(CAN_SuperCapTXDataTypeDef * TX_Temp)
return BSP_FDCAN_TransmitStdDataFrame( SUPERCAP_CAN , &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
}
/*******balance特供*******/
/**
* @brief 限制功率不超过power_limit（超电测量功率版本）
*
* @param power_limit 最大功率
* @param motor_out 电机输出值
* @param speed 电机转速
* @param len 电机数量
* @return int8_t 0对应没有错误
*/
int8_t PowerLimit_Output_by_cap(float power_limit, float *motor_out, uint32_t len)
{
/* power_limit小于0时不进行限制 */
if (motor_out == NULL || power_limit < 0) return -1;
float ChassisPower = CAN_SuperCapRXData.ChassisPower ;
/* 保持每个电机输出值缩小时比例不变 */
if (ChassisPower > power_limit) {
for (uint32_t i = 0; i < len; i++) {
motor_out[i] *= power_limit / ChassisPower;
}
}
return 0;
}
/**
* @brief 限制功率不超过power_limit（out[i]版本）（未完成）
*/
int8_t PowerLimit_Output(float power_limit, float *motor_out, uint32_t len)
{
/* power_limit小于0时不进行限制 */
if (motor_out == NULL || power_limit < 0) return -1;
if (power_limit < 0 ) return 0;
float ChassisPower = CAN_SuperCapRXData.ChassisPower ;
/* 保持每个电机输出值缩小时比例不变 */
if (ChassisPower > power_limit) {
for (uint32_t i = 0; i < len; i++) {
motor_out[i] *= power_limit / ChassisPower;
}
}
return 0;
}

15
User/device/supercap.h
View File

@ -11,8 +11,10 @@ extern "C" {
#define SUPERCAP_CAN BSP_FDCAN_1
//#define SUPERCAP_CAN BSP_FDCAN_2
#define SUPERCAP_TX_ID 0x001 //C板发给超级电容的ID
#define SUPERCAP_RX_ID 0x100 //超级电容发给C板的ID
//#define SUPERCAP_TX_ID 0x301 //C板发给超级电容的ID
//#define SUPERCAP_RX_ID 0x100 //超级电容发给C板的ID
#define SUPERCAP_TX_ID 0x30F //C板发给超级电容的ID
#define SUPERCAP_RX_ID 0x300 //超级电容发给C板的ID
//超级电容的状态标志位，超级电容运行或者保护的具体状态反馈
@ -35,7 +37,7 @@ typedef enum
{
SUPERCAP_STATUS_OFFLINE =0 ,
SUPERCAP_STATUS_RUNNING =1,
}SuperCap_Status_t;
}SuperCapReadyEnum;
// 发送给超级电容的数据
typedef struct {
@ -49,7 +51,7 @@ typedef struct {
typedef struct {
uint8_t SuperCapEnergy;//超级电容可用能量：0-100%
uint16_t ChassisPower; //底盘功率，0-512，由于传输的时候为了扩大量程右移了一位，所以接收的时候需要左移还原（丢精度）。
SuperCap_Status_t SuperCapReady;//超级电容【可用标志】：1为可用，0为不可用
SuperCapReadyEnum SuperCapReady;//超级电容【可用标志】：1为可用，0为不可用
SuperCapStateEnum SuperCapState;//超级电容【状态标志】：各个状态对应的状态码查看E_SuperCapState枚举。
uint8_t BatVoltage; //通过超级电容监控电池电压*10，
uint8_t BatPower;
@ -100,7 +102,10 @@ int8_t SuperCap_Update(void);
/* UI 导出结构（供 referee 系统绘制） */
typedef struct {
float percentage;
SuperCap_Status_t status;
SuperCapReadyEnum ready;//超电状态
SuperCapStateEnum status;//超电是否可用
uint8_t online;//超电是否离线
// CAN_CapStatus_t status;
} Cap_RefereeUI_t;
#ifdef __cplusplus

3
User/module/balance_chassis.c
View File

@ -831,6 +831,9 @@ void Chassis_Output(Chassis_t *c) {
// for (int i = 0; i < 2; i++) {
// c->output.wheel[i] = LowPassFilter2p_Apply(&c->filter.wheel_out[i], c->output.wheel[i]);
// }
//cap-add//supercap-add
PowerLimit_Output(c->power_limit, c->output.wheel, 2 );
//PowerLimit_ChassicOutput()
MOTOR_LK_SetOutput(&c->param->wheel_param[0], c->output.wheel[0]);
MOTOR_LK_SetOutput(&c->param->wheel_param[1], c->output.wheel[1]);
}

1
User/module/balance_chassis.h
View File

@ -178,6 +178,7 @@ typedef struct {
Chassis_Feedback_t feedback;
/* 控制信息*/
Chassis_Output_t output;
float power_limit;//cap-add//supercap-add
VMC_t vmc_[2]; /* 两条腿的VMC */
LQR_t lqr[2]; /* 两条腿的LQR控制器 */

138
User/module/cap.c Normal file
View File

@ -0,0 +1,138 @@
/*
* 电容模组
*/
/* 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();
}

39
User/module/cap.h Normal file
View File

@ -0,0 +1,39 @@
/*
* 电容模组
*/
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include "device\supercap.h"
#include "device\referee.h"
/* Exported constants ------------------------------------------------------- */
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
/* Exported functions prototypes -------------------------------------------- */
/**
* @brief 运行电容控制逻辑
*
* @param cap 电容数据结构体
* @param referee 裁判系统数据
* @param cap_out 电容输出结构体
*/
void Cap_Control(CAN_SuperCapRXDataTypeDef *cap, const Referee_ForCap_t *referee);
/**
* @brief 导出电容数据
*
* @param cap 电容数据
* @param ui 结构体
*/
void Cap_DumpUI(CAN_SuperCapRXDataTypeDef *cap, Cap_RefereeUI_t *ui);
#ifdef __cplusplus
}
#endif

77
User/task/cap.c Normal file
View File

@ -0,0 +1,77 @@
/*
super_cap Task
*/
/* Includes ----------------------------------------------------------------- */
#include "task/user_task.h"
/* USER INCLUDE BEGIN */
#include "device/supercap.h"
#include "device/referee.h"
#include "module/cap.h"
/* USER INCLUDE END */
/* Private typedef ---------------------------------------------------------- */
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */
/* USER STRUCT BEGIN */
float power_limit = 120;
bool cap_online;
CAN_SuperCapTXDataTypeDef SuperCap_CanTX;
Referee_ForCap_t referee_cap;
Cap_RefereeUI_t cap_ui;
/* USER STRUCT END */
/* Private function --------------------------------------------------------- */
/* USER PRIVATE CODE BEGIN */
/* USER PRIVATE CODE END */
/* Exported functions ------------------------------------------------------- */
void Task_super_cap(void *argument) {
(void)argument; /* 未使用argument，消除警告 */
/* 计算任务运行到指定频率需要等待的tick数 */
const uint32_t delay_tick = osKernelGetTickFreq() / SUPER_CAP_FREQ;
osDelay(SUPER_CAP_INIT_DELAY); /* 延时一段时间再开启任务 */
uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
/* USER CODE INIT BEGIN */
SuperCap_Init();
SuperCap_CanTX.Enable = 1 ; //超级电容使能。1使能，0失能
SuperCap_CanTX.Charge = 0 ; //此标志位无效，超电的充放电是自动的
SuperCap_CanTX.Powerlimit = 120 ; //裁判系统功率限制
SuperCap_CanTX.ChargePower = 1 ; //此参数无效，超电的充电功率随着底盘功率变化
/* USER CODE INIT END */
while (1) {
tick += delay_tick; /* 计算下一个唤醒时刻 */
/* USER CODE BEGIN */
//osMessageQueueGet(task_runtime.msgq.referee.ui.tocap, , NULL, 0);
cap_online = get_supercap_online_state();
osKernelLock(); /* 锁住RTOS内核防止控制过程中断，造成错误 */
/* 根据裁判系统数据计算输出功率 */
SuperCap_CanTX.Powerlimit = power_limit;
Cap_Control(&CAN_SuperCapRXData, &referee_cap);
SuperCap_Update();
CAN_TX_SuperCapData(&SuperCap_CanTX);
osKernelUnlock();
/* 将电容状态发送到Chassis */
// osMessageQueueReset(task_runtime.msgq.cap.for_chassis);
// osMessageQueuePut(task_runtime.msgq.cap.for_chassis, &CAN_SuperCapRXData, 0, 0);
osMessageQueueReset(task_runtime.msgq.cap.power_limit);
osMessageQueuePut(task_runtime.msgq.cap.power_limit, &power_limit, 0, 0);
/* 超电UI */
Cap_DumpUI(&CAN_SuperCapRXData,&cap_ui);
osMessageQueueReset(task_runtime.msgq.referee.ui.tocap);
osMessageQueuePut(task_runtime.msgq.referee.ui.tocap, &cap_ui, 0, 0);
/* USER CODE END */
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
}
}

4
User/task/ctrl_chassis.c
View File

@ -99,10 +99,10 @@ void Task_ctrl_chassis(void *argument) {
osMessageQueueGet(task_runtime.msgq.chassis.ref, &chassis_ref, NULL, 0);
Chassis_UpdateFeedback(&chassis);
osMessageQueueGet(task_runtime.msgq.cap.power_limit, &chassis.power_limit, NULL, 0);
Chassis_Control(&chassis, &chassis_cmd);
// /* 功率限制：裁判系统在线时使用下发上限，否则使用保守默认值 */
///* 功率限制：裁判系统在线时使用下发上限，否则使用保守默认值 */
// float power_limit = (chassis_ref.ref_status == REF_STATUS_RUNNING)
// ? chassis_ref.chassis_power_limit
// : 500.0f;

4
User/task/init.c
View File

@ -78,7 +78,9 @@ void Task_Init(void *argument) {
task_runtime.msgq.referee.ui.toshoot =osMessageQueueNew(2u, sizeof(Shoot_RefereeUI_t), NULL);
task_runtime.msgq.referee.ui.tocmd = osMessageQueueNew(2u, sizeof(bool), NULL);
task_runtime.msgq.referee.ui.frcmd = osMessageQueueNew(2u, sizeof(Referee_UI_CMD_t), NULL);
/*超电*/
task_runtime.msgq.cap.for_chassis = osMessageQueueNew(2u, sizeof(CAN_SuperCapRXDataTypeDef), NULL);
task_runtime.msgq.cap.power_limit = osMessageQueueNew(2u, sizeof(float), NULL);
/* USER MESSAGE END */
osKernelUnlock(); // 解锁内核

4
User/task/user_task.h
View File

@ -24,6 +24,7 @@ extern "C" {
#define DEBUG_FREQ (10.0)
#define CMD_FREQ (500.0)
#define REFEREE_FREQ (500.0)
#define SUPER_CAP_FREQ (500.0)
/* 任务初始化延时ms */
#define TASK_INIT_DELAY (100u)
@ -40,6 +41,7 @@ extern "C" {
#define DEBUG_INIT_DELAY (0)
#define CMD_INIT_DELAY (0)
#define REFEREE_INIT_DELAY (0)
#define SUPER_CAP_INIT_DELAY (0)
/* Exported defines --------------------------------------------------------- */
/* Exported macro ----------------------------------------------------------- */
@ -98,6 +100,8 @@ typedef struct {
}ai;
struct{
osMessageQueueId_t ref; /* Referee_ForCap_t, cmd转发 */
osMessageQueueId_t for_chassis;
osMessageQueueId_t power_limit;
}cap;
struct {
struct {