285 lines
6.6 KiB
C
285 lines
6.6 KiB
C
/*
|
||
底盘模组
|
||
*/
|
||
|
||
#pragma anon_unions
|
||
|
||
#ifdef __cplusplus
|
||
extern "C" {
|
||
#endif
|
||
|
||
/* Includes ----------------------------------------------------------------- */
|
||
#include <cmsis_os2.h>
|
||
#include "bsp/can.h"
|
||
#include "component\filter.h"
|
||
#include "component\mixer.h"
|
||
#include "component\pid.h"
|
||
#include "component\ahrs.h"
|
||
#include "device/motor_rm.h"
|
||
/* Exported constants ------------------------------------------------------- */
|
||
#define CHASSIS_OK (0) /* 运行正常 */
|
||
#define CHASSIS_ERR (-1) /* 运行时出现了一些小错误 */
|
||
#define CHASSIS_ERR_NULL (-2) /* 运行时发现NULL指针 */
|
||
#define CHASSIS_ERR_MODE (-3) /* 运行时出配置了错误的ChassisMode_t */
|
||
#define CHASSIS_ERR_TYPE (-4) /* 运行时出配置了错误的Chassis_Type_t */
|
||
|
||
|
||
|
||
|
||
/* 底盘控制模式 */
|
||
typedef enum {
|
||
CHASSIS_MODE_RELAX, /* 放松模式,电机不输出。一般情况底盘初始化之后的模式 */
|
||
CHASSIS_MODE_BREAK, /* 刹车模式，电机闭环控制静止。用于机器人停止状态 */
|
||
CHASSIS_MODE_FOLLOW_GIMBAL, /* 通过闭环控制使车头方向跟随云台 */
|
||
CHASSIS_MODE_FOLLOW_GIMBAL_35, /* 通过闭环控制使车头方向35°跟随云台 */
|
||
CHASSIS_MODE_ROTOR, /* 小陀螺模式，通过闭环控制使底盘不停旋转 */
|
||
CHASSIS_MODE_INDEPENDENT, /*独立模式。底盘运行不受云台影响 */
|
||
CHASSIS_MODE_OPEN, /* 开环模式。底盘运行不受PID控制，直接输出到电机 */
|
||
} ChassisMode_t;
|
||
|
||
|
||
/* 小陀螺转动模式 */
|
||
typedef enum {
|
||
ROTOR_MODE_CW, /* 顺时针转动 */
|
||
ROTOR_MODE_CCW, /* 逆时针转动 */
|
||
ROTOR_MODE_RAND, /* 随机转动 */
|
||
} RotorMode_t;
|
||
|
||
/* 底盘控制命令 */
|
||
typedef struct {
|
||
ChassisMode_t mode; /* 底盘运行模式 */
|
||
RotorMode_t mode_rotor; /* 小陀螺转动模式 */
|
||
MoveVector_t ctrl_vec; /* 底盘控制向量 */
|
||
} ChassisCmd_t;
|
||
|
||
|
||
/* 底盘类型（底盘的物理设计） */
|
||
typedef enum {
|
||
CHASSIS_TYPE_MECANUM, /* 麦克纳姆轮 */
|
||
CHASSIS_TYPE_PARLFIX4, /* 平行摆放的四个驱动轮 */
|
||
CHASSIS_TYPE_PARLFIX2, /* 平行摆放的两个驱动轮 */
|
||
CHASSIS_TYPE_OMNI_CROSS, /* 叉型摆放的四个全向轮 */
|
||
CHASSIS_TYPE_OMNI_PLUS, /* 十字型摆设的四个全向轮 */
|
||
CHASSIS_TYPE_DRONE, /* 无人机底盘 */
|
||
CHASSIS_TYPE_SINGLE, /* 单个摩擦轮 */
|
||
} Chassis_Type_t;
|
||
|
||
/* 底盘参数结构体,ALL初始化参数,一般是const */
|
||
typedef struct {
|
||
Chassis_Type_t type; /* 底盘类型，底盘的机械设计和轮子选型 */
|
||
KPID_Params_t motor_pid_param; /* 轮子控制PID */
|
||
KPID_Params_t follow_pid_param; /* 跟随云台PID */
|
||
|
||
/* 低通滤波器截至频率*/
|
||
struct {
|
||
float in; /* 输入 */
|
||
float out; /* 输出 */
|
||
} low_pass_cutoff_freq;
|
||
|
||
/* 电机反装，应该和云台设置相同*/
|
||
struct {
|
||
bool yaw;
|
||
} reverse;
|
||
|
||
} Chassis_Params_t;
|
||
|
||
/*
|
||
* 运行的主结构体￣
|
||
* 包含初始化参数,中间变量,输出变量
|
||
*/
|
||
typedef struct {
|
||
uint32_t last_wakeup;
|
||
float dt;
|
||
|
||
const Chassis_Params_t *param; /* 底盘参数,用Chassis_Init设定 */
|
||
AHRS_Eulr_t *mech_zero;
|
||
|
||
/* 模块通用 */
|
||
ChassisMode_t mode; /* 底盘模式 */
|
||
|
||
/* 底盘设计 */
|
||
int8_t num_wheel; /* 底盘轮子数量 */
|
||
Mixer_t mixer; /* 混合器,移动向量->电机目标值 */
|
||
MOTOR_RM_Param_t *motors_param[4]; /*指向底盘每个电机参数*/
|
||
MoveVector_t move_vec; /* 底盘实际的运动向量 */
|
||
|
||
/* 反馈 */
|
||
struct {
|
||
float gimbal_yaw_encoder; /* 云台Yaw编码器角度 */
|
||
float *motor_rpm; /* 电机转速的动态数组,单位:RPM */
|
||
float *motor_current; /* 转矩电流 单位:A */
|
||
} feedback;
|
||
|
||
float wz_multi; /* 小陀螺旋转模式 */
|
||
|
||
/* PID计算目标值 */
|
||
struct {
|
||
float *motor_rpm; /* 电机转速的动态数组,单位:RPM */
|
||
} setpoint;
|
||
|
||
/* 反馈控制用的PID */
|
||
struct {
|
||
KPID_t *motor; /* 控制轮子电机用的PID的动态数组 */
|
||
KPID_t follow; /* 跟随云台用的PID */
|
||
} pid;
|
||
|
||
/* 滤波器 */
|
||
struct {
|
||
LowPassFilter2p_t *in; /* 反馈值滤波器 */
|
||
LowPassFilter2p_t *out; /* 输出值滤波器 */
|
||
} filter;
|
||
|
||
float *out; /* 电机最终的输出值的动态数组 */
|
||
} Chassis_t;
|
||
|
||
|
||
|
||
//typedef struct {
|
||
// uint32_t recive_flag; /* 接收标志位 */
|
||
|
||
// /* 使用motor_rm 电机管理 */
|
||
// MOTOR_RM_t *motors[4]; /* 底盘四个3508电机实例指针 */
|
||
// MOTOR_RM_CANManager_t *can_mgr; /* 管理底盘电机CAN总线 */
|
||
|
||
// const Chassis_Params_t *param; /* 底盘参数 */
|
||
// struct {
|
||
// uint32_t chassis;
|
||
// uint32_t gimbal;
|
||
// uint32_t shoot;
|
||
// } mailbox;
|
||
|
||
// osMessageQueueId_t msgq_raw; /* 接收原始消息队列 */
|
||
//} Chassis_CAN_t;
|
||
|
||
|
||
|
||
|
||
typedef struct {
|
||
uint32_t receive_flag;
|
||
|
||
struct {
|
||
union {
|
||
struct {
|
||
MOTOR_RM_t fl;
|
||
MOTOR_RM_t fr;
|
||
MOTOR_RM_t bl;
|
||
MOTOR_RM_t br;
|
||
} named;
|
||
MOTOR_RM_t as_array[4];
|
||
};
|
||
} chassis;
|
||
|
||
struct {
|
||
union {
|
||
struct {
|
||
MOTOR_RM_t yaw;
|
||
MOTOR_RM_t pitch;
|
||
} named;
|
||
MOTOR_RM_t as_array[2];
|
||
};
|
||
} gimbal;
|
||
|
||
struct {
|
||
union {
|
||
struct {
|
||
MOTOR_RM_t trigger;
|
||
MOTOR_RM_t friction_l;
|
||
MOTOR_RM_t friction_r;
|
||
} named;
|
||
MOTOR_RM_t as_array[3];
|
||
};
|
||
} shoot;
|
||
|
||
const MOTOR_RM_Param_t *param;
|
||
struct {
|
||
uint32_t chassis;
|
||
uint32_t gimbal;
|
||
uint32_t shoot;
|
||
} mailbox;
|
||
|
||
osMessageQueueId_t msgq_raw;
|
||
} Chassis_CAN_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,
|
||
AHRS_Eulr_t *mech_zero, float target_freq);
|
||
|
||
/**
|
||
* \brief 更新底盘反馈信息
|
||
*
|
||
* \param c 包含底盘数据的结构体
|
||
* \param can CAN设备结构体
|
||
*
|
||
* \return 运行结果
|
||
*/
|
||
int8_t Chassis_UpdateFeedback(Chassis_t *c, const Chassis_CAN_t *can);
|
||
|
||
/**
|
||
* \brief 运行底盘控制逻辑
|
||
*
|
||
* \param c 包含底盘数据的结构体
|
||
* \param c_cmd 底盘控制指令
|
||
* \param dt_sec 两次调用的时间间隔
|
||
*
|
||
* \return 运行结果
|
||
*/
|
||
int8_t Chassis_Control(Chassis_t *c, const ChassisCmd_t *c_cmd,
|
||
uint32_t now);
|
||
|
||
|
||
/**
|
||
* \brief 复制底盘输出值
|
||
*
|
||
* \param s 包含底盘数据的结构体
|
||
* \param out CAN设备底盘输出结构体
|
||
*/
|
||
void Chassis_DumpOutput(Chassis_t *c, Chassis_CAN_t *can);
|
||
|
||
/**
|
||
* \brief 清空Chassis输出数据
|
||
*
|
||
* \param out CAN设备底盘输出结构体
|
||
*/
|
||
void Chassis_ResetOutput(Chassis_t *c);
|
||
|
||
|
||
/**
|
||
* @brief 底盘功率限制
|
||
*
|
||
* @param c 底盘数据
|
||
* @param cap 电容数据
|
||
* @param ref 裁判系统数据
|
||
* @return 函数运行结果
|
||
*/
|
||
//还没有加入，waiting。。。。。。int8_t Chassis_PowerLimit(Chassis_t *c, const CAN_Capacitor_t *cap,
|
||
// const Referee_ForChassis_t *ref);
|
||
|
||
|
||
/**
|
||
* @brief 导出底盘数据
|
||
*
|
||
* @param chassis 底盘数据结构体
|
||
* @param ui UI数据结构体
|
||
*/
|
||
//void Chassis_DumpUI(const Chassis_t *c, Referee_ChassisUI_t *ui);
|
||
|
||
#ifdef __cplusplus
|
||
}
|
||
#endif
|
||
|
||
|
||
|
||
|