/*********************************************************************************************************************
* RT1064DVL6A Opensourec Library RT1064DVL6A Դ⣩һڹٷ SDK ӿڵĵԴ
* Copyright (c) 2022 SEEKFREE ɿƼ
* 
* ļ RT1064DVL6A Դһ
* 
* RT1064DVL6A Դ 
* Ըᷢ GPLGNU General Public License GNUͨù֤
*  GPL ĵ3棨 GPL3.0ѡģκκİ汾·/޸
* 
* Դķϣܷãδκεı֤
* ûԻʺض;ı֤
* ϸμ GPL
* 
* ӦյԴͬʱյһ GPL ĸ
* ûУ<https://www.gnu.org/licenses/>
* 
* ע
* Դʹ GPL3.0 Դ֤Э Ϊİ汾
* Ӣİ libraries/doc ļµ GPL3_permission_statement.txt ļ
* ֤ libraries ļ ļµ LICENSE ļ
* ӭλʹò ޸ʱ뱣ɿƼİȨ
//* 
//* ļ          main
//* ˾          ɶɿƼ޹˾
//* 汾Ϣ          鿴 libraries/doc ļ version ļ 汾˵
//*           IAR 8.32.4 or MDK 5.33
* ƽ̨          RT1064DVL6A
*           https://seekfree.taobao.com/
* 
* ޸ļ¼
*                               ע
* 2022-09-21        SeekFree            first version
********************************************************************************************************************/



// µĹ̻߹ƶλִ²
// һ رд򿪵ļ
// ڶ project->clean  ȴ·

// ǿԴֲÿչ
#include "common.h"
#include "data.h"

