#ifndef _CONTROL_H_
#define _CONTROL_H_

/* 
	ǶȻ->˶ѧ->ٶȻ  
	ͣAngle2Inv2Speed
	˶ѧͣSPEED_YAW_SOLVE/XY_SPEED_SOLVE
	ƲĿǶȣrotation_yaw_target
			 1.Ŀٶȣlinear_speed_targetĿƽǣtranslation_yaw_target
			 2.ĿXٶȣx_speed_targetĿYٶȣy_speed_target
	ʹǰҪȽrotate_finsh_flagFalse
*/
void Angle2Inv2Speed_control(void);

/* 
	ǶȻ->˶ѧ->ٶȻ  
	ͣAngle2Inv2Speed
	˶ѧͣSPEED_YAW_SOLVE/XY_SPEED_SOLVE
	ƲĿǶȣrotation_yaw_target
			 1.Ŀٶȣlinear_speed_targetĿƽǣtranslation_yaw_target
			 2.ĿXٶȣx_speed_targetĿYٶȣy_speed_target
	ʹǰҪȽrotate_finsh_flagFalse
*/
void CircleAngle2Inv2Speed_control(void);

/* 
	ӶλX->˶ѧ->ٶȻ  
	ͣX2Inv2Speed
	˶ѧͣXY_SPEED_SOLVEĬϣ
	ƲĿYٶȣy_speed_targetʶX꣺detection_box_center_x
	ʹǰҪȽbox_X_finsh_flagFalse
*/
void Track_X2Inv2Speed_control(void);

/* 
	ӶλY->˶ѧ->ٶȻ  
	ͣX2Inv2Speed
	˶ѧͣXY_SPEED_SOLVEĬϣ
	ƲĿXٶȣx_speed_targetʶX꣺detection_box_center_x
	ʹǰҪȽbox_X_finsh_flagFalse
*/
void Track_Y2Inv2Speed_control(void);

/* 
	ӶλXY->˶ѧ->ٶȻ  
	ͣXY2Inv2Speed
	˶ѧͣXY_SPEED_SOLVEĬϣ
	ƲʶX꣺detection_box_center_xʶȣdetection_box_width
	ʹǰҪȽbox_XY_finsh_flagFalse
*/
void Track_XY2Inv2Speed_control(void);

/* 
	X->˶ѧ->ٶȻ  
	ͣX2Inv2Speed
	˶ѧͣXY_SPEED_SOLVEĬϣ
	ƲĿYٶȣy_speed_targetʶX꣺detection_box_center_x
	ʹǰҪȽbox_X_finsh_flagFalse
*/
void Fxxk_X2Inv2Speed_control(void);

/* 
	˶ѧ->ٶȻ  
	ͣInv2Speed
	˶ѧͣSPEED_YAW_SOLVE/XY_SPEED_SOLVE
	ƲĿǶȣrotation_yaw_target
			 1.Ŀٶȣlinear_speed_targetĿƽǣtranslation_yaw_target
			 2.ĿXٶȣx_speed_targetĿYٶȣy_speed_target
*/
void Inv2Speed_control(void);

/* 
	ٶȻ  
	ͣSpeed
	ƲӵĿת٣wheel_speed_target[0]wheel_speed_target[1]wheel_speed_target[2]
*/
void Speed_control(void);

/* 
	PWM  
	ͣPWM
	ƲӵĿת٣motor_pwm_duty[0]motor_pwm_duty[1]motor_pwm_duty[2]
*/
void PWM_control(void);

/* 
	PID 
	˶ѧͣXY_SPEED_SOLVEĬϣ
	ƲĿYٶȣy_speed_target
*/
void path_pid_calc(void);

/*  PID */
static void motor_pid_calc(void);

/* ǶȻת PID */
static void angle_rotate_pid_calc(void);

/* Բת PID */
static void circle_rotate_pid_calc(void);

/* BOX X PID */
static void box_track_x_pid_calu(void);
static void box_fxxk_x_pid_calu(void);

/* BOX Y PID */
static void box_track_y_pid_calu(void);

/* BOX X/Y PID */
static void box_track_xy_pid_calu(void);

/* 
	X 
	˵
	_path_x_speed_target_ Ŀٶ
	slow_change_rate 仯ʣԽСԽ
	speed_slow_change_enable_flag ʹܱ־λ
*/
void x_speed_slow_change(float _path_x_speed_target_, float slow_change_rate, _bool_ speed_slow_change_enable_flag);

/* 
	Y 
	˵
	_path_y_speed_target_ Ŀٶ
	slow_change_rate 仯ʣԽСԽ
	speed_slow_change_enable_flag ʹܱ־λ
*/
void y_speed_slow_change(float _path_y_speed_target_, float slow_change_rate, _bool_ speed_slow_change_enable_flag);

#endif