/*
 * fsm.c
 *
 *  Created on: Oct 18, 2024
 *      Author: akeguo
 */

#include <bsp_tempature.h>
#include "fsm.h"
#include <math.h>

#include "BHBF_ROBOT.h"
#include "BSP/bsp_include.h"

#include "MSP/msp_PID.h"
#include "robot_state.h"
#include "MSP/msp_MK32_1.h"
#include "motors.h"

#define Move_Manual 1
#define Move_Horizontal_Move 2
#define Move_Vertical_Move_Left 3
#define Move_Vertical_Move_Right 4
#define Move_Automation_Horizontal_Move 5
#define Move_Automation_Vertical_Move_Left 6
#define Move_Automation_Vertical_Move_Right 7

#define Horizontal_Angle_threshold 80  //角度识别阈值 80度

#define LeftRightCalbrationTime 250

#define LanChangeAngleHolds  100


MoveSTATE_t CurrentMoveState;

SwingSTATE_t CurrentSwingState;

TiltSTATE_t CurrentTiltState; //点头，抬头

LaneChangeSTATE_t CurrentLaneChangeSTATE;

RougheningEndState_t CurrentRougheningEndState; //拉毛运动或者不动

SetSTATE_t CurrentSetState; //设置

Lane_Horizontal_ChangeState CurrentHorizontal_ChangeState;
Lane_Vertical_ChangeState Current_Vertical_ChangeState;

LaneChangeControlSTATE HorizontalLaneChangeState, VerticalLaneChangeState; //水平和竖直换道标志位

AutoMoveSTATE_t Current_Auto_Move_State;

RegionAuto_t CurrentRegionAuto;

Horizontal_LaneChangeFlag_t Horizontal_LaneChangeFlag;
Vertical_LaneChangeFlag_t Vertical_LaneChangeFlag;
RegionAutoMoveStart_t RegionAutoMoveStart;
RegionAutoMoveStart_t AutoMoveStart;
TiltWorkingMode_t TiltWorking_Mode;

GV_Main_Core_Mode_t GV_Main_Core_Mode;

MoveTaskTimeCheck_t MoveTaskTimeCheck;
InitialState_Detection_t InitialState_Detection;

Wind_State_t Wind_State;

double PI = 3.14159265;

int Auto_Move_Cycle_Total_Count = 0;
int Auto_Move_Cycle_Count = 0;

int LaneChangeWaittime = 0;

int Region_Move_time = 0;

#define VerticalLaneChangeDistanceCalibration 5

int LaneChangeWaittimeConstant = 50;

int32_t speed_selection;

char is_upper_computer_take_over_control = 0;

int index_counter = 0;

double MK32_LY_V_Forwards_Angle = 90;
double MK32_LY_V_Backwards_Angle = -90;
double MK32_LY_H_Left_Angle = 180;
double MK32_LY_H_Right_Angle = 0;
double MK32_LY_CatchAngle = 20;
double MK32_LY_DeadZone = 600;

int LaneChangeControl();

void DHRougheningControl(int mode);
void DHRougheningControl_Auto_TiltControl();



void TiltControl();

int Auto_TiltControl();
void Movement_control();

void IV_control();
void compensation_control();
void compensation_control2();
void compensation_control3();

void MoveControl();

int32_t GetVehicleSpeed();

int LaneChangeControl();

void Horizontal_Lane_Change_Turn_To_Left_Control();
void Horizontal_Lane_Change_Turn_To_Right_Control();

void Vertical_Lane_Change_Right_Control();
void Vertical_Lane_Change_Left_Control();

void RegionAuto_Horizontal_Distance_Err();
void RegionAuto_Vertical_Distance_Err();

void RegionAuto_Horizontal_RightToLeft();
void RegionAuto_Horizontal_LeftToRight();

void RegionAuto_Vertical_RightToLeft();
void RegionAuto_Vertical_LeftToRight();

void Manual_Mode_Move_Forwards();
void Manual_Mode_Move_Backwards();

int RegionAutoMoveControl();


void Manual_Mode_Joystick_Control();
void Manual_Mode_Joystick_Control1();
void Manual_Mode_Joystick_Control2();
void PV_Data_Reading();
void DLT_Log_Hardware_Failure();

int MoveTaskIsOk(double DistanceMeters);
int MoveTask_Start();
int MoveTask_Reset();
void RegionAuto_MoveTask_Test(int mode);
int Mk32_InitialState_Detection();

//void WindControl();

void EmegencyStopHandle();

int EmegencyStop_Step = 10;
int EmegencyStop_StepNum = 10;
int RegionAuto_MoveTask_Test_Result = 0;


//手动下 机器人运动
transition_t MoveTransitions[] =
{

{ Move_Forwards, Forwards_State_Do },
{ Move_Backwards, Backwards_State_Do },
{ Move_TurnLeft, TurnLeft_State_Do },
{ Move_TurnRight, TurnRight_State_Do },
{ Move_HALT, HALT_State_Do },
{ Move_Vertical_Task_Forwards, Move_Vertical_Task_Forwards_Do },
{ Move_Vertical_Task_Backwards, Move_Vertical_Task_Backwards_Do },
{ Move_Horizontal_Task_Forwards_Left, Move_Horizontal_Task_Forwards_Left_Do },
{ Move_Horizontal_Task_Backwards_Left, Move_Horizontal_Task_Backwards_Left_Do },
{ Move_Horizontal_Task_Forwards_Right, Move_Horizontal_Task_Forwards_Right_Do },
{ Move_Horizontal_Task_Backwards_Right, Move_Horizontal_Task_Backwards_Right_Do },
{ Move_Head_To_Left_Enum, Move_Head_To_Left_Do },
{ Move_Head_To_UP_Enum, Move_Head_To_UP_Do },
{ Move_Head_To_Right_Enum, Move_Head_To_Right_Do },
{ Move_Head_To_Down_Enum, Move_Head_To_Down_Do },

};

transition_t SetTransitions[] =
{

{ OtherMode, OtherMode_State_Do },
{ SetLaneChangeDistance, LaneChangeDistance_Setting_State_Do },
{ SetBackwardsDistance, BackWardsDistance_Setting_State_Do },

};

transition_t TiltTransitions[] =
{
{ TiltHALT, TiltHalt_Do },
{ TiltUP, TiltUp_Do },
{ TiltDown, TiltDown_Do },

};

transition_t RougheningEndTransitions[] =
{
{ RougheningEndHALT, RougheningEndHALT_Do },
{ RougheningEndSwingClockwise, RougheningEndSwingClockWise_Do },
{ RougheningEndSwingAnticlockwise, RougheningEndSwingAnticlockWise_Do }, };


transition_t WindTransitions[] =
{
{ Wind_State_OFF, Wind_OFF },
{ Wind_State_ON, Wind_ON },
};


void Fsm_Init()
{
	VerticalLaneChangeState = Lane_Change_Stop;
	Current_Vertical_ChangeState = VerticalChange_StateZero;
	HorizontalLaneChangeState = Lane_Change_Stop;
	CurrentHorizontal_ChangeState = HorizontalChange_StateZero;  //设定初始方向
	InitialState_Detection = InitialState_Detection_start;

	GF_BSP_Interrupt_Add_CallBack(DF_BSP_InterCall_TIM8_2ms_PeriodElapsedCallback, GF_Dispatch);
}
void action_perfrom(transition_t transitions[], int length, int state)
{

	for (index_counter = 0; index_counter < length; index_counter++)
	{
		if (transitions[index_counter].State == state)
		{
			if (transitions[index_counter].robotRun != NULL)
			{
				transitions[index_counter].robotRun();
			}

			break;
			//return;
		}
	}
}
void IV_control()
{

	IV.Tempature = GV.Tempature;
	IV.LeftCompensation = GV.Left_Compensation;
	IV.RightCompensation = GV.Right_Compensation;
//	IV.CurrentAngle = GV.Robot_Angle-80;// 本台拉毛02号，倾角仪左偏了0.8度左右
//	IV.CurrentAngle = GV.Robot_Angle+90;// 本台拉毛06号，倾角仪右偏了0.9度左右
//	IV.CurrentAngle = GV.Robot_Angle-100;// 本台拉毛03号，倾角仪左右偏了1度左右
	IV.CurrentAngle = GV.Robot_Angle;
	//IV.RobotMoveSpeed = GV.PV.RobotSpeed;
	IV.RobotMoveSpeed = (int32_t) GetVehicleSpeed();
	IV.EndPressure = GV.ForceValue - CV.ForceSensorCalibration;
	IV.SystemError = GV.SystemErrorData.Com_Error_Code;
	IV.Left_Motor_Err = GV.SystemErrorData.Left_Motor_Error_Code;
	IV.Right_Motor_Err = GV.SystemErrorData.Right_Motor_Error_Code;
	IV.Left_Motor_Temp = GV.LeftMotor.Tempature;
	IV.Right_Motor_Temp = GV.RightMotor.Tempature;
}

/*
 * 暂存App传输的PV数据，用于在业务执行过程中进行数据隔离
 */
void PV_Data_Reading()
{
	GV.PV = decoded_PV;
	PV_App.RunMode = GV.PV.RunMode;
	PV_App.RobotSpeed = GV.PV.RobotSpeed;
	PV_App.LaneChangeDistance = GV.PV.LaneChangeDistance;
	PV_App.WorkDistanceMeters = GV.PV.WorkDistanceMeters;
	PV_App.WorkWidthMeters = GV.PV.WorkWidthMeters;
	PV_App.ToolRotationDirection = GV.PV.ToolRotationDirection;
	PV_App.EndPushForceValue = GV.PV.EndPushForceValue;
	PV_App.Vertical_Calibration = GV.PV.Vertical_Calibration;

	//只能在车体未启动功能性按键的时候且未使用波轮调整补偿时可由APP下发
	if(GV.PV.LeftCompensation != 0 || GV.PV.RightCompensation != 0)
	{
		if(GV.Left_Compensation != GV.PV.LeftCompensation|| GV.Right_Compensation != GV.PV.RightCompensation)
		{
			if(P_MK32->CH14_LT == 0 && P_MK32->CH15_RT == 0)
			{
				PV_App.LeftCompensation = GV.PV.LeftCompensation;
				PV_App.RightCompensation = GV.PV.RightCompensation;
				GV.Left_Compensation = PV_App.LeftCompensation;
				GV.Right_Compensation = PV_App.RightCompensation;
			}
		}
	}


}

char Switch_Off_Flag = 0;
void GF_Dispatch()
{

	switch (InitialState_Detection)
	{
		case InitialState_Detection_start:
		{
			if (Mk32_InitialState_Detection() == 1)
			{
				InitialState_Detection = InitialState_Detection_finish;
			}
			break;
		}
		case InitialState_Detection_finish:
		{

			Movement_control();
			break;
		}
		default:
			break;
	}

	IV_control();

}

