#ifndef UP_UTILS_H
#define UP_UTILS_H

#include "gpio.h"
#include "user_math.h"
#include "bsp_buzzer.h"
#include "bsp_delay.h"
#include "struct_typedef.h"
#include "pid.h"
#include "user_math.h"
#include "ahrs.h"
#include "filter.h"	
#include "bsp_usart.h"
#include "GO_M8010_6_Driver.h"


#define CAN2_G3508_ID (0x200)
#define GEAR_RATIO_2006         (36)   // 2006减速比
#define GEAR_RATIO_3508         (19)

#define CAN_MOTOR_ENC_RES  8191   // 编码器分辨率
#define MOTOR2006_ECD_TO_ANGLE  (360.0 / 8191.0 / (GEAR_RATIO_2006))   //2006编码值转轴角度
#define MOTOR3508_ECD_TO_ANGLE  (360.0 / 8191.0 / (GEAR_RATIO_3508))   //3508编码值转轴角度


typedef struct
{	
	  fp32 ecd;
	  fp32 rpm;
		uint8_t id;
    fp32 orig_angle;     
    fp32 last_angle;     
    int32_t round_cnt;    
    int init_cnt;   
	  fp32 total_angle;	
}DJmotor_feedback_t;


typedef enum {
    CURVE_58,
    CURVE_66
} CurveType;


static float curve_58(float d) ;
static float curve_66(float d) ;
float CurveChange(float d, float x, float y, CurveType *cs);

int8_t GO_SendData( GO_MotorData_t *go_data,GO_MotorCmd_t *go_cmd,float pos, float limit);

int8_t DJ_processdata(DJmotor_feedback_t *f,fp32 ecd_to_angle);




#endif