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

/* ඨ */	
DOG_MOTOR motor_1;						// 1
DOG_MOTOR motor_2;						// 2
DOG_MOTOR motor_3;						// 3
DOG_ENCODER encoder_1;					// 1
DOG_ENCODER encoder_2;					// 2
DOG_ENCODER encoder_3;					// 3
DOG_CURRENT current_1;					// 1
DOG_CURRENT current_2;					// 2
DOG_CURRENT current_3;					// 3
DOG_VOLTAGE gray_sensor;				// Ҷȴ
DOG_VOLTAGE bat_voltage;				// صѹ
DOG_KARMAN_FILTER current_1_karman;		// 1 ˲ 
DOG_KARMAN_FILTER current_2_karman;		// 2 ˲ 
DOG_KARMAN_FILTER current_3_karman;		// 3 ˲ 
DOG_KARMAN_FILTER path_gyro_karman;		// · ˲ 
DOG_KARMAN_FILTER box_center_x_karman;	// еX ˲ 
DOG_KARMAN_FILTER box_width_karman;		// ӿ ˲ 
DOG_KARMAN_FILTER displacement_solve_gyro_karman;	// λƽ ˲
DOG_PID motor_1_pid;					// PID1
DOG_PID motor_2_pid;					// PID2
DOG_PID motor_3_pid;					// PID3
DOG_PID current_1_pid;					// PID1
DOG_PID current_2_pid;					// PID2
DOG_PID current_3_pid;					// PID3
DOG_PID path_pid;						// ·PID
DOG_PID path_gyroz_pid;					// ·PID
DOG_PID angle_rotate_pid;				// ǶȻתPID
DOG_PID circle_rotate_pid;				// ԲתPID
DOG_PID box_track_x_pid;				// ӶλX PID
DOG_PID box_fxxk_x_pid;					// X PID
DOG_PID box_track_y_pid;				// ӶλY PID
DOG_VOFA wireless_vofa;					// ߴVOFA
DOG_IMU imu660ra;						// IMU660RA
DOG_SOLVE euler_angle_solve;			// ŷǽ
DOG_SOLVE rotate_euler_angle_solve;		// תŷǽ
DOG_SOLVE circle_euler_angle_solve;		// Բŷǽ
DOG_SOLVE box_euler_angle_solve;		// ŷǽ
DOG_SOLVE chassis_solve;				// ̽
DOG_SOLVE displacement_solve;			// λƽ
DOG_TIMER zebra_path_timer;				// ߼ʱ
DOG_TIMER motor_debug_timer;       	 	// Լʱ
DOG_TIMER rwr_timer;					// RWRʱ
DOG_CV dog_cv;							// Ӿ
DOG_PATH dog_path;						// ѭ
DOG_RWR dog_rwr;						// RWR

/* ʹܱ־λ */
_bool_ circle_enable_flag = True;			// Բ ʹܱ־λ
_bool_ zebra_enable_flag = True;			//  ʹܱ־λ
_bool_ ai_camera_0_enable_flag = True;		// AI0 ʹܱ־λ
_bool_ ai_camera_1_enable_flag = True;		// AI1 ʹܱ־λ
_bool_ ai_camera_2_enable_flag = True;		// AI2 ʹܱ־λ
uint8 supplement_lamp_enable_flag = 0;		//  ʹܱ־λ

/* ɱ־λ */
_bool_ angle_rotate_finsh_flag = False;		// ǶȻתɱ־λ
_bool_ circle_rotate_finsh_flag = False;	// Բתɱ־λ
_bool_ box_X_finsh_flag = False;			// Xλɱ־λ
_bool_ box_Y_finsh_flag = False;			// Yλɱ־λ
_bool_ box_XY_finsh_flag = False;			// XYλɱ־λ

/* ͨ־λ */
_bool_ out_last_box_circle_flag = True;	// һԲڱ־λ

/* ʼ־λ */
_bool_ ai_camera_0_init_flag = False;	// AIͷ0 ʼ־λ

/* ȫֱ */
/* ͷ */
uint16 exp_time = 512;					//ͷعʱ
/* ȡ */
int16 path_err;							// ·
uint16 path_start = 95;					// ·ʼ߶
uint16 path_end = 30;					// ·߽߶
uint16 side_extract_start_y = 80;		// ȡʼ߶
uint16 side_extract_end_y = 20;			// ȡ߶
uint16 prediction_point = 30;			// Ԥ߶ȣ꽫Ϊһ֡

