/*
 * StateManager.c
 *
 *  Created on: Mar 24, 2025
 *      Author: bihon
 */

#include <stdio.h>
#include <stdint.h>
#include "BHBF_ROBOT.h"
#include "bsp_include.h"
#include "StateManager.h"

static uint8_t ManualControlMark = 0;	//记录调用手动控制模式时的状态，便于恢复
static uint8_t RecoverMark = 0;			//恢复标记，0表示正常调用，1表示恢复状态

// 当前状态变量
Paint_Mode currentModeCMD = STATE_STANDBY;//喷涂模式初始为待机模式
Paint_Control currentControlCMD = INVALID_CMD;//喷涂控制初始为无效指令

// 模拟接收停止信号
uint8_t stopSignalReceived = 0;
start_position_dir paintDir = notwork;

uint8_t InverseStart_Flag;
void InverseStartSet()
{
	InverseStart_Flag = 1;
}
void InverseStartReset()
{
	InverseStart_Flag = 0;
}
uint8_t InverseStartFlag()
{
	return InverseStart_Flag;
}

uint8_t StopPaintingFlag;
void StopPaintingSet()
{
	StopPaintingFlag = 1;
}
void StopPaintingReset()
{
	StopPaintingFlag = 0;
}
uint8_t StopPainting()
{
	return StopPaintingFlag;
}
/*
 * @brief:	控制程序切换到手动控制状态
 * @Param:	CallMark: 用于记录调用时的程序状态，用于恢复
 */
void switch2ManualState()
{
	Paint_Gun_Control(paint_gun_close);//喷枪关闭（默认值）
	paintDir = notwork;//未作业（默认值）
	currentControlCMD = INVALID_CMD;//无效状态（默认值）
	currentModeCMD = STATE_MANUAL_CONTROL;//手动控制状态
}

/*
 * @brief:	控制程序切换到喷漆作业状态
 */
void switch2PaintState()
{
	Paint_Gun_Control(paint_gun_close);//喷枪关闭（默认值）
	paintDir = notwork;//未作业（默认值）

	currentModeCMD = STATE_PAINTING;//喷漆状态
	switch(ManualControlMark)
	{
	case 0:
		RecoverMark = 0;
		currentControlCMD = INVALID_CMD;//无效状态（默认值）
		break;
	case 1:
		RecoverMark = 1;
		currentControlCMD = GOTO_STARTPOSITION;//初始位置
		break;
	case 7://洗枪位置
		RecoverMark = 1;
		currentControlCMD = GOTO_WASH_GUN_POSITION;//洗枪位置
		break;
	case 8:
		RecoverMark = 1;
		currentControlCMD = TRIAL_POSITION;//试枪位置
		break;
	default:
		RecoverMark = 0;
		currentControlCMD = INVALID_CMD;//无效状态（默认值）
		break;
	}
}

/*
 * @brief:	控制程序切换到异常状态
 */
void switch2AbnormalState()
{
	Paint_Gun_Control(paint_gun_close);//喷枪关闭（默认值）
	paintDir = notwork;//未作业（默认值）
	currentControlCMD = INVALID_CMD;//无效状态（默认值）
	currentModeCMD = STATE_ABNORMAL;//异常状态
}

/*
 * @brief:	控制程序切换到待机状态
 */
void switch2StandByState()
{
	Paint_Gun_Control(paint_gun_close);//喷枪关闭（默认值）
	paintDir = notwork;//未作业（默认值）
	currentControlCMD = INVALID_CMD;//无效状态（默认值）
	currentModeCMD = STATE_STANDBY;//待机状态
}

/*
 * @brief 	手动作业状态下的模式切换
 * @note	除执行手动操作指令外，可切换至喷漆控制模式
 */
