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

75
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 void BSP_CAN_RxFifo0Callback(void);
static void BSP_CAN_RxFifo1Callback(void);
static void BSP_CAN_TxCompleteCallback_Internal(BSP_CAN_t can);
static void BSP_CAN_1_TxCompleteCallback(void);
static void BSP_CAN_2_TxCompleteCallback(void);
static void BSP_CAN_TxCompleteCallback(void);
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 void BSP_CAN_TxQueueInit(BSP_CAN_t can);
@ -214,50 +212,39 @@ static bool BSP_CAN_TxQueueIsEmpty(BSP_CAN_t can) {
}
/**
* @brief 处理指定CAN实例的发送队列
* @brief 处理所有CAN实例的发送队列
*/
static void BSP_CAN_TxCompleteCallback_Internal(BSP_CAN_t can) {
CAN_HandleTypeDef *hcan = BSP_CAN_GetHandle(can);
if (hcan == NULL) return;
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);
if (hcan == NULL) continue;
BSP_CAN_TxMessage_t msg;
uint32_t mailbox;
BSP_CAN_TxMessage_t msg;
uint32_t mailbox;
// 尝试发送队列中的消息
while (!BSP_CAN_TxQueueIsEmpty(can)) {
// 检查是否有空闲邮箱
if (HAL_CAN_GetTxMailboxesFreeLevel(hcan) == 0) {
break; // 没有空闲邮箱，等待下次中断
}
// 尝试发送队列中的消息
while (!BSP_CAN_TxQueueIsEmpty(can)) {
// 检查是否有空闲邮箱
if (HAL_CAN_GetTxMailboxesFreeLevel(hcan) == 0) {
break; // 没有空闲邮箱，等待下次中断
}
// 从队列中取出消息
if (!BSP_CAN_TxQueuePop(can, &msg)) {
break;
}
// 从队列中取出消息
if (!BSP_CAN_TxQueuePop(can, &msg)) {
break;
}
// 发送消息
if (HAL_CAN_AddTxMessage(hcan, &msg.header, msg.data, &mailbox) != HAL_OK) {
// 发送失败，消息已经从队列中移除，直接丢弃
break;
// 发送消息
if (HAL_CAN_AddTxMessage(hcan, &msg.header, msg.data, &mailbox) != HAL_OK) {
// 发送失败，消息已经从队列中移除，直接丢弃
break;
}
}
}
}
/**
* @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接收处理函数
@ -478,9 +465,9 @@ int8_t BSP_CAN_Init(void) {
// 自动注册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_TX_MAILBOX0_CPLT_CB, BSP_CAN_1_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_MAILBOX2_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_TxCompleteCallback);
BSP_CAN_RegisterCallback(BSP_CAN_1, HAL_CAN_TX_MAILBOX2_CPLT_CB, BSP_CAN_TxCompleteCallback);
// 激活CAN1中断
HAL_CAN_ActivateNotification(&hcan1, CAN_IT_RX_FIFO0_MSG_PENDING |
@ -494,9 +481,9 @@ int8_t BSP_CAN_Init(void) {
// 自动注册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_TX_MAILBOX0_CPLT_CB, BSP_CAN_2_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_MAILBOX2_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_TxCompleteCallback);
BSP_CAN_RegisterCallback(BSP_CAN_2, HAL_CAN_TX_MAILBOX2_CPLT_CB, BSP_CAN_TxCompleteCallback);
// 激活CAN2中断
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 heat_increase, bool is_big);

31
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};
target_state.theta = c->param->theta; // 目标摆杆角度
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.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; // 目标速度
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_Control(&c->lqr[0]);
} else {
target_state.theta = 0.13f; // 非接触时，腿摆角目标为0.1rad，防止腿完全悬空
target_state.theta = 0.15f; // 非接触时，腿摆角目标为0.1rad，防止腿完全悬空
LQR_SetTargetState(&c->lqr[0], &target_state);
c->lqr[0].control_output.T = 0.0f;
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_Control(&c->lqr[1]);
}else {
target_state.theta = 0.13f; // 非接触时，腿摆角目标为0.1rad，防止腿完全悬空
target_state.theta = 0.15f; // 非接触时，腿摆角目标为0.1rad，防止腿完全悬空
LQR_SetTargetState(&c->lqr[1], &target_state);
c->lqr[1].control_output.T = 0.0f;
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);
// 限制补偿力范围，防止过度补偿
if (roll_compensation_force > 20.0f)
roll_compensation_force = 20.0f;
if (roll_compensation_force < -20.0f)
roll_compensation_force = -20.0f;
// 如果任一腿离地，限制补偿力为20
if (!c->vmc_[0].leg.is_ground_contact || !c->vmc_[1].leg.is_ground_contact) {
if (roll_compensation_force > 20.0f)
roll_compensation_force = 20.0f;
if (roll_compensation_force < -20.0f)
roll_compensation_force = -20.0f;
}
// 目标腿长设定（移除横滚角补偿）
switch (c->state) {
case 0: // 正常状态，根据高度指令调节腿长
target_L0[0] = 0.13f + c_cmd->height * 0.22f; // 左腿：基础腿长 + 高度调节
target_L0[1] = 0.13f + c_cmd->height * 0.22f; // 右腿：基础腿长 + 高度调节
target_L0[0] = 0.15f + c_cmd->height * 0.22f; // 左腿：基础腿长 + 高度调节
target_L0[1] = 0.15f + c_cmd->height * 0.22f; // 右腿：基础腿长 + 高度调节
break;
case 1: // 准备阶段，腿长收缩
target_L0[0] = 0.13f; // 左腿：收缩到基础腿长
target_L0[1] = 0.13f; // 右腿：收缩到基础腿长
target_L0[0] = 0.15f; // 左腿：收缩到基础腿长
target_L0[1] = 0.15f; // 右腿：收缩到基础腿长
break;
case 2: // 起跳阶段，腿长快速伸展
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; // 右腿：缓冲腿长
break;
default:
target_L0[0] = 0.13f + c_cmd->height * 0.22f;
target_L0[1] = 0.13f + c_cmd->height * 0.22f;
target_L0[0] = 0.15f + c_cmd->height * 0.22f;
target_L0[1] = 0.15f + c_cmd->height * 0.22f;
break;
}

12
User/module/config.c
View File

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