/*
 *  Created on: Oct 18, 2024
 *      Author: akeguo
 */
#include <logic.h>
#include <math.h>
#include "BHBF_ROBOT.h"
int direction;
/*
 * @brief 喷漆装置主程序，实现程序状态切换
 */
void Decive_MainLoop()
{
	static int count = 0;
	count++;
	//设备状态切换
	stateMachineLoop();//主程序控制函数
	//测距
	Position_Detect();
	RaspberryGetOffset(GetOffsetFromVehicle());
	if(count == 3)
	{//返回距离信息
		Elevated_Distance_Stutas();//10ms发送一次，向大臂车
	}else if(count >= 5)
	{//返回运行状态
		Elevated_Stutas_Control();//10ms发送一次，向大臂车
		count = 0;//计数置零
	}
	//Vehicle_Heart_Beat();//机械臂心跳
	Globle_Time_Count++;//全局计时
    //VehicleConnectDetect();//大臂车心跳检测
}

/*
 * @brief 喷漆装置初始化函数
 * @note 主循环每2ms运行一次
 */
void Painting_Device_Init()
{
	//为大臂车分配CAN1接口
	Elevator_Connection_Init(&FD_CAN_2_Handler);
	//为机械臂分配串口2
	Elite_initialize(&RS_485_2_UART_Handler);
	//为树莓派分配串口1
	Raspberry_initialize(&RS_485_1_UART_Handler);
	//为超声波测距传感器分配串口4
	Sonic_intialize(&RS_485_4_UART_Handler);
//	Elite_Remote_PowerOn2();
	//主程序每2ms调度一次
	GF_BSP_Interrupt_Add_CallBack(
			DF_BSP_InterCall_TIM8_2ms_PeriodElapsedCallback,
			Decive_MainLoop
			);
}