int main(void)
{
    clock_init(SYSTEM_CLOCK_600M);  // ɾ
    debug_init();                   // Զ˿ڳʼ

    // ˴дû ʼ
	
	flash_init();

	/*  */
	motor(&motor_1, MOTOR_1_DIR, MOTOR_1_PWM, 10000, MOTOR_1_FRONT_DIR);
	motor(&motor_2, MOTOR_2_DIR, MOTOR_2_PWM, 10000, MOTOR_2_FRONT_DIR);
	motor(&motor_3, MOTOR_3_DIR, MOTOR_3_PWM, 10000, MOTOR_3_FRONT_DIR);
	
	/*  */
	encoder(&encoder_1, ENCODER_1_MODULE_NUM, ENCODER_1_CH1, ENCODER_1_CH2, ENCODER_1_FRONT_DIR, 4096, SENSOR_SOLVE_IT_TIME, GEAR_RATIO, WHEEL_CIRCUMFERENCE);
	encoder(&encoder_2, ENCODER_2_MODULE_NUM, ENCODER_2_CH1, ENCODER_2_CH2, ENCODER_2_FRONT_DIR, 4096, SENSOR_SOLVE_IT_TIME, GEAR_RATIO, WHEEL_CIRCUMFERENCE);
	encoder(&encoder_3, ENCODER_3_MODULE_NUM, ENCODER_3_CH1, ENCODER_3_CH2, ENCODER_3_FRONT_DIR, 4096, SENSOR_SOLVE_IT_TIME, GEAR_RATIO, WHEEL_CIRCUMFERENCE);
	
	/*  */
	current(&current_1, CURRENT_1_PIN, ADC_12BIT, CURRENT_RATIO, 500,CURRENT_1_FRONT_DIR);
	current(&current_2, CURRENT_2_PIN, ADC_12BIT, CURRENT_RATIO, 500,CURRENT_2_FRONT_DIR);
	current(&current_3, CURRENT_3_PIN, ADC_12BIT, CURRENT_RATIO, 500,CURRENT_1_FRONT_DIR);
	
	/* Ҷȴ */
	voltage(&gray_sensor, GRAYSCALE_SENSOR_PIN, ADC_12BIT);
	
	/* صѹ */
	voltage(&bat_voltage, BAT_VOLTAGE_PIN, ADC_12BIT);
	
	/* ˲ */
	karman(&current_1_karman, I_KARMAN[0], I_KARMAN[1]);
	karman(&current_2_karman, I_KARMAN[0], I_KARMAN[1]);
	karman(&current_3_karman, I_KARMAN[0], I_KARMAN[1]);
	
	/* AI CAMERA 0˲ */
	karman(&box_center_x_karman, BOX_CENTER_X_KARMAN[0], BOX_CENTER_X_KARMAN[1]);
	karman(&box_width_karman, BOX_WIDTH_KARMAN[0], BOX_WIDTH_KARMAN[1]);
    
    /* ·˲ */
    karman(&path_gyro_karman, PATH_GYRO_KARMAN[0], PATH_GYRO_KARMAN[1]);
	
	/* λƽ˲ */
	karman(&displacement_solve_gyro_karman, PATH_GYRO_KARMAN[0], PATH_GYRO_KARMAN[1]);
	
	/* VOFA */
	vofa(&wireless_vofa, WIRELESS_UART_INDEX, 115200, WIRELESS_UART_TX_PIN, WIRELESS_UART_RX_PIN);
	
	/* IMU */
	imu(&imu660ra, IMU_X_FRONT_DIR, IMU_Y_FRONT_DIR, IMU_Z_FRONT_DIR, 500);
	
	/* ʱ */
	timer(&zebra_path_timer, 5);
    timer(&motor_debug_timer, 5);
	timer(&rwr_timer, 5);
	
	/* ŷǽ */
	solve(&euler_angle_solve, RADIUS, SENSOR_SOLVE_IT_TIME);
	solve(&rotate_euler_angle_solve, RADIUS, SENSOR_SOLVE_IT_TIME);
	solve(&circle_euler_angle_solve, RADIUS, SENSOR_SOLVE_IT_TIME);
	solve(&box_euler_angle_solve, RADIUS, SENSOR_SOLVE_IT_TIME);
	
	/* ̽ */
	solve(&chassis_solve, RADIUS, SENSOR_SOLVE_IT_TIME);
	
	/* λƽ */
	solve(&displacement_solve, RADIUS, SENSOR_SOLVE_IT_TIME);
	
	/* Ӿ */
	cv(&dog_cv);
	system_delay_ms(50);
	
	/* · */
	path(&dog_path, path_start, path_end, side_extract_start_y, side_extract_end_y, prediction_point);
	
	/* RWR */
	rwr(&dog_rwr, BUZZER_PIN, &rwr_timer.time);
	
	/* PID */
	pid(&motor_1_pid);
	pid(&motor_2_pid);
	pid(&motor_3_pid);
	#ifdef FUZZY_SPEED_AND_CURRENT
	motor_1_pid.fuzzy_pid_init(&motor_1_pid, motor_fuzzy_rules, MOTOR_RANGE);
	motor_2_pid.fuzzy_pid_init(&motor_2_pid, motor_fuzzy_rules, MOTOR_RANGE);
	motor_3_pid.fuzzy_pid_init(&motor_3_pid, motor_fuzzy_rules, MOTOR_RANGE);
	#endif
	pid(&current_1_pid);
	pid(&current_2_pid);
	pid(&current_3_pid);
	pid(&path_pid);
	path_pid.fuzzy_pid_init(&path_pid, path_fuzzy_rules, PATH_RANGE);
	pid(&path_gyroz_pid);
	path_gyroz_pid.fuzzy_pid_init(&path_gyroz_pid, gyro_fuzzy_rules, GYRO_RANGE);
	pid(&angle_rotate_pid);
	angle_rotate_pid.fuzzy_pid_init(&angle_rotate_pid, angle_rotate_fuzzy_rules, ROTATE_RANGE);
	pid(&circle_rotate_pid);
	circle_rotate_pid.fuzzy_pid_init(&circle_rotate_pid, circle_rotate_fuzzy_rules, ROTATE_RANGE);
	pid(&box_track_x_pid);
	box_track_x_pid.fuzzy_pid_init(&box_track_x_pid, xy_fuzzy_rules, BOX_TRACK_X_RANGE);
	pid(&box_fxxk_x_pid);
	box_fxxk_x_pid.fuzzy_pid_init(&box_fxxk_x_pid, xy_fuzzy_rules, BOX_TRACK_X_RANGE);
	pid(&box_track_y_pid);
	box_track_y_pid.fuzzy_pid_init(&box_track_y_pid, xy_fuzzy_rules, BOX_TRACK_Y_RANGE);
	
	/* ӽ */
	symmetry_rectificate_init();
	
	/*  */
	key_init(10);
	
	// /жϳʼ
	pit_ms_init (SENSOR_SOLVE_IT_CH, SENSOR_SOLVE_IT_TIME);
	// жϳʼ
	pit_ms_init (CONTROL_IT_CH, CONTROL_IT_TIME);
	/* ʱжϳʼ */
	pit_ms_init (TIMER_IT_CH, TIMER_IT_TIME);
	/* жϳʼ */
	pit_ms_init (MENU_KEY_SCAN_IT_CH, MENU_KEY_SCAN_IT_TIME);
	// жʹ
	pit_enable(SENSOR_SOLVE_IT_CH);
	pit_enable(CONTROL_IT_CH);
	pit_enable(TIMER_IT_CH);
	pit_enable(MENU_KEY_SCAN_IT_CH);
	
	/* AIͷŵжʹܺ */
	ai_camera_init();
	
	/* ˵ */
	menu_init();
	
	circle_enable_flag = True;			// Բ ʹܱ־λ
	zebra_enable_flag = True;			//  ʹܱ־λ
	ai_camera_0_enable_flag = True;		// AI0 ʹܱ־λ
	ai_camera_1_enable_flag = True;		// AI1 ʹܱ־λ
	ai_camera_2_enable_flag = True;		// AI2 ʹܱ־λ
	
	ai_camera_0_init_flag = False;
	detection_result.ai_camera_init_flag[0] = False;
	detection_result.ai_camera_init_flag[1] = False;
	if(ai_camera_0_enable_flag == False)
		ai_camera_0_init_flag = True;
	if(ai_camera_1_enable_flag == False)
		detection_result.ai_camera_init_flag[0] = True;
	if(ai_camera_2_enable_flag == False)
		detection_result.ai_camera_init_flag[1] = True;

    // ˴дû ʼ
    while(1)
    {	
		// ˴дҪѭִеĴ
		#ifdef SPEED
		if(imu660ra.gyro_calibration_flag == True && imu660ra.acc_calibration_flag == True && ai_camera_0_init_flag == True && detection_result.ai_camera_init_flag[0] == True && detection_result.ai_camera_init_flag[1] == True)
		{
		#else
		if(imu660ra.gyro_calibration_flag == True && imu660ra.acc_calibration_flag == True && current_1.current_calibration_flag == True && current_2.current_calibration_flag == True && current_3.current_calibration_flag == True && ai_camera_0_init_flag == True && detection_result.ai_camera_init_flag[0] == True && detection_result.ai_camera_init_flag[1] == True)
		{
		#endif
			// ˴дҪѭִеĴ
			menu_service_start(); 
			
            // ̬ǰհ
//			auto_control_point();
//            vofa_debug();
            
			// λ
//			control_kind = Fxxk_X2Inv2Speed;
//			wireless_vofa.justfloat_add(&wireless_vofa, 4, (float)detection_box_center_x, (float)detection_box_width, (float)box_center_x_karman.value, (float)box_width_karman.value);
//			wireless_vofa.justfloat_send(&wireless_vofa);
			
			// ǶȻ
//			control_kind = Angle2Inv2Speed;
//			rotate_euler_angle_solve.solve_flag = True;
//			wireless_vofa.justfloat_add(&wireless_vofa, 1, rotate_euler_angle_solve.yaw);
//			wireless_vofa.justfloat_send(&wireless_vofa);
		}
    }
	
	return 0;
}



