617 lines
21 KiB
C
617 lines
21 KiB
C
/**
|
||
* @file balance_chassis.c
|
||
* @brief 平衡底盘控制模块
|
||
*/
|
||
|
||
#include "module/balance_chassis.h"
|
||
#include "bsp/can.h"
|
||
#include "bsp/time.h"
|
||
#include "component/filter.h"
|
||
#include "component/user_math.h"
|
||
#include "device/motor_lk.h"
|
||
#include "device/motor_lz.h"
|
||
#include <math.h>
|
||
#include <stddef.h>
|
||
#include <string.h>
|
||
|
||
/* Private defines ---------------------------------------------------------- */
|
||
#define LIMIT(x, min, max) ((x) < (min) ? (min) : ((x) > (max) ? (max) : (x)))
|
||
|
||
#define WHEEL_RADIUS 0.068f /* 轮子半径 (m) */
|
||
#define MAX_ACCELERATION 2.0f /* 最大加速度 (m/s²) */
|
||
#define WHEEL_GEAR_RATIO 8.0f /* 轮毂电机扭矩 */
|
||
#define BASE_LEG_LENGTH 0.16f /* 基础腿长 (m) */
|
||
#define LEG_LENGTH_RANGE 0.22f /* 腿长调节范围 (m) */
|
||
#define MIN_LEG_LENGTH 0.10f /* 最小腿长 (m) */
|
||
#define MAX_LEG_LENGTH 0.34f /* 最大腿长 (m) */
|
||
#define NON_CONTACT_THETA 0.16f /* 离地时的摆角目标 (rad) */
|
||
#define LEFT_BASE_FORCE 60.0f /* 左腿基础支撑力 (N) */
|
||
#define RIGHT_BASE_FORCE 60.0f /* 右腿基础支撑力 (N) */
|
||
float L_fn = 0.0f, L_tp = 0.0f, R_fn = 0.0f, R_tp = 0.0f;
|
||
/* Private function prototypes ---------------------------------------------- */
|
||
static int8_t Chassis_MotorEnable(Chassis_t *c);
|
||
static int8_t Chassis_MotorRelax(Chassis_t *c);
|
||
static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode);
|
||
static void Chassis_UpdateChassisState(Chassis_t *c);
|
||
static void Chassis_ResetControllers(Chassis_t *c);
|
||
static void Chassis_SelectYawZeroPoint(Chassis_t *c);
|
||
|
||
/* Private functions -------------------------------------------------------- */
|
||
|
||
/**
|
||
* @brief 使能所有电机
|
||
* @param c 底盘结构体指针
|
||
* @return 操作结果
|
||
*/
|
||
static int8_t Chassis_MotorEnable(Chassis_t *c) {
|
||
if (c == NULL) return CHASSIS_ERR_NULL;
|
||
|
||
for (int i = 0; i < 4; i++) {
|
||
MOTOR_LZ_Enable(&c->param->joint_param[i]);
|
||
}
|
||
for (int i = 0; i < 2; i++) {
|
||
MOTOR_LK_MotorOn(&c->param->wheel_param[i]);
|
||
}
|
||
return CHASSIS_OK;
|
||
}
|
||
|
||
/**
|
||
* @brief 放松所有电机
|
||
* @param c 底盘结构体指针
|
||
* @return 操作结果
|
||
*/
|
||
static int8_t Chassis_MotorRelax(Chassis_t *c) {
|
||
if (c == NULL) return CHASSIS_ERR_NULL;
|
||
|
||
for (int i = 0; i < 4; i++) {
|
||
MOTOR_LZ_Relax(&c->param->joint_param[i]);
|
||
}
|
||
for (int i = 0; i < 2; i++) {
|
||
MOTOR_LK_Relax(&c->param->wheel_param[i]);
|
||
}
|
||
return CHASSIS_OK;
|
||
}
|
||
|
||
/**
|
||
* @brief 重置所有控制器
|
||
* @param c 底盘结构体指针
|
||
*/
|
||
static void Chassis_ResetControllers(Chassis_t *c) {
|
||
/* 重置PID控制器 */
|
||
PID_Reset(&c->pid.leg_length[0]);
|
||
PID_Reset(&c->pid.leg_length[1]);
|
||
PID_Reset(&c->pid.yaw);
|
||
PID_Reset(&c->pid.roll);
|
||
PID_Reset(&c->pid.tp[0]);
|
||
PID_Reset(&c->pid.tp[1]);
|
||
|
||
/* 重置LQR控制器 */
|
||
LQR_Reset(&c->lqr[0]);
|
||
LQR_Reset(&c->lqr[1]);
|
||
|
||
/* 重置滤波器 */
|
||
for (int i = 0; i < 4; i++) {
|
||
LowPassFilter2p_Reset(&c->filter.joint_out[i], 0.0f);
|
||
}
|
||
for (int i = 0; i < 2; i++) {
|
||
LowPassFilter2p_Reset(&c->filter.wheel_out[i], 0.0f);
|
||
}
|
||
|
||
/* 清空机体状态 */
|
||
c->chassis_state.position_x = 0.0f;
|
||
c->chassis_state.velocity_x = 0.0f;
|
||
c->chassis_state.last_velocity_x = 0.0f;
|
||
c->chassis_state.target_x = 0.0f;
|
||
c->chassis_state.target_velocity_x = 0.0f;
|
||
c->chassis_state.last_target_velocity_x = 0.0f;
|
||
}
|
||
|
||
/**
|
||
* @brief 选择yaw轴零点（双零点自动选择）
|
||
* @param c 底盘结构体指针
|
||
*/
|
||
static void Chassis_SelectYawZeroPoint(Chassis_t *c) {
|
||
float current_yaw = c->feedback.yaw.rotor_abs_angle;
|
||
float zero_point_1 = c->param->mech_zero_yaw;
|
||
float zero_point_2 = zero_point_1 + M_PI;
|
||
|
||
float error_to_zero1 = CircleError(zero_point_1, current_yaw, M_2PI);
|
||
float error_to_zero2 = CircleError(zero_point_2, current_yaw, M_2PI);
|
||
|
||
if (fabsf(error_to_zero1) <= fabsf(error_to_zero2)) {
|
||
c->yaw_control.target_yaw = zero_point_1;
|
||
c->yaw_control.is_reversed = false;
|
||
} else {
|
||
c->yaw_control.target_yaw = zero_point_2;
|
||
c->yaw_control.is_reversed = true;
|
||
}
|
||
}
|
||
|
||
/**
|
||
* @brief 设置底盘模式
|
||
* @param c 底盘结构体指针
|
||
* @param mode 目标模式
|
||
* @return 操作结果
|
||
*/
|
||
static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode) {
|
||
if (c == NULL) return CHASSIS_ERR_NULL;
|
||
if (mode == c->mode) return CHASSIS_OK;
|
||
|
||
Chassis_MotorEnable(c);
|
||
Chassis_ResetControllers(c);
|
||
Chassis_SelectYawZeroPoint(c);
|
||
|
||
c->mode = mode;
|
||
return CHASSIS_OK;
|
||
}
|
||
|
||
/**
|
||
* @brief 更新机体状态估计
|
||
* @param c 底盘结构体指针
|
||
* @note 基于轮腿机器人运动学的精确速度估计算法
|
||
* 综合考虑轮子速度、关节运动和机体姿态的影响
|
||
*/
|
||
static void Chassis_UpdateChassisState(Chassis_t *c) {
|
||
if (c == NULL) return;
|
||
|
||
/* 获取轮子原始速度数据 (dps -> rad/s) */
|
||
float left_wheel_speed = c->feedback.wheel[0].rotor_speed * (M_PI / 180.0f);
|
||
float right_wheel_speed = c->feedback.wheel[1].rotor_speed * (M_PI / 180.0f);
|
||
|
||
/* 获取关节速度和陀螺仪数据 (dps -> rad/s) */
|
||
float left_joint_speed = c->feedback.joint[1].rotor_speed * (M_PI / 180.0f); /* 左前关节 */
|
||
float right_joint_speed = c->feedback.joint[2].rotor_speed * (M_PI / 180.0f); /* 右前关节 */
|
||
float pitch_gyro = c->feedback.imu.gyro.y; /* 俯仰角速度 rad/s */
|
||
|
||
/*
|
||
* 轮子角速度补偿算法
|
||
* 补偿关节运动对轮子速度的影响，并消除机体俯仰运动的干扰
|
||
* 公式来源：五连杆机构运动学分析
|
||
*/
|
||
float left_w_wheel_compensated = left_wheel_speed + left_joint_speed - pitch_gyro;
|
||
float right_w_wheel_compensated = right_wheel_speed + right_joint_speed - pitch_gyro;
|
||
|
||
/*
|
||
* 机体速度计算算法
|
||
* 综合考虑：
|
||
* 1. 轮子接触点线速度：omega_wheel * r_wheel
|
||
* 2. 摆杆运动贡献：L0 * d_theta (摆杆角速度在x方向的分量)
|
||
* 3. 腿长变化贡献：d_L0 * sin(theta) (腿长变化在x方向的分量)
|
||
*/
|
||
float left_v_body = left_w_wheel_compensated * WHEEL_RADIUS +
|
||
c->vmc_[0].leg.L0 * c->vmc_[0].leg.d_theta +
|
||
c->vmc_[0].leg.d_L0 * sinf(c->vmc_[0].leg.theta);
|
||
|
||
float right_v_body = right_w_wheel_compensated * WHEEL_RADIUS +
|
||
c->vmc_[1].leg.L0 * c->vmc_[1].leg.d_theta +
|
||
c->vmc_[1].leg.d_L0 * sinf(c->vmc_[1].leg.theta);
|
||
|
||
/* 保存历史速度 */
|
||
c->chassis_state.last_velocity_x = c->chassis_state.velocity_x;
|
||
|
||
/* 计算平均速度 */
|
||
float vel_measured = (left_v_body + right_v_body) / 2.0f;
|
||
|
||
/*
|
||
* 接地状态检测与速度修正
|
||
* 当双腿都离地时，基于轮子的速度估计不可靠，将速度设为0
|
||
*/
|
||
if (c->vmc_[0].leg.Fn < 20.0f && c->vmc_[1].leg.Fn < 20.0f) {
|
||
vel_measured = 0.0f;
|
||
}
|
||
|
||
/* 使用二阶低通滤波器平滑速度估计 */
|
||
c->chassis_state.velocity_x = LowPassFilter2p_Apply(&c->filter.velocity_est, vel_measured);
|
||
|
||
/* 位置积分更新 */
|
||
c->chassis_state.position_x += c->chassis_state.velocity_x * c->dt;
|
||
}
|
||
|
||
/* Public functions --------------------------------------------------------- */
|
||
|
||
/**
|
||
* @brief 初始化底盘
|
||
* @param c 底盘结构体指针
|
||
* @param param 底盘参数指针
|
||
* @param target_freq 目标控制频率
|
||
* @return 操作结果
|
||
*/
|
||
int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq) {
|
||
if (c == NULL || param == NULL || target_freq <= 0.0f) {
|
||
return CHASSIS_ERR;
|
||
}
|
||
|
||
c->param = param;
|
||
c->mode = CHASSIS_MODE_RELAX;
|
||
|
||
/* 初始化CAN */
|
||
BSP_CAN_Init();
|
||
|
||
/* 初始化和注册电机 */
|
||
MOTOR_LZ_Init();
|
||
|
||
for (int i = 0; i < 4; i++) {
|
||
if (MOTOR_LZ_Register(&c->param->joint_param[i]) != DEVICE_OK) {
|
||
return DEVICE_ERR;
|
||
}
|
||
}
|
||
|
||
for (int i = 0; i < 2; i++) {
|
||
if (MOTOR_LK_Register(&c->param->wheel_param[i]) != DEVICE_OK) {
|
||
return DEVICE_ERR;
|
||
}
|
||
}
|
||
|
||
MOTOR_DM_Register(&c->param->yaw_motor);
|
||
|
||
/* 初始化VMC控制器 */
|
||
VMC_Init(&c->vmc_[0], ¶m->vmc_param[0], target_freq);
|
||
VMC_Init(&c->vmc_[1], ¶m->vmc_param[1], target_freq);
|
||
|
||
/* 初始化PID控制器 */
|
||
PID_Init(&c->pid.leg_length[0], KPID_MODE_CALC_D, target_freq, ¶m->leg_length);
|
||
PID_Init(&c->pid.leg_length[1], KPID_MODE_CALC_D, target_freq, ¶m->leg_length);
|
||
PID_Init(&c->pid.yaw, KPID_MODE_CALC_D, target_freq, ¶m->yaw);
|
||
PID_Init(&c->pid.roll, KPID_MODE_CALC_D, target_freq, ¶m->roll);
|
||
PID_Init(&c->pid.tp[0], KPID_MODE_CALC_D, target_freq, ¶m->tp);
|
||
PID_Init(&c->pid.tp[1], KPID_MODE_CALC_D, target_freq, ¶m->tp);
|
||
|
||
/* 初始化LQR控制器 */
|
||
LQR_Init(&c->lqr[0], ¶m->lqr_gains);
|
||
LQR_Init(&c->lqr[1], ¶m->lqr_gains);
|
||
|
||
/* 初始化滤波器 */
|
||
for (int i = 0; i < 4; i++) {
|
||
LowPassFilter2p_Init(&c->filter.joint_out[i], target_freq, param->low_pass_cutoff_freq.out);
|
||
}
|
||
for (int i = 0; i < 2; i++) {
|
||
LowPassFilter2p_Init(&c->filter.wheel_out[i], target_freq, param->low_pass_cutoff_freq.out);
|
||
}
|
||
LowPassFilter2p_Init(&c->filter.velocity_est, target_freq, 30.0f); /* 速度估计滤波截止频率30Hz */
|
||
|
||
Chassis_MotorEnable(c);
|
||
|
||
/* 初始化状态变量 */
|
||
memset(&c->chassis_state, 0, sizeof(c->chassis_state));
|
||
memset(&c->yaw_control, 0, sizeof(c->yaw_control));
|
||
|
||
return CHASSIS_OK;
|
||
}
|
||
|
||
/**
|
||
* @brief 更新底盘反馈数据
|
||
* @param c 底盘结构体指针
|
||
* @return 操作结果
|
||
*/
|
||
int8_t Chassis_UpdateFeedback(Chassis_t *c) {
|
||
if (c == NULL) return CHASSIS_ERR_NULL;
|
||
|
||
/* 更新所有电机数据 */
|
||
MOTOR_LZ_UpdateAll();
|
||
MOTOR_LK_UpdateAll();
|
||
|
||
/* 更新关节电机反馈 */
|
||
for (int i = 0; i < 4; i++) {
|
||
MOTOR_LZ_t *joint_motor = MOTOR_LZ_GetMotor(&c->param->joint_param[i]);
|
||
if (joint_motor != NULL) {
|
||
c->feedback.joint[i] = joint_motor->motor.feedback;
|
||
}
|
||
|
||
/* 机械零点调整 */
|
||
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;
|
||
}
|
||
|
||
/* 更新轮子电机反馈 */
|
||
for (int i = 0; i < 2; i++) {
|
||
MOTOR_LK_t *wheel_motor = MOTOR_LK_GetMotor(&c->param->wheel_param[i]);
|
||
if (wheel_motor != NULL) {
|
||
c->feedback.wheel[i] = wheel_motor->motor.feedback;
|
||
}
|
||
}
|
||
|
||
/* 更新机体状态估计 */
|
||
Chassis_UpdateChassisState(c);
|
||
|
||
return CHASSIS_OK;
|
||
}
|
||
|
||
/**
|
||
* @brief 更新IMU数据
|
||
* @param c 底盘结构体指针
|
||
* @param imu IMU数据
|
||
* @return 操作结果
|
||
*/
|
||
int8_t Chassis_UpdateIMU(Chassis_t *c, const Chassis_IMU_t imu) {
|
||
if (c == NULL) return CHASSIS_ERR_NULL;
|
||
|
||
c->feedback.imu = imu;
|
||
return CHASSIS_OK;
|
||
}
|
||
|
||
/**
|
||
* @brief 底盘控制主函数
|
||
* @param c 底盘结构体指针
|
||
* @param c_cmd 控制指令
|
||
* @return 操作结果
|
||
*/
|
||
int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
|
||
if (c == NULL || c_cmd == NULL) return CHASSIS_ERR_NULL;
|
||
|
||
/* 计算时间间隔 */
|
||
c->dt = (BSP_TIME_Get_us() - c->lask_wakeup) / 1000000.0f;
|
||
c->lask_wakeup = BSP_TIME_Get_us();
|
||
|
||
/* 设置底盘模式 */
|
||
if (Chassis_SetMode(c, c_cmd->mode) != CHASSIS_OK) {
|
||
return CHASSIS_ERR_MODE;
|
||
}
|
||
|
||
/* VMC正解算 */
|
||
VMC_ForwardSolve(&c->vmc_[0], c->feedback.joint[0].rotor_abs_angle,
|
||
c->feedback.joint[1].rotor_abs_angle,
|
||
c->feedback.imu.euler.pit, c->feedback.imu.gyro.y);
|
||
VMC_ForwardSolve(&c->vmc_[1], c->feedback.joint[3].rotor_abs_angle,
|
||
c->feedback.joint[2].rotor_abs_angle,
|
||
c->feedback.imu.euler.pit, c->feedback.imu.gyro.y);
|
||
|
||
/* 根据模式执行控制 */
|
||
switch (c->mode) {
|
||
case CHASSIS_MODE_RELAX:
|
||
Chassis_MotorRelax(c);
|
||
Chassis_LQRControl(c, c_cmd);
|
||
break;
|
||
|
||
case CHASSIS_MODE_RECOVER: {
|
||
float fn = -20.0f, tp = 0.0f;
|
||
Chassis_LQRControl(c, c_cmd);
|
||
|
||
VMC_InverseSolve(&c->vmc_[0], fn, tp);
|
||
VMC_GetJointTorques(&c->vmc_[0], &c->output.joint[0], &c->output.joint[1]);
|
||
VMC_InverseSolve(&c->vmc_[1], fn, tp);
|
||
VMC_GetJointTorques(&c->vmc_[1], &c->output.joint[3], &c->output.joint[2]);
|
||
|
||
c->output.wheel[0] = c_cmd->move_vec.vx * 0.2f;
|
||
c->output.wheel[1] = c_cmd->move_vec.vx * 0.2f;
|
||
Chassis_Output(c);
|
||
} break;
|
||
|
||
case CHASSIS_MODE_CALIBRATE: {
|
||
|
||
Chassis_LQRControl(c, c_cmd);
|
||
|
||
VMC_InverseSolve(&c->vmc_[0], L_fn, L_tp);
|
||
VMC_GetJointTorques(&c->vmc_[0], &c->output.joint[0], &c->output.joint[1]);
|
||
VMC_InverseSolve(&c->vmc_[1], R_fn, R_tp);
|
||
VMC_GetJointTorques(&c->vmc_[1], &c->output.joint[3], &c->output.joint[2]);
|
||
|
||
c->output.wheel[0] = 0.0f;
|
||
c->output.wheel[1] = 0.0f;
|
||
Chassis_Output(c);
|
||
} break;
|
||
|
||
case CHASSIS_MODE_WHELL_LEG_BALANCE:
|
||
Chassis_LQRControl(c, c_cmd);
|
||
Chassis_Output(c);
|
||
break;
|
||
|
||
default:
|
||
return CHASSIS_ERR_MODE;
|
||
}
|
||
|
||
return CHASSIS_OK;
|
||
}
|
||
|
||
/**
|
||
* @brief 底盘输出
|
||
* @param c 底盘结构体指针
|
||
*/
|
||
void Chassis_Output(Chassis_t *c) {
|
||
if (c == NULL) return;
|
||
|
||
/* 关节输出滤波 */
|
||
// for (int i = 0; i < 4; i++) {
|
||
// c->output.joint[i] = LowPassFilter2p_Apply(&c->filter.joint_out[i], c->output.joint[i]);
|
||
// }
|
||
|
||
/* 发送关节控制指令 */
|
||
MOTOR_LZ_MotionParam_t motion_param = {
|
||
.torque = 0.0f,
|
||
.target_angle = 0.0f,
|
||
.target_velocity = 0.0f,
|
||
.kp = 0.0f,
|
||
.kd = 0.0f,
|
||
};
|
||
|
||
for (int i = 0; i < 4; i++) {
|
||
motion_param.torque = c->output.joint[i];
|
||
MOTOR_LZ_MotionControl(&c->param->joint_param[i], &motion_param);
|
||
}
|
||
|
||
/* 轮子输出滤波并发送 */
|
||
// for (int i = 0; i < 2; i++) {
|
||
// c->output.wheel[i] = LowPassFilter2p_Apply(&c->filter.wheel_out[i], c->output.wheel[i]);
|
||
// }
|
||
MOTOR_LK_SetOutput(&c->param->wheel_param[0], c->output.wheel[0]);
|
||
MOTOR_LK_SetOutput(&c->param->wheel_param[1], c->output.wheel[1]);
|
||
}
|
||
|
||
/**
|
||
* @brief LQR控制
|
||
* @param c 底盘结构体指针
|
||
* @param c_cmd 控制指令
|
||
* @return 操作结果
|
||
*/
|
||
int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
|
||
if (c == NULL || c_cmd == NULL) return CHASSIS_ERR_NULL;
|
||
|
||
/* ==================== 速度控制 ==================== */
|
||
float raw_vx = c_cmd->move_vec.vx * 2.0f;
|
||
float desired_velocity = c->yaw_control.is_reversed ? -raw_vx : raw_vx;
|
||
|
||
/* 加速度限制 */
|
||
float velocity_change = desired_velocity - c->chassis_state.last_target_velocity_x;
|
||
float max_velocity_change = MAX_ACCELERATION * c->dt;
|
||
velocity_change = LIMIT(velocity_change, -max_velocity_change, max_velocity_change);
|
||
|
||
float target_dot_x = c->chassis_state.last_target_velocity_x + velocity_change;
|
||
c->chassis_state.target_velocity_x = target_dot_x;
|
||
c->chassis_state.last_target_velocity_x = target_dot_x;
|
||
c->chassis_state.target_x += target_dot_x * c->dt;
|
||
|
||
/* ==================== 状态更新 ==================== */
|
||
LQR_State_t current_state = {
|
||
.theta = c->vmc_[0].leg.theta,
|
||
.d_theta = c->vmc_[0].leg.d_theta,
|
||
.x = c->chassis_state.position_x,
|
||
.d_x = c->chassis_state.velocity_x,
|
||
.phi = c->feedback.imu.euler.pit,
|
||
.d_phi = -c->feedback.imu.gyro.x,
|
||
};
|
||
|
||
LQR_UpdateState(&c->lqr[0], ¤t_state);
|
||
|
||
current_state.theta = c->vmc_[1].leg.theta;
|
||
current_state.d_theta = c->vmc_[1].leg.d_theta;
|
||
LQR_UpdateState(&c->lqr[1], ¤t_state);
|
||
|
||
/* 根据腿长更新增益矩阵 */
|
||
LQR_CalculateGainMatrix(&c->lqr[0], c->vmc_[0].leg.L0);
|
||
LQR_CalculateGainMatrix(&c->lqr[1], c->vmc_[1].leg.L0);
|
||
|
||
/* ==================== 目标状态 ==================== */
|
||
LQR_State_t target_state = {
|
||
.theta = 0.0f,
|
||
.d_theta = 0.0f,
|
||
.phi = -0.2f,
|
||
.d_phi = 0.0f,
|
||
.x = c->chassis_state.target_x,
|
||
.d_x = c->chassis_state.target_velocity_x,
|
||
};
|
||
|
||
/* ==================== Yaw轴控制 ==================== */
|
||
c->yaw_control.current_yaw = c->feedback.yaw.rotor_abs_angle;
|
||
|
||
float manual_offset = c_cmd->move_vec.vy * M_PI_2;
|
||
float base_target_1 = c->param->mech_zero_yaw + manual_offset;
|
||
float base_target_2 = c->param->mech_zero_yaw + M_PI + manual_offset;
|
||
|
||
float error_to_target1 = CircleError(base_target_1, c->yaw_control.current_yaw, M_2PI);
|
||
float error_to_target2 = CircleError(base_target_2, c->yaw_control.current_yaw, M_2PI);
|
||
|
||
if (fabsf(error_to_target1) <= fabsf(error_to_target2)) {
|
||
c->yaw_control.target_yaw = base_target_1;
|
||
c->yaw_control.is_reversed = false;
|
||
} else {
|
||
c->yaw_control.target_yaw = base_target_2;
|
||
c->yaw_control.is_reversed = true;
|
||
}
|
||
|
||
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);
|
||
|
||
/* ==================== 左腿LQR控制 ==================== */
|
||
if (c->vmc_[0].leg.is_ground_contact) {
|
||
LQR_SetTargetState(&c->lqr[0], &target_state);
|
||
LQR_Control(&c->lqr[0]);
|
||
} else {
|
||
target_state.theta = NON_CONTACT_THETA;
|
||
LQR_SetTargetState(&c->lqr[0], &target_state);
|
||
c->lqr[0].control_output.T = 0.0f;
|
||
c->lqr[0].control_output.Tp =
|
||
c->lqr[0].K_matrix[1][0] * (-c->vmc_[0].leg.theta + NON_CONTACT_THETA) +
|
||
c->lqr[0].K_matrix[1][1] * (-c->vmc_[0].leg.d_theta);
|
||
c->yaw_control.yaw_force = 0.0f;
|
||
}
|
||
|
||
/* ==================== 右腿LQR控制 ==================== */
|
||
if (c->vmc_[1].leg.is_ground_contact) {
|
||
LQR_SetTargetState(&c->lqr[1], &target_state);
|
||
LQR_Control(&c->lqr[1]);
|
||
} else {
|
||
target_state.theta = NON_CONTACT_THETA;
|
||
LQR_SetTargetState(&c->lqr[1], &target_state);
|
||
c->lqr[1].control_output.T = 0.0f;
|
||
c->lqr[1].control_output.Tp =
|
||
c->lqr[1].K_matrix[1][0] * (-c->vmc_[1].leg.theta + NON_CONTACT_THETA) +
|
||
c->lqr[1].K_matrix[1][1] * (-c->vmc_[1].leg.d_theta);
|
||
c->yaw_control.yaw_force = 0.0f;
|
||
}
|
||
|
||
/* ==================== 轮毂输出 ==================== */
|
||
c->output.wheel[0] = c->lqr[0].control_output.T / WHEEL_GEAR_RATIO + c->yaw_control.yaw_force;
|
||
c->output.wheel[1] = c->lqr[1].control_output.T / WHEEL_GEAR_RATIO - c->yaw_control.yaw_force;
|
||
|
||
/* ==================== 横滚角补偿 ==================== */
|
||
float roll_compensation = PID_Calc(&c->pid.roll, 0.0f, c->feedback.imu.euler.rol,
|
||
c->feedback.imu.gyro.x, c->dt);
|
||
|
||
/* 限制补偿范围 */
|
||
float max_compensation = (c->vmc_[0].leg.is_ground_contact && c->vmc_[1].leg.is_ground_contact)
|
||
? 0.05f : 0.02f;
|
||
roll_compensation = LIMIT(roll_compensation, -max_compensation, max_compensation);
|
||
|
||
/* ==================== 腿长控制 ==================== */
|
||
float target_L0[2];
|
||
target_L0[0] = BASE_LEG_LENGTH + c_cmd->height * LEG_LENGTH_RANGE + roll_compensation;
|
||
target_L0[1] = BASE_LEG_LENGTH + c_cmd->height * LEG_LENGTH_RANGE - roll_compensation;
|
||
|
||
/* 腿长限幅 */
|
||
target_L0[0] = LIMIT(target_L0[0], MIN_LEG_LENGTH, MAX_LEG_LENGTH);
|
||
target_L0[1] = LIMIT(target_L0[1], MIN_LEG_LENGTH, MAX_LEG_LENGTH);
|
||
|
||
/* 获取腿长变化率 */
|
||
float leg_d_length[2];
|
||
VMC_GetVirtualLegState(&c->vmc_[0], NULL, NULL, &leg_d_length[0], NULL);
|
||
VMC_GetVirtualLegState(&c->vmc_[1], NULL, NULL, &leg_d_length[1], NULL);
|
||
|
||
/* ==================== 左右腿摆角同步补偿 ==================== */
|
||
float Delta_Tp[2];
|
||
Delta_Tp[0] = c->vmc_[0].leg.L0 * 10.0f *
|
||
PID_Calc(&c->pid.tp[0], c->vmc_[1].leg.theta,
|
||
c->vmc_[0].leg.theta, c->vmc_[0].leg.d_theta, c->dt);
|
||
Delta_Tp[1] = c->vmc_[1].leg.L0 * 10.0f *
|
||
PID_Calc(&c->pid.tp[1], c->vmc_[0].leg.theta,
|
||
c->vmc_[1].leg.theta, c->vmc_[1].leg.d_theta, c->dt);
|
||
|
||
/* ==================== 左腿VMC控制 ==================== */
|
||
float pid_output_left = PID_Calc(&c->pid.leg_length[0], target_L0[0],
|
||
c->vmc_[0].leg.L0, leg_d_length[0], c->dt);
|
||
float virtual_force_left = c->lqr[0].control_output.Tp * sinf(c->vmc_[0].leg.theta) +
|
||
pid_output_left + LEFT_BASE_FORCE;
|
||
|
||
if (VMC_InverseSolve(&c->vmc_[0], virtual_force_left,
|
||
c->lqr[0].control_output.Tp + Delta_Tp[0]) == 0) {
|
||
VMC_GetJointTorques(&c->vmc_[0], &c->output.joint[0], &c->output.joint[1]);
|
||
} else {
|
||
c->output.joint[0] = 0.0f;
|
||
c->output.joint[1] = 0.0f;
|
||
}
|
||
|
||
/* ==================== 右腿VMC控制 ==================== */
|
||
float pid_output_right = PID_Calc(&c->pid.leg_length[1], target_L0[1],
|
||
c->vmc_[1].leg.L0, leg_d_length[1], c->dt);
|
||
float virtual_force_right = c->lqr[1].control_output.Tp * sinf(c->vmc_[1].leg.theta) +
|
||
pid_output_right + RIGHT_BASE_FORCE;
|
||
|
||
if (VMC_InverseSolve(&c->vmc_[1], virtual_force_right,
|
||
c->lqr[1].control_output.Tp + Delta_Tp[1]) == 0) {
|
||
VMC_GetJointTorques(&c->vmc_[1], &c->output.joint[3], &c->output.joint[2]);
|
||
} else {
|
||
c->output.joint[2] = 0.0f;
|
||
c->output.joint[3] = 0.0f;
|
||
}
|
||
|
||
/* ==================== 安全限制 ==================== */
|
||
for (int i = 0; i < 2; i++) {
|
||
c->output.wheel[i] = LIMIT(c->output.wheel[i], -1.0f, 1.0f);
|
||
}
|
||
for (int i = 0; i < 4; i++) {
|
||
c->output.joint[i] = LIMIT(c->output.joint[i], -15.0f, 15.0f);
|
||
}
|
||
|
||
return CHASSIS_OK;
|
||
}
|