#include "common.h"
#include "data.h"

/* ǶȻ->˶ѧ->ٶȻ  */
void Angle2Inv2Speed_control(void){
	angle_rotate_pid_calc(); 
	switch(move_solve_kind){ 
		case SPEED_YAW_SOLVE:{ chassis_solve.move_inv_solve(&chassis_solve, SPEED_YAW_SOLVE, linear_speed_target, translation_yaw_target, angular_speed_target); break; } 
		case XY_SPEED_SOLVE:{ chassis_solve.move_inv_solve(&chassis_solve, XY_SPEED_SOLVE, x_speed_target, y_speed_target, angular_speed_target); break; }
	}   
	wheel_speed_target[0] = chassis_solve.wheel_1_speed; 
	wheel_speed_target[1] = chassis_solve.wheel_2_speed;
	wheel_speed_target[2] = chassis_solve.wheel_3_speed;
	motor_pid_calc(); 
	PWM_control(); 
}

/* ԲǶȻ->˶ѧ->ٶȻ  */
void CircleAngle2Inv2Speed_control(void){
	circle_rotate_pid_calc(); 
	switch(move_solve_kind){ 
		case SPEED_YAW_SOLVE:{ chassis_solve.move_inv_solve(&chassis_solve, SPEED_YAW_SOLVE, linear_speed_target, translation_yaw_target, angular_speed_target); break; } 
		case XY_SPEED_SOLVE:{ chassis_solve.move_inv_solve(&chassis_solve, XY_SPEED_SOLVE, x_speed_target, y_speed_target, angular_speed_target); break; }
	}   
	wheel_speed_target[0] = chassis_solve.wheel_1_speed; 
	wheel_speed_target[1] = chassis_solve.wheel_2_speed;
	wheel_speed_target[2] = chassis_solve.wheel_3_speed;
	motor_pid_calc(); 
	PWM_control(); 
}

/* ӶλX->˶ѧ->ٶȻ  */
void Track_X2Inv2Speed_control(void){
	box_track_x_pid_calu(); 
	chassis_solve.move_inv_solve(&chassis_solve, XY_SPEED_SOLVE, x_speed_target, y_speed_target, angular_speed_target);  
	wheel_speed_target[0] = chassis_solve.wheel_1_speed; 
	wheel_speed_target[1] = chassis_solve.wheel_2_speed; 
	wheel_speed_target[2] = chassis_solve.wheel_3_speed;
	motor_pid_calc(); 
	PWM_control(); 
}

/* ӶλY->˶ѧ->ٶȻ  */
void Track_Y2Inv2Speed_control(void){
	box_track_y_pid_calu(); 
	chassis_solve.move_inv_solve(&chassis_solve, XY_SPEED_SOLVE, x_speed_target, y_speed_target, angular_speed_target);  
	wheel_speed_target[0] = chassis_solve.wheel_1_speed; 
	wheel_speed_target[1] = chassis_solve.wheel_2_speed; 
	wheel_speed_target[2] = chassis_solve.wheel_3_speed;
	motor_pid_calc(); 
	PWM_control(); 
}

/* ӶλXY->˶ѧ->ٶȻ  */
void Track_XY2Inv2Speed_control(void){
	box_track_xy_pid_calu(); 
	chassis_solve.move_inv_solve(&chassis_solve, XY_SPEED_SOLVE, x_speed_target, y_speed_target, angular_speed_target);  
	wheel_speed_target[0] = chassis_solve.wheel_1_speed; 
	wheel_speed_target[1] = chassis_solve.wheel_2_speed; 
	wheel_speed_target[2] = chassis_solve.wheel_3_speed;
	motor_pid_calc(); 
	PWM_control(); 
}

/* X->˶ѧ->ٶȻ  */
void Fxxk_X2Inv2Speed_control(void){
	box_fxxk_x_pid_calu(); 
	chassis_solve.move_inv_solve(&chassis_solve, XY_SPEED_SOLVE, x_speed_target, y_speed_target, angular_speed_target);  
	wheel_speed_target[0] = chassis_solve.wheel_1_speed; 
	wheel_speed_target[1] = chassis_solve.wheel_2_speed; 
	wheel_speed_target[2] = chassis_solve.wheel_3_speed;
	motor_pid_calc(); 
	PWM_control(); 
}

/* ˶ѧ->ٶȻ  */
void Inv2Speed_control(void){
	switch(move_solve_kind){ 	
		case SPEED_YAW_SOLVE:{ chassis_solve.move_inv_solve(&chassis_solve, SPEED_YAW_SOLVE, linear_speed_target, translation_yaw_target, angular_speed_target); break; } 
		case XY_SPEED_SOLVE:{ chassis_solve.move_inv_solve(&chassis_solve, XY_SPEED_SOLVE, x_speed_target, y_speed_target, angular_speed_target); break; }
	}   
	wheel_speed_target[0] = chassis_solve.wheel_1_speed; wheel_speed_target[1] = chassis_solve.wheel_2_speed; wheel_speed_target[2] = chassis_solve.wheel_3_speed;
	motor_pid_calc();
	PWM_control();
}