uint16 control_point[4] = {60, 70, 70, 55};	// Ƶ߶ȣ0лٿƵ㣻1/2Ƶ/ԶƵ㣻3·ȡƵ㣩

uint16 longest_white_control_point = 73;	// пƵ
/* Բ */
uint16 circle_check_y = 45;				// Բ߸߶ȣԽԽ
uint16 side_x_delta_range[2] = {3, 15};	// XֵֵΧС
float circle_in_distance = 0;			// Բʼ·
float circle_out_distance = 0;			// Բʼ·
float circle_in_distance_limit = 150;	// Բʼ·ֵ·̴ڸֵܳ
float circle_out_distance_limit = 100;	// Բʼ·ֵ·̴ڸֵ뻷
/*  */
uint16 zebra_check_y = 1;				// ߼߸߶
uint16 zebra_stop_distance = 0;			// ͣ
/*  */
_path_state_ path_state = common_path;		// ״̬
_path_state_ last_path_state = common_path;	// һ״̬
/*  */
_control_kind_ control_kind = Stop;				// 
float wheel_speed_target[3] = {0, 0, 0};		// Ŀٶ
float motor_current_target[3] = {0, 0, 0};		// Ŀֵ
float motor_pwm_duty[3] = {0, 0, 0};			// PWMռձ
_move_solve_kind_ move_solve_kind;				// ˶
float linear_speed_target;						// Ŀٶ
float translation_yaw_target;					// ĿƽǶ
float x_speed_target;							// Ŀxٶ
float y_speed_target;							// Ŀyٶ
float angular_speed_target;						// Ŀתٶ
float angle_rotation_yaw_target;				// ĿǶȻתǶ
float circle_rotation_yaw_target;				// ĿԲתǶ
float data_1;									// ˶ѧ1ٶ/Xٶȣ
float data_2;									// ˶ѧ2/Yٶȣ

float x_speed_rate = 2.3;						// ѭxٶȱĿѭxٶ/Ŀѭתٶȣ

uint16 auto_control_point_normalize_range[2] = {100, 200};	// ̬ǰհһΧ
/*  */
uint8 detection_box_width;						// ʶ
uint8 detection_box_width_limit = 26;			// ʶֵڴֵſԽ׷ģʽ
uint8 detection_box_width_target = 81;			// ʶĿ
uint8 detection_box_height;						// ʶ߶ȶ
uint8 detection_box_height_limit = 30;			// ʶ߶ֵڴֵſԽ׷ģʽ
int16 detection_box_center_x;					// ʶĺ
uint16 detection_box_center_x_limit = 65;		// ʶĺֵֵΧڲſԽ׷ģʽ
uint8 box_y_track_enable_center_x_limit = 40;	// Yλʹܵĺֵ
_ai_camera_detection_result_ detection_result;	// ʶ
_ai_camera_detection_result_ detection_result_list[BOX_NUM_MAX] = {0};	// ʶб
uint8 detection_result_num = 0;					// ʶб
uint16 rectificate_weight[4] = {1 ,5 ,55 ,85};	// ȨأߡMT9V03X_W/8 ߡ2*MT9V03X_W/8 ߡ3*MT9V03X_W/8 ߡ4*MT9V03X_W/8
uint32 sum_weight = 0;							// Ȩ
uint16 symmetry_rectificate_start_y = 99;		// ԳƷͼʼ߶
uint16 symmetry_rectificate_end_y = 40;			// ԳƷͼ߶
float sum_weight_normalization = 0;				// Ȩ͹һ	
float sum_weight_normalization_limit[2] = {0.93, 0.80};	// Ȩ͹һֵ
float frame_white_num_normalization[2] = {0};			// ԳƷͼұ߿׵һ
float frame_white_num_normalization_limit = 0.25;		// ԳƷͼұ߿׵һֵ
uint16 frame_offset = 15;								// ͼ߿ƫƫҿƫֹʳ޷ʹöԳƷн 
float last_box_world_x[BOX_NUM_MAX] = {0};				// һʼX
float last_box_world_y[BOX_NUM_MAX] = {0};				// һʼY
uint8 box_num = 0;										// Ѿƹ
float box_distance = 50;								// Ӽࣨcm
/* ٶ/Ƕ/ʱ */
uint8 plan_idx = 0;									// ɵߣٶ𽥱죩			
float path_y_speed_target[4] = {180, 150, 190, 210};		// ĿѭYٶ
float circle_y_speed_target[4] = {180, 150, 180, 190};		// ĿԲYٶ
float path_x_speed_enable_y_speed_rate = 0.8;		// ĿѭYٶȱʵʱĿٶ/Ŀٶ ڸñſXٶȣ
float circle_x_speed_enable_y_speed_rate = 0.8;		// ĿԲYٶȱʵʱĿٶ/Ŀٶ ڸñſXٶȣ
float circle_angular_speed_target[4] = {55, 50, 50, 55 };	// 뻷Ŀٶ
float circle_angle_target[2] = {70, 65};			// 뻷ĿתǶ
float box_x_speed_target = 75;						// ĿXٶ
float box_x_angular_speed_rate = 0.33;				//  תٶ/Xٶ Խת뾶ԽС
float box_fxxk_y_speed_target[4] = {110, 120, 120, 120};	// YٶĿֵ

