#ifndef UP_H
#define UP_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"
#include "chassis.h" 
#include "vofa.h"	
#include "GO_M8010_6_Driver.h"
	
typedef enum {
    STATE_IDLE,          // 空闲状态
    STATE_PRE_DRIBBLE,   // 运球前
    STATE_POST_DRIBBLE,  // 运球后
    STATE_PRE_LAUNCH,    // 发射前
    STATE_POST_LAUNCH    // 发射后
} OperationState;
	


typedef struct {

 BMI088_t bmi088;

 AHRS_Eulr_t imu_eulr;//解算后存放欧拉角（弧度制）
	
}UP_Imu_t;
	
	
typedef struct
{	
	/*该部分决定PID的参数整定在config中修改*/
	pid_param_t VESC_5065_M1_param;
	pid_param_t VESC_5065_M2_param;
	
	pid_param_t UP_GM6020_speed_param;
	pid_param_t UP_GM6020_angle_param;
	
	pid_param_t M2006_speed_param;	
	pid_param_t M2006_angle_param;	
	
	pid_param_t M3508_speed_param;
}UP_Param_t;

typedef struct
{		
	float orig_angle;
  int8_t offset_flag;
	float angle;
	float last_angle;
	float total_angle;
	
}M2006_data;


typedef struct{
	
   uint8_t up_task_run; 
   const	UP_Param_t *param;  
	 UP_Imu_t pos088; 
	
	OperationState ctrl;
      struct {
        float orig_angle;     // 初始偏移量
        float last_angle;     // 上一次编码器值
        int32_t round_cnt;    // 圈数计数器
        uint16_t init_cnt;    // 初始化计数器（前50次记录初始值）
				float total_angle;
    } M2006;
	 struct{
					
	  fp32 rotor_pit6020ecd;
	  fp32 rotor_pit6020rpm;

	  fp32 VESC_5065_M1_rpm;
	  fp32 VESC_5065_M2_rpm;

	  fp32 M2006_rpm;
	  fp32 M2006_angle;
		 
		fp32 M3508_speed[3];

	}motorfeedback;
 

	
	 struct{
					
	  fp32 rotor_pit6020angle;

	  fp32 VESC_5065_M1_rpm;
	  fp32 VESC_5065_M2_rpm;

	  fp32 M2006_angle;
		fp32 M3508_speed;

	}motor_target;
  
	 struct{
		 
		 pid_type_def GM6020_speed;
		 pid_type_def GM6020_angle;
		 pid_type_def VESC_5065_M1;
		 pid_type_def VESC_5065_M2;
		 pid_type_def M2006_angle;
		 pid_type_def M2006_speed;
		 pid_type_def M3508_speed[3];
				
	}pid;
 
	
	
	
	
	
		/*经PID计算后的实际发送给电机的实时输出值*/
    struct 
	{
		fp32 final_2006out;
		fp32 final_3508out[3];
		fp32 final_pitchout;
		fp32 final_VESC_5065_M1out;
		fp32 final_VESC_5065_M2out;

	}final_out;
	
	 LowPassFilter2p_t filled[6]; /* 输出滤波器滤波器数组 */
	
	   fp32 vofa_send[8];

	
}	UP_t;


int8_t up_init(UP_t *u,const UP_Param_t *param,float target_freq);

int8_t UP_UpdateFeedback(UP_t *u, const CAN_t *can) ;
int8_t GM6020_control(UP_t *u,fp32 angle);
int8_t VESC_M5065_Control(UP_t *u,fp32 speed);

int8_t UP_M2006_angle(UP_t *u,fp32 target_angle);
int8_t UP_control(UP_t *u,CAN_Output_t *out);
int8_t ALL_Motor_Control(UP_t *u,CAN_Output_t *out);
int8_t UP_M2006_angle(UP_t *u,fp32 target_angle);
int8_t UP_M3508_speed(UP_t *u,fp32 speed);



#endif