239 lines
6.2 KiB
C
239 lines
6.2 KiB
C
/*
|
||
底盘模组
|
||
*/
|
||
#pragma once
|
||
|
||
|
||
#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 */
|
||
|
||
#define MAX_MOTOR_CURRENT 20.0f
|
||
/* 底盘控制模式 */
|
||
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控制，直接输出到电机 */
|
||
} Chassis_Mode_t;
|
||
|
||
/* 小陀螺转动模式 */
|
||
typedef enum {
|
||
ROTOR_MODE_CW, /* 顺时针转动 */
|
||
ROTOR_MODE_CCW, /* 逆时针转动 */
|
||
ROTOR_MODE_RAND, /* 随机转动 */
|
||
} Chassis_RotorMode_t;
|
||
|
||
/* 底盘控制命令 */
|
||
typedef struct {
|
||
Chassis_Mode_t mode; /* 底盘运行模式 */
|
||
Chassis_RotorMode_t mode_rotor; /* 小陀螺转动模式 */
|
||
MoveVector_t ctrl_vec; /* 底盘控制向量 */
|
||
} Chassis_CMD_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, /* 叉型摆放的四个全向轮 */
|
||
CHASSIS_TYPE_OMNI_PLUS, /* 十字型摆设的四个全向轮 */
|
||
CHASSIS_TYPE_DRONE, /* 无人机底盘 */
|
||
CHASSIS_TYPE_SINGLE, /* 单个摩擦轮 */
|
||
} Chassis_Type_t;
|
||
|
||
|
||
/* 底盘参数结构体,ALL初始化参数 */
|
||
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;
|
||
|
||
/* 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 can CAN设备结构体
|
||
*
|
||
* \return 运行结果
|
||
*/
|
||
int8_t Chassis_UpdateFeedback(Chassis_t *c);
|
||
|
||
/**
|
||
* \brief 运行底盘控制逻辑
|
||
*
|
||
* \param c 包含底盘数据的结构体
|
||
* \param c_cmd 底盘控制指令
|
||
* \param dt_sec 两次调用的时间间隔
|
||
*
|
||
* \return 运行结果
|
||
*/
|
||
int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd,
|
||
uint32_t now);
|
||
|
||
|
||
/**
|
||
* \brief 复制底盘输出值
|
||
*
|
||
* \param s 包含底盘数据的结构体
|
||
* \param out CAN设备底盘输出结构体
|
||
*/
|
||
void Chassis_Output(Chassis_t *c);
|
||
|
||
/**
|
||
* \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
|
||
|
||
|
||
|
||
|