/* ٶȻ  */
void Speed_control(void){
	motor_pid_calc();
	PWM_control();
}

/* PWM  */
void PWM_control(void){
	/* 1 */
	if(motor_pwm_duty[0] >= 0)
		motor_1.motor_run(&motor_1, positive, fabsf(motor_pwm_duty[0]));
	else 
		motor_1.motor_run(&motor_1, negative, fabsf(motor_pwm_duty[0]));
	/* 2 */
	if(motor_pwm_duty[1] >= 0)
		motor_2.motor_run(&motor_2, positive, fabsf(motor_pwm_duty[1]));
	else 
		motor_2.motor_run(&motor_2, negative, fabsf(motor_pwm_duty[1]));
	/* 3 */
	if(motor_pwm_duty[2] >= 0)
		motor_3.motor_run(&motor_3, positive, fabsf(motor_pwm_duty[2]));
	else 
		motor_3.motor_run(&motor_3, negative, fabsf(motor_pwm_duty[2]));
}

/* PID */
void path_pid_calc(void){
	path_gyro_karman.karman_filter(&path_gyro_karman, imu660ra.gyro_z);
    float err[2] = { path_err,path_gyro_karman.value  };
	float err_gyro[2] = { path_gyro_karman.value , path_err };
    
	// ѭPID
	float path_Kp[4] = { PATH_PID[plan_idx][0][0], PATH_PID[plan_idx][1][0], PATH_PID[plan_idx][2][0], PATH_PID[plan_idx][3][0] };
	float path_Ki[4] = { PATH_PID[plan_idx][0][1], PATH_PID[plan_idx][1][1], PATH_PID[plan_idx][2][1], PATH_PID[plan_idx][3][1] };
	float path_Kd[4] = { PATH_PID[plan_idx][0][2], PATH_PID[plan_idx][1][2], PATH_PID[plan_idx][2][2], PATH_PID[plan_idx][3][2] };
	float path_i_limit[4] = { PATH_PID[plan_idx][0][3], PATH_PID[plan_idx][1][3], PATH_PID[plan_idx][2][3], PATH_PID[plan_idx][3][3] };
	float path_output_limit[4] = { PATH_PID[plan_idx][0][4], PATH_PID[plan_idx][1][4], PATH_PID[plan_idx][2][4], PATH_PID[plan_idx][3][4] };
	// ٶPID
	float path_gyroz_Kp[4] = { 0, 0, 0, 0 };
	float path_gyroz_Ki[4] = { 0, 0, 0, 0 };
	float path_gyroz_Kd[4] = { PATH_PID[plan_idx][0][5], PATH_PID[plan_idx][1][5], PATH_PID[plan_idx][2][5], PATH_PID[plan_idx][3][5] };
	float path_gyroz_i_limit[4] = { 0, 0, 0, 0 };
	float path_gyroz_output_limit[4] = { 0, 0, 0, 0 };
	
	path_pid.fuzzy_pid(&path_pid, err, path_Kp, path_Ki, path_Kd, path_i_limit, path_output_limit, 2);
	path_gyroz_pid.fuzzy_pid(&path_gyroz_pid, err_gyro, path_gyroz_Kp, path_gyroz_Ki, path_gyroz_Kd, path_gyroz_i_limit, path_gyroz_output_limit, 1);

	move_solve_kind = XY_SPEED_SOLVE;
	control_kind = Inv2Speed;
	angular_speed_target = path_pid.positional_pid(&path_pid, 0, -path_err)-path_gyroz_pid.Kd*path_gyro_karman.value;	// ˴עgyro_z
}

