/*
 * 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"

#define Move_Manual 0
#define Move_Horizontal_Move_Left 1
#define Move_Horizontal_Move_Right 2
#define Move_Vertical_Move 3
#define Move_Horizontal_Move 1
#define LeftRightCalbrationTime 500

double LaneChangeSpeed = 4;
IV_Information_State IV_MCU_Run_State; //返回安卓程序的部分
void IV_control();
void compensation_control();
void MoveControl();

int32_t GetVehicleSpeed(int speed_selection);

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();
int LaneChangeWaittime = 0;
int32_t speed_selection;

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; //水平和竖直换道标志位

int index_counter = 0;

int LaneChangeControl();
void DHRougheningControl();
void TiltControl();

//手动下 机器人运动
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 },

};

//Move_Horizontal_Task_Forwards_Left,//水平方向前进，保持水平方向
//Move_Horizontal_Task_Backwards_Left,//水平方向后退，保持水平方向
//Move_Horizontal_Task_Forwards_Right,//水平方向前进，保持水平方向
//Move_Horizontal_Task_Backwards_Right,//水平方向后退，保持水平方向

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 }, };

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

	GF_BSP_Interrupt_Add_CallBack(
	        DF_BSP_InterCall_TIM8_2ms_PeriodElapsedCallback, GF_Dispatch);
}

void GF_Dispatch()
{




	if (Get_BIT(SystemErrorCode, mk32_sbus) == 1)
	{
		CurrentMoveState = Move_HALT;
		CurrentTiltState = TiltHALT; //点头，抬头
		CurrentRougheningEndState = RougheningEndHALT;
	}
	else
	{
		if (P_MK32->CH8_SE == -1000 && P_MK32->CH9_SF == -1000)
		{
			CurrentMoveState = Move_HALT;
			CurrentTiltState = TiltHALT; //点头，抬头
			CurrentRougheningEndState = RougheningEndHALT;

		}
		else
		{
			MoveControl();
			TiltControl();
			DHRougheningControl();

		}

	}

	GV.LaneChangeDistance = CV.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

	//double length=sqrt(P_MK32->CH0_RY_H*P_MK32->CH0_RY_H+P_MK32->CH1_RY_V*P_MK32->CH1_RY_V);
	//double MoveDir=

	compensation_control();
	IV_control();

	//点头、抬头 这里是上下移动
	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);

}
void IV_control()
{
	IV.Tempature = GV.Tempature;
	IV.RunMode = IV_MCU_Run_State;
	IV.LeftCompensation = GV.Left_Compensation;
	IV.RightCompensation = GV.Right_Compensation;
	IV.CurrentAngle = GV.Robot_Angle;
	IV.RobotMoveSpeed = (int32_t) GetVehicleSpeed(speed_selection);
	IV.EndPressure = GV.ForceValue;

}

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 MoveControl()
{
	//换道优先级优于普通的运行;
	if (LaneChangeControl() != 0)
	{
		return;
	}

	speed_selection = (P_MK32->CH11_RD1 + 1000) / 200;

	GV.Robot_Move_Speed = (int32_t) GetVehicleSpeed(speed_selection) / 0.880
	        * 101 * 10;
	//自动巡航
	if (P_MK32->CH5_SB == -1000)	//前进
	{
		//#define Move_Manual 0
		if (CV.PV.RunMode == Move_Horizontal_Move)
		{

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

		}
//		else if (CV.PV.RunMode == Move_Horizontal_Move_Right)
//		{
//			CurrentMoveState = Move_Horizontal_Task_Forwards_Right;
//			IV_MCU_Run_State = IV_Ding_Su_Shu_Zhi_Qian_Jin;
//		}
		else if (CV.PV.RunMode == Move_Vertical_Move)
		{
			CurrentMoveState = Move_Vertical_Task_Forwards;
			IV_MCU_Run_State = IV_Ding_Su_Shu_Zhi_Qian_Jin;
		}
		else	//0
		{
			CurrentMoveState = Move_Forwards;
			IV_MCU_Run_State = IV_Ding_Su_Shou_Dong_Hou_Tui;
		}
	}
	else if (P_MK32->CH5_SB == 1000)	//后退
	{
//		if (CV.PV.RunMode == Move_Horizontal_Move_Left)
//		{
//			CurrentMoveState = Move_Horizontal_Task_Backwards_Left;
//			IV_MCU_Run_State = IV_Shui_Ping_Mo_Shi_Hou_Tui;
//		}
//		else if (CV.PV.RunMode == Move_Horizontal_Move_Right)
//		{
//			CurrentMoveState = Move_Horizontal_Task_Backwards_Right;
//			IV_MCU_Run_State = IV_Shui_Ping_Mo_Shi_Hou_Tui;
//		}
//		else if (CV.PV.RunMode == Move_Vertical_Move)
//		{
//			CurrentMoveState = Move_Vertical_Task_Backwards;
//			IV_MCU_Run_State = IV_Ding_Su_Shu_Zhi_Hou_Tui;
//		}
//
//		else	//-1000
//		{
//			CurrentMoveState = Move_Backwards;
//			IV_MCU_Run_State = IV_Ding_Su_Shou_Dong_Hou_Tui;
//		}

		if (CV.PV.RunMode == Move_Horizontal_Move)
		{

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

		}
//				else if (CV.PV.RunMode == Move_Horizontal_Move_Right)
//				{
//					CurrentMoveState = Move_Horizontal_Task_Backwards_Right;
//					IV_MCU_Run_State = IV_Shui_Ping_Mo_Shi_Hou_Tui;
//				}
		else if (CV.PV.RunMode == Move_Vertical_Move)
		{
			CurrentMoveState = Move_Vertical_Task_Backwards;
			IV_MCU_Run_State = IV_Ding_Su_Shu_Zhi_Hou_Tui;
		}

		else	//-1000
		{
			CurrentMoveState = Move_Backwards;
			IV_MCU_Run_State = IV_Ding_Su_Shou_Dong_Hou_Tui;
		}
	}
	else
	{
		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 (CV.PV.RunMode == Move_Horizontal_Move)
					{

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

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

						}else
						{
							CurrentMoveState = Move_HALT;
						}

					}

//					if (CV.PV.RunMode == Move_Horizontal_Move_Left)
//					{
//						CurrentMoveState = Move_Horizontal_Task_Forwards_Left;
//					}
//					else if (CV.PV.RunMode == Move_Horizontal_Move_Right)
//					{
//						CurrentMoveState = Move_Horizontal_Task_Forwards_Right;
//					}
					else if (CV.PV.RunMode == Move_Vertical_Move)
					{
						CurrentMoveState = Move_Vertical_Task_Forwards;
					}
					else	//0
					{
						CurrentMoveState = Move_Forwards;
					}

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

					if (CV.PV.RunMode == Move_Horizontal_Move)
					{

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

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

						}else
						{
							CurrentMoveState = Move_HALT;
						}

					}
//					if (CV.PV.RunMode == Move_Horizontal_Move_Left)
//					{
//						CurrentMoveState = Move_Horizontal_Task_Backwards_Left;
//					}
//					else if (CV.PV.RunMode == Move_Horizontal_Move_Right)
//					{
//						CurrentMoveState = Move_Horizontal_Task_Backwards_Right;
//					}
					else if (CV.PV.RunMode == Move_Vertical_Move)
					{
						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;
			}
		}
	}

}

//超过某个角度，则一直向左转，在误差允许范围内，进行水平左移动，然后计算时间，随后停止

void TiltControl()
{
	//Tilt 部分 前端上升、前端下降
	if (P_MK32->CH1_RY_V == 1000)
	{
		CurrentTiltState = TiltDown;
	}
	else if (P_MK32->CH1_RY_V == -1000)
	{
		CurrentTiltState = TiltUP;

	}
	else
	{
		CurrentTiltState = TiltHALT;
	}

}
//拉毛前端运动部分
void DHRougheningControl()
{
	//拉毛部分
	if (P_MK32->CH7_SD == -1000)
	{
		CurrentRougheningEndState = RougheningEndSwingClockWise;
	}
	else if (P_MK32->CH7_SD == 1000)
	{
		CurrentRougheningEndState = RougheningEndSwingAntiClockWise;
	}
	else
	{
		CurrentRougheningEndState = RougheningEndHALT;
	}

}

#define Horizontal_Head_To_Left_Flag 0
#define Horizontal_Head_To_Right_Flag 1
#define Veritical_To_Left_Flag 2
#define Veritical_To_Right_Flag 3
char LaneChangeFlage = -1;
int LaneChangeControl()
{
	//m/min = 100cm/60s  = 10/6 =    5/3 cm/s  =5*1000/3 cm/ms
	GV.Robot_Move_Speed = LaneChangeSpeed / 0.880 * 101 * 10;

	if (P_MK32->CH4_SA == 1000)  //水平换道
	{

		if (HorizontalLaneChangeState == Lane_Change_Start)
		{
			if (LaneChangeFlage == Horizontal_Head_To_Left_Flag)
			{
				Horizontal_Lane_Change_Turn_To_Left_Control();
			}
			else if (LaneChangeFlage == Horizontal_Head_To_Right_Flag)
			{
				Horizontal_Lane_Change_Turn_To_Right_Control();
			}

		}
		IV_MCU_Run_State = IV_Shui_Ping_Huang_Dao;

	}
	else if (P_MK32->CH4_SA == -1000)  //竖直换道
	{
//		if (HorizontalLaneChangeState == Lane_Change_Start)
//		{
//
//			Horizontal_Lane_Change_Turn_To_Right_Control();
//		}
//		IV_MCU_Run_State = IV_Shu_Zhi_Huang_Dao;

		if (VerticalLaneChangeState == Lane_Change_Start)
		{

			if (LaneChangeFlage == Veritical_To_Left_Flag)
			{
				//Vertical_Lane_Change_Right_Control();
				Vertical_Lane_Change_Left_Control();
			}
			else
			{
				Vertical_Lane_Change_Right_Control();
			}

		}
	}
	else
	{
		//头朝左
		if (abs(GV.Robot_Angle - CV.RobotLeftAngleValue) <= 45 * 100)
		{
			// 头朝左，换道意味着头
			LaneChangeFlage = Horizontal_Head_To_Right_Flag;
		}
		else if (abs(GV.Robot_Angle - CV.RobotRightAngleValue) <= 45 * 100)
		{
			LaneChangeFlage = Horizontal_Head_To_Left_Flag;
		}
		else if (abs(GV.Robot_Angle - CV.RobotUpAngleValue) <= 45 * 100)
		{
			LaneChangeFlage = Veritical_To_Left_Flag;
		}
		else
		{
			LaneChangeFlage = -1;
		}

		HorizontalLaneChangeState = Lane_Change_Start;
		CurrentHorizontal_ChangeState = HorizontalChange_TurnToUP;  //设定初始方向

		VerticalLaneChangeState = Lane_Change_Start;
		Current_Vertical_ChangeState = VerticalChange_StateZero;

	}

	return P_MK32->CH4_SA;
//	if (P_MK32->CH4_SA != 0)
//	{
//		return P_MK32->CH4_SA;
//	}
//
//	if (P_MK32->CH6_SC == -1000)  //竖直左换道
//	{
//		if (VerticalLaneChangeState == Lane_Change_Start)
//		{
//			Vertical_Lane_Change_Left_Control();
//		}
//
//	}
//	else if (P_MK32->CH6_SC == 1000)  //竖直右换道
//	{
//		if (VerticalLaneChangeState == Lane_Change_Start)
//		{
//			Vertical_Lane_Change_Right_Control();
//		}
//	}
//	else
//	{
//		VerticalLaneChangeState = Lane_Change_Start;
//		Current_Vertical_ChangeState = VerticalChange_StateZero;
//	}
//
//	return P_MK32->CH6_SC;
}

//水平  向下换道 最终朝右
void Horizontal_Lane_Change_Turn_To_Right_Control()
{
	switch (CurrentHorizontal_ChangeState)
	{
		case HorizontalChange_TurnToUP:
		{
			if (abs(GV.Robot_Angle - CV.RobotUpAngleValue) >= 200)
			{
				CurrentMoveState = Move_Head_To_UP_Enum; //转到上面
			}
			else
			{
				CurrentMoveState = Move_Vertical_Task_Backwards; //转到左边，前进
				//开启计时
				timer_handler_1.start_timer = 1;
				CurrentHorizontal_ChangeState = HorizontalChange_DelayMove;
			}
			break;
		}
		case HorizontalChange_DelayMove:
		{
			LaneChangeWaittime = 60 * CV.PV.LaneChangeDistance
			        / LaneChangeSpeed;
			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) <= 100)
			{
				CurrentHorizontal_ChangeState = HorizontalChange_End;
			}
			break;
		}
		case HorizontalChange_End:
		{
			HorizontalLaneChangeState = Lane_Change_Stop;
			break;
		}

	}

}

void Horizontal_Lane_Change_Turn_To_Left_Control()
{
	switch (CurrentHorizontal_ChangeState)
	{
		case HorizontalChange_TurnToUP:
		{
			if (abs(GV.Robot_Angle - CV.RobotUpAngleValue) >= 200)
			{
				CurrentMoveState = Move_Head_To_UP_Enum; //转到上面
			}
			else
			{
				CurrentMoveState = Move_Vertical_Task_Backwards; //转到左边，前进
				//开启计时
				timer_handler_1.start_timer = 1;
				CurrentHorizontal_ChangeState = HorizontalChange_DelayMove;
			}
			break;
		}
		case HorizontalChange_DelayMove:
		{
			LaneChangeWaittime = 60 * CV.PV.LaneChangeDistance
			        / LaneChangeSpeed;
			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) <= 100)
			{
				CurrentHorizontal_ChangeState = HorizontalChange_End;
			}
			break;
		}
		case HorizontalChange_End:
		{
			HorizontalLaneChangeState = Lane_Change_Stop;
			break;
		}

	}

}

void Vertical_Lane_Change_Left_Control()
{
	switch (Current_Vertical_ChangeState)
	{
		case VerticalChange_StateZero:
		{
			Current_Vertical_ChangeState = VerticalChange_TurnToRight;

			break;
		}
		case VerticalChange_TurnToRight:
		{
			if (abs(GV.Robot_Angle - CV.RobotRightAngleValue) >= 100)
			{
				CurrentMoveState = Move_Head_To_Right_Enum;
			}
			else
			{
				CurrentMoveState = Move_Horizontal_Task_Backwards_Right; //转到左边，前进
				//开启计时
				timer_handler_1.start_timer = 1;
				Current_Vertical_ChangeState = VerticalChange_DelayMove;
			}
			break;
		}
		case VerticalChange_DelayMove:
		{
			LaneChangeWaittime = 60 * CV.PV.LaneChangeDistance
			        / LaneChangeSpeed;
			if (CompareTimer(LaneChangeWaittime, &timer_handler_1))
			{
				CurrentMoveState = Move_Head_To_UP_Enum; //转到朝上
				Current_Vertical_ChangeState = VerticalChange_TurnToUP;
				// Current_Vertical_ChangeState = VerticalChange_TurnToDown;
			}
			break;
		}
		case VerticalChange_TurnToUP:
			//case VerticalChange_TurnToDown:
		{
			if (abs(GV.Robot_Angle - CV.RobotUpAngleValue) >= 20 * 100)
			{
				CurrentMoveState = Move_Head_To_UP_Enum;
			}
			else
			{
				Current_Vertical_ChangeState = VerticalChange_End;
			}
//			if (abs(GV.Robot_Angle - CV.RobotDownAngleValue) >= 20 * 100)
//						{
//							CurrentMoveState = Move_Head_To_Down_Enum;
//						}
//						else
//						{
//							Current_Vertical_ChangeState = VerticalChange_End;
//						}

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

	}

}

void Vertical_Lane_Change_Right_Control()
{
	switch (Current_Vertical_ChangeState)
	{
		case VerticalChange_StateZero:
		{
			Current_Vertical_ChangeState = VerticalChange_TurnToLeft;
			break;
		}
		case VerticalChange_TurnToLeft:
		{
			if (abs(GV.Robot_Angle - CV.RobotLeftAngleValue) >= 100)
			{
				CurrentMoveState = Move_Head_To_Left_Enum; //
			}
			else
			{
				CurrentMoveState = Move_Horizontal_Task_Backwards_Left; //转到左边，前进
				//开启计时
				timer_handler_1.start_timer = 1;
				Current_Vertical_ChangeState = VerticalChange_DelayMove;
			}
			break;
		}
		case VerticalChange_DelayMove:
		{
			LaneChangeWaittime = 60 * CV.PV.LaneChangeDistance
			        / LaneChangeSpeed;
			if (CompareTimer(LaneChangeWaittime, &timer_handler_1))
			{
				CurrentMoveState = Move_Head_To_UP_Enum; //转到朝上
				Current_Vertical_ChangeState = VerticalChange_TurnToUP;
			}
			break;
		}
		case VerticalChange_TurnToUP:
		{
			if (abs(GV.Robot_Angle - CV.RobotUpAngleValue) >= 20 * 100)
			{
				CurrentMoveState = Move_Head_To_UP_Enum;
			}
			else
			{
				Current_Vertical_ChangeState = VerticalChange_End;
			}

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

	}

}

int32_t GetVehicleSpeed(int speed_selection)
{
	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;
	}

}
// every 2 miliseconds
bool Left_Time_Start = false;
bool Right_Time_Start = false;
void compensation_control()
{
	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 <= 0)
				{
					GV.Left_Compensation = 0;
				}
				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 <= 0)
				{
					GV.Right_Compensation = 0;
				}
				Right_Time_Start = false;
			}
		}

	}
	else
	{
		Right_Time_Start = false;
	}

}