void Movement_control()
{
	if (is_upper_computer_take_over_control == 0)
	{
		//DLT_Log_Hardware_Failure();
		if (Get_BIT(SystemErrorCode, ComError_MK32_Serial) == 1 )
		{
			//GV.PV.RunMode = Move_Manual;
		}
		if (Get_BIT(SystemErrorCode, ComError_TL720D) == 1)
		{
			GV.PV.RunMode = Move_Manual;
		}
		if (Get_BIT(SystemErrorCode, ComError_MK32_SBus) == 1 )
		{
			EmegencyStopHandle();
		}
		if(Get_BIT(SystemErrorCode, ComError_MK32_SBus) == 1 || Get_BIT(SystemErrorCode, ComError_MK32_SBus) == 1)
		{

		}
		else
		{
			if (P_MK32->IsOnline == 1)
			{
				if (P_MK32->CH8_SE == -1000 && P_MK32->CH9_SF == -1000) //MK32急停
				{
					EmegencyStopHandle();

					IV.RunMode = IV_Run_Mode_EmergencyStop;

				}
				else
				{

					IV.RunMode = IV_Run_Mode_Maunal;
					//Motor Power on
					GF_BSP_GPIO_SetIO(Motor_Power_IO_CTL, K_ON);
					if (Switch_Off_Flag == 1)
					{
						EmegencyStop_Step = EmegencyStop_StepNum;
						Switch_Off_Flag = 0;
						TT_Motor_Need_To_Activate = 1;
					}
					else
					{

						if ((fabs(P_MK32->CH2_LY_V) <= 300) && (fabs(P_MK32->CH3_LY_H) <= 300)
						        && (fabs(P_MK32->CH0_RY_H) <= 300) && (fabs(P_MK32->CH1_RY_V) <= 300)
						        && (P_MK32->CH4_SA != -1000) && (P_MK32->CH5_SB == 0)
						        && (P_MK32->CH6_SC != -1000) && (P_MK32->CH7_SD != -1000))
						{
							PV_Data_Reading();
							IV.IsWorking = 0;
						}
						else
						{

							IV.IsWorking = 1;
						}
						MoveControl();
					}
//					WindControl();
					TiltControl();

					compensation_control3();
				}
			}
			else
			{
				EmegencyStopHandle();
			}

		}
	}
	else
	{
		//Motor Power on
		GF_BSP_GPIO_SetIO(Motor_Power_IO_CTL, K_ON);
		//GF_BSP_GPIO_SetIO(1, Motor_Power_ON);
		if (Switch_Off_Flag == 1)
		{
			EmegencyStop_Step = EmegencyStop_StepNum;
			CurrentMoveState = Move_HALT;
			Switch_Off_Flag = 0;
			TT_Motor_Need_To_Activate = 1;
		}
		else
		{

		}
	}

	GV.LaneChangeDistance = GV.PV.LaneChangeDistance;

	//电机直径280mm/101传动比  发送的数据单位是0.1 rpm
	//	1 rpm 意思是1分钟转1圈，英文是 1rotation per minite。一圈的弧度是2π rad，一分钟60秒，那么
	//	1 rpm = 1(r)/1(min) = 1(r)/60(s) = 2π (rad)/60(s) =π/30 (rad/s)
	//	所以，1 rpm = π/30 (rad/s), 即 1 rpm = 0.1047 （rad/s）
	//   v=ω×r

	//前端上下移动
	action_perfrom(WindTransitions, sizeof(WindTransitions) / sizeof(Wind_State_t), Wind_State);

	//前端上下移动
	action_perfrom(TiltTransitions, sizeof(TiltTransitions) / sizeof(transition_t), CurrentTiltState);

	//拉毛
	action_perfrom(RougheningEndTransitions, sizeof(RougheningEndTransitions) / sizeof(transition_t),
	        CurrentRougheningEndState);
	//车体
	action_perfrom(MoveTransitions, sizeof(MoveTransitions) / sizeof(transition_t), CurrentMoveState);

	if (P_MK32->CH8_SE == -1000 && P_MK32->CH9_SF == -1000) //MK32急停
	{
		IV.RunMode = IV_Run_Mode_EmergencyStop;
	}
}



//机器人运动部分：old ，需增加区域自动模式下，准备阶段可手动控制功能
//void MoveControl()
//{
//
//	GV.Robot_Move_Speed = (int32_t) GetVehicleSpeed() / 0.880 * 101 * 10;
//
//	DHRougheningControl_Auto_TiltControl();
//
//	if (RegionAutoMoveControl() != 0)
//	{
//		return;
//	}
//
//	//换道优先级优于普通的运行;
//	if (LaneChangeControl() != 0)
//	{
//		return;
//	}
//
//
///*********该代码段用于调试各模式下的定距离移动***********/
////	if (P_MK32->CH7_SD == -1000)
////	{
////		RegionAuto_MoveTask_Test(1);
////		return;
////	}
////	else if (P_MK32->CH7_SD == 1000)
////	{
////		RegionAuto_MoveTask_Test(2);
////		return;
////	}
////	else
////	{
////		MoveTask_Reset();
////		RegionAuto_MoveTask_Test_Result = 0;
////		Current_Auto_Move_State = Auto_Move_Start;
////	}
///******************************************/
//
//	//自动巡航
//	if (P_MK32->CH5_SB == -1000)	//自动巡航前进
//	{
//		Manual_Mode_Move_Forwards();
//	}
//	else if (P_MK32->CH5_SB == 1000)	//自动巡航后退
//	{
//		Manual_Mode_Move_Backwards();
//	}
//	else
//	{
//		Manual_Mode_Joystick_Control();
//	}
//
//}


void MoveControl()
{

	GV.Robot_Move_Speed = (int32_t) GetVehicleSpeed() / 0.880 * 101 * 10;

	DHRougheningControl_Auto_TiltControl();

	if (RegionAutoMoveControl() != 0)
	{
		return;
	}

	//换道优先级优于普通的运行;
	if (LaneChangeControl() != 0)
	{
		return;
	}

	//自动巡航
	if (P_MK32->CH5_SB == -1000)	//自动巡航前进
	{
		Manual_Mode_Move_Forwards();
	}
	else if (P_MK32->CH5_SB == 1000)	//自动巡航后退
	{
		Manual_Mode_Move_Backwards();
	}
	else
	{
		Manual_Mode_Joystick_Control();
	}

}


/*
 * 升降电推杆控制
 */
//void TiltControl()
//{
//
//	if (TiltWorking_Mode == Tilt_Manual_Mode && PV_App.RunMode != 0)
//	{
//		//Tilt 部分 前端上升、前端下降
//		if (P_MK32->CH1_RY_V <= -900)
//		{
//			if (GV.ForceValue >= PV_App.EndPushForceValue - 50)
//			{
//				CurrentTiltState = TiltHALT;
//			}
//			else
//			{
//				CurrentTiltState = TiltDown;
//
//			}
//		}
//		else if (P_MK32->CH1_RY_V >= 900)
//		{
//
//			CurrentTiltState = TiltUP;
//		}
//		else
//		{
//			CurrentTiltState = TiltHALT;
//		}
//	}
//
//
//}

// ：实际压力值 - 50 >= 设定压力值时,允许开启。改成百分比的形式。
//int Auto_TiltControl()
//{
//	if (PV_App.RunMode != 0)
//	{
//		TiltWorking_Mode = Tilt_Auto_Mode;
//		if (GV.ForceValue >= PV_App.EndPushForceValue - 50 && GV.ForceValue <= PV_App.EndPushForceValue + 100)
//		{
//			CurrentTiltState = TiltHALT;
//			TiltWorking_Mode = Tilt_Manual_Mode;
//			return 1;
//		}
//		else if (GV.ForceValue >= PV_App.EndPushForceValue + 100)
//		{
//			CurrentTiltState = TiltUP;
//			return 0;
//		}
//		else if (GV.ForceValue <= PV_App.EndPushForceValue - 50)
//		{
//			CurrentTiltState = TiltDown;
//			return 0;
//		}
//	}
//	else
//	{
//		return 0;
//	}
//}




// 压力值界限改成百分比形式
#define FORCE_LOWER_PERCENT 5
#define FORCE_UPPER_PERCENT 10




void TiltControl()
{

    if (TiltWorking_Mode == Tilt_Manual_Mode && PV_App.RunMode != 0)
    {

        int lowerBound = PV_App.EndPushForceValue - PV_App.EndPushForceValue * FORCE_LOWER_PERCENT / 100;

        // 前端下降操作判断
        if (P_MK32->CH1_RY_V <= -900)
        {
            if (GV.ForceValue >= lowerBound)
            {
                CurrentTiltState = TiltHALT;
            }
            else
            {
                CurrentTiltState = TiltDown;
            }
        }
        // 前端上升操作判断
        else if (P_MK32->CH1_RY_V >= 900)
        {
            CurrentTiltState = TiltUP;
        }
        // 无操作时停止
        else
        {
            CurrentTiltState = TiltHALT;
        }
    }
}



int Auto_TiltControl()
{
    if (PV_App.RunMode != 0)
    {
        TiltWorking_Mode = Tilt_Auto_Mode;

        // 计算边界值
        int lowerBound = PV_App.EndPushForceValue - PV_App.EndPushForceValue * FORCE_LOWER_PERCENT / 100;
        int upperBound = PV_App.EndPushForceValue + PV_App.EndPushForceValue * FORCE_UPPER_PERCENT / 100;

        // 压力判断逻辑
        if (GV.ForceValue >= lowerBound && GV.ForceValue <= upperBound)
        {
            CurrentTiltState = TiltHALT;
            TiltWorking_Mode = Tilt_Manual_Mode;
            return 1;
        }
        else if (GV.ForceValue >= upperBound)
        {
            CurrentTiltState = TiltUP;
            return 0;
        }
        else if (GV.ForceValue <= lowerBound)
        {
            CurrentTiltState = TiltDown;
            return 0;
        }
    }
    else
    {
        return 0;
    }

}


/*
 * 拉毛前端控制
 * mode_0 ：停止； mode_1 ：直接启动拉毛电机
 */
void DHRougheningControl(int mode)
{
	//拉毛部分
	if (mode == 1 && PV_App.RunMode != 0)
	{
		if (PV_App.ToolRotationDirection == 1)
		{
			CurrentRougheningEndState = RougheningEndSwingClockwise;
		}
		else if (PV_App.ToolRotationDirection == 2)
		{
			CurrentRougheningEndState = RougheningEndSwingAnticlockwise;
		}
		else if (PV_App.ToolRotationDirection == 0)
		{
			CurrentRougheningEndState = RougheningEndHALT;
		}
	}
	else
	{
		CurrentRougheningEndState = RougheningEndHALT;

	}

}





/*
 * 拉毛前端控制
 * mode_0 ：停止； mode_1 ：直接启动拉毛电机
 */
void DHRougheningControl_Auto_TiltControl()
{

	if (PV_App.RunMode == Move_Manual || PV_App.RunMode == Move_Horizontal_Move
	        || PV_App.RunMode == Move_Vertical_Move_Left || PV_App.RunMode == Move_Vertical_Move_Right)
	{
		if (P_MK32->CH6_SC == -1000)  //
		{

			switch (AutoMoveStart)
			{
				case Adjust_Pressure:
				{
					if (Auto_TiltControl() == 1)
					{
						AutoMoveStart = Launch_Tool;
					}
					break;
				}
				case Launch_Tool:
				{
					DHRougheningControl(1);
					break;
				}

				default:
					break;

			}

		}
		else
		{
			TiltWorking_Mode = Tilt_Manual_Mode;
			AutoMoveStart = Adjust_Pressure;
			CurrentTiltState = TiltHALT;
			CurrentRougheningEndState = RougheningEndHALT;

		}
	}
	else
	{

	}

}