/*  PID */
static void motor_pid_calc(void){
	if(path_state == box_first_track || path_state == box_calibration || path_state == box_inv_calibration || path_state == box_second_track || path_state == box_fxxk || path_state == path_back){
		// ٶȻ
		#ifdef SPEED
		/* 1 */
		motor_1_pid.Kp = MOTOR_1_PID[0];
		motor_1_pid.Ki = MOTOR_1_PID[1];
		motor_1_pid.Kd = MOTOR_1_PID[2];
		motor_1_pid.i_limit = MOTOR_1_PID[3];
		motor_1_pid.output_limit = MOTOR_1_PID[4];
		motor_pwm_duty[0] = motor_1_pid.incremental_pid(&motor_1_pid, wheel_speed_target[0], encoder_1.wheel_speed);
		/* 2 */
		motor_2_pid.Kp = MOTOR_1_PID[0];
		motor_2_pid.Ki = MOTOR_1_PID[1];
		motor_2_pid.Kd = MOTOR_1_PID[2];
		motor_2_pid.i_limit = MOTOR_1_PID[3];
		motor_2_pid.output_limit = MOTOR_1_PID[4];
		motor_pwm_duty[1] = motor_2_pid.incremental_pid(&motor_2_pid, wheel_speed_target[1], encoder_2.wheel_speed);
		/* 3 */
		motor_3_pid.Kp = MOTOR_1_PID[0];
		motor_3_pid.Ki = MOTOR_1_PID[1];
		motor_3_pid.Kd = MOTOR_1_PID[2];
		motor_3_pid.i_limit = MOTOR_1_PID[3];
		motor_3_pid.output_limit = MOTOR_1_PID[4];
		motor_pwm_duty[2] = motor_3_pid.incremental_pid(&motor_3_pid, wheel_speed_target[2], encoder_3.wheel_speed);
		#endif
		// ٶȻ+
		#ifdef SPEED_AND_CURRENT
		/* 1 */
		motor_1_pid.Kp = MOTOR_1_PID[0];
		motor_1_pid.Ki = MOTOR_1_PID[1];
		motor_1_pid.Kd = MOTOR_1_PID[2];
		motor_1_pid.i_limit = MOTOR_1_PID[3];
		motor_1_pid.output_limit = MOTOR_1_PID[4];
		motor_current_target[0] = motor_1_pid.incremental_pid(&motor_1_pid, wheel_speed_target[0], encoder_1.wheel_speed);
		current_1_pid.Kp = I_1_PID[0];
		current_1_pid.Ki = I_1_PID[1];
		current_1_pid.Kd = I_1_PID[2];
		current_1_pid.i_limit = I_1_PID[3];
		current_1_pid.output_limit = I_1_PID[4];
		motor_pwm_duty[0] = current_1_pid.incremental_pid(&current_1_pid, motor_current_target[0], current_1.current);
		/* 2 */
		motor_2_pid.Kp = MOTOR_1_PID[0];
		motor_2_pid.Ki = MOTOR_1_PID[1];
		motor_2_pid.Kd = MOTOR_1_PID[2];
		motor_2_pid.i_limit = MOTOR_1_PID[3];
		motor_2_pid.output_limit = MOTOR_1_PID[4];
		motor_current_target[1] = motor_2_pid.incremental_pid(&motor_2_pid, wheel_speed_target[1], encoder_2.wheel_speed);
		current_2_pid.Kp = I_2_PID[0];
		current_2_pid.Ki = I_2_PID[1];
		current_2_pid.Kd = I_2_PID[2];
		current_2_pid.i_limit = I_2_PID[3];
		current_2_pid.output_limit = I_2_PID[4];
		motor_pwm_duty[1] = current_2_pid.incremental_pid(&current_2_pid, motor_current_target[1], current_2.current);
		/* 3 */
		motor_3_pid.Kp = MOTOR_1_PID[0];
		motor_3_pid.Ki = MOTOR_1_PID[1];
		motor_3_pid.Kd = MOTOR_1_PID[2];
		motor_3_pid.i_limit = MOTOR_1_PID[3];
		motor_3_pid.output_limit = MOTOR_1_PID[4];
		motor_current_target[2] = motor_3_pid.incremental_pid(&motor_3_pid, wheel_speed_target[2], encoder_3.wheel_speed);
		current_3_pid.Kp = I_3_PID[0];
		current_3_pid.Ki = I_3_PID[1];
		current_3_pid.Kd = I_3_PID[2];
		current_3_pid.i_limit = I_3_PID[3];
		current_3_pid.output_limit = I_3_PID[4];
		motor_pwm_duty[2] = current_3_pid.incremental_pid(&current_3_pid, motor_current_target[2], current_3.current);
		#endif
		#ifdef FUZZY_SPEED_AND_CURRENT
		float motor_1_err = wheel_speed_target[0];
		float motor_2_err = wheel_speed_target[1];
		float motor_3_err = wheel_speed_target[2];
		float Kp[4] = { MOTOR_PID[0][0], MOTOR_PID[1][0], MOTOR_PID[2][0], MOTOR_PID[3][0] };
		float Ki[4] = { MOTOR_PID[0][1], MOTOR_PID[1][1], MOTOR_PID[2][1], MOTOR_PID[3][1] };
		float Kd[4] = { MOTOR_PID[0][2], MOTOR_PID[1][2], MOTOR_PID[2][2], MOTOR_PID[3][2] };
		float i_limit[4] = { MOTOR_PID[0][3], MOTOR_PID[1][3], MOTOR_PID[2][3], MOTOR_PID[3][3] };
		float output_limit[4] = { MOTOR_PID[0][4], MOTOR_PID[1][4], MOTOR_PID[2][4], MOTOR_PID[3][4] };
		/* 1 */
		motor_1_pid.fuzzy_pid(&motor_1_pid, &motor_1_err, Kp, Ki, Kd, i_limit, output_limit, 1);
		motor_current_target[0] = motor_1_pid.incremental_pid(&motor_1_pid, wheel_speed_target[0], encoder_1.wheel_speed);
		current_1_pid.Kp = I_1_PID[0];
		current_1_pid.Ki = I_1_PID[1];
		current_1_pid.Kd = I_1_PID[2];
		current_1_pid.i_limit = I_1_PID[3];
		current_1_pid.output_limit = I_1_PID[4];
		motor_pwm_duty[0] = current_1_pid.incremental_pid(&current_1_pid, motor_current_target[0], current_1.current);
		/* 2 */
		motor_2_pid.fuzzy_pid(&motor_2_pid, &motor_2_err, Kp, Ki, Kd, i_limit, output_limit, 1);
		motor_current_target[1] = motor_2_pid.incremental_pid(&motor_2_pid, wheel_speed_target[1], encoder_2.wheel_speed);
		current_2_pid.Kp = I_2_PID[0];
		current_2_pid.Ki = I_2_PID[1];
		current_2_pid.Kd = I_2_PID[2];
		current_2_pid.i_limit = I_2_PID[3];
		current_2_pid.output_limit = I_2_PID[4];
		motor_pwm_duty[1] = current_2_pid.incremental_pid(&current_2_pid, motor_current_target[1], current_2.current);
		/* 3 */
		motor_3_pid.fuzzy_pid(&motor_3_pid, &motor_3_err, Kp, Ki, Kd, i_limit, output_limit, 1);
		motor_current_target[2] = motor_3_pid.incremental_pid(&motor_3_pid, wheel_speed_target[2], encoder_3.wheel_speed);
		current_3_pid.Kp = I_3_PID[0];
		current_3_pid.Ki = I_3_PID[1];
		current_3_pid.Kd = I_3_PID[2];
		current_3_pid.i_limit = I_3_PID[3];
		current_3_pid.output_limit = I_3_PID[4];
		motor_pwm_duty[2] = current_3_pid.incremental_pid(&current_3_pid, motor_current_target[2], current_3.current);
		#endif
	}
	else{
		// ٶȻ
		#ifdef SPEED
		/* 1 */
		motor_1_pid.Kp = MOTOR_1_PID[0];
		motor_1_pid.Ki = MOTOR_1_PID[1];
		motor_1_pid.Kd = MOTOR_1_PID[2];
		motor_1_pid.i_limit = MOTOR_1_PID[3];
		motor_1_pid.output_limit = MOTOR_1_PID[4];
		motor_pwm_duty[0] = motor_1_pid.incremental_pid(&motor_1_pid, wheel_speed_target[0], encoder_1.wheel_speed);
		/* 2 */
		motor_2_pid.Kp = MOTOR_2_PID[0];
		motor_2_pid.Ki = MOTOR_2_PID[1];
		motor_2_pid.Kd = MOTOR_2_PID[2];
		motor_2_pid.i_limit = MOTOR_2_PID[3];
		motor_2_pid.output_limit = MOTOR_2_PID[4];
		motor_pwm_duty[1] = motor_2_pid.incremental_pid(&motor_2_pid, wheel_speed_target[1], encoder_2.wheel_speed);
		/* 3 */
		motor_3_pid.Kp = MOTOR_3_PID[0];
		motor_3_pid.Ki = MOTOR_3_PID[1];
		motor_3_pid.Kd = MOTOR_3_PID[2];
		motor_3_pid.i_limit = MOTOR_3_PID[3];
		motor_3_pid.output_limit = MOTOR_3_PID[4];
		motor_pwm_duty[2] = motor_3_pid.incremental_pid(&motor_3_pid, wheel_speed_target[2], encoder_3.wheel_speed);
		#endif
		// ٶȻ+
		#ifdef SPEED_AND_CURRENT
		/* 1 */
		motor_1_pid.Kp = MOTOR_1_PID[0];
		motor_1_pid.Ki = MOTOR_1_PID[1];
		motor_1_pid.Kd = MOTOR_1_PID[2];
		motor_1_pid.i_limit = MOTOR_1_PID[3];
		motor_1_pid.output_limit = MOTOR_1_PID[4];
		motor_current_target[0] = motor_1_pid.incremental_pid(&motor_1_pid, wheel_speed_target[0], encoder_1.wheel_speed);
		current_1_pid.Kp = I_1_PID[0];
		current_1_pid.Ki = I_1_PID[1];
		current_1_pid.Kd = I_1_PID[2];
		current_1_pid.i_limit = I_1_PID[3];
		current_1_pid.output_limit = I_1_PID[4];
		motor_pwm_duty[0] = current_1_pid.incremental_pid(&current_1_pid, motor_current_target[0], current_1.current);
		/* 2 */
		motor_2_pid.Kp = MOTOR_2_PID[0];
		motor_2_pid.Ki = MOTOR_2_PID[1];
		motor_2_pid.Kd = MOTOR_2_PID[2];
		motor_2_pid.i_limit = MOTOR_2_PID[3];
		motor_2_pid.output_limit = MOTOR_2_PID[4];
		motor_current_target[1] = motor_2_pid.incremental_pid(&motor_2_pid, wheel_speed_target[1], encoder_2.wheel_speed);
		current_2_pid.Kp = I_2_PID[0];
		current_2_pid.Ki = I_2_PID[1];
		current_2_pid.Kd = I_2_PID[2];
		current_2_pid.i_limit = I_2_PID[3];
		current_2_pid.output_limit = I_2_PID[4];
		motor_pwm_duty[1] = current_2_pid.incremental_pid(&current_2_pid, motor_current_target[1], current_2.current);
		/* 3 */
		motor_3_pid.Kp = MOTOR_3_PID[0];
		motor_3_pid.Ki = MOTOR_3_PID[1];
		motor_3_pid.Kd = MOTOR_3_PID[2];
		motor_3_pid.i_limit = MOTOR_3_PID[3];
		motor_3_pid.output_limit = MOTOR_3_PID[4];
		motor_current_target[2] = motor_3_pid.incremental_pid(&motor_3_pid, wheel_speed_target[2], encoder_3.wheel_speed);
		current_3_pid.Kp = I_3_PID[0];
		current_3_pid.Ki = I_3_PID[1];
		current_3_pid.Kd = I_3_PID[2];
		current_3_pid.i_limit = I_3_PID[3];
		current_3_pid.output_limit = I_3_PID[4];
		motor_pwm_duty[2] = current_3_pid.incremental_pid(&current_3_pid, motor_current_target[2], current_3.current);
		#endif
		#ifdef FUZZY_SPEED_AND_CURRENT
		float motor_1_err = wheel_speed_target[0];
		float motor_2_err = wheel_speed_target[1];
		float motor_3_err = wheel_speed_target[2];
		float Kp[4] = { MOTOR_PID[0][0], MOTOR_PID[1][0], MOTOR_PID[2][0], MOTOR_PID[3][0] };
		float Ki[4] = { MOTOR_PID[0][1], MOTOR_PID[1][1], MOTOR_PID[2][1], MOTOR_PID[3][1] };
		float Kd[4] = { MOTOR_PID[0][2], MOTOR_PID[1][2], MOTOR_PID[2][2], MOTOR_PID[3][2] };
		float i_limit[4] = { MOTOR_PID[0][3], MOTOR_PID[1][3], MOTOR_PID[2][3], MOTOR_PID[3][3] };
		float output_limit[4] = { MOTOR_PID[0][4], MOTOR_PID[1][4], MOTOR_PID[2][4], MOTOR_PID[3][4] };
		/* 1 */
		motor_1_pid.fuzzy_pid(&motor_1_pid, &motor_1_err, Kp, Ki, Kd, i_limit, output_limit, 1);
		motor_current_target[0] = motor_1_pid.incremental_pid(&motor_1_pid, wheel_speed_target[0], encoder_1.wheel_speed);
		current_1_pid.Kp = I_1_PID[0];
		current_1_pid.Ki = I_1_PID[1];
		current_1_pid.Kd = I_1_PID[2];
		current_1_pid.i_limit = I_1_PID[3];
		current_1_pid.output_limit = I_1_PID[4];
		motor_pwm_duty[0] = current_1_pid.incremental_pid(&current_1_pid, motor_current_target[0], current_1.current);
		/* 2 */
		motor_2_pid.fuzzy_pid(&motor_2_pid, &motor_2_err, Kp, Ki, Kd, i_limit, output_limit, 1);
		motor_current_target[1] = motor_2_pid.incremental_pid(&motor_2_pid, wheel_speed_target[1], encoder_2.wheel_speed);
		current_2_pid.Kp = I_2_PID[0];
		current_2_pid.Ki = I_2_PID[1];
		current_2_pid.Kd = I_2_PID[2];
		current_2_pid.i_limit = I_2_PID[3];
		current_2_pid.output_limit = I_2_PID[4];
		motor_pwm_duty[1] = current_2_pid.incremental_pid(&current_2_pid, motor_current_target[1], current_2.current);
		/* 3 */
		motor_3_pid.fuzzy_pid(&motor_3_pid, &motor_3_err, Kp, Ki, Kd, i_limit, output_limit, 1);
		motor_current_target[2] = motor_3_pid.incremental_pid(&motor_3_pid, wheel_speed_target[2], encoder_3.wheel_speed);
		current_3_pid.Kp = I_3_PID[0];
		current_3_pid.Ki = I_3_PID[1];
		current_3_pid.Kd = I_3_PID[2];
		current_3_pid.i_limit = I_3_PID[3];
		current_3_pid.output_limit = I_3_PID[4];
		motor_pwm_duty[2] = current_3_pid.incremental_pid(&current_3_pid, motor_current_target[2], current_3.current);
		#endif
	}
}