void handleManualControlState()
{
	/*👇设备状态👇*/
	GV.Vehicle_Cmd.Device_Status = 0x01;	//设备正常
	GV.Vehicle_Cmd.Paint_Stutas = 0xFF;		//位于停机位置
	GV.Vehicle_Cmd.Target_Position = GV.Vehicle_Cmd.Paint_Gun_Control;	//回传接收到的指令高字节
	GV.Vehicle_Cmd.Paint_Process = GV.Vehicle_Cmd.Paint_Speed;	//回传接收到的指令低字节
	/*👆设备状态👆*/

	Manual_Control_Mode();

    //运行状态切换

	if (GV.Vehicle_Cmd.Paint_Mode == 0x01 && GV.Vehicle_Cmd.Paint_Control == 0x00)
	{//切换到喷漆状态 RX:0x01 0x00 0x00 0x00
		switch2PaintState();//根据ManualControlMark恢复
	}

    LOG("STATE: ManualState");
}
uint8_t ArmSpeedSet;
void WorkModeSet()
{
	static uint8_t LastSpeed = 0;

	if(LastSpeed != VehicleSetSpeedData)
	{
		Raspberry_Command_SpeedSet(VehicleSetSpeedData);
		LastSpeed = VehicleSetSpeedData;
	}
}
/*
 * @Brief:	无效指令处理
 * @Note:	机械臂上电默认处于无效指令状态，
 * @Note:	此状态下仅可以接收【前往初始位置】命令
 */
void handleInvalidCMD()
{
	/*👇设备状态👇*/
	GV.Vehicle_Cmd.Device_Status = 0x01;	//设备正常
	GV.Vehicle_Cmd.Paint_Stutas = 0x00;		//位于停机位置
	GV.Vehicle_Cmd.Target_Position = 0x00;	//无目标位置
	GV.Vehicle_Cmd.Paint_Process = 0x00;	//未收到结束指令
	/*👆设备状态👆*/

//	WorkModeSet();

	if(GV.Vehicle_Cmd.Paint_Mode == 0x01 && GV.Vehicle_Cmd.Paint_Control == 0x01)
	{//前往初始位置 RX:0x01 0x01 0x00 0x00
		currentControlCMD = GOTO_STARTPOSITION;
	}
	LOG("STATE: InvalidState");
}

uint8_t Trial_Mark;//试运行标志，0表示未试运行，1表示已进行试运行

void handleTrialPositionCMD()
{
	static uint8_t state = 0;
	GV.Vehicle_Cmd.Device_Status = 0x01;	//设备正常
	GV.Vehicle_Cmd.Paint_Process = 0x00;	//试运行
	switch(state)
	{
	case 0:
		GV.Vehicle_Cmd.Paint_Stutas = 0x04;		//前往目标位置
		GV.Vehicle_Cmd.Target_Position = 0x08;	//试枪位置
		LOG("PAINTSTATE: TrialOperation");
		if(RecoverMark == 1)
		{
			/*👇设备状态👇*/
			GV.Vehicle_Cmd.Paint_Stutas = 0x08;		//位于初始位置
			GV.Vehicle_Cmd.Target_Position = 0x00;	//无目标位置
			/*👆设备状态👆*/
			RecoverMark = 0;
			state++;
		}
		if(TrialOperation() == 1)
		{

			/*👇设备状态👇*/
			GV.Vehicle_Cmd.Paint_Stutas = 0x08;		//位于初始位置
			GV.Vehicle_Cmd.Target_Position = 0x00;	//无目标位置
			/*👆设备状态👆*/

			LOG("PAINTSTATE: AtTrailPosition");

			state++;
		}
		break;
	case 1:
		if(GV.Vehicle_Cmd.Paint_Mode == 0x00 && GV.Vehicle_Cmd.Paint_Control == 0xAA)
		{//切换到手动模式 RX:0x00 0xAA 0x00 0x00
			switch2ManualState();
			ManualControlMark = 8;
			state = 0;
		}else if(GV.Vehicle_Cmd.Paint_Mode == 0x01 && GV.Vehicle_Cmd.Paint_Control == 0x01)
		{//前往初始位置 RX:0x01 0x01 0x00 0x00
			currentControlCMD = GOTO_STARTPOSITION;
			state = 0;
		}
		break;
	}
}

