233 lines
4.7 KiB
C
233 lines
4.7 KiB
C
|
#ifndef CHASSIS_H
|
|||
|
|
|
|||
|
|
/**
|
|||
|
|
* @brief 底盘控制类头文件
|
|||
|
|
*
|
|||
|
|
* 本头文件定义了底盘控制类的接口和数据结构，用于控制车辆的底盘。
|
|||
|
|
*
|
|||
|
|
* @param ChassisImu_t 底盘的 IMU 数据结构体
|
|||
|
|
* @param ops_t 操作数据结构体
|
|||
|
|
* @param Chassis_t 底盘数据结构体
|
|||
|
|
*
|
|||
|
|
* @brief 底盘控制类的功能包括：
|
|||
|
|
* 速度控制
|
|||
|
|
* 方向控制
|
|||
|
|
* PID 控制
|
|||
|
|
* 速度和方向的综合控制
|
|||
|
|
*
|
|||
|
|
* @attention PID 控制器的配置需要在 config 文件中设置
|
|||
|
|
*
|
|||
|
|
*/
|
|||
|
|
|
|||
|
|
|
|||
|
|
#define CHASSIS_H
|
|||
|
|
|
|||
|
|
#include "struct_typedef.h"
|
|||
|
|
#include "pid.h"
|
|||
|
|
#include "bmi088.h"
|
|||
|
|
#include "map.h"
|
|||
|
|
#include "user_math.h"
|
|||
|
|
#include "ahrs.h"
|
|||
|
|
#include "can_use.h"
|
|||
|
|
#include "cmd.h"
|
|||
|
|
#include "filter.h"
|
|||
|
|
#include "Action.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 */
|
|||
|
|
|
|||
|
|
//m3508的电机转速转换为底盘的实际速度
|
|||
|
|
#define M3508_MOTOR_RPM_TO_VECTOR 0.0008809748903494517209f
|
|||
|
|
|
|||
|
|
|
|||
|
|
#define M6020_MOTOR_RPM_TO_VECTOR 0.003664f
|
|||
|
|
#define PI 3.1415926535f
|
|||
|
|
|
|||
|
|
typedef struct {
|
|||
|
|
|
|||
|
|
BMI088_t bmi088;
|
|||
|
|
|
|||
|
|
/*可通过该枚举类型来决定Imu的数据量纲*/
|
|||
|
|
enum {
|
|||
|
|
IMU_DEGREE,//角度值（0-360）
|
|||
|
|
IMU_RADIAN//弧度制（0-2pi)
|
|||
|
|
}angle_mode;
|
|||
|
|
|
|||
|
|
AHRS_Eulr_t imu_eulr;//解算后存放欧拉角（弧度制）
|
|||
|
|
}ChassisImu_t;
|
|||
|
|
|
|||
|
|
/*底盘的类型*/
|
|||
|
|
typedef enum {
|
|||
|
|
CHASSIS_TYPE_MECANUM, /* 麦轮 */
|
|||
|
|
CHASSIS_TYPE_OMNI_CROSS, /* 全向轮*/
|
|||
|
|
CHASSIS_TYPE_AGV, /* AGV舵轮 */
|
|||
|
|
} Chassis_Type_e;
|
|||
|
|
|
|||
|
|
/*底盘的电机轮组*/
|
|||
|
|
typedef enum {
|
|||
|
|
DJI_M3508,
|
|||
|
|
DJI_G6020,
|
|||
|
|
AGV_Group,
|
|||
|
|
}Chassis_Motortype_e;
|
|||
|
|
|
|||
|
|
|
|||
|
|
/* 该结构体用于存取固定的一些参数 在config.c中更改后不再变化 */
|
|||
|
|
typedef struct
|
|||
|
|
{
|
|||
|
|
Chassis_Type_e chassis_type; /* */
|
|||
|
|
Chassis_Motortype_e motor_type; /**/
|
|||
|
|
|
|||
|
|
|
|||
|
|
/*该部分决定PID的参数整定在config中修改*/
|
|||
|
|
pid_param_t M3508_param;
|
|||
|
|
pid_param_t AngleCor_param;
|
|||
|
|
pid_param_t OmegaCor_param;
|
|||
|
|
pid_param_t DisCamera_param;
|
|||
|
|
pid_param_t ImuCor_param;
|
|||
|
|
pid_param_t C6020pitAngle_param;
|
|||
|
|
pid_param_t C6020pitOmega_param;
|
|||
|
|
pid_param_t Gimbal_yawAngle_param;
|
|||
|
|
pid_param_t Gimbal_yawOmega_param;
|
|||
|
|
pid_param_t Gimbal_pitchAngle_param;
|
|||
|
|
pid_param_t Gimbal_pitchOmega_param;
|
|||
|
|
pid_param_t NaviVx_param;
|
|||
|
|
pid_param_t NaviVy_param;
|
|||
|
|
pid_param_t NaviVw_param;
|
|||
|
|
pid_param_t Sick_CaliYparam;
|
|||
|
|
pid_param_t Sick_CaliXparam;
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
}Chassis_Param_t;
|
|||
|
|
|
|||
|
|
|
|||
|
|
/*该结构体用于底盘的期望运动向量*/
|
|||
|
|
typedef struct
|
|||
|
|
{
|
|||
|
|
fp32 Vx;
|
|||
|
|
fp32 Vy;
|
|||
|
|
fp32 Vw;
|
|||
|
|
fp32 mul;//油门倍率
|
|||
|
|
}ChassisMove_Vec;
|
|||
|
|
|
|||
|
|
|
|||
|
|
/**
|
|||
|
|
* @brief
|
|||
|
|
*
|
|||
|
|
*/
|
|||
|
|
typedef struct{
|
|||
|
|
|
|||
|
|
uint8_t chassis_task_run; //线程的运行
|
|||
|
|
|
|||
|
|
const Chassis_Param_t *param; //一些固定的参数
|
|||
|
|
|
|||
|
|
ChassisImu_t pos088; //088的实时姿态
|
|||
|
|
Action_POS_t Action_pos;
|
|||
|
|
CMD_Chassis_CtrlType_e ctrl;
|
|||
|
|
CMD_Chassis_mode_e mode;
|
|||
|
|
|
|||
|
|
ChassisMove_Vec move_vec; //由控制任务决定
|
|||
|
|
|
|||
|
|
struct{
|
|||
|
|
|
|||
|
|
fp32 rotor_rpm3508[4];
|
|||
|
|
fp32 rotor_current3508[4];
|
|||
|
|
|
|||
|
|
fp32 rotor_pit6020angle;
|
|||
|
|
fp32 rotor_pit6020rpm;
|
|||
|
|
|
|||
|
|
fp32 rotor_gimbal_yawangle;
|
|||
|
|
fp32 rotor_gimbal_yawrpm;
|
|||
|
|
|
|||
|
|
fp32 rotor_gimbal_pitchangle;
|
|||
|
|
fp32 rotor_gimbal_pitchrpm;
|
|||
|
|
|
|||
|
|
}motorfeedback;
|
|||
|
|
|
|||
|
|
|
|||
|
|
/*期望的底盘输出值*/
|
|||
|
|
struct{
|
|||
|
|
fp32 OmniSpeedOut[4];
|
|||
|
|
|
|||
|
|
}hopemotorout;
|
|||
|
|
|
|||
|
|
|
|||
|
|
/*经PID计算后的实际发送给电机的实时输出值*/
|
|||
|
|
struct
|
|||
|
|
{
|
|||
|
|
fp32 final_3508out[4];
|
|||
|
|
fp32 final_pitchout;
|
|||
|
|
fp32 final_gimbal_yawout;
|
|||
|
|
fp32 final_gimbal_pitchout;
|
|||
|
|
|
|||
|
|
}final_out;
|
|||
|
|
|
|||
|
|
struct{
|
|||
|
|
|
|||
|
|
pid_type_def chassis_3508VecPID[4];
|
|||
|
|
pid_type_def chassis_pitAngle6020;
|
|||
|
|
pid_type_def chassis_pitOmega6020;
|
|||
|
|
pid_type_def chassis_gimbal_yawAnglePID;
|
|||
|
|
pid_type_def chassis_gimbal_yawOmegaPID;
|
|||
|
|
pid_type_def chassis_gimbal_pitchAnglePID;
|
|||
|
|
pid_type_def chassis_gimbal_pitchOmegaPID;
|
|||
|
|
pid_type_def chassis_NaviVxPID;
|
|||
|
|
pid_type_def chassis_NaviVyPID;
|
|||
|
|
pid_type_def chassis_NaviWzPID;
|
|||
|
|
pid_type_def sick_CaliforYPID;
|
|||
|
|
pid_type_def sick_CaliforXPID;
|
|||
|
|
|
|||
|
|
pid_type_def Action_VxPID;
|
|||
|
|
pid_type_def Action_VyPID;
|
|||
|
|
pid_type_def Action_WzPID;
|
|||
|
|
}pid;
|
|||
|
|
|
|||
|
|
fp32 vofa_send[8];
|
|||
|
|
|
|||
|
|
LowPassFilter2p_t filled[6]; /* 输出滤波器滤波器数组 */
|
|||
|
|
|
|||
|
|
|
|||
|
|
int32_t sick_dis[4]; //获取到的sick激光值
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
}Chassis_t;
|
|||
|
|
|
|||
|
|
/**
|
|||
|
|
* @brief
|
|||
|
|
*
|
|||
|
|
* @param c
|
|||
|
|
* @param param
|
|||
|
|
* @param mech_zero
|
|||
|
|
* @param wheelPolar
|
|||
|
|
* @return
|
|||
|
|
*/
|
|||
|
|
int8_t Chassis_init(Chassis_t *c,const Chassis_Param_t *param,float target_freq);
|
|||
|
|
|
|||
|
|
|
|||
|
|
/**
|
|||
|
|
* \brief
|
|||
|
|
|
|||
|
|
*/
|
|||
|
|
int8_t Chassis_UpdateFeedback(Chassis_t *c, const CAN_t *can);
|
|||
|
|
|
|||
|
|
|
|||
|
|
/**
|
|||
|
|
* \brief
|
|||
|
|
|
|||
|
|
*/
|
|||
|
|
int8_t Chassis_Control(Chassis_t *c,CMD_t *ctrl,CAN_Output_t *out);
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
/// @brief
|
|||
|
|
/// @param c
|
|||
|
|
void vesc_current_detection(Chassis_t *c);
|
|||
|
|
#endif
|