/* ǶȻ PID */
static void angle_rotate_pid_calc(void){
	static uint8 angle_rotate_finsh_num_count = 0;
	
	float err = angle_rotation_yaw_target-rotate_euler_angle_solve.yaw;
	float Kp[4] = { ROTATE_PID[0][0], ROTATE_PID[1][0], ROTATE_PID[2][0], ROTATE_PID[3][0] };
	float Ki[4] = { ROTATE_PID[0][1], ROTATE_PID[1][1], ROTATE_PID[2][1], ROTATE_PID[3][1] };
	float Kd[4] = { ROTATE_PID[0][2], ROTATE_PID[1][2], ROTATE_PID[2][2], ROTATE_PID[3][2] };
	float i_limit[4] = { ROTATE_PID[0][3], ROTATE_PID[1][3], ROTATE_PID[2][3], ROTATE_PID[3][3] };
	float output_limit[4] = { ROTATE_PID[0][4], ROTATE_PID[1][4], ROTATE_PID[2][4], ROTATE_PID[3][4] };
	
	angle_rotate_pid.fuzzy_pid(&angle_rotate_pid, &err, Kp, Ki, Kd, i_limit, output_limit, 1);
	
	angular_speed_target = angle_rotate_pid.positional_pid(&angle_rotate_pid, angle_rotation_yaw_target, rotate_euler_angle_solve.yaw);

	if(fabsf(err) < 4.5)
		angle_rotate_finsh_num_count++;
	if(angle_rotate_finsh_num_count >= 3){
		angle_rotate_finsh_flag = True;
		angle_rotate_finsh_num_count = 0;
	}		
}