/*
 * @Brief:	前往初始位置指令处理
 * @Note:	可通过该函数命令机械臂【前往上升喷涂起点】或【前往下降喷涂起点】
 * @Note:	结束作业后可接收【前往停机位置】命令
 */
void handleStartPosCMD()
{
	static uint8_t state = 0;

	/*👇设备状态👇*/
	GV.Vehicle_Cmd.Device_Status = 0x01;	//设备正常
	GV.Vehicle_Cmd.Paint_Process = 0x00;	//未收到结束指令
	/*👆设备状态👆*/
	WorkModeSet();
	switch(state)
	{
	case 0://执行动作

		/*👇设备状态👇*/
		GV.Vehicle_Cmd.Paint_Stutas = 0x04;		//前往目标位置
		GV.Vehicle_Cmd.Target_Position = 0x01;	//初始位置
		/*👆设备状态👆*/

		LOG("PAINTSTATE: GoToStartPosition");
		if(RecoverMark == 1)
		{
			/*👇设备状态👇*/
			GV.Vehicle_Cmd.Paint_Stutas = 0x01;		//位于初始位置
			GV.Vehicle_Cmd.Target_Position = 0x00;	//无目标位置
			/*👆设备状态👆*/
			RecoverMark = 0;
			state++;
		}
		if(GoToStartPosition() == 1)
		{//已到达起始位置

			/*👇设备状态👇*/
			GV.Vehicle_Cmd.Paint_Stutas = 0x01;		//位于初始位置
			GV.Vehicle_Cmd.Target_Position = 0x00;	//无目标位置
			/*👆设备状态👆*/

			LOG("PAINTSTATE: AtStartPosition");

			state++;
		}
		break;
	case 1://等待指令
		if(GV.Vehicle_Cmd.Paint_Mode == 0x01 && GV.Vehicle_Cmd.Paint_Control == 0x02)
		{//前往下降喷涂起点 RX:0x01 0x02 0x00 0x00
			if(GV.Vehicle_Cmd.Paint_Gun_Control == 0xFF && GV.Vehicle_Cmd.Paint_Speed == 0xFF)
			{
				InverseStartSet();
			}else
			{
				InverseStartReset();
			}
			currentControlCMD = GOTO_DESENDPAINTING_STARTPOSITION;
			state = 0;
		}else if(GV.Vehicle_Cmd.Paint_Mode == 0x01 && GV.Vehicle_Cmd.Paint_Control == 0x03)
		{//前往上升喷涂起点 RX:0x01 0x03 0x00 0x00
			if(GV.Vehicle_Cmd.Paint_Gun_Control == 0xFF && GV.Vehicle_Cmd.Paint_Speed == 0xFF)
			{
				InverseStartSet();
			}else
			{
				InverseStartReset();
			}
			currentControlCMD = GOTO_ASCENDPAINTING_STARTPOSITION;
			state = 0;
		}else if(GV.Vehicle_Cmd.Paint_Mode == 0x01 && GV.Vehicle_Cmd.Paint_Control == 0x06)
		{//前往停机位置 RX:0x01 0x06 0x00 0x00
			currentControlCMD = GOTO_STOPPOSITION;
			state = 0;
		}else if(GV.Vehicle_Cmd.Paint_Mode == 0x01 && GV.Vehicle_Cmd.Paint_Control == 0x07)
		{
			currentControlCMD = GOTO_STOPPOSITION;
			state = 0;
		}else if(GV.Vehicle_Cmd.Paint_Mode == 0x00 && GV.Vehicle_Cmd.Paint_Control == 0xAA)
		{//切换到手动模式 RX:0x00 0xAA 0x00 0x00
			ManualControlMark = 7;
			switch2ManualState();
			state = 0;
		}
		break;
	default:
		break;
	}
}