/*
 * 换道控制
 *
 */
int LaneChangeControl()
{
	//m/min = 100cm/60s  = 10/6 =    5/3 cm/s  =5*1000/3 cm/ms

	if (P_MK32->CH4_SA == -1000)
	{

		if (PV_App.RunMode == Move_Horizontal_Move)	//水平换道
		{
			IV.RunMode = IV_Run_Mode_Horizontal_LaneChange;
			if (HorizontalLaneChangeState == Lane_Change_Start)
			{
				if (Horizontal_LaneChangeFlag == Horizontal_Head_To_Left_Flag)
				{
					Horizontal_Lane_Change_Turn_To_Left_Control();
				}
				else if (Horizontal_LaneChangeFlag == Horizontal_Head_To_Right_Flag)
				{
					Horizontal_Lane_Change_Turn_To_Right_Control();
				}

			}
		}
		else if (PV_App.RunMode == Move_Vertical_Move_Left || PV_App.RunMode == Move_Vertical_Move_Right)
		{
			IV.RunMode = IV_Run_Mode_Vertical_LaneChange;
			if (VerticalLaneChangeState == Lane_Change_Start)
			{

				if (Vertical_LaneChangeFlag == Veritical_To_Left_Flag)
				{
					Vertical_Lane_Change_Left_Control();
				}
				else if (Vertical_LaneChangeFlag == Veritical_To_Right_Flag)
				{
					Vertical_Lane_Change_Right_Control();
				}

			}
		}

	}
//	else if (P_MK32->CH4_SA == 1000)  //竖直换道
//	{
//
//
//	}
	else
	{
		//换道初始化设定初始方向
		HorizontalLaneChangeState = Lane_Change_Start;
		CurrentHorizontal_ChangeState = HorizontalChange_StateZero;
		VerticalLaneChangeState = Lane_Change_Start;
		Current_Vertical_ChangeState = VerticalChange_StateZero;

		//头朝左
		if (abs(GV.Robot_Angle - CV.RobotLeftAngleValue) <= Horizontal_Angle_threshold * 100)
		{
			// 头朝左，换道意味着头
			Horizontal_LaneChangeFlag = Horizontal_Head_To_Right_Flag;
		}
		else if (abs(GV.Robot_Angle - CV.RobotRightAngleValue) <= Horizontal_Angle_threshold * 100)
		{
			Horizontal_LaneChangeFlag = Horizontal_Head_To_Left_Flag;
		}
		else
		{
			Horizontal_LaneChangeFlag = Horizontal_No;
		}

		if (PV_App.RunMode == Move_Vertical_Move_Left)
		{
			Vertical_LaneChangeFlag = Veritical_To_Left_Flag;
		}
		else if (PV_App.RunMode == Move_Vertical_Move_Right)
		{
			Vertical_LaneChangeFlag = Veritical_To_Right_Flag;
		}
		else
		{
			Vertical_LaneChangeFlag = Veritical_No;
		}

	}

	return P_MK32->CH4_SA;
}

/*
 * 水平向下换道 完成后头朝右
 */
void Horizontal_Lane_Change_Turn_To_Right_Control()
{
	GV.Robot_Move_Speed = CV.Lane_Change_Speed_M_Min / 0.880 * 101 * 10;
	switch (CurrentHorizontal_ChangeState)
	{
		case HorizontalChange_StateZero:
		{
			CurrentHorizontal_ChangeState = HorizontalChange_TurnToUP;
			break;
		}
		case HorizontalChange_TurnToUP:
		{
			if (abs(GV.Robot_Angle - CV.RobotUpAngleValue) >= LanChangeAngleHolds)
			{
				CurrentMoveState = Move_Head_To_UP_Enum; //转到上面
			}
			else
			{

				//开启计时
				timer_handler_1.start_timer = 1;
				CurrentHorizontal_ChangeState = HorizontalChange_Delay1;

//				CurrentMoveState = Move_Vertical_Task_Backwards; //转到左边，前进
//				//开启计时
//				timer_handler_1.start_timer = 1;
//				CurrentHorizontal_ChangeState = HorizontalChange_DelayMove;

			}
			break;
		}

		case HorizontalChange_Delay1:
		{
			CurrentMoveState = Move_HALT; //
			if (CompareTimer(LaneChangeWaittimeConstant, &timer_handler_1))	//计时结束
			{
				CurrentMoveState = Move_Vertical_Task_Backwards; //转到左边，前进
				//开启计时
				timer_handler_1.start_timer = 1;
				CurrentHorizontal_ChangeState = HorizontalChange_DelayMove;

			}
			break;
		}

		case HorizontalChange_DelayMove:
		{
			//m/min 100cm/60s  100cm/(60*1000)mm 1cm/600ms

			if (PV_App.LaneChangeDistance > 15)
			{
				LaneChangeWaittime = 600 * (PV_App.LaneChangeDistance - VerticalLaneChangeDistanceCalibration)//6cm是下降距离
				/ CV.Lane_Change_Speed_M_Min;
			}
			else
			{
				LaneChangeWaittime = 600 * (PV_App.LaneChangeDistance)				//6cm是下降距离
				/ CV.Lane_Change_Speed_M_Min;
			}

			if (CompareTimer(LaneChangeWaittime, &timer_handler_1))
			{
				CurrentHorizontal_ChangeState = HorizontalChange_TurnToRight;
				CurrentMoveState = Move_Head_To_Right_Enum; //头朝RightAngle
			}
			break;
		}
		case HorizontalChange_TurnToRight:
		{
			CurrentMoveState = Move_Head_To_Right_Enum; //头朝RightAngle
			if (abs(GV.Robot_Angle - CV.RobotRightAngleValue) <= LanChangeAngleHolds)
			{
				CurrentHorizontal_ChangeState = HorizontalChange_End;
			}
			break;
		}
		case HorizontalChange_End:
		{
			HorizontalLaneChangeState = Lane_Change_Stop;
			break;
		}

	}

}

/*
 * 水平向下换道 完成后头朝左
 */
void Horizontal_Lane_Change_Turn_To_Left_Control()
{
	GV.Robot_Move_Speed = CV.Lane_Change_Speed_M_Min / 0.880 * 101 * 10;
	switch (CurrentHorizontal_ChangeState)
	{
		case HorizontalChange_StateZero:
		{
			CurrentHorizontal_ChangeState = HorizontalChange_TurnToUP;
			break;
		}
		case HorizontalChange_TurnToUP:
		{
			if (abs(GV.Robot_Angle - CV.RobotUpAngleValue) >= LanChangeAngleHolds)
			{
				CurrentMoveState = Move_Head_To_UP_Enum; //转到上面
			}
			else
			{

				CurrentMoveState = Move_HALT;
				timer_handler_1.start_timer = 1;
				CurrentHorizontal_ChangeState = HorizontalChange_Delay1;
//				CurrentMoveState = Move_Vertical_Task_Backwards; //转到左边，前进
//				//开启计时
//				timer_handler_1.start_timer = 1;
//				CurrentHorizontal_ChangeState = HorizontalChange_DelayMove;
			}
			break;
		}
		case HorizontalChange_Delay1:
		{
			CurrentMoveState = Move_HALT; //
			if (CompareTimer(LaneChangeWaittimeConstant, &timer_handler_1))	//计时结束
			{
				CurrentMoveState = Move_Vertical_Task_Backwards; //转到左边，前进
				//开启计时
				timer_handler_1.start_timer = 1;
				CurrentHorizontal_ChangeState = HorizontalChange_DelayMove;

			}
			break;
		}

		case HorizontalChange_DelayMove:
		{

			if (PV_App.LaneChangeDistance > 15)
			{
				LaneChangeWaittime = 600 * (PV_App.LaneChangeDistance - VerticalLaneChangeDistanceCalibration)//6cm是下降距离
				/ CV.Lane_Change_Speed_M_Min;
			}
			else
			{
				LaneChangeWaittime = 600 * (PV_App.LaneChangeDistance)				//5cm是下降距离
				/ CV.Lane_Change_Speed_M_Min;
			}

			if (CompareTimer(LaneChangeWaittime, &timer_handler_1))		//计时结束
			{
				CurrentHorizontal_ChangeState = HorizontalChange_TurnToLeft;
				CurrentMoveState = Move_Head_To_Left_Enum; //头朝LeftAngle
			}
			break;
		}
		case HorizontalChange_TurnToLeft:
		{
			CurrentMoveState = Move_Head_To_Left_Enum; //头朝LeftAngle
			if (abs(GV.Robot_Angle - CV.RobotLeftAngleValue) <= LanChangeAngleHolds)
			{
				CurrentHorizontal_ChangeState = HorizontalChange_End;
			}
			break;
		}
		case HorizontalChange_End:
		{
			HorizontalLaneChangeState = Lane_Change_Stop;
			break;
		}

	}

}

/*
 * 竖直向左换道
 */
void Vertical_Lane_Change_Left_Control()
{

	GV.Robot_Move_Speed = CV.Lane_Change_Speed_M_Min / 0.880 * 101 * 10;
	switch (Current_Vertical_ChangeState)
	{
		case VerticalChange_StateZero:
		{
			Current_Vertical_ChangeState = VerticalChange_TurnToRight;

			break;
		}
		case VerticalChange_TurnToRight:
		{
			if (abs(GV.Robot_Angle - CV.RobotRightAngleValue) >= LanChangeAngleHolds)
			{
				CurrentMoveState = Move_Head_To_Right_Enum;
			}
			else
			{

//				CurrentMoveState = VerticalChange_Delay1;
//				timer_handler_1.start_timer = 1;
				CurrentMoveState = Move_Horizontal_Task_Backwards_Right; //转到左边，前进
				//开启计时
				timer_handler_1.start_timer = 1;
				//GV.RightMotor.Start_Measuring = 1;
				Current_Vertical_ChangeState = VerticalChange_DelayMove;
			}
			break;
		}

		case VerticalChange_DelayMove:
		{
			//竖直换道不管，经测试，基本无误差
			LaneChangeWaittime = 600 * PV_App.LaneChangeDistance / CV.Lane_Change_Speed_M_Min;
			if (CompareTimer(LaneChangeWaittime, &timer_handler_1))
			{

				Current_Vertical_ChangeState = VerticalChange_TurnToUP;
			}
			break;
		}
		case VerticalChange_TurnToUP:

		{
			if (abs(GV.Robot_Angle - CV.RobotUpAngleValue) >= LanChangeAngleHolds)
			{
				CurrentMoveState = Move_Head_To_UP_Enum;
			}
			else
			{
				Current_Vertical_ChangeState = VerticalChange_End;
			}

			break;
		}
		case VerticalChange_End:
		{
			VerticalLaneChangeState = Lane_Change_Stop;
			break;
		}

	}

}

/*
 * 竖直向右换道
 */