/*    PID     			Kp     Ki     Kd     ޷     ޷     Kd */
// 
#ifdef FUZZY_SPEED_AND_CURRENT
float MOTOR_RANGE[3] = { 0.0,   10.0, 160.0 };		// ٶ
float MOTOR_PID[4][5] =  {{ 0.050,    0.010,    0.0,  0,         13},
						  { 0.070,    0.020,    0.0,  0,         13},
						  { 0.040,    0.009,    0.0,  0,         13},
						  { 0.040,    0.003,    0.0,  0,         13}};
#endif
#ifdef SPEED_AND_CURRENT
float MOTOR_1_PID[5] = 	  { 0.0263,   0.0050,   0,  500,         13 };		// Ӳ
float MOTOR_2_PID[5] =    { 0.0422,   0.0027,   0,  500,         13 };		// 
float MOTOR_3_PID[5] =    { 0.0422,   0.0027,   0,  500,         13 };		// 

#endif
#ifdef SPEED
float MOTOR_1_PID[5] = 	  { 8.8,   6.1,   0.98,  500,         9000 };
float MOTOR_2_PID[5] =    { 8.8,   6.1,   0.98,  500,         9000 };
float MOTOR_3_PID[5] =    { 8.8,   6.1,   0.98,  500,         9000 };
#endif

// 
float I_1_PID[5] = 		  { 60,     170,     0,     500,         9000 };
float I_2_PID[5] = 		  { 60,     170,     0,     500,         9000 };
float I_3_PID[5] = 		  { 60,     170,     0,     500,         9000 };
// ѭ
float PATH_RANGE[2][3] = {{ 8.0,   30.0,  60.0 },		// ѭ
						  { 40.0,  200.0, 400.0 }};		// ٶ

float GYRO_RANGE[2][3] = {{ 40.0,  200.0,  400.0 },		// ѭ
						  { 12.0,   25.0,  50.0}};		// ٶ

//float PATH_PID[4][6] =  {{ 2.3, 0,     0.0,   2,           75,          0.25},
//						   { 2.0, 0,     0.0,   2,          75,           0.19},
//						   { 1.4, 0,     0.0,   2,           75,          0.16},
//						   { 0.6, 0,     0.0,   2,           75,          0.13}};
						  
float PATH_PID[4][4][6] ={{{ 1.165, 0,   0.0,   2,           75,          0.149},		/*  190*/
						   { 1.120, 0,   0.0,   2,           75,          0.149},
						   { 1.200, 0,   0.0,   2,           75,          0.149},
						   { 1.100, 0,   0.0,   2,           75,          0.149}},

						  {{ 2.725, 0,   0.0,   2,           75,          0.200},		/*  190*/
						   { 2.100, 0,   0.0,   2,           75,          0.150},
						   { 1.600, 0,   0.0,   2,           75,          0.095},
						   { 0.635, 0,   0.0,   2,           75,          0.080}},
						  
						  {{ 2.725, 0,   0.0,   2,           75,          0.200},		/*  190*/
						   { 2.100, 0,   0.0,   2,           75,          0.150},
						   { 1.600, 0,   0.0,   2,           75,          0.095},
						   { 0.635, 0,   0.0,   2,           75,          0.080}},

						  {{ 2.725, 0,   0.0,   2,           75,          0.200},		/*  190*/
						   { 2.100, 0,   0.0,   2,           75,          0.150},
						   { 1.600, 0,   0.0,   2,           75,          0.095},
						   { 0.635, 0,   0.0,   2,           75,          0.080}}};