/*
 * @Brief:	前往下降喷涂起点指令处理
 * @Note:	机械臂执行前往下降喷涂起点动作
 * @Note:	到达位置后可接收【前往初始位置】指令或【开始喷涂】指令
 */
void handleDesendPaintCMD()
{
	static uint8_t state;

	/*👇设备状态👇*/
	GV.Vehicle_Cmd.Device_Status = 0x01;	//设备正常
	GV.Vehicle_Cmd.Paint_Process = 0x00;	//未收到结束指令
	/*👆设备状态👆*/

	switch(state)
	{
	case 0://执行动作

		/*👇设备状态👇*/
		GV.Vehicle_Cmd.Paint_Stutas = 0x04;		//前往目标位置
		GV.Vehicle_Cmd.Target_Position = 0x02;	//下降喷涂起点
		/*👆设备状态👆*/

		LOG("PAINTSTATE: GoToDesendPaintPosition");

		if(GoToPaintPosition(decend) == 1)
		{//已到达下降喷涂起点

			/*👇设备状态👇*/
			GV.Vehicle_Cmd.Paint_Stutas = 0x02;		//位于下降喷涂起点
			GV.Vehicle_Cmd.Target_Position = 0x00;	//无目标位置
			/*👆设备状态👆*/

			LOG("PAINTSTATE: AtDesendPaintPosition");

			state++;
		}
		break;
	case 1://等待指令
		if(GV.Vehicle_Cmd.Paint_Mode == 0x01 && GV.Vehicle_Cmd.Paint_Control == 0x01)
		{//回到起始位置 RX:0x01 0x01 0x00 0x00
			currentControlCMD = GOTO_STARTPOSITION;
			state = 0;
		}else if(GV.Vehicle_Cmd.Paint_Mode == 0x01 && GV.Vehicle_Cmd.Paint_Control == 0x04)
		{//开始喷涂 RX:0x01 0x04 0x00 0x00
			currentControlCMD = START_PAINTING;
			paintDir = decend;
			state = 0;
		}
		break;
	default:
		break;
	}
}

/*
 * @Brief:	前往上升喷涂起点指令处理
 * @Note:	机械臂执行前往下上升喷涂起点动作
 * @Note:	到达位置后可接收【前往初始位置】指令或【开始喷涂】指令
 */
void handleAscendPaintCMD()
{
	static uint8_t state;

	/*👇设备状态👇*/
	GV.Vehicle_Cmd.Device_Status = 0x01;	//设备正常
	GV.Vehicle_Cmd.Paint_Process = 0x00;	//未收到结束指令
	/*👆设备状态👆*/

	switch(state)
	{
	case 0:

		/*👇设备状态👇*/
		GV.Vehicle_Cmd.Paint_Stutas = 0x04;		//前往目标位置
		GV.Vehicle_Cmd.Target_Position = 0x03;	//上升喷涂起点
		/*👆设备状态👆*/
		LOG("PAINTSTATE: GoToAscendPaintPosition");
		if(GoToPaintPosition(ascend) == 1)
		{//已到达下降喷涂起点

			/*👇设备状态👇*/
			GV.Vehicle_Cmd.Paint_Stutas = 0x03;		//位于下降喷涂起点
			GV.Vehicle_Cmd.Target_Position = 0x00;	//无目标位置
			/*👆设备状态👆*/

			LOG("PAINTSTATE: AtAscendPaintPosition");

			state++;
		}
		break;
	case 1:
		if(GV.Vehicle_Cmd.Paint_Mode == 0x01 && GV.Vehicle_Cmd.Paint_Control == 0x01)
		{//回到起始位置 RX:0x01 0x01 0x00 0x00
			currentControlCMD = GOTO_STARTPOSITION;
			state = 0;
		}else if(GV.Vehicle_Cmd.Paint_Mode == 0x01 && GV.Vehicle_Cmd.Paint_Control == 0x04)
		{//开始喷涂 RX:0x01 0x04 0x00 0x00
			currentControlCMD = START_PAINTING;
			paintDir = ascend;
			state = 0;
		}
		break;
	default:
		break;
	}
}

