210 lines
4.5 KiB
C
210 lines
4.5 KiB
C
#ifndef CHASSIS_H
|
||
#define CHASSIS_H
|
||
|
||
#include "struct_typedef.h"
|
||
#include "pid.h"
|
||
#include "bmi088.h"
|
||
#include "user_math.h"
|
||
#include "ahrs.h"
|
||
#include "can_use.h"
|
||
#include "cmd.h"
|
||
#include "filter.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
|
||
|
||
//纵向/横向
|
||
#define radians atan(1.0*422/440)//角度制
|
||
//四个舵轮的安装误差
|
||
typedef struct {
|
||
fp32 MOTOR_OFFSET[4];
|
||
} MotorOffset_t;
|
||
|
||
|
||
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_3, /* AGV舵轮 */
|
||
CHASSIS_TYPE_AGV_4, /* AGV舵轮 */
|
||
} Chassis_Type_e;
|
||
/*底盘的电机轮组*/
|
||
typedef enum {
|
||
DJI_M3508,
|
||
DJI_G6020,
|
||
AGV_Group,
|
||
}Chassis_Motortype_e;
|
||
|
||
/*舵轮的实际坐标值*/
|
||
typedef struct
|
||
{
|
||
fp32 WheelXpos[4];
|
||
fp32 WheelYpos[4];
|
||
|
||
}AgvNormalPos;
|
||
|
||
/* 该结构体用于存取固定的一些参数 在config.c中更改后不再变化 */
|
||
typedef struct
|
||
{
|
||
Chassis_Type_e chassis_type; /* */
|
||
Chassis_Motortype_e motor_type; /**/
|
||
|
||
AgvNormalPos AgvZeroPos_3;
|
||
const fp32 Agv6020zeroPos_3[3];
|
||
|
||
AgvNormalPos AgvZeroPos;
|
||
const fp32 Agv6020zeroPos[4];
|
||
/*该部分决定PID的参数整定在config中修改*/
|
||
pid_param_t M3508_param;
|
||
pid_param_t C6020Omega_param;
|
||
pid_param_t C6020Angle_param;
|
||
pid_param_t CameraAngle_param;
|
||
pid_param_t CameraSpeed_param;
|
||
pid_param_t C6020pitAngle_param;
|
||
pid_param_t C6020pitOmega_param;
|
||
|
||
pid_param_t NaviVx_param;
|
||
pid_param_t NaviVy_param;
|
||
pid_param_t NaviVw_param;
|
||
}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的实时姿态
|
||
|
||
CMD_Chassis_mode mode;
|
||
CMD_Chassis_navi_pos pos;
|
||
|
||
ChassisMove_Vec move_vec; //由控制任务决定
|
||
|
||
struct{
|
||
fp32 rotor_rpm3508[4];
|
||
fp32 rotor_current3508[4];
|
||
|
||
fp32 rotor_rpm6020[4];
|
||
fp32 rotor_angle6020[4];
|
||
fp32 rotor_current6020[4];
|
||
|
||
fp32 rotor_rpm5065[4];
|
||
fp32 torque_current5065[4];
|
||
fp32 rotor_5065pos[4];
|
||
|
||
|
||
}motorfeedback;
|
||
|
||
MotorOffset_t motoroffset;
|
||
|
||
/*期望的底盘输出值(此处为舵轮解算出的各个电机的期望输出值)ֵ*/
|
||
struct{
|
||
fp32 rotor5065_jiesuan_1[4];
|
||
fp32 rotor5065_jiesuan_2[4];
|
||
fp32 rotor6020_jiesuan_1[4];
|
||
fp32 rotor6020_jiesuan_2[4];
|
||
fp32 rotor6020_elur_yaw;
|
||
|
||
fp32 motor6020_target[4];
|
||
fp32 motor5065_target[4];
|
||
|
||
}hopemotorout;
|
||
|
||
fp32 pit6020_set;
|
||
/*经PID计算后的实际发送给电机的实时输出值*/
|
||
struct
|
||
{
|
||
fp32 final_6020out[4]; //最终经由PID计算出的发送给电机的实际输出值
|
||
fp32 final_5065out[4]; //
|
||
fp32 final_3508out[4];
|
||
fp32 final_pitchout;
|
||
}final_out;
|
||
|
||
struct{
|
||
pid_type_def chassis_6020anglePid;
|
||
pid_type_def chassis_6020OmegaPid;
|
||
pid_type_def chassis_CameraanglePID;
|
||
pid_type_def chassis_CameraspeedPID;
|
||
pid_type_def chassis_3508VecPID[4];
|
||
pid_type_def chassis_pitAngle6020;
|
||
pid_type_def chassis_pitOmega6020;
|
||
}pid;
|
||
|
||
fp32 vofa_send[8];
|
||
|
||
LowPassFilter2p_t filled[6];
|
||
|
||
uint8_t pick_ready; //该状态位用来检测与上层C板的通信状态
|
||
|
||
uint8_t keeping_angle_flag; // 是否处于保持角度模式
|
||
float keep_angle[4]; // 保持的 6020 角度
|
||
|
||
fp32 chassis_yaw;
|
||
fp32 chassis_yaw_pid;
|
||
|
||
}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, Action_POS_t*pos);
|
||
|
||
#endif
|