// ת
float ROTATE_RANGE[3] =   { 15.0,  50.0,  120.0 };		// ǶȻ						  
float ROTATE_PID[4][5] = {{ 0.8,   0,     1.54,  20,          45 },
						  { 1.35,  0,     1.20,  20,          75 },
						  { 1.6,   0,     0.84,  20,          105 },
						  { 2.1,   0,     0.58,  20,	      130 }};							  
// BOX X
float BOX_TRACK_X_RANGE[3] =    { 10.0,  20.0,  40.0 };		// BOX X	
float BOX_TRACK_X_PID[4][5] =  {{ 0.45,  0,     6.27,  2,           35 },
								{ 0.75,  0,     6.00,  2,           60 },
								{ 1.5,   0,     4.37,  2,           90 },
								{ 2.1,   0,     2.47,  2,           130 }};	
float BOX_FXXK_X_PID[4][5] =   {{ 0.35,  0,     2.27,  2,           35 },
								{ 0.55,  0,     2.00,  2,           60 },
								{ 0.45,  0,     1.37,  2,           60 },
								{ 0.25,  0,     0.47,  2,           60 }};	

// BOX Y
float BOX_TRACK_Y_RANGE[3] =    { 10.0,  20.0,  45.0 };		// BOX Y	
float BOX_TRACK_Y_PID[4][5] =  {{ 0.45,  0,     6.27,  2,           35 },
								{ 0.85,  0,     6.00,  2,           60 },
								{ 1.5,   0,     4.37,  2,           90 },
								{ 2.1,   0,     2.47,  2,           130 }};
 
 
/* KARMAN˲		   	 Q     R     QԽСԽƽ   RԽСԽӽ(Խ)*/
float I_KARMAN[2] = 	   { 0.01, 0.1};
float PATH_GYRO_KARMAN[2] ={ 0.01, 0.1};
float BOX_CENTER_X_KARMAN[2] ={ 0.01, 0.1};
float BOX_WIDTH_KARMAN[2] ={ 0.01, 0.1};

/* ģPID  */
// ٶȻ
_fuzzy_subset_ motor_fuzzy_rules[8][8] = 
{  
{7,7,7,6,7,7,7, 7},
{7,7,7,5,7,7,7, 7},
{7,7,7,4,7,7,7, 7},
{6,5,4,3,4,5,6, 7},
{7,7,7,4,7,7,7, 7},
{7,7,7,5,7,7,7, 7},
{7,7,7,6,7,7,7, 7},
{7,7,7,7,7,7,7,7}};
// 
_fuzzy_subset_ path_fuzzy_rules[8][8] =     
{  
{6,6,6,6,6,6,6, 7},
{6,5,5,4,5,5,6, 7},
{6,5,4,3,4,5,6, 7},
{6,5,4,3,4,5,6, 7},
{6,5,4,3,4,5,6, 7},
{6,5,5,4,5,5,6, 7},
{6,6,6,6,6,6,6, 7},

{7,7,7,7,7,7,7,7}};
// 
_fuzzy_subset_ gyro_fuzzy_rules[8][8] = 
{  
{7,7,7,6,7,7,7, 7},
{7,7,7,5,7,7,7, 7},
{7,7,7,4,7,7,7, 7},
{6,5,4,3,4,5,6, 7},
{7,7,7,4,7,7,7, 7},
{7,7,7,5,7,7,7, 7},
{7,7,7,6,7,7,7, 7},

{7,7,7,7,7,7,7,7}};
// X/Yλ
_fuzzy_subset_ xy_fuzzy_rules[8][8] =     
{  
{6,6,5,5,5,6,6, 7},
{6,5,4,4,4,5,6, 7},
{5,4,3,3,3,4,5, 7},
{6,5,4,3,4,5,6, 7},
{5,4,3,3,3,4,5, 7},
{6,5,4,4,4,5,6, 7},
{6,6,5,5,5,6,6, 7},

{7,7,7,7,7,7,7,7}};
// ǶȻ
_fuzzy_subset_ angle_rotate_fuzzy_rules[8][8] =     
{  
{6,6,5,5,5,6,6, 7},
{6,5,4,4,4,5,6, 7},
{5,4,3,3,3,4,5, 7},
{6,5,4,3,4,5,6, 7},
{5,4,3,3,3,4,5, 7},
{6,5,4,4,4,5,6, 7},
{6,6,5,5,5,6,6, 7},

{7,7,7,7,7,7,7,7}};
// ԲǶȻ
_fuzzy_subset_ circle_rotate_fuzzy_rules[8][8] =     
{  
{6,6,5,5,5,6,6, 7},
{6,5,4,4,4,5,6, 7},
{5,4,3,3,3,4,5, 7},
{6,5,4,3,4,5,6, 7},
{5,4,3,3,3,4,5, 7},
{6,5,4,4,4,5,6, 7},
{6,6,5,5,5,6,6, 7},

{7,7,7,7,7,7,7,7}};

