/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "BHBF_ROBOT.h"
#include "bsp_FDCAN.h"
#include <bsp_client_setting.h>
#include <bsp_tempature.h>
#include "paint_robot_old.h"
//#include "msp_DMKE_485_Motor.h"
/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */
void GF_Robot_Init();
/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */

#define RS485_1_WaitTime  6

#define RS485_2_WaitTime 6
#define RS485_3_WaitTime 6
#define RS485_4_WaitTime 6
#define LTE_7S0_Serial_WaitTime 6
#define InterCall_DEBUG_WaitTime 6
#define E28_SBUS_WaitTime 4
#define LPUART1_UART_WaitTime 6
#define RS485_1_Dispacher_Time 100
#define RS485_2_Dispacher_Time 100
#define RS485_3_Dispacher_Time 100
#define RS485_4_Dispacher_Time 100
#define LTE_7S0_Serial_Dispacher_Time 100
#define InterCall_DEBUG_Dispacher_Time 100
#define E28_SBUS_Dispacher_Time  100
#define LPUART1_UART_Dispacher_Time 100
int can1_sendListPeriod = 10;
int can1_DispacherPeriod = 4;
int can2_sendListPeriod = 10;
int can2_DispacherPeriod = 20;

int SAVE_To_CV = 0;

/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/

/* USER CODE BEGIN PV */
void protobuf_mqtt_setting_test(void);
/* USER CODE END PV */

/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
static void MPU_Config(void);
/* USER CODE BEGIN PFP */
char ReadIO[8];

/* USER CODE END PFP */

/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */

/* USER CODE END 0 */

void GF_Robot_Init()
{



	//2ms调度
	GF_BSP_EEPROM_Init();

	CV_GV_Init();

	//初始化encoder
	GF_BSP_UARTHandlers_Intialize(
	RS485_1_WaitTime,
	RS485_2_WaitTime,
	RS485_3_WaitTime,
	RS485_4_WaitTime,
	LTE_7S0_Serial_WaitTime,
	InterCall_DEBUG_WaitTime,
	E28_SBUS_WaitTime,
	LPUART1_UART_WaitTime,
	RS485_1_Dispacher_Time,
	RS485_2_Dispacher_Time,
	RS485_3_Dispacher_Time,
	RS485_4_Dispacher_Time,
	LTE_7S0_Serial_Dispacher_Time,
	InterCall_DEBUG_Dispacher_Time,
	E28_SBUS_Dispacher_Time,
	LPUART1_UART_Dispacher_Time);



	is_upper_computer_take_over_control = 0;
	DLT_LOG_ENABLE_LEVEL = 7;	  //7 send all information 	  //0 send nothing
	//dLT_Log_intialize(&InterCall_DEBUG_UART_Handler); //huart4
	//dLT_Log_intialize(&RS_485_4_UART_Handler);
	udp_client_init();
	tcp_server_init(3490);
	dLT_Log_intialize_udp_tcp();
	upper_Computer_UART_Handler_intialize(&RS_485_4_UART_Handler);//send to computer to 485
	TL720D_intialize(&RS_485_1_UART_Handler);
	MK32_Sbus_UART_Handler_intialize(&E28_SBUS_UART_Handler); //huart5
	client_setting_intialize(&LPUART1_UART_Handler);

	//WH_LTE_7S0_intialize(&LTE_7S0_Serial_UART_Handler);//huart2

	GF_BSP_FDCAN_Init();//初始化FDCAN

	GF_BSP_CANHandler_Init(can1_sendListPeriod, can1_DispacherPeriod,
	        can2_sendListPeriod, can2_DispacherPeriod);


	LS_Motor_Controller_intialize(&FD_CAN_1_Handler);

	Fsm_Init();

	GF_BSP_TIMER_Init(); //定时器最后启


}
/*
 * */
void CV_GV_Init()
{
	//Read CV
	CV = GF_BSP_EEPROM_Get_CV();
	CV.has_PID_high=true;
	CV.has_PID_mid=true;
	CV.has_PID_low=true;

	SystemErrorCode = &GV.SystemErrorData.Com_Error_Code;

	//电机绑定
	LS_Motor[1]=&GV.LeftMotor;
	LS_Motor[2]=&GV.RightMotor;

	LS_Motor[1]->MotorID=1;
	LS_Motor[2]->MotorID=2;

	P_MK32 = &GV.P_MK32;

	SP_MSP_RF_TL720D_Parameters_In = &GV.TL720DParameters;



	RobotAngle = &GV.Robot_Angle;

	tempature = &GV.TempatureE_2C;

	GV.has_P_MK32 = true;
	GV.has_LeftMotor = true;
	GV.has_RightMotor = true;
	GV.has_TL720DParameters = true;
	GV.has_IO = true;
	GV.has_SystemErrorData = true;

}

int BHBF_robot_init(void)
{
    SystemTimer_Intialize();

    Error_Detect_Intialzie(1000);	//every 1 seconds
    GF_Robot_Init();
    GV.Robot_Move_Speed = 1;
    return 0;
}

void BHBF_robot_task(void)
{
    /* 刷新看门狗计数器（喂狗） */
    //HAL_IWDG_Refresh(&hiwdg1);

    if (SAVE_To_CV == 1)
    {
        SAVE_To_CV = 0;
        //CV写入falsh�???????
        GF_BSP_EEPROM_Set_CV(CV);
        CV = GF_BSP_EEPROM_Get_CV();
        //FS_SetZero();
    }
}