From 57b47b800ed4fc751dc3443d10b1ca249ed53cbb Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Wed, 1 Oct 2025 03:01:32 +0800 Subject: [PATCH] =?UTF-8?q?=E6=B7=BB=E5=8A=A0yaw?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- User/module/balance_chassis.c | 40 +++++++++++++++++++---------------- User/module/balance_chassis.h | 5 +++-- User/module/config.c | 11 +++++++++- 3 files changed, 35 insertions(+), 21 deletions(-) diff --git a/User/module/balance_chassis.c b/User/module/balance_chassis.c index 5939ff1..fe925b7 100644 --- a/User/module/balance_chassis.c +++ b/User/module/balance_chassis.c @@ -122,6 +122,7 @@ int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq){ for (int i = 0; i < 2; i++) { MOTOR_LK_Register(&c->param->wheel_motors[i]); } + MOTOR_DM_Register(&c->param->yaw_motor); // Chassis_MotorEnable(c); @@ -173,12 +174,6 @@ int8_t Chassis_UpdateFeedback(Chassis_t *c){ for (int i = 0; i < 4; i++) { MOTOR_LZ_t *joint_motor = MOTOR_LZ_GetMotor(&c->param->joint_motors[i]); if (joint_motor != NULL) { - // c->feedback.joint[i] = joint_motor->motor.feedback; - // // c->feedback.joint[i].rotor_abs_angle = joint_motor->motor.feedback.rotor_abs_angle - M_PI; // 机械零点调整 - // if (c->feedback.joint[i].rotor_abs_angle > M_PI ){ - // c->feedback.joint[i].rotor_abs_angle -= M_2PI; - // } - // c->feedback.joint[i].rotor_abs_angle = - c->feedback.joint[i].rotor_abs_angle; // 机械零点调整 if (joint_motor->motor.feedback.rotor_abs_angle > M_PI ){ joint_motor->motor.feedback.rotor_abs_angle -= M_2PI; } @@ -195,7 +190,14 @@ int8_t Chassis_UpdateFeedback(Chassis_t *c){ c->feedback.wheel[i] = wheel_motor->motor.feedback; } } - + + /* 更新云台电机反馈数据（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); @@ -443,19 +445,21 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { } } - c->yaw_control.current_yaw = c->feedback.imu.euler.yaw; + // c->yaw_control.current_yaw = c->feedback.imu.euler.yaw; - c->yaw_control.target_yaw -= c_cmd->move_vec.vy * 0.005f; // 目标偏航角，假设遥控器输入范围为[-10, 10]，映射到[-0.02, 0.02] rad/s + // c->yaw_control.target_yaw -= c_cmd->move_vec.vy * 0.005f; // 目标偏航角，假设遥控器输入范围为[-10, 10]，映射到[-0.02, 0.02] rad/s - // 修正目标yaw角度到[-pi, pi] - if (c->yaw_control.target_yaw > M_PI) { - c->yaw_control.target_yaw -= M_2PI; - } else if (c->yaw_control.target_yaw < -M_PI) { - c->yaw_control.target_yaw += M_2PI; - } + // // 修正目标yaw角度到[-pi, pi] + // if (c->yaw_control.target_yaw > M_PI) { + // c->yaw_control.target_yaw -= M_2PI; + // } else if (c->yaw_control.target_yaw < -M_PI) { + // c->yaw_control.target_yaw += M_2PI; + // } - c->yaw_control.yaw_force = PID_Calc(&c->pid.yaw, c->yaw_control.target_yaw, c->feedback.imu.euler.yaw, c->feedback.imu.gyro.z, c->dt); - + // c->yaw_control.yaw_force = PID_Calc(&c->pid.yaw, c->yaw_control.target_yaw, c->feedback.imu.euler.yaw, c->feedback.imu.gyro.z, c->dt); + c->yaw_control.current_yaw = c->feedback.yaw.rotor_abs_angle; + c->yaw_control.target_yaw = c->param->mech_zero_yaw; + c->yaw_control.yaw_force = PID_Calc(&c->pid.yaw, c->yaw_control.target_yaw, c->feedback.yaw.rotor_abs_angle, 0.0f, c->dt); /* 轮毂力矩输出（参考C++版本的减速比） */ c->output.wheel[0] = Tw[0] / 4.5f - c->yaw_control.yaw_force; @@ -490,7 +494,7 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { // 腿长控制力 = LQR摆杆力矩的径向分量 + PID腿长控制输出 + 基础支撑力 virtual_force[leg] = (Tp[leg]) * sinf(leg_theta[leg]) + - pid_output + 40.0f; + pid_output + 30.0f; // + // PID腿长控制输出 // 45.0f; // 基础支撑力 diff --git a/User/module/balance_chassis.h b/User/module/balance_chassis.h index b87e2a0..d102d34 100644 --- a/User/module/balance_chassis.h +++ b/User/module/balance_chassis.h @@ -24,6 +24,7 @@ extern "C" { #include "device/motor.h" #include "device/motor_lk.h" #include "device/motor_lz.h" +#include "device/motor_dm.h" /* Exported constants ------------------------------------------------------- */ #define CHASSIS_OK (0) /* 运行正常 */ #define CHASSIS_ERR (-1) /* 运行时发现了其他错误 */ @@ -56,8 +57,8 @@ typedef struct { typedef struct { MOTOR_Feedback_t joint[4]; /* 四个关节电机反馈 */ MOTOR_Feedback_t wheel[2]; /* 两个轮子电机反馈 */ + MOTOR_Feedback_t yaw; /* 云台Yaw轴电机反馈 */ Chassis_IMU_t imu; /* IMU数据 */ - MOTOR_Feedback_t yaw; /* 云台Yaw轴电机反馈 */ }Chassis_Feedback_t; typedef struct { @@ -71,7 +72,7 @@ typedef struct { MOTOR_LZ_Param_t joint_motors[4]; /* 四个关节电机参数 */ MOTOR_LK_Param_t wheel_motors[2]; /* 两个轮子电机参数 */ - + MOTOR_DM_Param_t yaw_motor; /* 云台Yaw轴电机参数 */ VMC_Param_t vmc_param[2]; /* VMC参数 */ LQR_GainMatrix_t lqr_gains; /* LQR增益矩阵参数 */ diff --git a/User/module/config.c b/User/module/config.c index dc49643..982703c 100644 --- a/User/module/config.c +++ b/User/module/config.c @@ -5,6 +5,7 @@ /* Includes ----------------------------------------------------------------- */ #include "module/config.h" #include "bsp/can.h" +#include "device/motor_dm.h" #include "device/rc_can.h" /* Private typedef ---------------------------------------------------------- */ @@ -132,7 +133,15 @@ Config_RobotParam_t robot_config = { .reverse = true, }, }, - .mech_zero_yaw = 0.0f, + .yaw_motor = { // 云台Yaw轴电机 + .can = BSP_CAN_2, + .can_id = 0x1, + .master_id = 0x11, + .module = MOTOR_DM_J4310, + .reverse = false, + }, + + .mech_zero_yaw = 4.37085676f, /* 250.5度，机械零点 */ .vmc_param = { { // 左腿