/* Բת PID */
static void circle_rotate_pid_calc(void){
	static uint8 circle_rotate_finsh_num_count = 0;
	
	float err = circle_rotation_yaw_target-circle_euler_angle_solve.yaw;
	float Kp[4] = { ROTATE_PID[0][0], ROTATE_PID[1][0], ROTATE_PID[2][0], ROTATE_PID[3][0] };
	float Ki[4] = { ROTATE_PID[0][1], ROTATE_PID[1][1], ROTATE_PID[2][1], ROTATE_PID[3][1] };
	float Kd[4] = { ROTATE_PID[0][2], ROTATE_PID[1][2], ROTATE_PID[2][2], ROTATE_PID[3][2] };
	float i_limit[4] = { ROTATE_PID[0][3], ROTATE_PID[1][3], ROTATE_PID[2][3], ROTATE_PID[3][3] };
	float output_limit[4] = { circle_angular_speed_target[plan_idx], circle_angular_speed_target[plan_idx], circle_angular_speed_target[plan_idx], circle_angular_speed_target[plan_idx] };
	
	circle_rotate_pid.fuzzy_pid(&circle_rotate_pid, &err, Kp, Ki, Kd, i_limit, output_limit, 1);
	
	angular_speed_target = circle_rotate_pid.positional_pid(&circle_rotate_pid, circle_rotation_yaw_target, circle_euler_angle_solve.yaw);

	if(fabsf(err) < 4.5)
		circle_rotate_finsh_num_count++;
	if(circle_rotate_finsh_num_count >= 3){
		circle_rotate_finsh_flag = True;
		circle_rotate_finsh_num_count = 0;
	}		
}

