146 lines
3.7 KiB
C
146 lines
3.7 KiB
C
#pragma once
|
||
|
||
#ifdef __cplusplus
|
||
extern "C" {
|
||
#endif
|
||
|
||
#include "bsp/struct_typedef.h"
|
||
#include "component/ahrs.h"
|
||
#include "component/filter.h"
|
||
#include "component/mixer.h"
|
||
#include "component/pid.h"
|
||
#include "component/user_math.h"
|
||
#include "device/bmi088.h"
|
||
#include "device/motor_rm.h"
|
||
#include "module/cmd.h"
|
||
|
||
#define CHASSIS_OK (0)
|
||
#define CHASSIS_ERR (-1)
|
||
#define CHASSIS_ERR_NULL (-2)
|
||
#define CHASSIS_ERR_MODE (-3) /* 底盘模式错误：CMD_ChassisMode_t */
|
||
#define CHASSIS_ERR_TYPE (-4) /* 底盘类型错误：Chassis_Type_t */
|
||
|
||
|
||
typedef enum {
|
||
CHASSIS_MODE_RELAX, /* 放松模式，电机不输出，一般用于底盘初始化后 */
|
||
CHASSIS_MODE_MOVE
|
||
} Chassis_Mode_t;
|
||
|
||
/* 限幅 */
|
||
typedef struct {
|
||
float max;
|
||
float min;
|
||
} Chassis_Limit_t;
|
||
|
||
/* 底盘类型（底盘的物理设计） */
|
||
typedef enum {
|
||
CHASSIS_TYPE_MECANUM, /* 麦克纳姆轮 */
|
||
CHASSIS_TYPE_PARLFIX4, /* 平行摆放的四个驱动轮 */
|
||
CHASSIS_TYPE_PARLFIX2, /* 平行摆放的两个驱动轮 */
|
||
CHASSIS_TYPE_OMNI_CROSS, /* X 型摆放的四个全向轮 */
|
||
CHASSIS_TYPE_OMNI_PLUS, /* 十字型摆放的四个全向轮 */
|
||
CHASSIS_TYPE_DRONE, /* 无人机底盘 */
|
||
CHASSIS_TYPE_SINGLE, /* 单个麦克纳姆轮 */
|
||
} Chassis_Type_t;
|
||
|
||
|
||
/* 底盘参数结构体，初始化所需参数 */
|
||
typedef struct {
|
||
MOTOR_RM_Param_t motor_param[4];
|
||
struct {
|
||
KPID_Params_t motor_pid_param; /* 底盘电机 PID 参数 */
|
||
KPID_Params_t follow_pid_param; /* 跟随云台 PID 参数 */
|
||
} pid;
|
||
Chassis_Type_t type; /* 底盘类型，决定机械设计和轮组配置 */
|
||
|
||
/* 低通滤波器截止频率 */
|
||
struct {
|
||
float in; /* 输入 */
|
||
float out; /* 输出 */
|
||
} low_pass_cutoff_freq;
|
||
|
||
/* 电机反转设置，应与云台方向配置一致 */
|
||
struct {
|
||
bool yaw;
|
||
} reverse;
|
||
struct {
|
||
float max_vx, max_vy, max_wz;
|
||
float max_current;
|
||
} limit;
|
||
} Chassis_Params_t;
|
||
|
||
typedef struct {
|
||
AHRS_Gyro_t gyro;
|
||
AHRS_Eulr_t eulr;
|
||
} Chassis_IMU_t;
|
||
|
||
typedef struct {
|
||
MOTOR_Feedback_t motor[4]; // 四个 3508 电机反馈
|
||
float encoder_gimbalYawMotor;
|
||
} Chassis_Feedback_t;
|
||
|
||
/* 底盘输出结构体 */
|
||
typedef struct {
|
||
float motor[4];
|
||
} Chassis_Output_t;
|
||
|
||
/*
|
||
* 运行时主结构体
|
||
* 包含初始化参数、中间变量和输出变量
|
||
*/
|
||
typedef struct {
|
||
uint64_t last_wakeup;
|
||
float dt;
|
||
|
||
Chassis_Params_t *param; /* 底盘参数，在 Chassis_Init 中设置 */
|
||
|
||
/* 模块通用 */
|
||
Chassis_mode_t mode; /* 底盘模式 */
|
||
|
||
|
||
/* 底盘设计参数 */
|
||
int8_t num_wheel; /* 底盘轮子数量 */
|
||
Mixer_t mixer; /* 混控器：移动向量 -> 电机目标值 */
|
||
MoveVector_t move_vec; /* 底盘实际运动向量 */
|
||
MOTOR_RM_t *motors[4];/* 指向底盘各个电机参数 */
|
||
float mech_zero;
|
||
float wz_multi; /* 小陀螺旋转模式倍率 */
|
||
|
||
/* PID 计算目标值 */
|
||
struct {
|
||
float motor_rpm[4]; /* 电机转速动态数组，单位：RPM */
|
||
} setpoint;
|
||
|
||
/* 反馈控制使用的 PID */
|
||
struct {
|
||
KPID_t motor[4]; /* 控制轮组电机的 PID 动态数组 */
|
||
KPID_t follow; /* 跟随云台使用的 PID */
|
||
} pid;
|
||
|
||
struct {
|
||
Chassis_Limit_t vx, vy, wz;
|
||
} limit;
|
||
|
||
/* 滤波器 */
|
||
struct {
|
||
LowPassFilter2p_t in[4]; /* 反馈值滤波器 */
|
||
LowPassFilter2p_t out[4]; /* 输出值滤波器 */
|
||
} filter;
|
||
|
||
Chassis_Output_t out; /* 电机输出 */
|
||
Chassis_Feedback_t feedback;
|
||
//float out_motor[4];
|
||
} Chassis_t;
|
||
|
||
int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param,
|
||
float target_freq);
|
||
|
||
int8_t Chassis_UpdateFeedback(Chassis_t *c);
|
||
int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd, uint32_t now);
|
||
|
||
void Chassis_Output(Chassis_t *c);
|
||
void Chassis_ResetOutput(Chassis_t *c);
|
||
#ifdef __cplusplus
|
||
}
|
||
#endif
|