TuxMonkey/half-steering-basic-framework
1
0
Fork 0
mirror of https://gitee.com/dlmu-cone/half-steering-basic-framework synced 2026-10-23 17:42:20 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge remote-tracking branch 'origin/codex/chassis-power-control' into codex/chassis-power-control

Browse Source
This commit is contained in:
guoxy06
2026-08-29 19:58:09 +08:00
parent c40525234f 63d4b7ae7d
commit a9fe5069f4
8 changed files with 825 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

169
application/chassis/chassis.c
View File

@@ -17,6 +17,9 @@
#include "dji_motor.h"
#include "LKMotor.h"
#include "super_cap.h"
#include "chassis_power.h"
#include "power_meter.h"
#include "vofa.h"
#include "message_center.h"
#include "referee_task.h"
#include "user_lib.h"
@@ -25,6 +28,31 @@
#include "bsp_dwt.h"
#include "referee_UI.h"
#include "arm_math.h"
#include <math.h>
#ifndef CHASSIS_POWER_LIMIT_ENABLE
#define CHASSIS_POWER_LIMIT_ENABLE 0
#endif
#ifndef CHASSIS_POWER_USE_POWER_METER
#define CHASSIS_POWER_USE_POWER_METER 1
#endif
#ifndef CHASSIS_POWER_RLS_ENABLE
#define CHASSIS_POWER_RLS_ENABLE 0
#endif
#ifndef CHASSIS_POWER_VOFA_ENABLE
#define CHASSIS_POWER_VOFA_ENABLE 0
#endif
#ifndef CHASSIS_POWER_VOFA_DIV
#define CHASSIS_POWER_VOFA_DIV 10u
#endif
ChassisPowerInstance chassis_power_control;
PowerMeterInstance *chassis_power_meter;
ChassisPower_Limit_Input_s chassis_power_debug_input;
/* 根据robot_def.h中的macro自动计算的参数 */
#define HALF_WHEEL_BASE (WHEEL_BASE / 2.0f) // 半轴距
@@ -181,6 +209,26 @@ void ChassisInit()
// （新增）设置超级电容功率补偿，例如补偿5W，可根据实际需求调整
SuperCapSetPowerOffset(cap, 5);
#if CHASSIS_POWER_USE_POWER_METER
chassis_power_meter = PowerMeterInitCan(&hcan1, POWER_METER_DEFAULT_CAN_ID);
#endif
ChassisPowerMotorModel_s wheel_model = ChassisPowerGetDefault3508Model();
ChassisPower_Init_Config_s power_config = {
.wheel_model = wheel_model,
.default_power_limit = 60.0f,
.min_power_limit = 20.0f,
.buffer_low = 30.0f,
.buffer_high = 50.0f,
.buffer_gain = 0.25f,
.steering_power_reserve = 6.0f,
.min_scale = 0.20f,
.rls_lambda = 0.995f,
.rls_init_covariance = 1000.0f,
.rls_enable = CHASSIS_POWER_RLS_ENABLE,
};
ChassisPowerInit(&chassis_power_control, &power_config);
// 发布订阅初始化,如果为双板,则需要can comm来传递消息
#ifdef CHASSIS_BOARD
Chassis_IMU_data = INS_Init(); // 底盘IMU初始化
@@ -340,6 +388,123 @@ static void SteeringWheelCalculate()
*/
static void LimitChassisOutput()
{
#if CHASSIS_POWER_LIMIT_ENABLE
float limited_rf = vt_rf;
float limited_lb = vt_lb;
const PowerMeterInstance *meter_data = 0;
#if CHASSIS_POWER_USE_POWER_METER
meter_data = PowerMeterGetData(chassis_power_meter);
#endif
ChassisPower_Limit_Input_s power_input = {
.rf_speed_ref = vt_rf,
.lb_speed_ref = vt_lb,
.rf_speed = motor_rf->measure.speed_rads,
.lb_speed = motor_lb->measure.speed_rads,
.rf_current_cmd = motor_rf->speed_PID.Output,
.lb_current_cmd = motor_lb->speed_PID.Output,
.rf_output_cmd = motor_rf->current_PID.Output,
.lb_output_cmd = motor_lb->current_PID.Output,
.rf_feedback_current = motor_rf->measure.real_current,
.lb_feedback_current = motor_lb->measure.real_current,
.referee_limit = referee_data != 0 ? referee_data->GameRobotState.chassis_power_limit : 0.0f,
.referee_power = referee_data != 0 ? referee_data->PowerHeatData.chassis_power : 0.0f,
.power_buffer = referee_data != 0 ? referee_data->PowerHeatData.chassis_power_buffer : 0.0f,
.measured_power = meter_data != 0 ? meter_data->power : 0.0f,
.steering_power_estimate = 0.0f,
.power_meter_online = meter_data != 0 ? meter_data->online : 0u,
};
chassis_power_debug_input = power_input;
ChassisPowerUpdateMeasureState(&chassis_power_control, &power_input);
ChassisPowerLimitWheelRefs(&chassis_power_control, &power_input, &limited_rf, &limited_lb);
if (power_input.power_meter_online) {
ChassisPowerUpdateRLS(&chassis_power_control, &power_input, power_input.measured_power);
}
vt_rf = limited_rf;
vt_lb = limited_lb;
#if CHASSIS_POWER_VOFA_ENABLE
static uint16_t vofa_divider = 0u;
if (++vofa_divider >= CHASSIS_POWER_VOFA_DIV) {
vofa_divider = 0u;
const ChassisPower_State_s *power_state = ChassisPowerGetState(&chassis_power_control);
float vofa_data[] = {
power_state->estimated_total_power,
power_state->allowed_power,
power_state->scale,
power_input.measured_power,
meter_data != 0 ? meter_data->voltage : 0.0f,
meter_data != 0 ? meter_data->current : 0.0f,
power_state->rf_ref_raw,
power_state->rf_ref_limited,
power_state->lb_ref_raw,
power_state->lb_ref_limited,
motor_rf->measure.speed_rads,
motor_lb->measure.speed_rads,
motor_rf->speed_PID.Output,
motor_lb->speed_PID.Output,
motor_rf->measure.real_current,
motor_lb->measure.real_current,
};
vofa_justfloat_output(vofa_data, sizeof(vofa_data) / sizeof(vofa_data[0]), &huart1);
}
#endif
#else
const PowerMeterInstance *meter_data = 0;
#if CHASSIS_POWER_USE_POWER_METER
meter_data = PowerMeterGetData(chassis_power_meter);
#endif
ChassisPower_Limit_Input_s power_input = {
.rf_speed_ref = vt_rf,
.lb_speed_ref = vt_lb,
.rf_speed = motor_rf->measure.speed_rads,
.lb_speed = motor_lb->measure.speed_rads,
.rf_current_cmd = motor_rf->speed_PID.Output,
.lb_current_cmd = motor_lb->speed_PID.Output,
.rf_output_cmd = motor_rf->current_PID.Output,
.lb_output_cmd = motor_lb->current_PID.Output,
.rf_feedback_current = motor_rf->measure.real_current,
.lb_feedback_current = motor_lb->measure.real_current,
.referee_limit = referee_data != 0 ? referee_data->GameRobotState.chassis_power_limit : 0.0f,
.referee_power = referee_data != 0 ? referee_data->PowerHeatData.chassis_power : 0.0f,
.power_buffer = referee_data != 0 ? referee_data->PowerHeatData.chassis_power_buffer : 0.0f,
.measured_power = meter_data != 0 ? meter_data->power : 0.0f,
.steering_power_estimate = 0.0f,
.power_meter_online = meter_data != 0 ? meter_data->online : 0u,
};
chassis_power_debug_input = power_input;
ChassisPowerUpdateMeasureState(&chassis_power_control, &power_input);
#if CHASSIS_POWER_VOFA_ENABLE
static uint16_t vofa_divider = 0u;
if (++vofa_divider >= CHASSIS_POWER_VOFA_DIV) {
vofa_divider = 0u;
const ChassisPower_State_s *power_state = ChassisPowerGetState(&chassis_power_control);
float vofa_data[] = {
power_state->estimated_total_power,
power_state->allowed_power,
power_state->scale,
power_input.measured_power,
meter_data != 0 ? meter_data->voltage : 0.0f,
meter_data != 0 ? meter_data->current : 0.0f,
power_state->rf_ref_raw,
power_state->rf_ref_limited,
power_state->lb_ref_raw,
power_state->lb_ref_limited,
motor_rf->measure.speed_rads,
motor_lb->measure.speed_rads,
motor_rf->speed_PID.Output,
motor_lb->speed_PID.Output,
motor_rf->measure.real_current,
motor_lb->measure.real_current,
};
vofa_justfloat_output(vofa_data, sizeof(vofa_data) / sizeof(vofa_data[0]), &huart1);
}
#endif
#endif
// 功率限制待添加
// referee_data->PowerHeatData.chassis_power;
// referee_data->PowerHeatData.chassis_power_buffer;
@@ -416,7 +581,7 @@ void ChassisTask() {
default:
break;
}
static float sin_theta, cos_theta;
float sin_theta = 0.0f, cos_theta = 1.0f;
// ------------------ 【修改部分：加入相位滞后动态补偿】 ------------------
// 提取当前的云台-底盘夹角
float real_offset_angle = chassis_cmd_recv.offset_angle;
@@ -477,4 +642,4 @@ void ChassisTask() {
#ifdef CHASSIS_BOARD
CANCommSend(chasiss_can_comm, (void *)&chassis_feedback_data);
#endif // CHASSIS_BOARD
}
}