void Vertical_Lane_Change_Right_Control()
{
	GV.Robot_Move_Speed = CV.Lane_Change_Speed_M_Min / 0.880 * 101 * 10;
	switch (Current_Vertical_ChangeState)
	{
		case VerticalChange_StateZero:
		{
			Current_Vertical_ChangeState = VerticalChange_TurnToLeft;
			break;
		}
		case VerticalChange_TurnToLeft:
		{
			if (abs(GV.Robot_Angle - CV.RobotLeftAngleValue) >= LanChangeAngleHolds)
			{
				CurrentMoveState = Move_Head_To_Left_Enum; //
			}
			else
			{
				CurrentMoveState = Move_HALT; //转到左边，前进
				//开启计时
				timer_handler_1.start_timer = 1;

//				CurrentMoveState = Move_Horizontal_Task_Backwards_Left; //转到左边，前进
//				//开启计时
//				timer_handler_1.start_timer = 1;
//				//GV.RightMotor.Start_Measuring = 1;
				//Current_Vertical_ChangeState = VerticalChange_DelayMove;
				Current_Vertical_ChangeState = VerticalChange_Delay1;
			}
			break;
		}

		case VerticalChange_Delay1:
		{
			if (CompareTimer(LaneChangeWaittimeConstant, &timer_handler_1))	//计时结束
			{
				CurrentMoveState = Move_Horizontal_Task_Backwards_Left; //
				//开启计时
				timer_handler_1.start_timer = 1;
				//GV.RightMotor.Start_Measuring = 1;
				Current_Vertical_ChangeState = VerticalChange_DelayMove;

			}
			break;
		}
		case VerticalChange_DelayMove:
		{
			LaneChangeWaittime = 600 * PV_App.LaneChangeDistance / CV.Lane_Change_Speed_M_Min;
			if (CompareTimer(LaneChangeWaittime, &timer_handler_1))
			{
				Current_Vertical_ChangeState = VerticalChange_TurnToUP;
			}
			break;
		}
		case VerticalChange_TurnToUP:
		{
			if (abs(GV.Robot_Angle - CV.RobotUpAngleValue) >= LanChangeAngleHolds)
			{
				CurrentMoveState = Move_Head_To_UP_Enum;
			}
			else
			{
				Current_Vertical_ChangeState = VerticalChange_End;
			}

			break;
		}
		case VerticalChange_End:
		{
			VerticalLaneChangeState = Lane_Change_Stop;
			break;
		}

	}

}

int AutoMoveStartFlag = 0;

double Err_Left = 0;

double Err_Right = 0;

double Err_Horizontal_Hold = 0.06;

double Err_Vertical_Up_Hold = 0.04;

double Err_Vertical_Down_Hold = 0.04;

/*
 * 计算水平 实际距离与理论距离误差
 */
void RegionAuto_Horizontal_Distance_Err()
{
	Err_Left = fabs(GV.LeftMotor.Real_Disatnce)
	//* cos(PI * GV.Left_Compensation / 180.0)
	        - PV_App.WorkDistanceMeters;
	Err_Right = fabs(GV.RightMotor.Real_Disatnce)
	//* cos(PI * GV.Left_Compensation / 180.0)
	        - PV_App.WorkDistanceMeters;
}
/*
 * 计算竖直 实际距离与理论距离误差
 */
void RegionAuto_Vertical_Distance_Err()
{
	Err_Left = fabs(GV.LeftMotor.Real_Disatnce)
	//* cos(PI * GV.Left_Compensation / 180.0)
	        - PV_App.WorkWidthMeters;
	Err_Right = fabs(GV.RightMotor.Real_Disatnce)
	//* cos(PI * GV.Left_Compensation / 180.0)
	        - PV_App.WorkWidthMeters;
}

/*
 *
 * DistanceMeters: m
 * return: 1 true,0 false
 */
int MoveTaskIsOk(double DistanceMeters)
{
	Region_Move_time = 60000 * DistanceMeters / PV_App.RobotSpeed;
	if (CompareTimer(Region_Move_time, &timer_handler_1))
	{
		MoveTask_Reset();
		return 1;
	}
	else
	{
		return 0;
	}

}

/*
 *
 *
 */
int MoveTask_Start()

{
	if (MoveTaskTimeCheck == MoveTaskTimeCheck_finish)
	{
		MoveTaskTimeCheck = MoveTaskTimeCheck_start;
		timer_handler_1.start_timer = 1;
	}

}

/*
 *
 *
 */
int MoveTask_Reset()
{
	MoveTaskTimeCheck = MoveTaskTimeCheck_finish;
}

/*
 * 测试定距离行进
 *
 */
void RegionAuto_MoveTask_Test(int mode)
{

	switch (Current_Auto_Move_State)
	{

		case Auto_Move_Start:
		{
			timer_handler_1.start_timer = 1;
			Current_Auto_Move_State = Auto_Move_Time_Wait;
			break;
		}

		case Auto_Move_Time_Wait:
		{
			CurrentMoveState = Move_HALT;
			if (CompareTimer(LaneChangeWaittimeConstant, &timer_handler_1))
			{
				GV.RightMotor.Start_Measuring = 1;
				GV.LeftMotor.Start_Measuring = 1;
				Current_Auto_Move_State = Auto_Move_HeadToRight_Backwards;

			}

			break;
		}
		case Auto_Move_HeadToRight_Backwards:
		{
			if (GV.RightMotor.Start_Measuring == 0 && GV.LeftMotor.Start_Measuring == 0) //已经记录运动起始点
			{
				if (mode == 1)
				{
					Manual_Mode_Move_Forwards();
				}
				else if (mode == 2)
				{
					Manual_Mode_Move_Backwards();
				}

				MoveTask_Start();
				RegionAuto_Horizontal_Distance_Err();

				if (PV_App.WorkDistanceMeters > 2)
				{
					if (Err_Left - Err_Horizontal_Hold >= 0 || Err_Right - Err_Horizontal_Hold >= 0)
					{
						HorizontalLaneChangeState = Lane_Change_Start;
						Current_Auto_Move_State = Auto_Move_Stop;
						CurrentHorizontal_ChangeState = HorizontalChange_StateZero;
						Auto_Move_Cycle_Count++;
						RegionAuto_MoveTask_Test_Result = 1;
					}
					if (MoveTaskIsOk(PV_App.WorkDistanceMeters + Err_Horizontal_Hold) == 1)
					{
						HorizontalLaneChangeState = Lane_Change_Start;
						Current_Auto_Move_State = Auto_Move_Stop;
						CurrentHorizontal_ChangeState = HorizontalChange_StateZero;
						Auto_Move_Cycle_Count++;
						RegionAuto_MoveTask_Test_Result = 2;
					}
				}
				else
				{
					if (Err_Left >= 0 || Err_Right >= 0)
					{
						HorizontalLaneChangeState = Lane_Change_Start;
						Current_Auto_Move_State = Auto_Move_Stop;
						CurrentHorizontal_ChangeState = HorizontalChange_StateZero;
						Auto_Move_Cycle_Count++;
						RegionAuto_MoveTask_Test_Result = 3;
					}
					if (MoveTaskIsOk(PV_App.WorkDistanceMeters + Err_Horizontal_Hold) == 1)
					{
						HorizontalLaneChangeState = Lane_Change_Start;
						Current_Auto_Move_State = Auto_Move_Stop;
						CurrentHorizontal_ChangeState = HorizontalChange_StateZero;
						Auto_Move_Cycle_Count++;
						RegionAuto_MoveTask_Test_Result = 4;
					}
				}

			}
			break;
		}
		case Auto_Move_Stop:
		{
			CurrentMoveState = Move_HALT;
			break;
		}
	}
}

/*
 * 自动区域作业 水平右向左后退
 */
void RegionAuto_Horizontal_RightToLeft()
{
	if (PV_App.LaneChangeDistance == 0)
	{
		return;
	}
	Auto_Move_Cycle_Total_Count = PV_App.WorkWidthMeters * 100 / PV_App.LaneChangeDistance;

	if (Auto_Move_Cycle_Count >= Auto_Move_Cycle_Total_Count)
	{

		CurrentMoveState = Move_HALT;
		CurrentRougheningEndState = RougheningEndHALT;
	}
	else
	{
		switch (Current_Auto_Move_State)
		{
			case Auto_Move_Stop:
			{
				CurrentMoveState = Move_HALT;
				break;
			}

			case Auto_Move_Start:
			{
				if (abs(GV.Robot_Angle - CV.RobotRightAngleValue) >= 200)
				{
					CurrentMoveState = Move_Head_To_Right_Enum; //右
				}
				else
				{
					timer_handler_1.start_timer = 1;
					Current_Auto_Move_State = Auto_Move_Time_Wait;
				}
				break;
			}

			case Auto_Move_Time_Wait:
			{
				CurrentMoveState = Move_HALT;
				if (CompareTimer(LaneChangeWaittimeConstant, &timer_handler_1))
				{
					GV.RightMotor.Start_Measuring = 1;
					GV.LeftMotor.Start_Measuring = 1;
					Current_Auto_Move_State = Auto_Move_HeadToRight_Backwards;
				}

				break;
			}

			case Auto_Move_HeadToRight_Backwards:
			{
				if (GV.RightMotor.Start_Measuring == 0 && GV.LeftMotor.Start_Measuring == 0) //已经记录运动起始点
				{
					CurrentMoveState = Move_Horizontal_Task_Backwards_Right; //backwards

					MoveTask_Start();
					RegionAuto_Horizontal_Distance_Err();

					if (PV_App.WorkDistanceMeters > 2)
					{
						if (MoveTaskIsOk(PV_App.WorkDistanceMeters + Err_Horizontal_Hold) == 1
						        || Err_Left - Err_Horizontal_Hold >= 0 || Err_Right - Err_Horizontal_Hold >= 0)
						{
							MoveTask_Reset();
							HorizontalLaneChangeState = Lane_Change_Start;
							CurrentHorizontal_ChangeState = HorizontalChange_StateZero;
							Current_Auto_Move_State = Auto_Move_Horizontal_LaneChange_Left;
							Auto_Move_Cycle_Count++;
						}
					}
					else
					{
						if (MoveTaskIsOk(PV_App.WorkDistanceMeters + Err_Horizontal_Hold) == 1 || Err_Left >= 0
						        || Err_Right >= 0)
						{
							MoveTask_Reset();
							HorizontalLaneChangeState = Lane_Change_Start;
							CurrentHorizontal_ChangeState = HorizontalChange_StateZero;
							Current_Auto_Move_State = Auto_Move_Horizontal_LaneChange_Left;
							Auto_Move_Cycle_Count++;
						}
					}
				}

				break;
			}

			case Auto_Move_Horizontal_LaneChange_Left:
			{
				Horizontal_Lane_Change_Turn_To_Left_Control();
				if (HorizontalLaneChangeState == Lane_Change_Stop)
				{
					timer_handler_1.start_timer = 1;
					Current_Auto_Move_State = Auto_Move_Time_Wait2;
				}
				break;
			}
			case Auto_Move_Time_Wait2:
			{
				CurrentMoveState = Move_HALT;
				if (CompareTimer(LaneChangeWaittimeConstant, &timer_handler_1))
				{
					GV.RightMotor.Start_Measuring = 1;
					GV.LeftMotor.Start_Measuring = 1;
					Current_Auto_Move_State = Auto_Move_HeadToLeft_Backwards;

				}
				break;
			}

			case Auto_Move_HeadToLeft_Backwards:
			{
				if (GV.RightMotor.Start_Measuring == 0 && GV.LeftMotor.Start_Measuring == 0) //已经属于在等到了，跟其他的一样
				{
					CurrentMoveState = Move_Horizontal_Task_Backwards_Left; //backwards

					MoveTask_Start();
					RegionAuto_Horizontal_Distance_Err();

					if (PV_App.WorkDistanceMeters > 2)
					{
						if (MoveTaskIsOk(PV_App.WorkDistanceMeters + Err_Horizontal_Hold) == 1
						        || Err_Left - Err_Horizontal_Hold >= 0 || Err_Right - Err_Horizontal_Hold >= 0)
						{
							MoveTask_Reset();
							HorizontalLaneChangeState = Lane_Change_Start;
							CurrentHorizontal_ChangeState = HorizontalChange_StateZero;
							Current_Auto_Move_State = Auto_Move_Horizontal_LaneChange_Right;
							Auto_Move_Cycle_Count++;
						}
					}
					else
					{
						if (MoveTaskIsOk(PV_App.WorkDistanceMeters + Err_Horizontal_Hold) == 1 || Err_Left >= 0
						        || Err_Right >= 0)
						{
							MoveTask_Reset();
							HorizontalLaneChangeState = Lane_Change_Start;
							CurrentHorizontal_ChangeState = HorizontalChange_StateZero;
							Current_Auto_Move_State = Auto_Move_Horizontal_LaneChange_Right;
							Auto_Move_Cycle_Count++;
						}
					}

				}

				break;
			}
			case Auto_Move_Horizontal_LaneChange_Right:
			{
				Horizontal_Lane_Change_Turn_To_Right_Control();
				if (HorizontalLaneChangeState == Lane_Change_Stop)
				{
					Current_Auto_Move_State = Auto_Move_Time_Wait;
					timer_handler_1.start_timer = 1;
				}
				break;
			}
		}
	}
}