/* ־λʼ */	
void flag_init(void){
	// ʹܱ־λ
	euler_angle_solve.solve_flag = False;
	rotate_euler_angle_solve.solve_flag = False;
	rotate_euler_angle_solve.encoder_merge_enable_flag = False;
	circle_euler_angle_solve.solve_flag = False;
	circle_euler_angle_solve.encoder_merge_enable_flag = False;
	box_euler_angle_solve.solve_flag = False;
	box_euler_angle_solve.encoder_merge_enable_flag = False;
	chassis_solve.solve_flag = False;
	displacement_solve.solve_flag = False;
	// ʱʹܱ־λ
	zebra_path_timer.ticking_flag = False;
	rwr_timer.ticking_flag = False;
	// ɱ־λ
	angle_rotate_finsh_flag = False;	// ǶȻתɱ־λ
	circle_rotate_finsh_flag = False;	// Բתɱ־λ
	box_X_finsh_flag = False;			// Xλɱ־λ
	box_Y_finsh_flag = False;			// Yλɱ־λ
	box_XY_finsh_flag = False;			// XYλɱ־λ
	/* ͨ־λ */
	out_last_box_circle_flag = True;	// һԲڱ־λ
}
	
/* ʼ */	
void variable_init(void){
	zebra_stop_distance = 0;			// ͣ
	longest_white_control_point = 73;	// пƵ߶
	path_state = common_path;			// ״̬
	last_path_state = common_path;		// һ״̬
	control_kind = Stop;				// 
	memset(wheel_speed_target, 0, sizeof(wheel_speed_target));		// Ŀٶ
	memset(motor_current_target, 0, sizeof(motor_current_target));	// Ŀֵ
	memset(motor_pwm_duty, 0, sizeof(motor_pwm_duty));				// PWMռձ
	sum_weight_normalization = 0;		// Ȩ͹һ	
	memset(last_box_world_x, 0, sizeof(last_box_world_x));			// һʼX    
	memset(last_box_world_y, 0, sizeof(last_box_world_y));			// һʼY
	box_num = 0;						// Ѿƹ
	linear_speed_target = 0;			// Ŀٶ
	translation_yaw_target = 0;			// ĿƽǶ
	x_speed_target = 0;					// Ŀxٶ    
	y_speed_target = 0;					// Ŀyٶ
	angular_speed_target = 0;			// Ŀתٶ
	angle_rotation_yaw_target = 0;		// ĿǶȻתǶ
	circle_rotation_yaw_target = 0;		// ĿԲתǶ
	data_1 = 0;							// ˶ѧ1ٶ/Xٶȣ
	data_2 = 0;							// ˶ѧ2/Yٶȣ
	circle_in_distance = 0;			// Բʼ·
	circle_out_distance = 0;		// Բʼ·
	dog_rwr.radar_scanning_enable_flag = False;
	dog_rwr.radar_tracking_enable_flag = False;
	dog_rwr.missile_launch_enable_flag = False;	
	
	dog_path.mid_x = MT9V03X_W/2;					// ̬
	memset(dog_path.path,0,sizeof(dog_path.path));	// ·xy
	dog_path.prediction_point = prediction_point;	// Ԥ߶ȣ꽫Ϊһ֡
	dog_path.longest_white_col_x = MT9V03X_W/2;		// X
	memset(dog_path.L_side,0,sizeof(dog_path.L_side));// 
	memset(dog_path.R_side,0,sizeof(dog_path.R_side));// ұ
	dog_path.L_side_point_num = 0;			// ߵ
	dog_path.R_side_point_num = 0;			// ұߵ
	dog_path.L_frame_point_num = 0;			// ߿
	dog_path.R_frame_point_num = 0;			// ұ߿
	memset(dog_path.L_bend_point,0,sizeof(dog_path.L_bend_point));// 
	memset(dog_path.R_bend_point,0,sizeof(dog_path.R_bend_point));// ұ
	dog_path.L_bend_point_num = 0;			// 
	dog_path.R_bend_point_num = 0;			// ұ
	memset(dog_path.path_width,0,sizeof(dog_path.path_width));	// 
	dog_path.point_distance = 10;			// յ/
	dog_path.bend_point_angle_min = 0;		// СǶֵ
	dog_path.bend_point_angle_max = 170;	// Ƕֵ
}