/* BOX X PID */
static void box_track_x_pid_calu(void){
	// AIͷ˲
	box_center_x_karman.karman_filter(&box_center_x_karman, detection_box_center_x);
	
	static uint16 num = 0;	// ƫֵͼ
	
	float detection_box_center_err = box_center_x_karman.value-AI_CAMERA_0_IMAGE_WIDTH/2; 
	move_solve_kind = XY_SPEED_SOLVE;
	
	/* BOX X */
	float X_Kp[4] = { BOX_TRACK_X_PID[0][0], BOX_TRACK_X_PID[1][0], BOX_TRACK_X_PID[2][0], BOX_TRACK_X_PID[3][0] };
	float X_Ki[4] = { BOX_TRACK_X_PID[0][1], BOX_TRACK_X_PID[1][1], BOX_TRACK_X_PID[2][1], BOX_TRACK_X_PID[3][1] };
	float X_Kd[4] = { BOX_TRACK_X_PID[0][2], BOX_TRACK_X_PID[1][2], BOX_TRACK_X_PID[2][2], BOX_TRACK_X_PID[3][2] };
	float X_i_limit[4] = { BOX_TRACK_X_PID[0][3], BOX_TRACK_X_PID[1][3], BOX_TRACK_X_PID[2][3], BOX_TRACK_X_PID[3][3] };
	float X_output_limit[4] = { BOX_TRACK_X_PID[0][4], BOX_TRACK_X_PID[1][4], BOX_TRACK_X_PID[2][4], BOX_TRACK_X_PID[3][4] };

	box_track_x_pid.fuzzy_pid(&box_track_x_pid, &detection_box_center_err, X_Kp, X_Ki, X_Kd, X_i_limit, X_output_limit, 1);
	x_speed_target = box_track_x_pid.positional_pid(&box_track_x_pid, 0, -detection_box_center_err);
	
	// ׷ٵֵΧ
	if(abs(detection_box_center_err) <= 6)
	{
		num++;
	}
	// ׷ٵֵ
	if(abs(detection_box_center_err) <= 4)
	{
		x_speed_target = 0;
		num = 0;
		box_X_finsh_flag = True;
	}
	// ׷ٵֵΧĦʹ޷ƶж
	if(num >= 3)
	{
		x_speed_target = 0;
		num = 0;
		box_X_finsh_flag = True;
	}
}