/*
 * 自动区域作业 水平左向右后退
 */
void RegionAuto_Horizontal_LeftToRight()
{
	if (PV_App.LaneChangeDistance == 0)
	{
		return;
	}
	Auto_Move_Cycle_Total_Count = PV_App.WorkWidthMeters * 100 / PV_App.LaneChangeDistance;

	if (Auto_Move_Cycle_Count >= Auto_Move_Cycle_Total_Count)
	{
		CurrentMoveState = Move_HALT;
		CurrentRougheningEndState = RougheningEndHALT;
	}
	else
	{
		switch (Current_Auto_Move_State)
		{
			case Auto_Move_Stop:
			{
				CurrentMoveState = Move_HALT;
				break;
			}
			case Auto_Move_Start:
			{
				if (abs(GV.Robot_Angle - CV.RobotLeftAngleValue) >= 200)
				{
					CurrentMoveState = Move_Head_To_Left_Enum; //左
				}
				else
				{
					timer_handler_1.start_timer = 1;
					Current_Auto_Move_State = Auto_Move_Time_Wait;
				}
				break;
			}

			case Auto_Move_Time_Wait:
			{
				CurrentMoveState = Move_HALT;
				if (CompareTimer(LaneChangeWaittimeConstant, &timer_handler_1))
				{
					GV.RightMotor.Start_Measuring = 1;
					GV.LeftMotor.Start_Measuring = 1;
					Current_Auto_Move_State = Auto_Move_HeadToLeft_Backwards;
				}

				break;
			}

			case Auto_Move_HeadToLeft_Backwards:
			{
				if (GV.RightMotor.Start_Measuring == 0 && GV.LeftMotor.Start_Measuring == 0) //已经记录运动起始点
				{
					CurrentMoveState = Move_Horizontal_Task_Backwards_Left; //backwards

					MoveTask_Start();
					RegionAuto_Horizontal_Distance_Err();

					if (PV_App.WorkDistanceMeters > 2)
					{
						if (MoveTaskIsOk(PV_App.WorkDistanceMeters + Err_Horizontal_Hold) == 1
						        || Err_Left - Err_Horizontal_Hold >= 0 || Err_Right - Err_Horizontal_Hold >= 0)
						{
							MoveTask_Reset();
							HorizontalLaneChangeState = Lane_Change_Start;
							CurrentHorizontal_ChangeState = HorizontalChange_StateZero;
							Current_Auto_Move_State = Auto_Move_Horizontal_LaneChange_Right;
							Auto_Move_Cycle_Count++;
						}
					}
					else
					{
						if (MoveTaskIsOk(PV_App.WorkDistanceMeters + Err_Horizontal_Hold) == 1 || Err_Left >= 0
						        || Err_Right >= 0)
						{
							MoveTask_Reset();
							HorizontalLaneChangeState = Lane_Change_Start;
							CurrentHorizontal_ChangeState = HorizontalChange_StateZero;
							Current_Auto_Move_State = Auto_Move_Horizontal_LaneChange_Right;
							Auto_Move_Cycle_Count++;
						}
					}
				}

				break;
			}

			case Auto_Move_Horizontal_LaneChange_Right:
			{
				Horizontal_Lane_Change_Turn_To_Right_Control();
				if (HorizontalLaneChangeState == Lane_Change_Stop)
				{
					timer_handler_1.start_timer = 1;
					Current_Auto_Move_State = Auto_Move_Time_Wait2;
				}
				break;
			}
			case Auto_Move_Time_Wait2:
			{
				CurrentMoveState = Move_HALT;
				if (CompareTimer(LaneChangeWaittimeConstant, &timer_handler_1))
				{
					GV.RightMotor.Start_Measuring = 1;
					GV.LeftMotor.Start_Measuring = 1;
					Current_Auto_Move_State = Auto_Move_HeadToRight_Backwards;

				}
				break;
			}

			case Auto_Move_HeadToRight_Backwards:
			{
				if (GV.RightMotor.Start_Measuring == 0 && GV.LeftMotor.Start_Measuring == 0) //已经属于在等到了，跟其他的一样
				{
					CurrentMoveState = Move_Horizontal_Task_Backwards_Right; //backwards

					MoveTask_Start();
					RegionAuto_Horizontal_Distance_Err();

					if (PV_App.WorkDistanceMeters > 2)
					{
						if (MoveTaskIsOk(PV_App.WorkDistanceMeters + Err_Horizontal_Hold) == 1
						        || Err_Left - Err_Horizontal_Hold >= 0 || Err_Right - Err_Horizontal_Hold >= 0)
						{
							MoveTask_Reset();
							HorizontalLaneChangeState = Lane_Change_Start;
							CurrentHorizontal_ChangeState = HorizontalChange_StateZero;
							Current_Auto_Move_State = Auto_Move_Horizontal_LaneChange_Left;
							Auto_Move_Cycle_Count++;
						}
					}
					else
					{
						if (MoveTaskIsOk(PV_App.WorkDistanceMeters + Err_Horizontal_Hold) == 1 || Err_Left >= 0
						        || Err_Right >= 0)
						{
							MoveTask_Reset();
							HorizontalLaneChangeState = Lane_Change_Start;
							CurrentHorizontal_ChangeState = HorizontalChange_StateZero;
							Current_Auto_Move_State = Auto_Move_Horizontal_LaneChange_Left;
							Auto_Move_Cycle_Count++;
						}
					}

				}

				break;
			}
			case Auto_Move_Horizontal_LaneChange_Left:
			{
				Horizontal_Lane_Change_Turn_To_Left_Control();
				if (HorizontalLaneChangeState == Lane_Change_Stop)
				{
					Current_Auto_Move_State = Auto_Move_Time_Wait;
					timer_handler_1.start_timer = 1;
				}
				break;
			}
		}
	}
}

/*
 * 自动区域作业 竖直后退 右向左
 */
void RegionAuto_Vertical_RightToLeft()
{
	if (PV_App.LaneChangeDistance == 0)
	{
		return;
	}
	Auto_Move_Cycle_Total_Count = PV_App.WorkDistanceMeters * 100 / PV_App.LaneChangeDistance;

	if (Auto_Move_Cycle_Count >= Auto_Move_Cycle_Total_Count)
	{
		CurrentMoveState = Move_HALT;
		CurrentRougheningEndState = RougheningEndHALT;
	}
	else
	{
		switch (Current_Auto_Move_State)
		{
			case Auto_Move_Stop:
			{
				CurrentMoveState = Move_HALT;
				break;
			}
			case Auto_Move_Start:
			{
				if (abs(GV.Robot_Angle - CV.RobotUpAngleValue) >= 200)
				{
					CurrentMoveState = Move_Head_To_UP_Enum; //右
				}
				else
				{
					timer_handler_1.start_timer = 1;
					Current_Auto_Move_State = Auto_Move_Time_Wait;
				}
				break;
			}

			case Auto_Move_Time_Wait:
			{
				CurrentMoveState = Move_HALT;
				if (CompareTimer(LaneChangeWaittimeConstant, &timer_handler_1))
				{
					GV.RightMotor.Start_Measuring = 1;
					GV.LeftMotor.Start_Measuring = 1;
					Current_Auto_Move_State = Auto_Move_HeadToUp_Forwards;
				}

				break;
			}

			case Auto_Move_HeadToUp_Forwards:
			{
				if (GV.RightMotor.Start_Measuring == 0 && GV.LeftMotor.Start_Measuring == 0) //已经记录运动起始点
				{
					CurrentMoveState = Move_Vertical_Task_Forwards; //Forwards

					MoveTask_Start();
					RegionAuto_Vertical_Distance_Err();

					if (MoveTaskIsOk(PV_App.WorkWidthMeters + Err_Vertical_Up_Hold) == 1
							|| Err_Left - PV_App.WorkWidthMeters * Err_Vertical_Up_Hold >= 0
							|| Err_Right - PV_App.WorkWidthMeters * Err_Vertical_Up_Hold >= 0)
					{
						MoveTask_Reset();
						VerticalLaneChangeState = Lane_Change_Start;
						Current_Vertical_ChangeState = VerticalChange_StateZero;
						Current_Auto_Move_State = Auto_Move_Vertical_LaneChange_Left;
						Auto_Move_Cycle_Count++;
					}

				}

				break;
			}

			case Auto_Move_Vertical_LaneChange_Left:
			{
				Vertical_Lane_Change_Left_Control();
				if (VerticalLaneChangeState == Lane_Change_Stop)
				{
					timer_handler_1.start_timer = 1;
					Current_Auto_Move_State = Auto_Move_Time_Wait2;
				}
				break;
			}
			case Auto_Move_Time_Wait2:
			{
				CurrentMoveState = Move_HALT;
				if (CompareTimer(LaneChangeWaittimeConstant, &timer_handler_1))
				{
					GV.RightMotor.Start_Measuring = 1;
					GV.LeftMotor.Start_Measuring = 1;
					Current_Auto_Move_State = Auto_Move_HeadToUp_Backwards;

				}
				break;
			}

			case Auto_Move_HeadToUp_Backwards:
			{
				if (GV.RightMotor.Start_Measuring == 0 && GV.LeftMotor.Start_Measuring == 0) //已经属于在等到了，跟其他的一样
				{
					CurrentMoveState = Move_Vertical_Task_Backwards; //backwards

					MoveTask_Start();
					RegionAuto_Vertical_Distance_Err();

					if (MoveTaskIsOk(PV_App.WorkWidthMeters + Err_Vertical_Down_Hold) == 1
							|| Err_Left + PV_App.WorkWidthMeters * Err_Vertical_Down_Hold >= 0
							|| Err_Right + PV_App.WorkWidthMeters * Err_Vertical_Down_Hold >= 0)
					{
						MoveTask_Reset();
						VerticalLaneChangeState = Lane_Change_Start;
						Current_Vertical_ChangeState = VerticalChange_StateZero;
						Current_Auto_Move_State = Auto_Move_Vertical_LaneChange_Left2;
						Auto_Move_Cycle_Count++;
					}

				}

				break;
			}
			case Auto_Move_Vertical_LaneChange_Left2:
			{
				Vertical_Lane_Change_Left_Control();
				if (VerticalLaneChangeState == Lane_Change_Stop)
				{
					Current_Auto_Move_State = Auto_Move_Time_Wait;
					timer_handler_1.start_timer = 1;
				}
				break;
			}
		}
	}
}