/* ˵ɱȡ */
void menu_changea_parameter_read(void){
	flash_read_page_to_buffer(127, FLASH_PAGE_3);

	uint16_t read_buffer_idx = 0;
	
	exp_time = flash_union_buffer[read_buffer_idx].uint16_type;
	read_buffer_idx++;
	detection_box_width_limit = flash_union_buffer[read_buffer_idx].uint8_type;
	read_buffer_idx++;
	detection_box_width_target = flash_union_buffer[read_buffer_idx].uint8_type;
	read_buffer_idx++;
	detection_box_center_x_limit = flash_union_buffer[read_buffer_idx].uint16_type;
	read_buffer_idx++;
	box_x_speed_target = flash_union_buffer[read_buffer_idx].float_type;
	read_buffer_idx++;
	box_x_angular_speed_rate = flash_union_buffer[read_buffer_idx].float_type;
	read_buffer_idx++;
	for(uint8_t i = 0;i < 4;i++){
		box_fxxk_y_speed_target[i] = flash_union_buffer[read_buffer_idx].float_type;
		read_buffer_idx++;
	}
	box_distance = flash_union_buffer[read_buffer_idx].float_type;
	read_buffer_idx++;
	for(uint8_t i = 0;i < 4;i++){
		path_y_speed_target[i] = flash_union_buffer[read_buffer_idx].float_type;
		read_buffer_idx++;
	}
	path_start = flash_union_buffer[read_buffer_idx].uint16_type;
	read_buffer_idx++;
	path_end = flash_union_buffer[read_buffer_idx].uint16_type;
	read_buffer_idx++;
	for(uint8_t i = 0;i < 4;i++){
		control_point[i] = flash_union_buffer[read_buffer_idx].uint16_type;
		read_buffer_idx++;
	}
	prediction_point = flash_union_buffer[read_buffer_idx].uint16_type;
	read_buffer_idx++;
	x_speed_rate = flash_union_buffer[read_buffer_idx].float_type;
	read_buffer_idx++;
	auto_control_point_normalize_range[0] = flash_union_buffer[read_buffer_idx].uint16_type;
	read_buffer_idx++;
	auto_control_point_normalize_range[1] = flash_union_buffer[read_buffer_idx].uint16_type;
	read_buffer_idx++;
	for(uint8_t i = 0;i < 4;i++){
		circle_y_speed_target[i] = flash_union_buffer[read_buffer_idx].float_type;
		read_buffer_idx++;
	}
	for(uint8_t i = 0;i < 4;i++){
		circle_angular_speed_target[i] = flash_union_buffer[read_buffer_idx].float_type;
		read_buffer_idx++;
	}
	circle_check_y = flash_union_buffer[read_buffer_idx].uint16_type;
	read_buffer_idx++;
	circle_angle_target[0] = flash_union_buffer[read_buffer_idx].float_type;
	read_buffer_idx++;
	circle_angle_target[1] = flash_union_buffer[read_buffer_idx].float_type;
	read_buffer_idx++;
	side_extract_start_y = flash_union_buffer[read_buffer_idx].uint16_type;
	read_buffer_idx++;
	side_extract_end_y = flash_union_buffer[read_buffer_idx].uint16_type;
	read_buffer_idx++;
	circle_in_distance_limit = flash_union_buffer[read_buffer_idx].float_type;
	read_buffer_idx++;
	circle_out_distance_limit = flash_union_buffer[read_buffer_idx].float_type;
	read_buffer_idx++;

	#ifndef FUZZY_SPEED_AND_CURRENT
	for(uint8_t i = 0;i < 5;i++){
		#ifdef SPEED_AND_CURRENT
		MOTOR_1_PID[i] = flash_union_buffer[read_buffer_idx].float_type;
		read_buffer_idx++;
		I_1_PID[i] = flash_union_buffer[read_buffer_idx].float_type;
		read_buffer_idx++;
		#endif
		#ifdef SPEED
		MOTOR_1_PID[i] = flash_union_buffer[read_buffer_idx].float_type;
		read_buffer_idx++;
		#endif
	}
	for(uint8_t i = 0;i < 5;i++){
		#ifdef SPEED_AND_CURRENT
		MOTOR_2_PID[i] = flash_union_buffer[read_buffer_idx].float_type;
		read_buffer_idx++;
		I_2_PID[i] = flash_union_buffer[read_buffer_idx].float_type;
		read_buffer_idx++;
		#endif
		#ifdef SPEED
		MOTOR_2_PID[i] = flash_union_buffer[read_buffer_idx].float_type;
		read_buffer_idx++;
		#endif
	}
	for(uint8_t i = 0;i < 5;i++){
		#ifdef SPEED_AND_CURRENT
		MOTOR_3_PID[i] = flash_union_buffer[read_buffer_idx].float_type;
		read_buffer_idx++;
		I_3_PID[i] = flash_union_buffer[read_buffer_idx].float_type;
		read_buffer_idx++;
		#endif
		#ifdef SPEED
		MOTOR_3_PID[i] = flash_union_buffer[read_buffer_idx].float_type;
		read_buffer_idx++;
		#endif
	}
	#else
	for(uint8_t i = 0;i < 4;i++){
		for(uint8_t j = 0;j < 5;j++){
			MOTOR_PID[i][j] = flash_union_buffer[read_buffer_idx].float_type;
			read_buffer_idx++;
		}
	}
	for(uint8_t i = 0;i < 5;i++){
		I_1_PID[i] = flash_union_buffer[read_buffer_idx].float_type;
		read_buffer_idx++;
	}
	for(uint8_t i = 0;i < 5;i++){
		I_2_PID[i] = flash_union_buffer[read_buffer_idx].float_type;
		read_buffer_idx++;
	}
	for(uint8_t i = 0;i < 5;i++){
		I_3_PID[i] = flash_union_buffer[read_buffer_idx].float_type;
		read_buffer_idx++;
	}
	#endif

	for(uint8_t i = 0;i < 4;i++){
		for(uint8_t j = 0;j < 4;j++){
			for(uint8_t k = 0;k < 6;k++){
				PATH_PID[i][j][k] = flash_union_buffer[read_buffer_idx].float_type;
				read_buffer_idx++;
			}
		}
	}
}

