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

好用

Browse Source
This commit is contained in:
Robofish 2025-10-15 03:42:44 +08:00
parent 7070545aa2
commit 8b5a7ec6cb
4 changed files with 56 additions and 67 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
User/bsp/can.c
View File

@ -45,9 +45,7 @@ static osMessageQueueId_t BSP_CAN_FindQueue(BSP_CAN_t can, uint32_t can_id);
static int8_t BSP_CAN_CreateIdQueue(BSP_CAN_t can, uint32_t can_id, uint8_t queue_size); static int8_t BSP_CAN_CreateIdQueue(BSP_CAN_t can, uint32_t can_id, uint8_t queue_size);
static void BSP_CAN_RxFifo0Callback(void); static void BSP_CAN_RxFifo0Callback(void);
static void BSP_CAN_RxFifo1Callback(void); static void BSP_CAN_RxFifo1Callback(void);
static void BSP_CAN_TxCompleteCallback_Internal(BSP_CAN_t can); static void BSP_CAN_TxCompleteCallback(void);
static void BSP_CAN_1_TxCompleteCallback(void);
static void BSP_CAN_2_TxCompleteCallback(void);
static BSP_CAN_FrameType_t BSP_CAN_GetFrameType(CAN_RxHeaderTypeDef *header); static BSP_CAN_FrameType_t BSP_CAN_GetFrameType(CAN_RxHeaderTypeDef *header);
static uint32_t BSP_CAN_DefaultIdParser(uint32_t original_id, BSP_CAN_FrameType_t frame_type); static uint32_t BSP_CAN_DefaultIdParser(uint32_t original_id, BSP_CAN_FrameType_t frame_type);
static void BSP_CAN_TxQueueInit(BSP_CAN_t can); static void BSP_CAN_TxQueueInit(BSP_CAN_t can);
@ -214,11 +212,14 @@ static bool BSP_CAN_TxQueueIsEmpty(BSP_CAN_t can) {
} }
/** /**
* @brief 处理指定CAN实例的发送队列 * @brief 处理所有CAN实例的发送队列
*/ */
static void BSP_CAN_TxCompleteCallback_Internal(BSP_CAN_t can) { static void BSP_CAN_TxCompleteCallback(void) {
// 处理所有CAN实例的发送队列
for (int i = 0; i < BSP_CAN_NUM; i++) {
BSP_CAN_t can = (BSP_CAN_t)i;
CAN_HandleTypeDef *hcan = BSP_CAN_GetHandle(can); CAN_HandleTypeDef *hcan = BSP_CAN_GetHandle(can);
if (hcan == NULL) return; if (hcan == NULL) continue;
BSP_CAN_TxMessage_t msg; BSP_CAN_TxMessage_t msg;
uint32_t mailbox; uint32_t mailbox;
@ -242,22 +243,8 @@ static void BSP_CAN_TxCompleteCallback_Internal(BSP_CAN_t can) {
} }
} }
} }
/**
* @brief 1发送完成回调
*/
static void BSP_CAN_1_TxCompleteCallback(void) {
BSP_CAN_TxCompleteCallback_Internal(BSP_CAN_1);
} }
/**
* @brief 2发送完成回调
*/
static void BSP_CAN_2_TxCompleteCallback(void) {
BSP_CAN_TxCompleteCallback_Internal(BSP_CAN_2);
}
/** /**
* @brief FIFO0接收处理函数 * @brief FIFO0接收处理函数
@ -478,9 +465,9 @@ int8_t BSP_CAN_Init(void) {
// 自动注册CAN1接收回调函数 // 自动注册CAN1接收回调函数
BSP_CAN_RegisterCallback(BSP_CAN_1, HAL_CAN_RX_FIFO0_MSG_PENDING_CB, BSP_CAN_RxFifo0Callback); BSP_CAN_RegisterCallback(BSP_CAN_1, HAL_CAN_RX_FIFO0_MSG_PENDING_CB, BSP_CAN_RxFifo0Callback);
BSP_CAN_RegisterCallback(BSP_CAN_1, HAL_CAN_TX_MAILBOX0_CPLT_CB, BSP_CAN_1_TxCompleteCallback); BSP_CAN_RegisterCallback(BSP_CAN_1, HAL_CAN_TX_MAILBOX0_CPLT_CB, BSP_CAN_TxCompleteCallback);
BSP_CAN_RegisterCallback(BSP_CAN_1, HAL_CAN_TX_MAILBOX1_CPLT_CB, BSP_CAN_1_TxCompleteCallback); BSP_CAN_RegisterCallback(BSP_CAN_1, HAL_CAN_TX_MAILBOX1_CPLT_CB, BSP_CAN_TxCompleteCallback);
BSP_CAN_RegisterCallback(BSP_CAN_1, HAL_CAN_TX_MAILBOX2_CPLT_CB, BSP_CAN_1_TxCompleteCallback); BSP_CAN_RegisterCallback(BSP_CAN_1, HAL_CAN_TX_MAILBOX2_CPLT_CB, BSP_CAN_TxCompleteCallback);
// 激活CAN1中断 // 激活CAN1中断
HAL_CAN_ActivateNotification(&hcan1, CAN_IT_RX_FIFO0_MSG_PENDING | HAL_CAN_ActivateNotification(&hcan1, CAN_IT_RX_FIFO0_MSG_PENDING |
@ -494,9 +481,9 @@ int8_t BSP_CAN_Init(void) {
// 自动注册CAN2接收回调函数 // 自动注册CAN2接收回调函数
BSP_CAN_RegisterCallback(BSP_CAN_2, HAL_CAN_RX_FIFO1_MSG_PENDING_CB, BSP_CAN_RxFifo1Callback); BSP_CAN_RegisterCallback(BSP_CAN_2, HAL_CAN_RX_FIFO1_MSG_PENDING_CB, BSP_CAN_RxFifo1Callback);
BSP_CAN_RegisterCallback(BSP_CAN_2, HAL_CAN_TX_MAILBOX0_CPLT_CB, BSP_CAN_2_TxCompleteCallback); BSP_CAN_RegisterCallback(BSP_CAN_2, HAL_CAN_TX_MAILBOX0_CPLT_CB, BSP_CAN_TxCompleteCallback);
BSP_CAN_RegisterCallback(BSP_CAN_2, HAL_CAN_TX_MAILBOX1_CPLT_CB, BSP_CAN_2_TxCompleteCallback); BSP_CAN_RegisterCallback(BSP_CAN_2, HAL_CAN_TX_MAILBOX1_CPLT_CB, BSP_CAN_TxCompleteCallback);
BSP_CAN_RegisterCallback(BSP_CAN_2, HAL_CAN_TX_MAILBOX2_CPLT_CB, BSP_CAN_2_TxCompleteCallback); BSP_CAN_RegisterCallback(BSP_CAN_2, HAL_CAN_TX_MAILBOX2_CPLT_CB, BSP_CAN_TxCompleteCallback);
// 激活CAN2中断 // 激活CAN2中断
HAL_CAN_ActivateNotification(&hcan2, CAN_IT_RX_FIFO1_MSG_PENDING | HAL_CAN_ActivateNotification(&hcan2, CAN_IT_RX_FIFO1_MSG_PENDING |

1
User/component/limiter.h
View File

@ -61,4 +61,3 @@ float PowerLimit_TargetPower(float power_limit, float power_buffer);
*/ */
float HeatLimit_ShootFreq(float heat, float heat_limit, float cooling_rate, float HeatLimit_ShootFreq(float heat, float heat_limit, float cooling_rate,
float heat_increase, bool is_big); float heat_increase, bool is_big);