/*
 * 自动区域作业 竖直后退 左向右
 */
void RegionAuto_Vertical_LeftToRight()
{
	if (PV_App.LaneChangeDistance == 0)
	{
		return;
	}
	Auto_Move_Cycle_Total_Count = PV_App.WorkDistanceMeters * 100 / PV_App.LaneChangeDistance;

	if (Auto_Move_Cycle_Count >= Auto_Move_Cycle_Total_Count)
	{
		CurrentMoveState = Move_HALT;
		CurrentRougheningEndState = RougheningEndHALT;
	}
	else
	{
		switch (Current_Auto_Move_State)
		{
			case Auto_Move_Stop:
			{
				CurrentMoveState = Move_HALT;
				break;
			}
			case Auto_Move_Start:
			{
				if (abs(GV.Robot_Angle - CV.RobotUpAngleValue) >= 200)
				{
					CurrentMoveState = Move_Head_To_UP_Enum; //右
				}
				else
				{
					timer_handler_1.start_timer = 1;
					Current_Auto_Move_State = Auto_Move_Time_Wait;
				}
				break;
			}

			case Auto_Move_Time_Wait:
			{
				CurrentMoveState = Move_HALT;
				if (CompareTimer(LaneChangeWaittimeConstant, &timer_handler_1))
				{
					GV.RightMotor.Start_Measuring = 1;
					GV.LeftMotor.Start_Measuring = 1;
					Current_Auto_Move_State = Auto_Move_HeadToUp_Forwards;
				}

				break;
			}

			case Auto_Move_HeadToUp_Forwards:
			{
				if (GV.RightMotor.Start_Measuring == 0 && GV.LeftMotor.Start_Measuring == 0) //已经记录运动起始点
				{
					CurrentMoveState = Move_Vertical_Task_Forwards; //Forwards

					MoveTask_Start();
					RegionAuto_Vertical_Distance_Err();


					if (MoveTaskIsOk(PV_App.WorkWidthMeters + Err_Vertical_Up_Hold) == 1
							|| Err_Left - PV_App.WorkWidthMeters * Err_Vertical_Up_Hold >= 0
							|| Err_Right - PV_App.WorkWidthMeters * Err_Vertical_Up_Hold >= 0)
					{
						MoveTask_Reset();
						VerticalLaneChangeState = Lane_Change_Start;
						Current_Vertical_ChangeState = VerticalChange_StateZero;
						Current_Auto_Move_State = Auto_Move_Vertical_LaneChange_Right;
						Auto_Move_Cycle_Count++;
					}

				}

				break;
			}

			case Auto_Move_Vertical_LaneChange_Right:
			{
				Vertical_Lane_Change_Right_Control();
				if (VerticalLaneChangeState == Lane_Change_Stop)
				{
					timer_handler_1.start_timer = 1;
					Current_Auto_Move_State = Auto_Move_Time_Wait2;
				}
				break;
			}
			case Auto_Move_Time_Wait2:
			{
				CurrentMoveState = Move_HALT;
				if (CompareTimer(LaneChangeWaittimeConstant, &timer_handler_1))
				{
					GV.RightMotor.Start_Measuring = 1;
					GV.LeftMotor.Start_Measuring = 1;
					Current_Auto_Move_State = Auto_Move_HeadToUp_Backwards;

				}
				break;
			}

			case Auto_Move_HeadToUp_Backwards:
			{
				if (GV.RightMotor.Start_Measuring == 0 && GV.LeftMotor.Start_Measuring == 0) //已经属于在等到了，跟其他的一样
				{
					CurrentMoveState = Move_Vertical_Task_Backwards; //backwards

					MoveTask_Start();
					RegionAuto_Vertical_Distance_Err();


					if (MoveTaskIsOk(PV_App.WorkWidthMeters + Err_Vertical_Down_Hold) == 1
							|| Err_Left + PV_App.WorkWidthMeters * Err_Vertical_Down_Hold >= 0
							|| Err_Right + PV_App.WorkWidthMeters * Err_Vertical_Down_Hold >= 0)
					{
						MoveTask_Reset();
						VerticalLaneChangeState = Lane_Change_Start;
						Current_Vertical_ChangeState = VerticalChange_StateZero;
						Current_Auto_Move_State = Auto_Move_Vertical_LaneChange_Right2;
						Auto_Move_Cycle_Count++;
					}

				}

				break;
			}
			case Auto_Move_Vertical_LaneChange_Right2:
			{
				Vertical_Lane_Change_Right_Control();
				if (VerticalLaneChangeState == Lane_Change_Stop)
				{
					Current_Auto_Move_State = Auto_Move_Time_Wait;
					timer_handler_1.start_timer = 1;
				}
				break;
			}
		}
	}

}


// 全局变量：当前区域自动作业阶段（初始为准备阶段）
RegionAuto_Phase Current_RegionAuto_Phase = RegionAuto_Ready;
/*
 * 区域自动作业
 */
int RegionAutoMoveControl()
{
	if (PV_App.RunMode == Move_Automation_Horizontal_Move
	                || PV_App.RunMode == Move_Automation_Vertical_Move_Left
	                || PV_App.RunMode == Move_Automation_Vertical_Move_Right)
	{
        // 准备阶段：直接执行手动控制（摇杆操作）
        if (Current_RegionAuto_Phase == RegionAuto_Ready) {
            Manual_Mode_Joystick_Control1();  // 强制允许手动操作
        }
//		if(P_MK32->CH7_SD != -1000)//自动按钮未按下，
//		{
//			 Manual_Mode_Joystick_Control1();  // 强制允许手动操作
//			 return;
//		}


		if (P_MK32->CH7_SD == -1000)  //
		{

			Current_RegionAuto_Phase = RegionAuto_Running;  // 新增：进入自动运行阶段
			switch (RegionAutoMoveStart)
			{
				case Define_Task:
				{
					Current_Auto_Move_State = Auto_Move_Start;

					Auto_Move_Cycle_Count = 0;
					if (PV_App.RunMode == Move_Automation_Horizontal_Move)  //自动水平作业
					{
						if (abs(GV.Robot_Angle - CV.RobotLeftAngleValue) < Horizontal_Angle_threshold * 100) //车头向左
						{
							CurrentRegionAuto = RegionAuto_Horizontal_Left;
						}
						else if (abs(GV.Robot_Angle - CV.RobotRightAngleValue) < Horizontal_Angle_threshold * 100) //车头向右
						{
							CurrentRegionAuto = RegionAuto_Horizontal_Right;
						}
					}
					else if (PV_App.RunMode == Move_Automation_Vertical_Move_Left) //自动竖直向左作业
					{
						CurrentRegionAuto = RegionAuto_Vertical_left;
					}
					else if (PV_App.RunMode == Move_Automation_Vertical_Move_Right) //自动竖直向右作业
					{
						CurrentRegionAuto = RegionAuto_Vertical_Right;
					}

					if (PV_App.ToolRotationDirection == 1 || PV_App.ToolRotationDirection == 2)
					{
						RegionAutoMoveStart = Adjust_Pressure;
					}
					else if (PV_App.ToolRotationDirection == 0)
					{
						RegionAutoMoveStart = Perform_Tasks;
					}

					break;
				}
				case Adjust_Pressure:
				{
					if (Auto_TiltControl() == 1)
					{
						RegionAutoMoveStart = Launch_Tool;
					}
					break;
				}
				case Launch_Tool:
				{
					DHRougheningControl(1);
					RegionAutoMoveStart = Perform_Tasks;
					break;
				}
//				case Launch_Tool_Delay:
//				{
//					timer_handler_1.start_timer = 1;
//					if (CompareTimer(10, &timer_handler_1))
//					{
//
//						RegionAutoMoveStart = Perform_Tasks;
//					}
//					break;
//				}
				case Perform_Tasks:
				{
					switch (CurrentRegionAuto)
					{
						case RegionAuto_Horizontal_Left:
						{
							RegionAuto_Horizontal_LeftToRight();
							break;
						}
						case RegionAuto_Horizontal_Right:
						{
							RegionAuto_Horizontal_RightToLeft();
							break;
						}
						case RegionAuto_Vertical_left:
						{
							RegionAuto_Vertical_RightToLeft();
							break;
						}
						case RegionAuto_Vertical_Right:
						{
							RegionAuto_Vertical_LeftToRight();
							break;
						}
						default:
							break;
					}
					break;
				}
				default:
					break;

			}

			IV.RunMode = IV_Run_Mode_Automation;
			return 1;
		}
		else
		{
			MoveTask_Reset();
			TiltWorking_Mode = Tilt_Manual_Mode;
			RegionAutoMoveStart = Define_Task;
			CurrentTiltState = TiltHALT;
			CurrentRougheningEndState = RougheningEndHALT;
			Current_RegionAuto_Phase = RegionAuto_Ready;  // 自动运行信号无效：重置为准备阶段（允许手动）
			return 0;
		}

	}
	else
	{
		Current_RegionAuto_Phase = RegionAuto_Ready;   // 非区域自动作业模式：重置阶段
		return 0;
	}




}




/*
 * 水平向右自动区域作业，距离检测是右轮
 */
