225 lines
6.2 KiB
C
225 lines
6.2 KiB
C
/*
|
||
* 平衡底盘模组
|
||
*/
|
||
|
||
#pragma once
|
||
|
||
#ifdef __cplusplus
|
||
extern "C" {
|
||
#endif
|
||
|
||
// Front
|
||
// | 1 4 |
|
||
// (1)Left| |Right(2)
|
||
// | 2 3 |
|
||
|
||
/* Includes ----------------------------------------------------------------- */
|
||
#include <stdint.h>
|
||
#include <stdbool.h>
|
||
#include "component/vmc.h"
|
||
#include "component/lqr.h"
|
||
#include "component/ahrs.h"
|
||
#include "component/filter.h"
|
||
#include "component/pid.h"
|
||
#include "device/motor.h"
|
||
#include "device/motor_lk.h"
|
||
#include "device/motor_lz.h"
|
||
/* Exported constants ------------------------------------------------------- */
|
||
#define CHASSIS_OK (0) /* 运行正常 */
|
||
#define CHASSIS_ERR (-1) /* 运行时发现了其他错误 */
|
||
#define CHASSIS_ERR_NULL (-2) /* 运行时发现NULL指针 */
|
||
#define CHASSIS_ERR_MODE (-3) /* 运行时配置了错误的CMD_ChassisMode_t */
|
||
#define CHASSIS_ERR_TYPE (-4) /* 运行时配置了错误的Chassis_Type_t */
|
||
|
||
/* Exported macro ----------------------------------------------------------- */
|
||
/* Exported types ----------------------------------------------------------- */
|
||
|
||
typedef enum {
|
||
CHASSIS_MODE_RELAX, /* 放松模式，电机不输出。一般情况底盘初始化之后的模式 */
|
||
CHASSIS_MODE_RECOVER, /* 复位模式 */
|
||
CHASSIS_MODE_WHELL_BALANCE, /* 平衡模式，底盘自我平衡 */
|
||
CHASSIS_MODE_WHELL_LEG_BALANCE, /* 轮子+腿平衡模式，底盘自我平衡 */
|
||
} Chassis_Mode_t;
|
||
|
||
typedef struct {
|
||
Chassis_Mode_t mode; /* 底盘模式 */
|
||
MoveVector_t move_vec; /* 底盘运动向量 */
|
||
float height; /* 目标高度 */
|
||
} Chassis_CMD_t;
|
||
|
||
typedef struct {
|
||
AHRS_Accl_t accl; /* IMU加速度计 */
|
||
AHRS_Gyro_t gyro; /* IMU陀螺仪 */
|
||
AHRS_Eulr_t euler; /* IMU欧拉角 */
|
||
}Chassis_IMU_t;
|
||
|
||
typedef struct {
|
||
MOTOR_Feedback_t joint[4]; /* 四个关节电机反馈 */
|
||
MOTOR_Feedback_t wheel[2]; /* 两个轮子电机反馈 */
|
||
Chassis_IMU_t imu; /* IMU数据 */
|
||
MOTOR_Feedback_t yaw; /* 云台Yaw轴电机反馈 */
|
||
}Chassis_Feedback_t;
|
||
|
||
typedef struct {
|
||
MOTOR_LZ_MotionParam_t joint[4]; /* 四个关节电机输出 */
|
||
float wheel[2]; /* 两个轮子电机输出 */
|
||
}Chassis_Output_t;
|
||
|
||
|
||
/* 底盘参数的结构体，包含所有初始化用的参数，通常是const，存好几组 */
|
||
typedef struct {
|
||
|
||
MOTOR_LZ_Param_t joint_motors[4]; /* 四个关节电机参数 */
|
||
MOTOR_LK_Param_t wheel_motors[2]; /* 两个轮子电机参数 */
|
||
|
||
VMC_Param_t vmc_param[2]; /* VMC参数 */
|
||
LQR_GainMatrix_t lqr_gains; /* LQR增益矩阵参数 */
|
||
|
||
/* LQR控制器参数 */
|
||
struct {
|
||
float max_wheel_torque; /* 轮毂电机最大力矩限制 */
|
||
float max_joint_torque; /* 关节电机最大力矩限制 */
|
||
} lqr_param;
|
||
|
||
KPID_Params_t yaw; /* yaw轴PID控制参数，用于控制底盘朝向 */
|
||
KPID_Params_t roll; /* roll轴PID控制参数，用于横滚角补偿 */
|
||
KPID_Params_t tp; /* 摆力矩PID控制参数，用于控制摆力矩 */
|
||
KPID_Params_t leg_length; /* 腿长PID控制参数，用于控制虚拟腿长度 */
|
||
KPID_Params_t leg_theta; /* 摆角PID控制参数，用于控制虚拟腿摆角 */
|
||
|
||
float mech_zero_yaw; /* 机械零点 */
|
||
|
||
/* 低通滤波器截止频率 */
|
||
struct {
|
||
float in; /* 输入 */
|
||
float out; /* 输出 */
|
||
} low_pass_cutoff_freq;
|
||
} Chassis_Params_t;
|
||
|
||
/*
|
||
* 运行的主结构体，所有这个文件里的函数都在操作这个结构体
|
||
* 包含了初始化参数，中间变量，输出变量
|
||
*/
|
||
typedef struct {
|
||
uint64_t lask_wakeup;
|
||
float dt;
|
||
|
||
Chassis_Params_t *param; /* 底盘的参数，用Chassis_Init设定 */
|
||
AHRS_Eulr_t *mech_zero;
|
||
|
||
/* 模块通用 */
|
||
Chassis_Mode_t mode; /* 底盘模式 */
|
||
|
||
/* 反馈信息 */
|
||
Chassis_Feedback_t feedback;
|
||
/* 控制信息*/
|
||
Chassis_Output_t output;
|
||
|
||
VMC_t vmc_[2]; /* 两条腿的VMC */
|
||
LQR_Controller_t lqr[2]; /* 两条腿的LQR控制器 */
|
||
|
||
int8_t state;
|
||
|
||
float angle;
|
||
float height;
|
||
|
||
/* 机体状态估计 */
|
||
struct {
|
||
float position_x; /* 机体x位置 */
|
||
float velocity_x; /* 机体x速度 */
|
||
float last_velocity_x; /* 上一次速度用于数值微分 */
|
||
} chassis_state;
|
||
|
||
/* yaw控制相关 */
|
||
struct {
|
||
float target_yaw; /* 目标yaw角度 */
|
||
float current_yaw; /* 当前yaw角度 */
|
||
float yaw_force; /* yaw轴控制力矩 */
|
||
} yaw_control;
|
||
|
||
float wz_multi; /* 小陀螺模式旋转方向 */
|
||
|
||
/* PID计算的目标值 */
|
||
struct {
|
||
AHRS_Eulr_t chassis;
|
||
} setpoint;
|
||
|
||
/* 反馈控制用的PID */
|
||
struct {
|
||
KPID_t yaw; /* 跟随云台用的PID */
|
||
KPID_t roll; /* 横滚角控制PID */
|
||
KPID_t tp[2];
|
||
KPID_t leg_length[2]; /* 两条腿的腿长PID */
|
||
KPID_t leg_theta[2]; /* 两条腿的摆角PID */
|
||
} pid;
|
||
|
||
/* 滤波器 */
|
||
struct {
|
||
LowPassFilter2p_t *in; /* 反馈值滤波器 */
|
||
LowPassFilter2p_t *out; /* 输出值滤波器 */
|
||
} filter;
|
||
|
||
} Chassis_t;
|
||
|
||
/* Exported functions prototypes -------------------------------------------- */
|
||
|
||
/**
|
||
* \brief 初始化底盘
|
||
*
|
||
* \param c 包含底盘数据的结构体
|
||
* \param param 包含底盘参数的结构体指针
|
||
* \param target_freq 任务预期的运行频率
|
||
*
|
||
* \return 函数运行结果
|
||
*/
|
||
int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq);
|
||
|
||
/**
|
||
* \brief 更新底盘的反馈信息
|
||
*
|
||
* \param c 包含底盘数据的结构体
|
||
* \return 函数运行结果
|
||
*/
|
||
int8_t Chassis_UpdateFeedback(Chassis_t *c);
|
||
|
||
|
||
/**
|
||
* @brief 更新IMU数据
|
||
* @param c 包含底盘数据的结构体
|
||
* @param imu IMU数据
|
||
* @return
|
||
*/
|
||
int8_t Chassis_UpdateIMU(Chassis_t *c, const Chassis_IMU_t imu);
|
||
/**
|
||
* \brief 运行底盘控制逻辑
|
||
*
|
||
* \param c 包含底盘数据的结构体
|
||
* \param c_cmd 底盘控制指令
|
||
*
|
||
* \return 函数运行结果
|
||
*/
|
||
int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd);
|
||
|
||
/**
|
||
* \brief LQR控制逻辑
|
||
*
|
||
* \param c 包含底盘数据的结构体
|
||
* \param c_cmd 底盘控制指令
|
||
*
|
||
* \return 函数运行结果
|
||
*/
|
||
int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd);
|
||
|
||
|
||
/**
|
||
* \brief 底盘输出值
|
||
*
|
||
* \param s 包含底盘数据的结构体
|
||
* \param out CAN设备底盘输出结构体
|
||
*/
|
||
void Chassis_Output(Chassis_t *c);
|
||
|
||
#ifdef __cplusplus
|
||
}
|
||
#endif
|