#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 <cmsis_os2.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	struct {
	int32_t w_error;//角度纠正时的允许误差量
	int32_t xy_error;//xy允许误差量		
	int32_t x_set;
	int32_t y_set;
}sickparam_t;

/* 该结构体用于存取固定的一些参数 在config.c中更改后不再变化 */
typedef struct
{
	
	
	/*该部分决定PID的参数整定在config中修改*/
  pid_param_t M3508_param;	

	pid_param_t chassis_PICKWzPID_HIGN_param;
	pid_param_t chassis_PICKWzPID_LOW_param;
	
  pid_param_t NaviVx_param;
  pid_param_t NaviVy_param;
  pid_param_t NaviVw_param;	
	
	pid_param_t SickCali_WInparam;
	pid_param_t SickCali_WOutparam;
	pid_param_t SickCali_YInparam;
	pid_param_t SickCali_YOutparam;
	pid_param_t SickCali_XInparam;
	pid_param_t SickCali_XOutparam;
	
	pid_param_t Chassis_AngleAdjust_param;
	
	sickparam_t sickparam;
	
}Chassis_Param_t;


/*该结构体用于底盘的期望运动向量*/
typedef struct 
{
   fp32 Vx;
   fp32 Vy;
   fp32 Vw;
	fp32 mul;//油门倍率
}ChassisMove_Vec;
typedef struct 
{
	
   fp32 send[4];
	
}NUC_send_t;

/**
 *  @brief  
 * 
 */
typedef struct{
	
   uint8_t chassis_task_run; //线程的运行

   const	Chassis_Param_t *param;  //一些固定的参数

    ChassisImu_t pos088; //088的实时姿态
	struct{
		
    CMD_CtrlType_t ctrl;            
    CMD_mode_t mode;
		
	}chassis_ctrl;
    ChassisMove_Vec move_vec;         //由控制任务决定

    struct{
			
	  fp32 rotor_rpm3508[4];
	  fp32 rotor_current3508[4];
			

	}motorfeedback;
    

	/*期望的底盘输出值*/
	struct{
	fp32 OmniSpeedOut[4];
	
	}hopemotorout;


	/*经PID计算后的实际发送给电机的实时输出值*/
    struct 
	{
		fp32 final_3508out[4];
	}final_out;
	
	struct{                

	pid_type_def chassis_3508VecPID[4];
		/*视觉*/
		/*存在较高误差*/
	pid_type_def chassis_PICKWzPID_HIGN;
		/*存在较低误差*/
	pid_type_def chassis_PICKWzPID_LOW;
	pid_type_def 	Chassis_AngleAdjust;
	
	/*sick */
	  pid_type_def SickCaliWzInPID;
	  pid_type_def SickCaliWzOutPID;
	  pid_type_def SickCaliVxInPID;
	  pid_type_def SickCaliVxOutPID;
	  pid_type_def SickCaliVyInPID;
	  pid_type_def SickCaliVyOutPID;
		
		
	}pid;

   fp32 vofa_send[8];
	
	 LowPassFilter2p_t filled[8]; /* 输出滤波器滤波器数组 */
	 
	struct {
	int32_t sick_dis[4];   //获取到的sick激光值
  sickparam_t sickparam;
	int is_reach;
	}sick_cali;
	
	NUC_send_t NUC_send;

	
}Chassis_t;

int8_t Chassis_init(Chassis_t *c,const Chassis_Param_t *param,float target_freq);


int8_t Chassis_UpdateFeedback(Chassis_t *c, const CAN_t *can);


int8_t Chassis_Control(Chassis_t *c,CMD_t *ctrl,CAN_Output_t *out);


int8_t sick_calibration(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) ;

int8_t Chassis_AngleCompensate(Chassis_t *c) ;


#endif
