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-01 03:01:32 +08:00
parent ad716705ff
commit 57b47b800e
3 changed files with 35 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
User/module/balance_chassis.c
View File

@ -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; // 基础支撑力

5
User/module/balance_chassis.h
View File

@ -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增益矩阵参数 */

11
User/module/config.c
View File

@ -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 = {
{ // 左腿