/*
 * @Brief:	开始喷涂指令处理
 * @Note:	按照作业方向进行喷涂作业
 * @Note:	可接受停止喷涂指令，
 * 			在执行完当前喷涂后进入结束喷涂状态
 */
void handleStartPaintCMD()
{
	uint8_t paintstate;

	/*👇设备状态👇*/
	GV.Vehicle_Cmd.Target_Position = 0x00;	//无目标位置
	/*👆设备状态👆*/

	if(paintDir == decend || paintDir == ascend)
	{//作业方向为上升或下降

		/*👇设备状态👇*/
		GV.Vehicle_Cmd.Device_Status = 0x01;	//设备正常
		GV.Vehicle_Cmd.Paint_Stutas = 0x05;		//正在进行喷涂
		GV.Vehicle_Cmd.Paint_Process = 0x00;	//未收到结束指令
		/*👆设备状态👆*/

		LOG("PAINTSTATE: AutoPaintingRuning...");

		paintstate = StartPainting(paintDir);

	}else
	{//收到错误方向

		paintDir = notwork;//恢复默认值

		LOG("ERROR: Invalid <paintDir> ");

	}

	if((GV.Vehicle_Cmd.Paint_Mode == 0x01) && (GV.Vehicle_Cmd.Paint_Control == 0x05))
	{//收到结束喷涂指令

		/*👇设备状态👇*/
		GV.Vehicle_Cmd.Device_Status = 0x01;	//设备正常
		GV.Vehicle_Cmd.Paint_Stutas = 0x05;		//正在进行喷涂
		GV.Vehicle_Cmd.Paint_Process = 0x01;	//收到结束指令（待执行）
		/*👆设备状态👆*/
		StopPaintingSet();
		LOG("PAINTSTATE: Received Stop Command.");
	}

	if(paintstate == 1)
	{//正常停止

		currentControlCMD = FINISH_PAINTING;
		paintDir = notwork;
		StopPaintingReset();
		LOG("PAINTSTATE: Paint Finished.");

	}else if(paintstate == 2)
	{//触发急停

		/*👇设备状态👇*/
		GV.Vehicle_Cmd.Device_Status = 0x65;	//触发急停（101）
		GV.Vehicle_Cmd.Paint_Stutas = 0x05;		//正在进行喷涂
		GV.Vehicle_Cmd.Paint_Process = 0x00;	//未收到结束指令（默认值）
		/*👆设备状态👆*/

		switch2AbnormalState();

		LOG("PAINTSTATE: Emergency Stop.");

	}
}

/*
 * @Brief:	结束喷涂指令处理
 * @Note:	可接受【前往初始位置】指令
 */
void handleFinishPaintCMD()
{//无动作执行

	/*👇设备状态👇*/
	GV.Vehicle_Cmd.Device_Status = 0x01;	//设备正常
	GV.Vehicle_Cmd.Paint_Stutas = 0x06;		//喷涂完成
	GV.Vehicle_Cmd.Target_Position = 0x00;	//无目标位置
	GV.Vehicle_Cmd.Paint_Process = 0x00;	//未收到结束指令
	/*👆设备状态👆*/

	LOG("PaintSTATE: Finish.");

	//等待指令
//	if((GV.Vehicle_Cmd.Paint_Mode == 0x01) && (GV.Vehicle_Cmd.Paint_Control = 0x01))
//	{//前往初始位置 RX:0x01 0x01 0x00 0x00
		currentControlCMD = GOTO_STARTPOSITION;
//	}
}

/*
 * @Brief:	前往停机位置指令处理
 * @Note:	机械臂执行前往停机位置动作
 * @Note:	到达停机位置后可接受【前往初始位置】指令
 */
