178 lines
4.8 KiB
C
178 lines
4.8 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/cmd.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 {
|
||
KPID_Params_t motor_pid_param; /* 轮子控制PID的参数 */
|
||
KPID_Params_t follow_pid_param; /* 跟随云台PID的参数 */
|
||
KPID_Params_t balance_pid_param; /* 平衡PID的参数 */
|
||
|
||
MOTOR_LZ_Param_t joint_motors[4]; /* 四个关节电机参数 */
|
||
MOTOR_LK_Param_t wheel_motors[2]; /* 两个轮子电机参数 */
|
||
|
||
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;
|
||
|
||
int8_t state;
|
||
|
||
float angle;
|
||
float height;
|
||
|
||
float wz_multi; /* 小陀螺模式旋转方向 */
|
||
|
||
/* PID计算的目标值 */
|
||
struct {
|
||
AHRS_Eulr_t chassis;
|
||
} setpoint;
|
||
|
||
/* 反馈控制用的PID */
|
||
struct {
|
||
KPID_t left_wheel; /* 左轮PID */
|
||
KPID_t right_wheel; /* 右轮PID */
|
||
KPID_t follow; /* 跟随云台用的PID */
|
||
KPID_t balance; /* 平衡用的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);
|
||
|
||
|
||
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 底盘输出值
|
||
*
|
||
* \param s 包含底盘数据的结构体
|
||
* \param out CAN设备底盘输出结构体
|
||
*/
|
||
void Chassis_Output(Chassis_t *c);
|
||
|
||
#ifdef __cplusplus
|
||
}
|
||
#endif
|