/* BOX X PID */
static void box_fxxk_x_pid_calu(void){
	// AIͷ˲
	box_center_x_karman.karman_filter(&box_center_x_karman, detection_box_center_x);
	
	static uint16 num = 0;	// ƫֵͼ
	
	float detection_box_center_err = box_center_x_karman.value-AI_CAMERA_0_IMAGE_WIDTH/2; 
	move_solve_kind = XY_SPEED_SOLVE;
	
	/* BOX X */
	float X_Kp[4] = { BOX_FXXK_X_PID[0][0], BOX_FXXK_X_PID[1][0], BOX_FXXK_X_PID[2][0], BOX_FXXK_X_PID[3][0] };
	float X_Ki[4] = { BOX_FXXK_X_PID[0][1], BOX_FXXK_X_PID[1][1], BOX_FXXK_X_PID[2][1], BOX_FXXK_X_PID[3][1] };
	float X_Kd[4] = { BOX_FXXK_X_PID[0][2], BOX_FXXK_X_PID[1][2], BOX_FXXK_X_PID[2][2], BOX_FXXK_X_PID[3][2] };
	float X_i_limit[4] = { BOX_FXXK_X_PID[0][3], BOX_FXXK_X_PID[1][3], BOX_FXXK_X_PID[2][3], BOX_FXXK_X_PID[3][3] };
	float X_output_limit[4] = { BOX_FXXK_X_PID[0][4], BOX_FXXK_X_PID[1][4], BOX_FXXK_X_PID[2][4], BOX_FXXK_X_PID[3][4] };

	box_fxxk_x_pid.fuzzy_pid(&box_fxxk_x_pid, &detection_box_center_err, X_Kp, X_Ki, X_Kd, X_i_limit, X_output_limit, 1);
	x_speed_target = box_fxxk_x_pid.positional_pid(&box_fxxk_x_pid, 0, -detection_box_center_err);
	
	// ׷ٵֵΧ
	if(abs(detection_box_center_err) <= 4)
	{
		num++;
	}
	// ׷ٵֵ
	if(abs(detection_box_center_err) <= 2)
	{
		x_speed_target = 0;
		num = 0;
		box_X_finsh_flag = True;
	}
	// ׷ٵֵΧĦʹ޷ƶж
	if(num >= 3)
	{
		x_speed_target = 0;
		num = 0;
		box_X_finsh_flag = True;
	}
}

/* BOX Y PID */
static void box_track_y_pid_calu(void){
	// AIͷ˲
	box_width_karman.karman_filter(&box_width_karman, detection_box_width);
	
	static uint16 num = 0;	// ƫֵͼ
	
	float detection_box_width_err = box_width_karman.value-detection_box_width_target;
	move_solve_kind = XY_SPEED_SOLVE;
	
	/* BOX Y */
	float Y_Kp[4] = { BOX_TRACK_Y_PID[0][0], BOX_TRACK_Y_PID[1][0], BOX_TRACK_Y_PID[2][0], BOX_TRACK_Y_PID[3][0] };
	float Y_Ki[4] = { BOX_TRACK_Y_PID[0][1], BOX_TRACK_Y_PID[1][1], BOX_TRACK_Y_PID[2][1], BOX_TRACK_Y_PID[3][1] };
	float Y_Kd[4] = { BOX_TRACK_Y_PID[0][2], BOX_TRACK_Y_PID[1][2], BOX_TRACK_Y_PID[2][2], BOX_TRACK_Y_PID[3][2] };
	float Y_i_limit[4] = { BOX_TRACK_Y_PID[0][3], BOX_TRACK_Y_PID[1][3], BOX_TRACK_Y_PID[2][3], BOX_TRACK_Y_PID[3][3] };
	float Y_output_limit[4] = { BOX_TRACK_Y_PID[0][4], BOX_TRACK_Y_PID[1][4], BOX_TRACK_Y_PID[2][4], BOX_TRACK_Y_PID[3][4] };

	box_track_y_pid.fuzzy_pid(&box_track_y_pid, &detection_box_width_err, Y_Kp, Y_Ki, Y_Kd, Y_i_limit, Y_output_limit, 1);
	y_speed_target = box_track_y_pid.positional_pid(&box_track_y_pid, detection_box_width_target, detection_box_width);
	
	// ׷ٵֵΧ
	if(abs(detection_box_width_err) <= 6)
	{
		num++;
	}
	// ׷ٵֵ
	if(abs(detection_box_width_err) <= 4)
	{
		x_speed_target = 0;
		num = 0;
		box_Y_finsh_flag = True;
	}
	// ׷ٵֵΧĦʹ޷ƶж
	if(num >= 3)
	{
		x_speed_target = 0;
		num = 0;
		box_X_finsh_flag = True;
	}
}