void handleStopPositionCMD()
{
	static uint8_t state;

	/*👇设备状态👇*/
	GV.Vehicle_Cmd.Device_Status = 0x01;	//设备正常
	GV.Vehicle_Cmd.Paint_Process = 0x00;	//未收到结束指令
	/*👆设备状态👆*/

	switch(state)
	{
	case 0://执行动作

		/*👇设备状态👇*/
		GV.Vehicle_Cmd.Paint_Stutas = 0x04;		//前往目标位置
		GV.Vehicle_Cmd.Target_Position = 0x06;	//停机位置
		/*👆设备状态👆*/

		LOG("PAINTSTATE: GoToStopPosition.");

		if(GoToStopPosition() == 1)
		{//已到达停机位置

			/*👇设备状态👇*/
			GV.Vehicle_Cmd.Paint_Stutas = 0x00;		//位于停机位置
			GV.Vehicle_Cmd.Target_Position = 0x00;	//无目标位置
			/*👆设备状态👆*/

			LOG("PAINTSTATE: AtStopPosition.");

			state++;
		}
		break;
	case 1://切换到待机状态
		switch2StandByState();
		state = 0;
		break;
	default:
		break;
	}
}

/*
 * @Brief:	前往洗枪位置指令处理
 * @Note:	机械臂执行前往洗枪位置动作
 * @Note:	到达洗枪位置后可接受【前往初始位置】指令或执行【切换为手动状态】
 */
void handleWashGunPositionCMD()
{
	static uint8_t state;

	/*👇设备状态👇*/
	GV.Vehicle_Cmd.Device_Status = 0x01;	//设备正常
	GV.Vehicle_Cmd.Paint_Process = 0x00;	//未收到结束指令
	/*👆设备状态👆*/

	switch(state)
	{
	case 0://执行动作

		/*👇设备状态👇*/
		GV.Vehicle_Cmd.Paint_Stutas = 0x04;		//前往目标位置
		GV.Vehicle_Cmd.Target_Position = 0x07;	//下降喷涂起点
		/*👆设备状态👆*/

		LOG("PAINTSTATE: GoToDesendPaintPosition");
		if(RecoverMark == 1)
		{//执行状态恢复，已在洗枪位置
			/*👇设备状态👇*/
			GV.Vehicle_Cmd.Paint_Stutas = 0x07;		//位于下降喷涂起点
			GV.Vehicle_Cmd.Target_Position = 0x00;	//无目标位置
			/*👆设备状态👆*/
			RecoverMark = 0;
			state++;
		}
		if(GoToWashGunPosition() == 1)
		{//已到达洗枪位置

			/*👇设备状态👇*/
			GV.Vehicle_Cmd.Paint_Stutas = 0x07;		//位于下降喷涂起点
			GV.Vehicle_Cmd.Target_Position = 0x00;	//无目标位置
			/*👆设备状态👆*/

			LOG("PAINTSTATE: AtDesendPaintPosition");

			state++;
		}
		break;
	case 1://等待指令
		if(GV.Vehicle_Cmd.Paint_Mode == 0x00 && GV.Vehicle_Cmd.Paint_Control == 0xAA)
		{//切换到手动模式 RX:0x00 0xAA 0x00 0x00
			switch2ManualState();
			ManualControlMark = 7;
			state = 0;
		}else if(GV.Vehicle_Cmd.Paint_Mode == 0x01 && GV.Vehicle_Cmd.Paint_Control == 0x01)
		{//前往初始位置 RX:0x01 0x01 0x00 0x00
			currentControlCMD = GOTO_STARTPOSITION;
			state = 0;
		}
		break;
	default:
		break;
	}
}

/*
 * @Brief:	喷漆作业模式
 * @Brief:	对喷漆流程进行控制，结束喷涂后主状态切换为待机状态
 */