23
User/module/balance_chassis.c
View File

@ -409,9 +409,9 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
LQR_State_t target_state = {0}; LQR_State_t target_state = {0};
target_state.theta = c->param->theta; // 目标摆杆角度 target_state.theta = c->param->theta; // 目标摆杆角度
target_state.d_theta = 0.0f; // 目标摆杆角速度 target_state.d_theta = 0.0f; // 目标摆杆角速度
target_state.phi = 11.9601*pow((0.13f + c_cmd->height * 0.25f),3) + -11.8715*pow((0.13f + c_cmd->height * 0.25f),2) + 3.87083*(0.13f + c_cmd->height * 0.25f) + -0.414154; // 目标俯仰角 target_state.phi = 11.9601*pow((0.15f + c_cmd->height * 0.25f),3) + -11.8715*pow((0.15f + c_cmd->height * 0.25f),2) + 3.87083*(0.15f + c_cmd->height * 0.25f) + -0.414154; // 目标俯仰角
target_state.d_phi = 0.0f; // 目标俯仰角速度 target_state.d_phi = 0.0f; // 目标俯仰角速度
target_state.x = c->chassis_state.target_x -2.07411f*(0.13f + c_cmd->height * 0.25f) + 0.41182f; target_state.x = c->chassis_state.target_x -2.07411f*(0.15f + c_cmd->height * 0.25f) + 0.41182f;
target_state.d_x = target_dot_x; // 目标速度 target_state.d_x = target_dot_x; // 目标速度
if (c->mode != CHASSIS_MODE_ROTOR){ if (c->mode != CHASSIS_MODE_ROTOR){
@ -434,7 +434,7 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
LQR_SetTargetState(&c->lqr[0], &target_state); LQR_SetTargetState(&c->lqr[0], &target_state);
LQR_Control(&c->lqr[0]); LQR_Control(&c->lqr[0]);
} else { } else {
target_state.theta = 0.13f; // 非接触时，腿摆角目标为0.1rad，防止腿完全悬空 target_state.theta = 0.15f; // 非接触时，腿摆角目标为0.1rad，防止腿完全悬空
LQR_SetTargetState(&c->lqr[0], &target_state); LQR_SetTargetState(&c->lqr[0], &target_state);
c->lqr[0].control_output.T = 0.0f; c->lqr[0].control_output.T = 0.0f;
c->lqr[0].control_output.Tp = c->lqr[0].control_output.Tp =
@ -446,7 +446,7 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
LQR_SetTargetState(&c->lqr[1], &target_state); LQR_SetTargetState(&c->lqr[1], &target_state);
LQR_Control(&c->lqr[1]); LQR_Control(&c->lqr[1]);
}else { }else {
target_state.theta = 0.13f; // 非接触时，腿摆角目标为0.1rad，防止腿完全悬空 target_state.theta = 0.15f; // 非接触时，腿摆角目标为0.1rad，防止腿完全悬空
LQR_SetTargetState(&c->lqr[1], &target_state); LQR_SetTargetState(&c->lqr[1], &target_state);
c->lqr[1].control_output.T = 0.0f; c->lqr[1].control_output.T = 0.0f;
c->lqr[1].control_output.Tp = c->lqr[1].control_output.Tp =
@ -472,20 +472,23 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
c->feedback.imu.gyro.x, c->dt); c->feedback.imu.gyro.x, c->dt);
// 限制补偿力范围，防止过度补偿 // 限制补偿力范围，防止过度补偿
// 如果任一腿离地，限制补偿力为20
if (!c->vmc_[0].leg.is_ground_contact || !c->vmc_[1].leg.is_ground_contact) {
if (roll_compensation_force > 20.0f) if (roll_compensation_force > 20.0f)
roll_compensation_force = 20.0f; roll_compensation_force = 20.0f;
if (roll_compensation_force < -20.0f) if (roll_compensation_force < -20.0f)
roll_compensation_force = -20.0f; roll_compensation_force = -20.0f;
}
// 目标腿长设定（移除横滚角补偿） // 目标腿长设定（移除横滚角补偿）
switch (c->state) { switch (c->state) {
case 0: // 正常状态，根据高度指令调节腿长 case 0: // 正常状态，根据高度指令调节腿长
target_L0[0] = 0.13f + c_cmd->height * 0.22f; // 左腿：基础腿长 + 高度调节 target_L0[0] = 0.15f + c_cmd->height * 0.22f; // 左腿：基础腿长 + 高度调节
target_L0[1] = 0.13f + c_cmd->height * 0.22f; // 右腿：基础腿长 + 高度调节 target_L0[1] = 0.15f + c_cmd->height * 0.22f; // 右腿：基础腿长 + 高度调节
break; break;
case 1: // 准备阶段，腿长收缩 case 1: // 准备阶段，腿长收缩
target_L0[0] = 0.13f; // 左腿：收缩到基础腿长 target_L0[0] = 0.15f; // 左腿：收缩到基础腿长
target_L0[1] = 0.13f; // 右腿：收缩到基础腿长 target_L0[1] = 0.15f; // 右腿：收缩到基础腿长
break; break;
case 2: // 起跳阶段，腿长快速伸展 case 2: // 起跳阶段，腿长快速伸展
target_L0[0] = 0.55f; // 左腿：伸展到最大腿长 target_L0[0] = 0.55f; // 左腿：伸展到最大腿长
@ -496,8 +499,8 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
target_L0[1] = 0.1f; // 右腿：缓冲腿长 target_L0[1] = 0.1f; // 右腿：缓冲腿长
break; break;
default: default:
target_L0[0] = 0.13f + c_cmd->height * 0.22f; target_L0[0] = 0.15f + c_cmd->height * 0.22f;
target_L0[1] = 0.13f + c_cmd->height * 0.22f; target_L0[1] = 0.15f + c_cmd->height * 0.22f;
break; break;
} }

12
User/module/config.c
View File

@ -179,12 +179,12 @@ Config_RobotParam_t robot_config = {
}, },
.roll={ .roll={
.k=20.0f, .k=30.0f,
.p=5.0f, /* 横滚角比例系数 */ .p=20.0f, /* 横滚角比例系数 */
.i=0.0f, /* 横滚角积分系数 */ .i=1.0f, /* 横滚角积分系数 */
.d=0.2f, /* 横滚角微分系数 */ .d=0.5f, /* 横滚角微分系数 */
.i_limit=0.0f, /* 积分限幅 */ .i_limit=20.0f, /* 积分限幅 */
.out_limit=50.0f, /* 输出限幅，腿长差值限制 */ .out_limit=100.0f, /* 输出限幅，腿长差值限制 */
.d_cutoff_freq=30.0f, /* 微分截止频率 */ .d_cutoff_freq=30.0f, /* 微分截止频率 */
.range=-1.0f, /* 不使用循环误差处理 */ .range=-1.0f, /* 不使用循环误差处理 */
}, },