/* ˵ɱд */
void menu_changea_parameter_write(void){
	flash_buffer_clear();

	uint16_t read_buffer_idx = 0;
	
	flash_union_buffer[read_buffer_idx].uint16_type = exp_time;
	read_buffer_idx++;
	flash_union_buffer[read_buffer_idx].uint8_type = detection_box_width_limit;
	read_buffer_idx++;
	flash_union_buffer[read_buffer_idx].uint8_type = detection_box_width_target;
	read_buffer_idx++;
	flash_union_buffer[read_buffer_idx].uint16_type = detection_box_center_x_limit;
	read_buffer_idx++;
	flash_union_buffer[read_buffer_idx].float_type = box_x_speed_target;
	read_buffer_idx++;
	flash_union_buffer[read_buffer_idx].float_type = box_x_angular_speed_rate;
	read_buffer_idx++;
	for(uint8_t i = 0;i < 4;i++){
		flash_union_buffer[read_buffer_idx].float_type = box_fxxk_y_speed_target[i];
		read_buffer_idx++;
	}
	flash_union_buffer[read_buffer_idx].float_type = box_distance;
	read_buffer_idx++;
	for(uint8_t i = 0;i < 4;i++){
		flash_union_buffer[read_buffer_idx].float_type = path_y_speed_target[i];
		read_buffer_idx++;
	}
	flash_union_buffer[read_buffer_idx].uint16_type = path_start;
	read_buffer_idx++;
	flash_union_buffer[read_buffer_idx].uint16_type = path_end;
	read_buffer_idx++;
	for(uint8_t i = 0;i < 4;i++){
		flash_union_buffer[read_buffer_idx].uint16_type = control_point[i];
		read_buffer_idx++;
	}
	flash_union_buffer[read_buffer_idx].uint16_type = prediction_point;
	read_buffer_idx++;
	flash_union_buffer[read_buffer_idx].float_type = x_speed_rate;
	read_buffer_idx++;
	flash_union_buffer[read_buffer_idx].uint16_type = auto_control_point_normalize_range[0];
	read_buffer_idx++;
	flash_union_buffer[read_buffer_idx].uint16_type = auto_control_point_normalize_range[1];
	read_buffer_idx++;
	for(uint8_t i = 0;i < 4;i++){
		flash_union_buffer[read_buffer_idx].float_type = circle_y_speed_target[i];
		read_buffer_idx++;
	}
	for(uint8_t i = 0;i < 4;i++){
		flash_union_buffer[read_buffer_idx].float_type = circle_angular_speed_target[i];
		read_buffer_idx++;
	}
	flash_union_buffer[read_buffer_idx].uint16_type = circle_check_y;
	read_buffer_idx++;
	flash_union_buffer[read_buffer_idx].float_type = circle_angle_target[0];
	read_buffer_idx++;
	flash_union_buffer[read_buffer_idx].float_type = circle_angle_target[1];
	read_buffer_idx++;
	flash_union_buffer[read_buffer_idx].uint16_type = side_extract_start_y;
	read_buffer_idx++;
	flash_union_buffer[read_buffer_idx].uint16_type = side_extract_end_y;
	read_buffer_idx++;
	flash_union_buffer[read_buffer_idx].float_type = circle_in_distance_limit;
	read_buffer_idx++;
	flash_union_buffer[read_buffer_idx].float_type = circle_out_distance_limit;
	read_buffer_idx++;

	#ifndef FUZZY_SPEED_AND_CURRENT
	for(uint8_t i = 0;i < 5;i++){
		#ifdef SPEED_AND_CURRENT
		flash_union_buffer[read_buffer_idx].float_type = MOTOR_1_PID[i];
		read_buffer_idx++;
		flash_union_buffer[read_buffer_idx].float_type = I_1_PID[i];
		read_buffer_idx++;
		#endif
		#ifdef SPEED
		flash_union_buffer[read_buffer_idx].float_type = MOTOR_1_PID[i];
		read_buffer_idx++;
		#endif
	}
	for(uint8_t i = 0;i < 5;i++){
		#ifdef SPEED_AND_CURRENT
		flash_union_buffer[read_buffer_idx].float_type = MOTOR_2_PID[i];
		read_buffer_idx++;
		flash_union_buffer[read_buffer_idx].float_type = I_2_PID[i];
		read_buffer_idx++;
		#endif
		#ifdef SPEED
		flash_union_buffer[read_buffer_idx].float_type = MOTOR_2_PID[i];
		read_buffer_idx++;
		#endif
	}
	for(uint8_t i = 0;i < 5;i++){
		#ifdef SPEED_AND_CURRENT
		flash_union_buffer[read_buffer_idx].float_type = MOTOR_3_PID[i];
		read_buffer_idx++;
		flash_union_buffer[read_buffer_idx].float_type = I_3_PID[i];
		read_buffer_idx++;
		#endif
		#ifdef SPEED
		flash_union_buffer[read_buffer_idx].float_type = MOTOR_3_PID[i];
		read_buffer_idx++;
		#endif
	}
	#else
	for(uint8_t i = 0;i < 4;i++){
		for(uint8_t j = 0;j < 5;j++){
			flash_union_buffer[read_buffer_idx].float_type = MOTOR_PID[i][j];
			read_buffer_idx++;
		}
	}
	for(uint8_t i = 0;i < 5;i++){
		flash_union_buffer[read_buffer_idx].float_type = I_1_PID[i];
		read_buffer_idx++;
	}
	for(uint8_t i = 0;i < 5;i++){
		flash_union_buffer[read_buffer_idx].float_type = I_2_PID[i];
		read_buffer_idx++;
	}
	for(uint8_t i = 0;i < 5;i++){
		flash_union_buffer[read_buffer_idx].float_type = I_3_PID[i];
		read_buffer_idx++;
	}
	#endif

	for(uint8_t i = 0;i < 4;i++){
		for(uint8_t j = 0;j < 4;j++){
			for(uint8_t k = 0;k < 6;k++){
				flash_union_buffer[read_buffer_idx].float_type = PATH_PID[i][j][k];
				read_buffer_idx++;
			}
		}
	}

	flash_write_page_from_buffer(127, FLASH_PAGE_3);
}