void Auto_Move_StartFrom_Horizontal()
{
	if (PV_App.LaneChangeDistance == 0)
	{
		return;
	}
	Auto_Move_Cycle_Total_Count = PV_App.WorkDistanceMeters * 100 / PV_App.LaneChangeDistance;

	if (Auto_Move_Cycle_Count >= Auto_Move_Cycle_Total_Count)
	{
		CurrentMoveState = Move_HALT;
	}
	else
	{
		switch (Current_Auto_Move_State)
		{
			case Auto_Move_Start:
			{
				//原地调整车头方向
				if (abs(GV.Robot_Angle - CV.RobotRightAngleValue) >= 200)
				{
					CurrentMoveState = Move_Head_To_Right_Enum; //right
				}
				else
				{
					GV.RightMotor.Start_Measuring = 1;
					Current_Auto_Move_State = Auto_Move_Time_Wait;
					timer_handler_1.start_timer = 1;
				}
				break;
			}

			case Auto_Move_Time_Wait:
			{
				CurrentMoveState = Move_HALT;
				if (GV.RightMotor.Start_Measuring == 0) //已经属于在等到了
				{
					if (CompareTimer(LaneChangeWaittimeConstant, &timer_handler_1))
					{
						Current_Auto_Move_State = Auto_Move_HeadToRight_Backwards;
						break;
					}
				}
				break;

			}
			case Auto_Move_HeadToRight_Backwards:
			{
				CurrentMoveState = Move_Horizontal_Task_Backwards_Right; //backwards

				RegionAuto_Vertical_Distance_Err();

				if (PV_App.WorkWidthMeters > 2)
				{
					if (Err_Right - Err_Horizontal_Hold >= 0)
					{
						HorizontalLaneChangeState = Lane_Change_Start;
						Current_Auto_Move_State = Auto_Move_Horizontal_LaneChange_Left;
						CurrentHorizontal_ChangeState = HorizontalChange_StateZero;
						Auto_Move_Cycle_Count++;
					}
				}
				else
				{
					if (Err_Right >= 0)
					{
						HorizontalLaneChangeState = Lane_Change_Start;
						Current_Auto_Move_State = Auto_Move_Horizontal_LaneChange_Left;
						CurrentHorizontal_ChangeState = HorizontalChange_StateZero;
						Auto_Move_Cycle_Count++;
					}

					break;
				}

				case Auto_Move_Horizontal_LaneChange_Left:
				{

					Horizontal_Lane_Change_Turn_To_Left_Control();

					if (HorizontalLaneChangeState == Lane_Change_Stop)
					{
						timer_handler_1.start_timer = 1;
						Current_Auto_Move_State = Auto_Move_Time_Wait2;
					}
					break;
				}
				case Auto_Move_Time_Wait2:
				{
					CurrentMoveState = Move_HALT;
					if (CompareTimer(LaneChangeWaittimeConstant, &timer_handler_1))
					{
						GV.RightMotor.Start_Measuring = 1;
						Current_Auto_Move_State = Auto_Move_HeadToLeft_Backwards;

					}
					break;
				}

				case Auto_Move_HeadToLeft_Backwards:
				{
					if (GV.RightMotor.Start_Measuring == 0) //已经属于在等到了，跟其他的一样
					{
						CurrentMoveState = Move_Horizontal_Task_Backwards_Left; //backwards

						RegionAuto_Vertical_Distance_Err();

						if (PV_App.WorkWidthMeters > 2)
						{
							if (Err_Right - Err_Horizontal_Hold >= 0)
							{
								HorizontalLaneChangeState = Lane_Change_Start;
								Current_Auto_Move_State = Auto_Move_Horizontal_LaneChange_Right;
								CurrentHorizontal_ChangeState = HorizontalChange_StateZero;
								Auto_Move_Cycle_Count++;
							}
						}
						else
						{
							if (Err_Right >= 0)
							{
								HorizontalLaneChangeState = Lane_Change_Start;
								Current_Auto_Move_State = Auto_Move_Horizontal_LaneChange_Right;
								CurrentHorizontal_ChangeState = HorizontalChange_StateZero;
								Auto_Move_Cycle_Count++;
							}
						}

					}

					break;
				}
				case Auto_Move_Horizontal_LaneChange_Right:
				{
					Horizontal_Lane_Change_Turn_To_Right_Control();
					if (HorizontalLaneChangeState == Lane_Change_Stop)
					{
						Current_Auto_Move_State = Auto_Move_Time_Wait;
						timer_handler_1.start_timer = 1;
					}
					break;
				}
			}
		}
	}
}


/*
 * 手动模式 前进
 */
void Manual_Mode_Move_Forwards()
{
	if (PV_App.RunMode == Move_Horizontal_Move) //1 手动水平作业
	{

		if (abs(GV.Robot_Angle - CV.RobotLeftAngleValue) < Horizontal_Angle_threshold * 100)
		{
			CurrentMoveState = Move_Horizontal_Task_Forwards_Left;
		}
		else if (abs(GV.Robot_Angle - CV.RobotRightAngleValue) < Horizontal_Angle_threshold * 100)
		{
			CurrentMoveState = Move_Horizontal_Task_Forwards_Right;
		}
		else
		{
			CurrentMoveState = Move_HALT;
		}
	}

	else if (PV_App.RunMode == Move_Vertical_Move_Left || PV_App.RunMode == Move_Vertical_Move_Right) //3 手动竖直向左作业 4 手动竖直向右作业
	{
		CurrentMoveState = Move_Vertical_Task_Forwards;
	}

}
/*
 * 手动模式 后退
 */
void Manual_Mode_Move_Backwards()
{
	if (PV_App.RunMode == Move_Horizontal_Move)	//1 手动水平作业
	{

		if (abs(GV.Robot_Angle - CV.RobotLeftAngleValue) < Horizontal_Angle_threshold * 100)
		{
			CurrentMoveState = Move_Horizontal_Task_Backwards_Left;
		}
		else if (abs(GV.Robot_Angle - CV.RobotRightAngleValue) < Horizontal_Angle_threshold * 100)
		{
			CurrentMoveState = Move_Horizontal_Task_Backwards_Right;
		}
		else
		{
			CurrentMoveState = Move_HALT;
		}

	}

	else if (PV_App.RunMode == Move_Vertical_Move_Left || PV_App.RunMode == Move_Vertical_Move_Right) //3 手动竖直向左作业 4 手动竖直向右作业
	{

		CurrentMoveState = Move_Vertical_Task_Backwards;

	}



}
double Rock_Angle = 0;
/*
 * 手动模式 摇杆控制 合理调整死区MK32_LY_*_*_Angle、MK32_LY_CatchAngle 和 MK32_LY_DeadZone
 */
void Manual_Mode_Joystick_Control()
{
	double y_value = P_MK32->CH2_LY_V; // 假设 Y 是纵向（前后）
	double x_value = P_MK32->CH3_LY_H; // 假设 X 是横向（左右）

	if ((fabs(y_value) >= MK32_LY_DeadZone) || (fabs(x_value) >= MK32_LY_DeadZone))
	{
		double Rock_Angle_t = atan2(y_value, x_value);
		Rock_Angle = Rock_Angle_t / 3.1415926 * 180;

		if ((Rock_Angle >= MK32_LY_V_Forwards_Angle - MK32_LY_CatchAngle)
		        && (Rock_Angle <= MK32_LY_V_Forwards_Angle + MK32_LY_CatchAngle)) //前进
		{
			Manual_Mode_Move_Forwards();
			if (PV_App.RunMode == Move_Manual)	//0 手动无校准作业
			{
				CurrentMoveState = Move_Forwards;
			}
		}
		else if ((Rock_Angle >= MK32_LY_V_Backwards_Angle - MK32_LY_CatchAngle)
		        && (Rock_Angle <= MK32_LY_V_Backwards_Angle + MK32_LY_CatchAngle)) //后退
		{
			Manual_Mode_Move_Backwards();
			if (PV_App.RunMode == Move_Manual)	//0 手动无校准作业
			{
				CurrentMoveState = Move_Backwards;

			}
		}
		else if ((abs(Rock_Angle) >= MK32_LY_H_Left_Angle - MK32_LY_CatchAngle)) //左转
		{
			if (PV_App.RunMode != 0)	//0 手动无校准作业
			{
				CurrentMoveState = Move_TurnLeft;
			}

		}
		else if ((abs(Rock_Angle) <= MK32_LY_H_Right_Angle + MK32_LY_CatchAngle)) //右转
		{
			if (PV_App.RunMode != 0)	//0 手动无校准作业
			{
				CurrentMoveState = Move_TurnRight;
			}

		}
		else
		{
			//只有在停止运动的时候，才可以进行换道命令
			CurrentMoveState = Move_HALT;
		}
	}
	else
	{
		//只有在停止运动的时候，才可以进行换道命令
		CurrentMoveState = Move_HALT;
	}
}


// for区域自动模式下：准备阶段的手动操作，原来只能左右转（调用原Joystick_Control函数）
void Manual_Mode_Joystick_Control1()
{
	double y_value = P_MK32->CH2_LY_V; // 假设 Y 是纵向（前后）
	double x_value = P_MK32->CH3_LY_H; // 假设 X 是横向（左右）

	if ((fabs(y_value) >= MK32_LY_DeadZone) || (fabs(x_value) >= MK32_LY_DeadZone))
	{
		double Rock_Angle_t = atan2(y_value, x_value);
		Rock_Angle = Rock_Angle_t / 3.1415926 * 180;

		if ((Rock_Angle >= MK32_LY_V_Forwards_Angle - MK32_LY_CatchAngle)
		        && (Rock_Angle <= MK32_LY_V_Forwards_Angle + MK32_LY_CatchAngle)) //前进
		{
			Manual_Mode_Move_Forwards();
			if (PV_App.RunMode != 0)	//0 手动无校准作业
			{
				CurrentMoveState = Move_Forwards;
			}
		}
		else if ((Rock_Angle >= MK32_LY_V_Backwards_Angle - MK32_LY_CatchAngle)
		        && (Rock_Angle <= MK32_LY_V_Backwards_Angle + MK32_LY_CatchAngle)) //后退
		{
			Manual_Mode_Move_Backwards();
			if (PV_App.RunMode != 0)	//0 手动无校准作业
			{
				CurrentMoveState = Move_Backwards;

			}
		}
		else if ((abs(Rock_Angle) >= MK32_LY_H_Left_Angle - MK32_LY_CatchAngle)) //左转
		{
			if (PV_App.RunMode != 0)	//0 手动无校准作业
			{
				CurrentMoveState = Move_TurnLeft;
			}

		}
		else if ((abs(Rock_Angle) <= MK32_LY_H_Right_Angle + MK32_LY_CatchAngle)) //右转
		{
			if (PV_App.RunMode != 0)	//0 手动无校准作业
			{
				CurrentMoveState = Move_TurnRight;
			}

		}
		else
		{
			//只有在停止运动的时候，才可以进行换道命令
			CurrentMoveState = Move_HALT;
		}
	}
	else
	{
		//只有在停止运动的时候，才可以进行换道命令
		CurrentMoveState = Move_HALT;
	}
}

