210 lines
6.3 KiB
C
210 lines
6.3 KiB
C
/*
|
||
* 底盘模组
|
||
*/
|
||
|
||
#pragma once
|
||
|
||
#ifdef __cplusplus
|
||
extern "C" {
|
||
#endif
|
||
|
||
/* Includes ----------------------------------------------------------------- */
|
||
#include "component/cmd.h"
|
||
#include "component/filter.h"
|
||
#include "component/kinematics.h"
|
||
#include "component/pid.h"
|
||
#include "device/go.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_TYPE_QUADRUPED, /* 四足机器人 */
|
||
} Chassis_Type_t;
|
||
|
||
typedef enum {
|
||
IMU_BOARD_BMI088,
|
||
IMU_WHEELREC_N100,
|
||
IMU_NUM,
|
||
IMU_ERROR,
|
||
} Chassis_ImuType_t;
|
||
|
||
// typedef union {
|
||
// float id[GO_MOTOR_NUM]; /* 零点角度，单位：弧度 */
|
||
// struct {
|
||
// float lf_hip; /* 左前腿髋关节零点 */
|
||
// float lf_thigh; /* 左前腿大腿零点 */
|
||
// float lf_calf; /* 左前腿小腿零点 */
|
||
// float rf_hip; /* 右前腿髋关节零点 */
|
||
// float rf_thigh; /* 右前腿大腿零点 */
|
||
// float rf_calf; /* 右前腿小腿零点 */
|
||
// float lr_hip; /* 左后腿髋关节零点 */
|
||
// float lr_thigh; /* 左后腿大腿零点 */
|
||
// float lr_calf; /* 左后腿小腿零点 */
|
||
// float rr_hip; /* 右后腿髋关节零点 */
|
||
// float rr_thigh; /* 右后腿大腿零点 */
|
||
// float rr_calf; /* 右后腿小腿零点 */
|
||
// } named;
|
||
// } Chassis_MotorZeroPoint_t;
|
||
|
||
typedef union {
|
||
float id[GO_MOTOR_NUM];
|
||
struct{
|
||
float lf_hip; /* 左前腿髋关节长度，单位：米 */
|
||
float lf_thigh; /* 左前腿大腿长度，单位：米 */
|
||
float lf_calf; /* 左前腿小腿长度，单位：米 */
|
||
float rf_hip; /* 右前腿髋关节长度，单位：米 */
|
||
float rf_thigh; /* 右前腿大腿长度，单位：米 */
|
||
float rf_calf; /* 右前腿小腿长度，单位：米 */
|
||
float lr_hip; /* 左后腿髋关节长度，单位：米 */
|
||
float lr_thigh; /* 左后腿大腿长度，单位：米 */
|
||
float lr_calf; /* 左后腿小腿长度，单位：米 */
|
||
float rr_hip; /* 右后腿髋关节长度，单位：米 */
|
||
float rr_thigh; /* 右后腿大腿长度，单位：米 */
|
||
float rr_calf; /* 右后腿小腿长度，单位：米 */
|
||
} named;
|
||
} joint_params;
|
||
|
||
typedef struct {
|
||
joint_params min;
|
||
joint_params max; /* 关节的最小和最大角度，单位：弧度 */
|
||
} joint_limits;
|
||
|
||
typedef union {
|
||
Kinematics_Sign_t leg[GO_MOTOR_NUM/3];
|
||
struct {
|
||
Kinematics_Sign_t lf; /* 左前腿 */
|
||
Kinematics_Sign_t rf; /* 右前腿 */
|
||
Kinematics_Sign_t lr; /* 左后腿 */
|
||
Kinematics_Sign_t rr; /* 右后腿 */
|
||
} named;
|
||
} Kinematics_DirectionSign_t;
|
||
|
||
|
||
typedef struct{
|
||
joint_params zero_point; /* 零点角度，单位：弧度 */
|
||
|
||
// joint_params length; /* 关节长度，单位：米 */
|
||
|
||
joint_params ratio; /* 关节减速比 */
|
||
joint_limits limit; /* 关节的最小和最大角度，单位：弧度 */
|
||
Kinematics_LinkLength_t length; /* 关节长度，单位：米 */
|
||
Kinematics_LegOffset_t offset; /* 关节偏移，单位：米 */
|
||
Kinematics_DirectionSign_t sign; /* 关节侧向标志（左前/左后为-1，右前/右后为1） */
|
||
|
||
}Chassis_Mech_Params_t;
|
||
|
||
/* 底盘参数的结构体，包含所有初始化用的参数，通常是const，存好几组 */
|
||
typedef struct {
|
||
Chassis_Type_t type; /* 底盘类型，底盘的机械设计和轮子选型 */
|
||
|
||
Chassis_Mech_Params_t mech_param; /* 机械参数 */
|
||
|
||
KPID_Params_t torque_pid_param; /* 力矩矩控制PID的参数 */
|
||
|
||
/* 低通滤波器截止频率 */
|
||
struct {
|
||
float in; /* 输入 */
|
||
float out; /* 输出 */
|
||
} low_pass_cutoff_freq;
|
||
|
||
} Chassis_Params_t;
|
||
|
||
/*
|
||
* 运行的主结构体，所有这个文件里的函数都在操作这个结构体
|
||
* 包含了初始化参数，中间变量，输出变量
|
||
*/
|
||
typedef struct {
|
||
uint32_t lask_wakeup;
|
||
float dt;
|
||
|
||
const Chassis_Params_t *param; /* 底盘的参数，用Chassis_Init设定 */
|
||
|
||
GO_ChassisFeedback_t feedback; /* 底盘反馈信息 */
|
||
GO_ChassisCMD_t output;
|
||
float height; /* 底盘高度，单位：米 */
|
||
/* 模块通用 */
|
||
CMD_ChassisMode_t mode; /* 底盘模式 */
|
||
|
||
/* 底盘设计 */
|
||
int8_t num_joint; /* 关节数量 */
|
||
|
||
MoveVector_t move_vec; /* 底盘实际的运动向量 */
|
||
|
||
/* 反馈控制用的PID */
|
||
struct {
|
||
KPID_t *motor; /* 控制轮子电机用的PID的动态数组 */
|
||
KPID_t follow; /* 跟随云台用的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, const Chassis_Params_t *param,float target_freq);
|
||
|
||
/**
|
||
* \brief 更新底盘的反馈信息
|
||
*
|
||
* \param c 包含底盘数据的结构体
|
||
* \param
|
||
*
|
||
* \return 函数运行结果
|
||
*/
|
||
int8_t Chassis_UpdateFeedback(Chassis_t *c, const GO_ChassisFeedback_t *go);
|
||
|
||
/**
|
||
* \brief 运行底盘控制逻辑
|
||
*
|
||
* \param c 包含底盘数据的结构体
|
||
* \param c_cmd 底盘控制指令
|
||
* \param dt_sec 两次调用的时间间隔
|
||
*
|
||
* \return 函数运行结果
|
||
*/
|
||
int8_t Chassis_Control(Chassis_t *c, const CMD_ChassisCmd_t *c_cmd,
|
||
uint32_t now);
|
||
|
||
/**
|
||
* \brief 底盘实际输出值
|
||
*
|
||
* \param s 包含底盘数据的结构体
|
||
* \param out CAN设备底盘输出结构体
|
||
*/
|
||
void Chassis_DumpOutput(const Chassis_t *c, GO_ChassisCMD_t *out);
|
||
|
||
/**
|
||
* \brief 清空Chassis输出数据
|
||
*
|
||
* \param out CAN设备底盘输出结构体
|
||
*/
|
||
// void Chassis_ResetOutput(CAN_ChassisOutput_t *out);
|
||
|
||
#ifdef __cplusplus
|
||
}
|
||
#endif
|