/* BOX X/Y PID */
static void box_track_xy_pid_calu(void){
	// AIͷ˲
	box_center_x_karman.karman_filter(&box_center_x_karman, detection_box_center_x);
	box_width_karman.karman_filter(&box_width_karman, detection_box_width);
	
	static uint16 num = 0;	// ƫֵͼ
	
	float detection_box_center_err = box_center_x_karman.value-AI_CAMERA_0_IMAGE_WIDTH/2; 
	float detection_box_width_err = box_width_karman.value-detection_box_width_target;
	move_solve_kind = XY_SPEED_SOLVE;
	
	/* BOX X */
	float X_Kp[4] = { BOX_TRACK_X_PID[0][0], BOX_TRACK_X_PID[1][0], BOX_TRACK_X_PID[2][0], BOX_TRACK_X_PID[3][0] };
	float X_Ki[4] = { BOX_TRACK_X_PID[0][1], BOX_TRACK_X_PID[1][1], BOX_TRACK_X_PID[2][1], BOX_TRACK_X_PID[3][1] };
	float X_Kd[4] = { BOX_TRACK_X_PID[0][2], BOX_TRACK_X_PID[1][2], BOX_TRACK_X_PID[2][2], BOX_TRACK_X_PID[3][2] };
	float X_i_limit[4] = { BOX_TRACK_X_PID[0][3], BOX_TRACK_X_PID[1][3], BOX_TRACK_X_PID[2][3], BOX_TRACK_X_PID[3][3] };
	float X_output_limit[4] = { BOX_TRACK_X_PID[0][4], BOX_TRACK_X_PID[1][4], BOX_TRACK_X_PID[2][4], BOX_TRACK_X_PID[3][4] };
	/* BOX Y */
	float Y_Kp[4] = { BOX_TRACK_Y_PID[0][0], BOX_TRACK_Y_PID[1][0], BOX_TRACK_Y_PID[2][0], BOX_TRACK_Y_PID[3][0] };
	float Y_Ki[4] = { BOX_TRACK_Y_PID[0][1], BOX_TRACK_Y_PID[1][1], BOX_TRACK_Y_PID[2][1], BOX_TRACK_Y_PID[3][1] };
	float Y_Kd[4] = { BOX_TRACK_Y_PID[0][2], BOX_TRACK_Y_PID[1][2], BOX_TRACK_Y_PID[2][2], BOX_TRACK_Y_PID[3][2] };
	float Y_i_limit[4] = { BOX_TRACK_Y_PID[0][3], BOX_TRACK_Y_PID[1][3], BOX_TRACK_Y_PID[2][3], BOX_TRACK_Y_PID[3][3] };
	float Y_output_limit[4] = { BOX_TRACK_Y_PID[0][4], BOX_TRACK_Y_PID[1][4], BOX_TRACK_Y_PID[2][4], BOX_TRACK_Y_PID[3][4] };

	box_track_x_pid.fuzzy_pid(&box_track_x_pid, &detection_box_center_err, X_Kp, X_Ki, X_Kd, X_i_limit, X_output_limit, 1);
	x_speed_target = box_track_x_pid.positional_pid(&box_track_x_pid, 0, -detection_box_center_err);
	if(abs(detection_box_center_err) < box_y_track_enable_center_x_limit){
		box_track_y_pid.fuzzy_pid(&box_track_y_pid, &detection_box_width_err, Y_Kp, Y_Ki, Y_Kd, Y_i_limit, Y_output_limit, 1);
		y_speed_target = box_track_y_pid.positional_pid(&box_track_y_pid, detection_box_width_target, detection_box_width);
	}
	else
		y_speed_target = 0;
		
	
//	if(x_speed_target/x_speed_rate >= 6)
//		angular_speed_target = 6;
//	else
//		angular_speed_target = x_speed_target/x_speed_rate;
	
	// ׷ٵֵΧ
	if(abs(detection_box_width_err) <= 8 && abs(detection_box_center_err) <= 8)
		num++;
	// ֵΧ
//	if(abs(detection_box_width_err) > 6 && abs(detection_box_center_err) > 6)
//		num = 0;
	// ׷ٵֵ
	if(abs(detection_box_width_err) <= 4 && abs(detection_box_center_err) <= 4)
	{
		x_speed_target = 0;
		y_speed_target = 0;
		angular_speed_target = 0;
		num = 0;
		box_XY_finsh_flag = True;
	}
	// ׷ٵֵΧĦʹ޷ƶж
	if(num >= 5)
	{
		x_speed_target = 0;
		y_speed_target = 0;
		angular_speed_target = 0;
		num = 0;
		box_XY_finsh_flag = True;
	}
	angular_speed_target = 0;
}

/* 
	X 
	˵
	_path_x_speed_target_ Ŀٶ
	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){
	// û٣
	if(speed_slow_change_enable_flag == True && fabsf(x_speed_target) < fabsf(_path_x_speed_target_)){
		// 
		x_speed_target += (_path_x_speed_target_-displacement_solve.x_speed)*slow_change_rate;
	}	
	// رջ
	else
		x_speed_target = _path_x_speed_target_;
}

/* 
	Y 
	˵
	_path_y_speed_target_ Ŀٶ
	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){
	// û٣
	if(speed_slow_change_enable_flag == True && fabsf(y_speed_target) < fabsf(_path_y_speed_target_)){
		// 
		y_speed_target += (_path_y_speed_target_-displacement_solve.y_speed)*slow_change_rate;
	}	
	// رջ
	else
		y_speed_target = _path_y_speed_target_;
}