void Manual_Mode_Joystick_Control2()
{
	if (abs(P_MK32->CH3_LY_H) <= 600)
	{
		if (abs(P_MK32->CH2_LY_V) >= 600)
		{
			if (P_MK32->CH2_LY_V >= 600)	//forward
			{

				if (PV_App.RunMode == Move_Horizontal_Move)
				{
					if (abs(GV.Robot_Angle - CV.RobotLeftAngleValue) < Horizontal_Angle_threshold * 100)
					{
						CurrentMoveState = Move_Horizontal_Task_Forwards_Left;

					}
					else if (abs(GV.Robot_Angle - CV.RobotRightAngleValue) < Horizontal_Angle_threshold * 100)
					{
						CurrentMoveState = Move_Horizontal_Task_Forwards_Right;

					}
					else
					{
						CurrentMoveState = Move_HALT;
					}

				}

				else if (PV_App.RunMode == Move_Vertical_Move_Left || PV_App.RunMode == Move_Vertical_Move_Right)
				{
					CurrentMoveState = Move_Vertical_Task_Forwards;
				}
				else	//0
				{
					CurrentMoveState = Move_Forwards;
				}

			}
			else if (P_MK32->CH2_LY_V <= 600)	//backward
			{

				if (PV_App.RunMode == Move_Horizontal_Move)
				{

					if (abs(GV.Robot_Angle - CV.RobotLeftAngleValue) < Horizontal_Angle_threshold * 100)
					{
						CurrentMoveState = Move_Horizontal_Task_Backwards_Left;

					}
					else if (abs(GV.Robot_Angle - CV.RobotRightAngleValue) < Horizontal_Angle_threshold * 100)
					{
						CurrentMoveState = Move_Horizontal_Task_Backwards_Right;

					}
					else
					{
						CurrentMoveState = Move_HALT;
					}

				}

				else if (PV_App.RunMode == Move_Vertical_Move_Left || PV_App.RunMode == Move_Vertical_Move_Right)
				{
					CurrentMoveState = Move_Vertical_Task_Backwards;
				}
				else	//-1000
				{
					CurrentMoveState = Move_Backwards;
				}
			}
		}
		else
		{
			//只有在停止运动的时候，才可以进行换道命令
			CurrentMoveState = Move_HALT;
		}
	}
	else  //>300
	{
		if (abs(P_MK32->CH2_LY_V) <= 600)
		{

			if (P_MK32->CH3_LY_H <= -600)
			{
				CurrentMoveState = Move_TurnLeft;
			}
			else if (P_MK32->CH3_LY_H >= 600)
			{
				CurrentMoveState = Move_TurnRight;
			}
		}
		else
		{
			//只有在停止运动的时候，才可以进行换道命令
			CurrentMoveState = Move_HALT;
		}
	}
}

/*
 * 车体速度
 */
int32_t GetVehicleSpeed()
{
	//PV_App.RobotSpeed = GV.PV.RobotSpeed;
	//return PV_App.RobotSpeed;
	speed_selection = (P_MK32->CH11_RD1 +1000)/185;
		switch (speed_selection)
		{
			case 0:
				return 0;
			case 1:
				return CV.Speed_1;
			case 2:
				return CV.Speed_2;
			case 3:
				return CV.Speed_3;
			case 4:
				return CV.Speed_4;
			case 5:
				return CV.Speed_5;
			case 6:
				return CV.Speed_6;
			case 7:
				return CV.Speed_7;
			case 8:
				return CV.Speed_8;
			case 9:
				return CV.Speed_9;
			case 10:
				return CV.Speed_10;
			default:
				return 0;
		}

}

/*
 * 左右补偿计算:这个是老版本的，用的是LD、RD两个旋钮控制的
 */
//void compensation_control()
//{
//	if (abs((int32_t) ((double) ((P_MK32->CH10_LD1 - 0) / 1000.0) * 1000)) <= 1000)
//	{
//		GV.Left_Compensation = (int32_t) ((double) ((P_MK32->CH10_LD1 - 0) / 1000.0) * 1000);
//
//	}
//	if (abs((int32_t) ((double) ((P_MK32->CH11_RD1 - 0) / 1000.0) * 1000)) <= 1000)
//	{
//		GV.Right_Compensation = (int32_t) ((double) ((P_MK32->CH11_RD1 - 0) / 1000.0) * 1000);
//
//	}
//
//}

// every 2 miliseconds
bool Left_Time_Start = false;
bool Right_Time_Start = false;
void compensation_control2()
{
	if (P_MK32->CH14_LT > 100)
	{
		if (!Left_Time_Start)
		{
			Left_Time_Start = true;
			timer_handler_3.start_timer = 1; //重新开始计时
		}
		else
		{
			if (CompareTimer(LeftRightCalbrationTime, &timer_handler_3)) //计时结束
			{
				GV.Left_Compensation += 10;
				if (GV.Left_Compensation >= 500)
				{
					GV.Left_Compensation = 500;
				}
				Left_Time_Start = false;
			}
		}

	}
	else if (P_MK32->CH14_LT < -100)
	{
		if (!Left_Time_Start)
		{
			Left_Time_Start = true;
			timer_handler_3.start_timer = 1; //重新开始计时
		}
		else
		{
			if (CompareTimer(LeftRightCalbrationTime, &timer_handler_3)) //计时结束
			{
				GV.Left_Compensation -= 10;
				if (GV.Left_Compensation <= -500)
				{
					GV.Left_Compensation = -500;
				}
				Left_Time_Start = false;
			}
		}
	}
	else
	{
		Left_Time_Start = false;
		//停止计时
	}
	if (P_MK32->CH15_RT < -100)
	{
		if (!Right_Time_Start)
		{
			Right_Time_Start = true;
			timer_handler_4.start_timer = 1; //重新开始计时
		}
		else
		{
			if (CompareTimer(LeftRightCalbrationTime, &timer_handler_4)) //计时结束
			{
				GV.Right_Compensation += 10;
				if (GV.Right_Compensation >= 500)
				{
					GV.Right_Compensation = 500;
				}
				Right_Time_Start = false;
			}
		}

	}
	else if (P_MK32->CH15_RT > 100)
	{

		if (!Right_Time_Start)
		{
			Right_Time_Start = true;
			timer_handler_4.start_timer = 1; //重新开始计时
		}
		else
		{
			if (CompareTimer(LeftRightCalbrationTime, &timer_handler_4)) //计时结束
			{
				GV.Right_Compensation -= 10;
				if (GV.Right_Compensation <= -500)
				{
					GV.Right_Compensation = -500;
				}
				Right_Time_Start = false;
			}
		}

	}
	else
	{
		Right_Time_Start = false;
	}

}

// 左右补偿值：25对应每波轮一次增减0.25；1000对应最大补偿角度为10度
void compensation_control3()
{
	static int Left_Compensation_Count;
	static int Right_Compensation_Count;

	if (P_MK32->CH14_LT > 300)
	{
		Left_Compensation_Count++;
		if (Left_Compensation_Count > 150)
		{
			GV.Left_Compensation = GV.Left_Compensation + 25;
			Left_Compensation_Count = 0;
		}
		if (GV.Left_Compensation > 1000)
		{
			GV.Left_Compensation = 1000;
		}
	}
	else if (P_MK32->CH14_LT < -300)
	{
		Left_Compensation_Count--;
		if (Left_Compensation_Count < -150)
		{
			GV.Left_Compensation = GV.Left_Compensation - 25;
			Left_Compensation_Count = 0;
			if (GV.Left_Compensation < -1000)
			{
				GV.Left_Compensation = -1000;
			}
		}
	}
	else
	{

		Left_Compensation_Count = 0;
	}

	if (P_MK32->CH15_RT > 300)
	{
		Right_Compensation_Count++;
		if (Right_Compensation_Count > 150)
		{
			GV.Right_Compensation = GV.Right_Compensation + 25;
			Right_Compensation_Count = 0;
			if (GV.Right_Compensation > 1000)
			{
				GV.Right_Compensation = 1000;
			}
		}
	}
	else if (P_MK32->CH15_RT < -300)
	{
		Right_Compensation_Count--;
		if (Right_Compensation_Count < -150)
		{
			GV.Right_Compensation = GV.Right_Compensation - 25;
			Right_Compensation_Count = 0;
			if (GV.Right_Compensation < -1000)
			{
				GV.Right_Compensation = -1000;
			}
		}
	}
	else
	{

		Right_Compensation_Count = 0;
	}

}



void DLT_Log_Hardware_Failure()
{
	if (Get_BIT(SystemErrorCode, ComError_MK32_Serial) == 1)
	{
		LOGFF(DL_FATAL, "ComError_MK32_Serial");
	}
	if (Get_BIT(SystemErrorCode, ComError_MK32_SBus) == 1)
	{
		LOGFF(DL_FATAL, "ComError_MK32_SBus");
	}
	if (Get_BIT(SystemErrorCode, ComError_TL720D) == 1)
	{
		LOGFF(DL_FATAL, "ComError_TL720D");
	}
	if (Get_BIT(SystemErrorCode, ComError_Force_sensor) == 1)
	{
		LOGFF(DL_FATAL, "ComError_Force_sensor");
	}
	if (Get_BIT(SystemErrorCode, ComError_ZQ_LeftMotor) == 1)
	{
		LOGFF(DL_FATAL, "ComError_ZQ_LeftMotor");
	}
	if (Get_BIT(SystemErrorCode, ComError_Force_sensor) == 1)
	{
		LOGFF(DL_FATAL, "ComError_ZQ_RightMotor");
	}
	if (Get_BIT(SystemErrorCode, ComError_Force_sensor) == 1)
	{
		LOGFF(DL_FATAL, "ComError_MK32_Serial");
	}
	if (P_MK32->IsOnline != 1)
	{
		LOGFF(DL_FATAL, "P_MK32->IsOnline failed");
	}

}

int Mk32_InitialState_Detection()
{

	if (P_MK32->RxIndex > 0 && P_MK32->CH4_SA == 0 && P_MK32->CH5_SB == 0 && P_MK32->CH6_SC == 0 && P_MK32->CH7_SD == 0)
	{
		Switch_Off_Flag = 1;
		SET_BIT_0(SystemErrorCode, ComError_MK32_InitialState);
		return 1;
	}
	else
	{

		SET_BIT_1(SystemErrorCode, ComError_MK32_InitialState);
		return 0;
	}

}

// 删除风刀功能，原SD按键改成自动控制拉毛启动：原调用关系见Movement_control
//void WindControl()
//{
//	if (PV_App.RunMode != 0)
//	{
//		if (P_MK32->CH7_SD == -1000)
//		{
//			Wind_State = Wind_State_ON;
//		}
//		else
//		{
//			Wind_State = Wind_State_OFF;
//		}
//	}
//	else
//	{
//		Wind_State = Wind_State_OFF;
//	}
//}


void EmegencyStopHandle()
{
	if(EmegencyStop_Step > 0)
	{
		// 关闭执行器
		CurrentMoveState = Move_HALT;
		CurrentTiltState = TiltHALT;
		CurrentRougheningEndState = RougheningEndHALT;
		Wind_State = Wind_State_OFF;

		EmegencyStop_Step --;
	}
	else
	{
		// 关闭执行器
		CurrentMoveState = Move_HALT;
		CurrentTiltState = TiltHALT;
		CurrentRougheningEndState = RougheningEndHALT;
		Wind_State = Wind_State_OFF;
		// 断开电源
		GF_BSP_GPIO_SetIO(Motor_Power_IO_CTL, K_OFF);
		Switch_Off_Flag = 1;
		TT_Motor_Need_To_Activate = 0;

		InitialState_Detection = InitialState_Detection_start;
	}

}