void handlePaintingState(){

	switch(currentControlCMD)
	{
	case INVALID_CMD://无效指令
		handleInvalidCMD();
		break;
	case GOTO_STARTPOSITION://机械臂前往初始位置
		handleStartPosCMD();
		break;
	case TRIAL_POSITION:
		handleTrialPositionCMD();
		break;
	case GOTO_DESENDPAINTING_STARTPOSITION://机械臂前往下降喷涂起点
		handleDesendPaintCMD();
		break;
	case GOTO_ASCENDPAINTING_STARTPOSITION://机械臂前往上升喷涂起点
		handleAscendPaintCMD();
		break;
	case START_PAINTING://开始喷涂
		handleStartPaintCMD();
		break;
	case FINISH_PAINTING://结束喷涂
		handleFinishPaintCMD();
		break;
	case GOTO_STOPPOSITION://前往停机位置
		handleStopPositionCMD();//到达停机位置程序自动切换到待机模式
		break;
	case GOTO_WASH_GUN_POSITION:

		break;
	default:
		break;
	}
	LOG("STATE: PaintingState");
}

/*
 * @brief:	待机模式
 * @note:	可将主状态切换到手动控制模式或喷漆作业模式
 */
void handleStandbyState()
{
//	static uint8_t state;
	static uint8_t paintgunstate = 0;
	/*👇设备状态👇*/
	GV.Vehicle_Cmd.Device_Status = 0xFF;	//设备正在启动
	GV.Vehicle_Cmd.Paint_Stutas = 0xFF;		//位于停机位置
	GV.Vehicle_Cmd.Target_Position = 0xFF;	//无目标位置
	GV.Vehicle_Cmd.Paint_Process = 0xFF;	//未收到结束指令
	/*👆设备状态👆*/

	if(GV.Vehicle_Cmd.Paint_Mode == 0x00 && GV.Vehicle_Cmd.Paint_Control == 0xAA)
	{//切换为手动控制模式 RX:0x00 0xAA 0x00 0x00
		switch2ManualState();
	}else if(GV.Vehicle_Cmd.Paint_Mode == 0x01 && GV.Vehicle_Cmd.Paint_Control == 0x00)
	{//切换为喷漆作业模式 RX:0x01 0x00 0x00 0x00
		ManualControlMark = 0;
		switch2PaintState();
	}else if(GV.Vehicle_Cmd.Paint_Mode == 0x55 && GV.Vehicle_Cmd.Paint_Control == 0xFF && !paintgunstate)
	{
		Paint_Gun_Control(paint_gun_open);//开启喷枪
		paintgunstate = 1;
	}else if(GV.Vehicle_Cmd.Paint_Mode == 0x55 && GV.Vehicle_Cmd.Paint_Control == 0xAA && paintgunstate)
	{
		Paint_Gun_Control(paint_gun_close);//关闭喷枪
		paintgunstate = 0;
	}

	LOG("STATE: StandbyState");
}

/*
 * @brief:	异常状态处理
 * @note:	处理急停
 * @note:	该状态下仅可以进入手动控制模式
 */
void handleAbnormalState()
{

	if(GV.Vehicle_Cmd.Paint_Mode == 0x00 && GV.Vehicle_Cmd.Paint_Control == 0xAA)
	{//切换为手动控制模式 RX:0x00 0xAA 0x00 0x00
		Elite_Command(CLEAR_ALARM_1);
		switch2ManualState();
	}

	LOG("STATE: AbnormalState");
}

/*
 * @brief:	状态机主循环函数
 * @note:	在待机模式/手动控制模式/喷漆作业模式间进行切换
 */
void stateMachineLoop() {
    switch (currentModeCMD) {
        case STATE_MANUAL_CONTROL://手动控制
            handleManualControlState();
            break;
        case STATE_PAINTING://喷漆作业
            handlePaintingState();
            break;
        case STATE_STANDBY://待机状态
        	handleStandbyState();
        	break;
        case STATE_ABNORMAL://异常状态
        	handleAbnormalState();
        	break;
        default:
            break;
    }

    LOG("FUNCTION: stateMachineLoop");
}