Compare commits
4 Commits
539dc0a2f4
...
480e41e68a
| Author | SHA1 | Date | |
|---|---|---|---|
| 480e41e68a | |||
| ec3d36ff08 | |||
| d933c59ae8 | |||
| 467567fae6 |
@ -158,7 +158,7 @@ int8_t CAN_TX_SuperCapData(CAN_SuperCapTXDataTypeDef * TX_Temp)
|
||||
|
||||
/*******balance特供*******/
|
||||
/**
|
||||
* @brief 限制功率不超过power_limit（超电测量功率版本）
|
||||
* @brief 限制功率不超过power_limit（out[i]版本）（未完成）
|
||||
*
|
||||
* @param power_limit 最大功率
|
||||
* @param motor_out 电机输出值
|
||||
@ -174,7 +174,7 @@ int8_t PowerLimit_Output_by_cap(float power_limit, float *motor_out, uint32_t le
|
||||
float ChassisPower = CAN_SuperCapRXData.ChassisPower ;
|
||||
|
||||
/* 保持每个电机输出值缩小时比例不变 */
|
||||
if (ChassisPower > power_limit) {
|
||||
if (ChassisPower > power_limit - 1) {
|
||||
for (uint32_t i = 0; i < len; i++) {
|
||||
motor_out[i] *= power_limit / ChassisPower;
|
||||
}
|
||||
@ -183,17 +183,17 @@ int8_t PowerLimit_Output_by_cap(float power_limit, float *motor_out, uint32_t le
|
||||
return 0;
|
||||
}
|
||||
/**
|
||||
* @brief 限制功率不超过power_limit（out[i]版本）（未完成）
|
||||
* @brief 限制功率不超过power_limit（超电测量功率版本）
|
||||
*/
|
||||
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) {
|
||||
if (ChassisPower > power_limit - 1) {
|
||||
for (uint32_t i = 0; i < len; i++) {
|
||||
motor_out[i] *= power_limit / ChassisPower;
|
||||
}
|
||||
|
||||
@ -1042,7 +1042,7 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
|
||||
c->chassis_state.target_velocity_x = -world_vx * sinf(gimbal_offset) + world_vy * cosf(gimbal_offset);
|
||||
c->chassis_state.last_target_velocity_x = c->chassis_state.target_velocity_x;
|
||||
} else {
|
||||
c->chassis_state.target_velocity_x = c_cmd->move_vec.vx * 2.0f;
|
||||
c->chassis_state.target_velocity_x = c_cmd->move_vec.vx * 3.0f;
|
||||
c->chassis_state.last_target_velocity_x = c->chassis_state.target_velocity_x;
|
||||
}
|
||||
|
||||
@ -1050,7 +1050,7 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
|
||||
* 无速度指令时：位移+速度双环控制，积分保持位置锁定
|
||||
* 位移误差钳位到±2m，防止积分漂移过大 */
|
||||
if (fabsf(c->chassis_state.target_velocity_x) > 0.01f) {
|
||||
c->chassis_state.target_x = c->chassis_state.position_x;
|
||||
c->chassis_state.target_x = c->chassis_state.position_x+0.8f; /* 直接锁定在当前位置，消除位移误差 */
|
||||
} else {
|
||||
c->chassis_state.target_x += c->chassis_state.target_velocity_x * c->dt;
|
||||
}
|
||||
|
||||
@ -27,9 +27,10 @@ extern Chassis_t chassis;
|
||||
void Cap_Control(CAN_SuperCapRXDataTypeDef *cap, const Referee_ForCap_t *referee)
|
||||
{
|
||||
|
||||
if (CAN_SuperCapRXData.SuperCapEnergy<=35)chassis.power_limit = referee->chassis_power_limit ;
|
||||
if (CAN_SuperCapRXData.SuperCapEnergy<=100)chassis.power_limit = referee->chassis_power_limit ;
|
||||
else chassis.power_limit = -1;
|
||||
/*
|
||||
|
||||
/*
|
||||
if (referee->ref_status != REF_STATUS_RUNNING) {
|
||||
//当裁判系统离线时，依然使用裁判系统进程传来的数据
|
||||
ChassisSetPower = referee->chassis_power_limit;
|
||||
|
||||
@ -5,6 +5,7 @@
|
||||
/* Includes ----------------------------------------------------------------- */
|
||||
#include "module/config.h"
|
||||
#include "module/balance_chassis.h"
|
||||
#include "module/gimbal.h"
|
||||
#include <stdbool.h>
|
||||
|
||||
|
||||
@ -72,18 +73,37 @@ Config_RobotParam_t robot_config = {
|
||||
.d_cutoff_freq = -1.0f,
|
||||
.range = M_2PI,
|
||||
},
|
||||
.recover = {
|
||||
.yaw_angle = {
|
||||
.k = 2.0f,
|
||||
.p = 1.0f,
|
||||
.i = 0.0f,
|
||||
.d = 0.0f,
|
||||
.i_limit = 0.0f,
|
||||
.out_limit = 5.0f,
|
||||
.d_cutoff_freq = -1.0f,
|
||||
.range = M_2PI,
|
||||
},
|
||||
.pit_angle = {
|
||||
.k = 1.5f,
|
||||
.p = 1.0f,
|
||||
.i = 0.0f,
|
||||
.d = 0.0f,
|
||||
.i_limit = 0.0f,
|
||||
.out_limit = 5.0f,
|
||||
.d_cutoff_freq = -1.0f,
|
||||
.range = M_2PI,
|
||||
},
|
||||
},
|
||||
},
|
||||
.mech_zero = {
|
||||
.yaw = 0.0f,
|
||||
.yaw = 2.96925735f, /* 机械零点 */
|
||||
.pit = 3.23056364f,
|
||||
},
|
||||
.travel = {
|
||||
.yaw = -1.0f,
|
||||
.pit = 0.85f,
|
||||
},
|
||||
.recover = {
|
||||
.yaw_mech_zero = 0.0f, /* yaw编码器机械零点，需要实测标定 */
|
||||
},
|
||||
.low_pass_cutoff_freq = {
|
||||
.out = -1.0f,
|
||||
.gyro = 1000.0f,
|
||||
@ -502,13 +522,15 @@ Config_RobotParam_t robot_config = {
|
||||
.rc_mode_map = {
|
||||
#if CMD_ENABLE_MODULE_GIMBAL
|
||||
.gimbal_sw_up = GIMBAL_MODE_RELAX,
|
||||
.gimbal_sw_mid = GIMBAL_MODE_RELATIVE,
|
||||
// .gimbal_sw_mid = GIMBAL_MODE_ABSOLUTE,
|
||||
.gimbal_sw_mid = GIMBAL_MODE_RECOVER,
|
||||
.gimbal_sw_down = GIMBAL_MODE_RELATIVE,
|
||||
#endif
|
||||
#if CMD_ENABLE_MODULE_BALANCE_CHASSIS
|
||||
.balance_sw_up = CHASSIS_MODE_RELAX,
|
||||
.balance_sw_mid = CHASSIS_MODE_WHELL_LEG_BALANCE,
|
||||
.balance_sw_down = CHASSIS_MODE_BALANCE_ROTOR,
|
||||
// .balance_sw_mid = CHASSIS_MODE_WHELL_LEG_BALANCE,
|
||||
.balance_sw_mid = CHASSIS_MODE_RELAX, // 先关闭轮腿平衡，避免不熟悉操作导致摔倒
|
||||
.balance_sw_down = CHASSIS_MODE_WHELL_LEG_BALANCE,
|
||||
#endif
|
||||
},
|
||||
},
|
||||
|
||||
@ -46,6 +46,8 @@ static int8_t Gimbal_SetMode(Gimbal_t *g, Gimbal_Mode_t mode) {
|
||||
PID_Reset(&g->pid.aimbot.yaw_omega);
|
||||
PID_Reset(&g->pid.aimbot.pit_angle);
|
||||
PID_Reset(&g->pid.aimbot.pit_omega);
|
||||
PID_Reset(&g->pid.recover.yaw_angle);
|
||||
PID_Reset(&g->pid.recover.pit_angle);
|
||||
LowPassFilter2p_Reset(&g->filter_out.yaw, 0.0f);
|
||||
LowPassFilter2p_Reset(&g->filter_out.pit, 0.0f);
|
||||
|
||||
@ -105,6 +107,12 @@ int8_t Gimbal_Init(Gimbal_t *g, const Gimbal_Params_t *param,
|
||||
PID_Init(&(g->pid.aimbot.pit_omega), KPID_MODE_SET_D, target_freq,
|
||||
&(g->param->pid.aimbot.pit_omega));
|
||||
|
||||
/* Recover模式PID（单环，编码器反馈） */
|
||||
PID_Init(&(g->pid.recover.yaw_angle), KPID_MODE_NO_D, target_freq,
|
||||
&(g->param->pid.recover.yaw_angle));
|
||||
PID_Init(&(g->pid.recover.pit_angle), KPID_MODE_NO_D, target_freq,
|
||||
&(g->param->pid.recover.pit_angle));
|
||||
|
||||
LowPassFilter2p_Init(&g->filter_out.yaw, target_freq,
|
||||
g->param->low_pass_cutoff_freq.out);
|
||||
LowPassFilter2p_Init(&g->filter_out.pit, target_freq,
|
||||
@ -211,14 +219,24 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd, Gimbal_AI_t *g_ai) {
|
||||
/* AI 模式：直接从指令获取角度设定值（每周期刷新） */
|
||||
g->setpoint.eulr.yaw = g_cmd->setpoint_yaw;
|
||||
g->setpoint.eulr.pit = g_cmd->setpoint_pit;
|
||||
}
|
||||
} else if (g_cmd->mode == GIMBAL_MODE_RECOVER) {
|
||||
/* 双零点选近：mech_zero 和 mech_zero+π，选离当前编码器角度最近的 */
|
||||
float yaw_enc = g->feedback.motor.yaw.rotor_abs_angle;
|
||||
float zero0 = g->param->mech_zero.yaw;
|
||||
float zero1 = zero0 + M_PI;
|
||||
if (zero1 >= M_2PI) zero1 -= M_2PI;
|
||||
float err0 = fabsf(CircleError(zero0, yaw_enc, M_2PI));
|
||||
float err1 = fabsf(CircleError(zero1, yaw_enc, M_2PI));
|
||||
g->setpoint.eulr.yaw = (err0 <= err1) ? zero0 : zero1;
|
||||
g->setpoint.eulr.pit = 0.5f * (g->limit.pit.max + g->limit.pit.min);
|
||||
}
|
||||
|
||||
/* 控制相关逻辑 */
|
||||
float yaw_omega_set_point, pit_omega_set_point;
|
||||
switch (g->mode) {
|
||||
case GIMBAL_MODE_RELAX:
|
||||
g->out.yaw = 0.0f;
|
||||
g->out.pit = 0.0f;
|
||||
/* fallthrough - AI控制模式也需要执行PID计算 */
|
||||
break;
|
||||
case GIMBAL_MODE_ABSOLUTE:
|
||||
case GIMBAL_MODE_RELATIVE:
|
||||
@ -233,7 +251,6 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd, Gimbal_AI_t *g_ai) {
|
||||
g->feedback.imu.gyro.y, 0.f, g->dt);
|
||||
|
||||
break;
|
||||
break;
|
||||
case GIMBAL_MODE_AI_CONTROL:
|
||||
/* --- YAW --- */
|
||||
// 位置环: Kp * (pos_ref - pos_fdb)
|
||||
@ -246,44 +263,22 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd, Gimbal_AI_t *g_ai) {
|
||||
g->feedback.imu.gyro.z, 0.0f, g->dt)
|
||||
+ g_cmd->ff_accl_yaw * GIMBAL_YAW_INERTIA; // 加速度前馈: J * acc
|
||||
|
||||
/* --- PITCH --- */
|
||||
/* --- PITCH --- (反馈轴与 ABSOLUTE 模式一致: eulr.rol / gyro.y) */
|
||||
pit_omega_set_point = PID_Calc(&(g->pid.aimbot.pit_angle),
|
||||
g->setpoint.eulr.pit,
|
||||
g->feedback.imu.eulr.pit, 0.0f, g->dt);
|
||||
g->feedback.imu.eulr.rol, 0.0f, g->dt);
|
||||
g->out.pit = PID_Calc(&(g->pid.aimbot.pit_omega),
|
||||
pit_omega_set_point + g_cmd->ff_vel_pit,
|
||||
g->feedback.imu.gyro.x, 0.0f, g->dt)
|
||||
g->feedback.imu.gyro.y, 0.0f, g->dt)
|
||||
+ g_cmd->ff_accl_pit * GIMBAL_PIT_INERTIA; // 加速度前馈: J * acc
|
||||
break;
|
||||
|
||||
case GIMBAL_MODE_RECOVER: {
|
||||
/* --- YAW: 编码器双环PID，双零点选近 --- */
|
||||
float yaw_enc = g->feedback.motor.yaw.rotor_abs_angle;
|
||||
float zero0 = g->param->recover.yaw_mech_zero;
|
||||
float zero1 = zero0 + M_PI;
|
||||
if (zero1 >= M_2PI) zero1 -= M_2PI;
|
||||
|
||||
/* 计算到两个零点的圆周距离，选近的 */
|
||||
float err0 = fabsf(CircleError(zero0, yaw_enc, M_2PI));
|
||||
float err1 = fabsf(CircleError(zero1, yaw_enc, M_2PI));
|
||||
float yaw_target = (err0 <= err1) ? zero0 : zero1;
|
||||
|
||||
yaw_omega_set_point = PID_Calc(&(g->pid.yaw_angle), yaw_target,
|
||||
yaw_enc, 0.0f, g->dt);
|
||||
g->out.yaw = PID_Calc(&(g->pid.yaw_omega), yaw_omega_set_point,
|
||||
g->feedback.motor.yaw.rotor_speed, 0.0f, g->dt);
|
||||
|
||||
/* --- PITCH: 编码器双环PID，锁定限位中心 --- */
|
||||
float pit_center = 0.5f * (g->limit.pit.max + g->limit.pit.min);
|
||||
float pit_enc = g->feedback.motor.pit.rotor_abs_angle;
|
||||
|
||||
pit_omega_set_point = PID_Calc(&(g->pid.pit_angle), pit_center,
|
||||
pit_enc, 0.0f, g->dt);
|
||||
g->out.pit = PID_Calc(&(g->pid.pit_omega), pit_omega_set_point,
|
||||
g->feedback.motor.pit.rotor_speed, 0.0f, g->dt);
|
||||
case GIMBAL_MODE_RECOVER:
|
||||
g->out.yaw = PID_Calc(&(g->pid.recover.yaw_angle), g->setpoint.eulr.yaw,
|
||||
g->feedback.motor.yaw.rotor_abs_angle, 0.0f, g->dt);
|
||||
g->out.pit = PID_Calc(&(g->pid.recover.pit_angle), g->setpoint.eulr.pit,
|
||||
g->feedback.motor.pit.rotor_abs_angle, 0.0f, g->dt);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
/* 输出滤波 */
|
||||
g->out.yaw = LowPassFilter2p_Apply(&g->filter_out.yaw, g->out.yaw);
|
||||
|
||||
@ -30,8 +30,8 @@ typedef enum {
|
||||
GIMBAL_MODE_RELAX, /* 放松模式，电机不输出。一般情况云台初始化之后的模式 */
|
||||
GIMBAL_MODE_ABSOLUTE, /* 绝对坐标系控制，控制在空间内的绝对姿态 */
|
||||
GIMBAL_MODE_RELATIVE, /* 相对坐标系控制，控制相对于底盘的姿态 */
|
||||
GIMBAL_MODE_AI_CONTROL, /* AI控制模式，直接接受AI下发的目标角度 */
|
||||
GIMBAL_MODE_RECOVER /* 回中模式，使用编码器双环PID控制yaw回零点，pitch锁定限位中心 */
|
||||
GIMBAL_MODE_AI_CONTROL, /* AI控制模式，直接接受AI下发的目标角度 */
|
||||
GIMBAL_MODE_RECOVER /* 自起模式，底盘翻倒后自动恢复到正常姿态 */
|
||||
} Gimbal_Mode_t;
|
||||
|
||||
/* UI 导出结构（供 referee 系统绘制） */
|
||||
@ -80,6 +80,10 @@ typedef struct {
|
||||
KPID_Params_t pit_angle; /* pitch轴角位置环PID参数 */
|
||||
KPID_Params_t pit_omega; /* pitch轴角速度环PID参数 */
|
||||
} aimbot;
|
||||
struct {
|
||||
KPID_Params_t yaw_angle; /* recover yaw位置环PID参数 */
|
||||
KPID_Params_t pit_angle; /* recover pitch位置环PID参数 */
|
||||
} recover;
|
||||
} pid;
|
||||
|
||||
/* 低通滤波器截止频率 */
|
||||
@ -98,11 +102,6 @@ typedef struct {
|
||||
float pit; /* pitch轴机械限位行程 -1表示无限位*/
|
||||
} travel;
|
||||
|
||||
/* Recover模式参数 */
|
||||
struct {
|
||||
float yaw_mech_zero; /* yaw机械零点（编码器角度） */
|
||||
} recover;
|
||||
|
||||
} Gimbal_Params_t;
|
||||
|
||||
/* 云台反馈数据的结构体，包含反馈控制用的反馈数据 */
|
||||
@ -147,12 +146,16 @@ typedef struct {
|
||||
KPID_t yaw_omega; /* yaw轴角速度环PID */
|
||||
KPID_t pit_angle; /* pitch轴角位置环PID */
|
||||
KPID_t pit_omega; /* pitch轴角速度环PID */
|
||||
struct {
|
||||
struct {
|
||||
KPID_t yaw_angle; /* yaw轴角位置环PID */
|
||||
KPID_t yaw_omega; /* yaw轴角速度环PID */
|
||||
KPID_t pit_angle; /* pitch轴角位置环PID */
|
||||
KPID_t pit_omega; /* pitch轴角速度环PID */
|
||||
} aimbot;
|
||||
struct {
|
||||
KPID_t yaw_angle; /* recover yaw位置环PID */
|
||||
KPID_t pit_angle; /* recover pitch位置环PID */
|
||||
} recover;
|
||||
} pid;
|
||||
|
||||
struct {
|
||||
|
||||
@ -49,7 +49,8 @@ void Task_cap(void *argument) {
|
||||
while (1) {
|
||||
tick += delay_tick; /* 计算下一个唤醒时刻 */
|
||||
/* USER CODE BEGIN */
|
||||
//osMessageQueueGet(task_runtime.msgq.referee.ui.tocap, , NULL, 0);
|
||||
osMessageQueueGet(task_runtime.msgq.referee.tocmd.cap, &referee_cap , NULL, 0);
|
||||
power_limit = referee_cap.chassis_power_limit;
|
||||
|
||||
cap_online = get_supercap_online_state();
|
||||
|
||||
@ -57,6 +58,7 @@ void Task_cap(void *argument) {
|
||||
/* 根据裁判系统数据计算输出功率 */
|
||||
SuperCap_CanTX.Powerlimit = power_limit;
|
||||
Cap_Control(&CAN_SuperCapRXData, &referee_cap);
|
||||
|
||||
SuperCap_Update();
|
||||
CAN_TX_SuperCapData(&SuperCap_CanTX);
|
||||
// osKernelUnlock();
|
||||
|
||||
Loading…
Reference in New Issue
Block a user