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

给底盘添加yaw

Browse Source
This commit is contained in:
Robofish 2025-10-06 19:28:34 +08:00
parent 39108dec77
commit cce8e7bdaf
5 changed files with 19 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
User/module/balance_chassis.c
View File

@ -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], &current_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],

1
User/task/ctrl_chassis.c
View File

@ -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);

6
User/task/ctrl_gimbal.c
View File

@ -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); /* 运行结束，等待下一次唤醒 */
}

2
User/task/init.c
View File

@ -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);

1
User/task/user_task.h
View File

@ -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;