From cce8e7bdafbf1e1ee787e4bf3d323e43613fa62f Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Mon, 6 Oct 2025 19:28:34 +0800 Subject: [PATCH] =?UTF-8?q?=E7=BB=99=E5=BA=95=E7=9B=98=E6=B7=BB=E5=8A=A0ya?= =?UTF-8?q?w?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- User/module/balance_chassis.c | 25 +++++++++---------------- User/task/ctrl_chassis.c | 1 + User/task/ctrl_gimbal.c | 6 ++++++ User/task/init.c | 2 ++ User/task/user_task.h | 1 + 5 files changed, 19 insertions(+), 16 deletions(-) diff --git a/User/module/balance_chassis.c b/User/module/balance_chassis.c index c0f2c6b..df51519 100644 --- a/User/module/balance_chassis.c +++ b/User/module/balance_chassis.c @@ -165,13 +165,6 @@ int8_t Chassis_UpdateFeedback(Chassis_t *c) { c->feedback.imu.gyro = virtual_chassis.data.imu.gyro; c->feedback.imu.euler = virtual_chassis.data.imu.euler; - /* 更新云台电机反馈数据（yaw轴） */ - MOTOR_DM_Update(&(c->param->yaw_motor)); - MOTOR_DM_t *dm_motor = MOTOR_DM_GetMotor(&(c->param->yaw_motor)); - if (dm_motor != NULL) { - c->feedback.yaw = dm_motor->motor.feedback; - } - // 更新机体状态估计 Chassis_UpdateChassisState(c); @@ -302,8 +295,8 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { current_state.d_theta = c->vmc_[0].leg.d_theta; current_state.x = xhat; current_state.d_x = x_dot_hat; - current_state.phi = -c->feedback.imu.euler.pit; - current_state.d_phi = -c->feedback.imu.gyro.y; + current_state.phi = c->feedback.imu.euler.pit; + current_state.d_phi = c->feedback.imu.gyro.y; LQR_UpdateState(&c->lqr[0], ¤t_state); @@ -319,7 +312,7 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { LQR_State_t target_state = {0}; target_state.theta = 0.0f; // 目标摆杆角度 target_state.d_theta = 0.0f; // 目标摆杆角速度 - target_state.phi = -0.2f; // 目标俯仰角 + target_state.phi = 0.0f; // 目标俯仰角 target_state.d_phi = 0.0f; // 目标俯仰角速度 target_state.x = c->chassis_state.target_x; // 目标位置 target_state.d_x = target_dot_x; // 目标速度 @@ -339,10 +332,10 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { c->feedback.yaw.rotor_abs_angle, 0.0f, c->dt); /* 轮毂力矩输出（参考C++版本的减速比） */ - // c->output.wheel[0] = c->lqr[0].control_output.T / 4.5f + c->yaw_control.yaw_force; - // c->output.wheel[1] = c->lqr[1].control_output.T / 4.5f - c->yaw_control.yaw_force; - c->output.wheel[0] = c->lqr[0].control_output.T / 4.5f; - c->output.wheel[1] = c->lqr[1].control_output.T / 4.5f; + c->output.wheel[0] = c->lqr[0].control_output.T / 4.5f + c->yaw_control.yaw_force; + c->output.wheel[1] = c->lqr[1].control_output.T / 4.5f - c->yaw_control.yaw_force; + // c->output.wheel[0] = c->lqr[0].control_output.T / 4.5f; + // c->output.wheel[1] = c->lqr[1].control_output.T / 4.5f; /* 腿长控制和VMC逆解算（使用PID控制） */ float virtual_force[2]; float target_L0[2]; @@ -384,7 +377,7 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { c->vmc_[0].leg.L0, leg_d_length[0], c->dt); // 腿长控制力 = LQR摆杆力矩的径向分量 + PID腿长控制输出 + 基础支撑力 - 横滚角补偿力（左腿减少支撑力） - virtual_force[0] = (c->lqr[0].control_output.Tp) * sinf(c->vmc_[0].leg.theta) + pid_output + 10.0f ; + virtual_force[0] = (c->lqr[0].control_output.Tp) * sinf(c->vmc_[0].leg.theta) + pid_output + 40.0f ; // VMC逆解算（包含摆角补偿） if (VMC_InverseSolve(&c->vmc_[0], virtual_force[0], @@ -405,7 +398,7 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { c->vmc_[1].leg.L0, leg_d_length[1], c->dt); // 腿长控制力 = LQR摆杆力矩的径向分量 + PID腿长控制输出 + 基础支撑力 + 横滚角补偿力（右腿增加支撑力） - virtual_force[1] = (c->lqr[1].control_output.Tp) * sinf(c->vmc_[1].leg.theta) + pid_output + 10.0f ; + virtual_force[1] = (c->lqr[1].control_output.Tp) * sinf(c->vmc_[1].leg.theta) + pid_output + 40.0f ; // VMC逆解算（包含摆角补偿） if (VMC_InverseSolve(&c->vmc_[1], virtual_force[1], diff --git a/User/task/ctrl_chassis.c b/User/task/ctrl_chassis.c index 176e034..f5c0456 100644 --- a/User/task/ctrl_chassis.c +++ b/User/task/ctrl_chassis.c @@ -58,6 +58,7 @@ void Task_ctrl_chassis(void *argument) { if (osMessageQueueGet(task_runtime.msgq.Chassis_cmd, &chassis_cmd, NULL, 0) != osOK) { // 没有新命令，保持上次命令 } + osMessageQueueGet(task_runtime.msgq.chassis_yaw, &chassis.feedback.yaw, NULL, 0); Chassis_UpdateFeedback(&chassis); diff --git a/User/task/ctrl_gimbal.c b/User/task/ctrl_gimbal.c index 05de732..9eed8a1 100644 --- a/User/task/ctrl_gimbal.c +++ b/User/task/ctrl_gimbal.c @@ -4,6 +4,7 @@ */ /* Includes ----------------------------------------------------------------- */ +#include "cmsis_os2.h" #include "task/user_task.h" /* USER INCLUDE BEGIN */ #include "component/ahrs.h" @@ -45,11 +46,16 @@ void Task_ctrl_gimbal(void *argument) { } osMessageQueueGet(task_runtime.msgq.gimbal.cmd, &gimbal_cmd, NULL, 0); + Gimbal_UpdateFeedback(&gimbal); + // osMessageQueueReset(task_runtime.msgq.chassis_yaw); + osMessageQueuePut(task_runtime.msgq.chassis_yaw, &gimbal.feedback.motor.yaw, 0, 0); + Gimbal_Control(&gimbal, &gimbal_cmd); Gimbal_Output(&gimbal); + /* USER CODE END */ osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */ } diff --git a/User/task/init.c b/User/task/init.c index d6baa6f..3fab228 100644 --- a/User/task/init.c +++ b/User/task/init.c @@ -7,6 +7,7 @@ #include "task/user_task.h" /* USER INCLUDE BEGIN */ +#include "device/motor.h" #include "module/balance_chassis.h" #include "module/gimbal.h" #include "module/shoot.h" @@ -46,6 +47,7 @@ void Task_Init(void *argument) { task_runtime.msgq.user_msg= osMessageQueueNew(2u, 10, NULL); task_runtime.msgq.chassis_imu = osMessageQueueNew(2u, sizeof(Chassis_IMU_t), NULL); task_runtime.msgq.Chassis_cmd = osMessageQueueNew(2u, sizeof(Chassis_CMD_t), NULL); + task_runtime.msgq.chassis_yaw = osMessageQueueNew(2u, sizeof(MOTOR_Feedback_t), NULL); task_runtime.msgq.gimbal.imu= osMessageQueueNew(2u, sizeof(Gimbal_IMU_t), NULL); task_runtime.msgq.gimbal.cmd= osMessageQueueNew(2u, sizeof(Gimbal_CMD_t), NULL); task_runtime.msgq.shoot.shoot_cmd = osMessageQueueNew(2u, sizeof(Shoot_CMD_t), NULL); diff --git a/User/task/user_task.h b/User/task/user_task.h index d25e52e..d10a055 100644 --- a/User/task/user_task.h +++ b/User/task/user_task.h @@ -57,6 +57,7 @@ typedef struct { osMessageQueueId_t chassis_imu; osMessageQueueId_t Chassis_cmd; + osMessageQueueId_t chassis_yaw; struct { osMessageQueueId_t imu; osMessageQueueId_t cmd;