/*TI5_Motor_MOdel_70_90_Pro_81,减速比为81，One_circle_resolution 5308416
 * Robot_Size[]; Robot_Size[0] Left wheel and right Wheel distance,  158mm，wheel width 46mm,total wheel width 250mm ; Robot_Size[1] wheel Radius 180mm ;
 *电机1，前轮电机（转向电机下）；电机2,后轮电机；电机3,转向电机。
 * BHBF_ROBOT.c
 *
 *  Created on: Oct 26, 2023
 *      Author: shiya
 */
#include "BHBF_ROBOT.h"
#include<stdbool.h>
struct CV_struct_define CV;
struct GV_struct_define GV;
struct PV_struct_define PV;

void Motor_Error_Query();
void GF_MainLoop_Jump(int32_t Next_Index_U,int32_t Next_Index_N);

int paint_onoff_status_flag=0;//喷枪开、闭标志位
int change_road_straight_count=0;
int screen_count=0;
int screen_count1=0;
int32_t change_road_speed=200;       //换道时的速度（RPM）   200/70*0.942=2.69m/min
int32_t compensate_angle=0;          //纠偏补偿角，客户端提供的

uint8_t DO1_ON[8]= {0x23, 0x10, 0x24, 0x00, 0x01, 0x00, 0x00, 0x00};
uint8_t DO1_OFF[8]={0x23, 0x10, 0x24, 0x00, 0x00, 0x00, 0x00, 0x00};

//以下变量是直接从喷漆C#中拷贝的PID参数
int32_t KP1 = 1;/**直线纠偏*/
int32_t KP2 = 1;
double KP3 = 0.5;
double KP4 = 0.2;

//KP11 = 0.3;KP22 = 0.2;KP33 = 0.2;KP44 = 0.15;
double KP11 = 0.3;/**换道纠偏*/                   //2~0
double KP22 = 0.25;                              //5~2
double KP33 = 0.2;                              //30~5
double KP44 = 0.13;                              //100~30
double KP55 = 0.1;                               //200~100
bool isTurn = false;//判断是否转弯完毕
bool isFinish=false;
bool isOpen = false;  //true
bool isIn = false;
int32_t deltaAngle1 = 2;
int32_t deltaAngle2 = 5;
int32_t deltaAngle3 = 30;
int32_t deltaAngle4 = 100;

int Backward_Vertical_Falg=0;
int Forward_Vertical_Falg=0;
int Backward_Horiz_Falg=0;
int Forward_Horiz_Falg=0;
//int32_t deltaSpeed;
//int32_t deltaAngle;

double ExpectationAngle=0;
int Paint_Gun_Close_Flag=0;
int Paint_Gun_Doub_Open_Flag=0;
//从C#中拷贝过来的每个档位对应的速度大小（此处为初始值，后续可以根据手机APP来改变此值）
//轮子直径为0.942m，传动比70，500/3为(P     X=500*V/3)(V为电机的转速RPM/min)

bool EnableFlag = 0;
double deltaAngle=0;
double deltaSpeed=0;
int change_road_status_flag=0;       //换道标志位
double change_road_distance_Ms=0;    //换道时的距离（执行换道前进程序的次数，可以理解为执行次数越多，机器人走的越远）
int Paint_Gun_Delay_Close_Flag=0;
double Back_Dis_CM=40;

int Auto_Spraying_Distance_CM=000;
int Auto_Spraying_Flag_Vertical=0;
int Button_Duration=0;

void VehicleAutoTurn(double InclinometerAngle, double expectationAngle);//换道自动转弯
void continueTurn_up(double Angle);
void continueTurn_down(double Angle);
void VehicleForward_Speed(void);
void VehicleAutoForward(double InclinometerAngle, double offsetAngle);
void VehicleBack_Speed(void);
void VehicleAutoBackward(double InclinometerAngle, double offsetAngle);
void Manu_No_MENS();
void Test_Void();
void Vertical_Tast();
void Paint_Gun_Contronl();
void Paint_Gun_Delay_Close_Func();
void Back_Dis_Compu();
void Back_Time_Compu_Hori();
void Back_Time_Compu_Ver();
void Back_Time_Compu();
void Vertical_Auto_Tast();

int Paint_Gun_Open1_Close0=0;
int Turn_Lef_Right_Flag=0;
int Vertical_Auto_Tast_Flag=0;
int Paint_Gun_Delay_Close_Flag_Modfiy=0;

int Angle_Error_Vlog_Flag=0;
int Paint_Gun_Delay_Close_Flag_Error=0;

void CV_GV_Init()        //这个函数用于明确一共两个电机，并给这两个电机ID赋值
{
	GV.Motor_LeiSai[0].MotorID=1;
	GV.Motor_LeiSai[1].MotorID=2;
}

void GF_Robot_Init()
{
	uint8_t _state=1;
	memset(&GV, 0, sizeof(struct GV_struct_define));                                   //把GV里的东西全部初始化为0
	memset(&PV, 0, sizeof(struct PV_struct_define));       //把PV里的东西全部初始化为0

	CV_GV_Init();
	_state = _state & GF_BSP_Interrupt_Add_CallBack(DF_BSP_InterCall_TIM1_100ms_PeriodElapsedCallback,GF_WatchDog_Loop);
	_state = _state & GF_BSP_Interrupt_Add_CallBack(DF_BSP_InterCall_TIM1_100ms_PeriodElapsedCallback,Motor_Error_Query);
	_state = _state & GF_BSP_Interrupt_Add_CallBack(DF_BSP_InterCall_TIM8_2ms_PeriodElapsedCallback,GF_Robot_MainLoop);              //给电机结构体赋值
	_state = _state & GF_BSP_Interrupt_Add_CallBack(DF_BSP_InterCall_TIM8_2ms_PeriodElapsedCallback,GF_MSP_LeisaiCan_Motor_Control_Timer);   //每2ms一次，进入电机发送函数
	//钛虎电机//锋华电机//雷塞电机

	_state = _state & GF_BSP_GPIO_Init();
	_state = _state & GF_BSP_FDCAN_Init();
	_state = _state & GF_BSP_EEPROM_Init();
	_state = _state & GF_BSP_UART_Init();

	HAL_Delay(10000);

	_state = _state & GF_MSP_LeiSaiCan_Motor_Init(2,(SP_MSP_LeiSaiCan_MotorParameters*)(&GV.Motor_LeiSai));  //初始化电机的结构体（GV）
	_state = _state & GF_MSP_RF_TL720D_Init(&huart3,(SP_MSP_RF_TL720D_Parameters*)&GV.RFTL720_D);
	_state = _state & GF_MSP_DH_Wash_H_Init(&huart6,(SP_MSP_DH_Handset_Parameters*)&GV.DH_WH_HandSet);

	//_state = _state & GF_MSP_RF_TPLINK_Init(&huart6,(SP_MSP_TPLINK_Parameters*)&GV.TPLINK);

	//_state = _state & GF_MSP_DH_CAN_Init(1, (SP_MSP_DH_CAN_Parameters*) (&GV.DH_CAN_Paint));
	_state = _state & GF_MSP_OBD_Init(2);
	_state = _state & GF_BSP_TIMER_Init(); //定时器最后启动

	if(_state == 1)
	{
          //进入此处证明前面执行的所有步骤都没有问题（这里没有要实现的功能，所以没在里面写东西）
	}
}

void GF_WatchDog_Loop()
{
//	screen_count1++;
//	if(screen_count1 >= 5)
//	{
//		GF_MSP_DH_Stu_Screen();
//		screen_count1 = 0;
//	}

//	GF_MSP_DH_40022_CAN_Inquiry_Paint(
//			((double)GV.RFTL720_D.RF_Angle_Roll)/100.0,
//			Back_Dis_CM,
//			Auto_Spraying_Distance_CM/10);
	//GF_MSP_DH_40022_CAN_Inquiry(((double)GV.RFTL720_D.RF_Angle_Roll));
	//硬件看门狗复位
	HAL_GPIO_TogglePin(RST_WDI_GPIO_Port, RST_WDI_Pin);//Hardware Watchdog,kicking the dog
}


void GF_Robot_MainLoop()
{

	//写屏幕485，机器人会有0.5S演示（把写屏幕屏蔽后延时可消除）
	screen_count++;
   if(screen_count == 12)
   {

	   GF_MSP_DH_Stu_Inquiry();

   }
   else if(screen_count==24)
   {
	   GF_MSP_DH_Stu_Screen();
	   screen_count=0;
   }

	//xcm/1500cm*60s*1000/2ms =20x  试试20x-40
//	int Robot_Mode=GV.DH_CAN_Paint.Mode_Close0_Ver_L1_R2_Hori3_Dis9;
//	int Robot_Change_Rode=GV.DH_CAN_Paint.Road_LU1_RD2_Close0;
//	int Robot_Mode = GV.DH_CAN_Paint.Vertical_change_road_up1_down2_close0;
    int Robot_Mode = GV.DH_WH_HandSet.Vertical_change_road_up1_down2_close0;
	switch(Robot_Mode)
	{
	case 0:   //作业关闭
		Auto_Job_Sub_Function();
		change_road_status_flag=0;

		break;
	case 1:   //竖直上换道
		 Back_Time_Compu_Ver();

		 Vertical_Right_Left_Change_Road_UP();
		 Angle_Error_Vlog_Flag=0;
		break;
	case 2:   //竖直下换道
		 Back_Time_Compu_Ver();
		 Vertical_Right_Left_Change_Road_Down();

		 Angle_Error_Vlog_Flag=0;
		break;
	default:
		GV.Motor_LeiSai[0].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
		GV.Motor_LeiSai[1].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
		GV.Motor_LeiSai[0].Target_Velcity=0;
		GV.Motor_LeiSai[1].Target_Velcity=0;
		break;
	}
}

void Auto_Job_Sub_Function()
{
	int Robot_Mode_Sub = GV.DH_WH_HandSet.working_start1_stop2;

	switch(Robot_Mode_Sub)
	{
		case 1:                                            //作业开始
			 Auto_Job_Sub_Vertical();
			 Auto_Spraying_Flag_Vertical=0;
			break;
		case 2:                                            //作业结束
			Auto_Job_Sub_No_MENS();
			Angle_Error_Vlog_Flag=0;
			Paint_Gun_Delay_Close_Flag_Error=0;
			Paint_Gun_Delay_Close_Flag=0;

			break;
		default:
			GV.Motor_LeiSai[0].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
			GV.Motor_LeiSai[1].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
			GV.Motor_LeiSai[0].Target_Velcity=0;
			GV.Motor_LeiSai[1].Target_Velcity=0;
			break;
	}

}

void Auto_Job_Sub_Vertical()
{
	Paint_Gun_Contronl();                        //IO控制电磁阀
	Vertical_Auto_Tast();

	Paint_Gun_Delay_Close_Func();//喷枪关闭，延时停机操作
	Paint_Gun_Delay_Close_Func_1();
}

int Paint_Flag=4;
int Paint_Gun_Count=0;
int Last_Paint_Gun_Flag=0;

void Paint_Gun_Contronl()
{
	switch(Paint_Flag)
	{
	case 1:
		GF_BSP_GPIO_SetIO(0,GPIO_PIN_RESET);
		//LeisaiCan_FDCANfunction(ID1,8,DO1_ON);
		Paint_Gun_Open1_Close0=1;

		Paint_Gun_Count++;
		if(Paint_Gun_Count>=2)
		{
			Paint_Flag=3;
		}
		break;
	case 2:
		GF_BSP_GPIO_SetIO(0,GPIO_PIN_RESET);
		//LeisaiCan_FDCANfunction(ID1,8,DO1_ON);
		Paint_Gun_Open1_Close0=1;
		Paint_Gun_Count++;
		if(Paint_Gun_Count>=2)
		{
			Paint_Flag=3;
		}
		break;
	case 3:
		Paint_Gun_Count=0;
		break;
	case 4:
		GF_BSP_GPIO_SetIO(0,GPIO_PIN_SET);
			//LeisaiCan_FDCANfunction(ID1,8,DO1_OFF);
		Paint_Gun_Open1_Close0=0;
		Last_Paint_Gun_Flag=GV.DH_WH_HandSet.Open_Up1_Down2;
		Paint_Gun_Count=0;
		Paint_Flag=3;
		break;
	case 5:
		break;
	default:
	break;
	}

	if(Last_Paint_Gun_Flag==GV.DH_WH_HandSet.Open_Up1_Down2)
	{

	}
	else
	{
		Paint_Flag=GV.DH_WH_HandSet.Open_Up1_Down2;
	}

	Last_Paint_Gun_Flag=GV.DH_WH_HandSet.Open_Up1_Down2;
}

////////////////////
/**
  * @brief 将读取到的电机故障代码赋值给遥控器的电机故障代码缓存中
  * @param 无参数传递
  * @retval 无返回数据
  */
void Motor_Error_Query()
{
	//左电机故障代码
	GV.DH_CAN_Paint.Motor1_Error = GV.Motor_LeiSai[0].Error_Flag;
	//右电机故障代码
	GV.DH_CAN_Paint.Motor2_Error = GV.Motor_LeiSai[1].Error_Flag;
}


void Auto_Job_Sub_No_MENS()
{
	Test_Void();
	Manu_No_MENS();
	Auto_Spraying_Flag_Vertical=0;
	change_road_status_flag=0;
}

////2ms 执行一次

void continueTurn_down(double Angle)
{
	double offset = 0.5;

	if (( ((double)GV.RFTL720_D.RF_Angle_Roll )/100.0) >= Angle - offset && ( ((double)GV.RFTL720_D.RF_Angle_Roll )/100.0) <= Angle + offset)
	{
		isTurn = true;
	}
}
void continueTurn_up(double Angle)
{
	double offset = 0.5;

	if (( ((double)GV.RFTL720_D.RF_Angle_Roll )/100.0) >= Angle - offset && ( ((double)GV.RFTL720_D.RF_Angle_Roll )/100.0) <= Angle + offset)
	{
		isTurn = true;
	}
}


void VehicleAutoForward(double InclinometerAngle, double offsetAngle)//自动前进
{
	double Angle_Error_Vlog=0;
	//水平
	//需要调整左右补偿
	if ((InclinometerAngle >= 45 && InclinometerAngle <= 135))
	{
		ExpectationAngle = 90;

	}
	if ((InclinometerAngle >= -135 && InclinometerAngle <= -45))
	{
		ExpectationAngle = -90;
	}

	//垂直
	if ((InclinometerAngle >= 0 && InclinometerAngle < 45) || (InclinometerAngle > -45 && InclinometerAngle <= 0))
	{
		ExpectationAngle = 0 + offsetAngle;

	}
	if ((InclinometerAngle > 135 && InclinometerAngle <= 180) || (InclinometerAngle >= -179 && InclinometerAngle < -135))
	{
		ExpectationAngle = 180 + offsetAngle;

	}

	 deltaAngle=( ((double)GV.RFTL720_D.RF_Angle_Roll )/100.0)-ExpectationAngle;
	 Angle_Error_Vlog=deltaAngle;
//	 Angle_Error_Vlog=5;
	 if(fabs(Angle_Error_Vlog)>4)
	 {
		 Angle_Error_Vlog_Flag++;
		 if(Angle_Error_Vlog_Flag>2)
		 {
			if(Paint_Gun_Delay_Close_Flag_Error<1)
			{
				Paint_Gun_Delay_Close_Flag_Error=1;
			}
		 }
	 }
	 else
	 {
		 Angle_Error_Vlog_Flag=0;
	 }

	 deltaSpeed=PositionalPID(ExpectationAngle, ( ((double)GV.RFTL720_D.RF_Angle_Roll )/100.0));


	if(deltaAngle<0)
	{
		deltaAngle=-deltaAngle;
	}

   if (deltaAngle >= 0 && deltaAngle <= deltaAngle1)
   {
	 GV.Motor_LeiSai[0].Target_Velcity = GV.Motor_LeiSai[0].Target_Velcity + KP1 * deltaSpeed;
	 GV.Motor_LeiSai[1].Target_Velcity = GV.Motor_LeiSai[1].Target_Velcity - KP1 * deltaSpeed;
   }
   if (deltaAngle > deltaAngle1 && deltaAngle <= deltaAngle2)
   {
	 GV.Motor_LeiSai[0].Target_Velcity = GV.Motor_LeiSai[0].Target_Velcity + KP2 * deltaSpeed;
	 GV.Motor_LeiSai[1].Target_Velcity = GV.Motor_LeiSai[1].Target_Velcity - KP2 * deltaSpeed;
   }
   if (deltaAngle > deltaAngle2 && deltaAngle <= deltaAngle3)
   {
	 GV.Motor_LeiSai[0].Target_Velcity = GV.Motor_LeiSai[0].Target_Velcity + KP3 * deltaSpeed;
	 GV.Motor_LeiSai[1].Target_Velcity = GV.Motor_LeiSai[1].Target_Velcity - KP3 * deltaSpeed;
   }
   if (deltaAngle > deltaAngle3 && deltaAngle <= 90)
   {
	 GV.Motor_LeiSai[0].Target_Velcity = GV.Motor_LeiSai[0].Target_Velcity + KP4 * deltaSpeed;
	 GV.Motor_LeiSai[1].Target_Velcity = GV.Motor_LeiSai[1].Target_Velcity - KP4 * deltaSpeed;
   }
}

void VehicleAutoBackward_12_18(double InclinometerAngle, double offsetAngle)//自动前进
{
	double Angle_Error_Vlog=0;
	//水平
	//需要调整左右补偿
	if ((InclinometerAngle >= 45 && InclinometerAngle <= 135))
	{
		ExpectationAngle = 90;

	}
	if ((InclinometerAngle >= -135 && InclinometerAngle <= -45))
	{
		ExpectationAngle = -90;
	}

	//垂直
	if ((InclinometerAngle >= 0 && InclinometerAngle < 45) || (InclinometerAngle > -45 && InclinometerAngle <= 0))
	{
		ExpectationAngle = 0 + offsetAngle;

	}
	if ((InclinometerAngle > 135 && InclinometerAngle <= 180) || (InclinometerAngle >= -179 && InclinometerAngle < -135))
	{
		ExpectationAngle = 180 + offsetAngle;

	}

	 deltaAngle=( ((double)GV.RFTL720_D.RF_Angle_Roll )/100.0)-ExpectationAngle;
	 Angle_Error_Vlog=deltaAngle;
//	 Angle_Error_Vlog=5;
	 if(fabs(Angle_Error_Vlog)>4)
	 {
		 Angle_Error_Vlog_Flag++;
		 if(Angle_Error_Vlog_Flag>2)
		 {
			if(Paint_Gun_Delay_Close_Flag_Error<7)
			{
				Paint_Gun_Delay_Close_Flag_Error=7;
			}
		 }
	 }
	 else
	 {
		 Angle_Error_Vlog_Flag=0;
	 }

	 deltaSpeed=PositionalPID(ExpectationAngle, ( ((double)GV.RFTL720_D.RF_Angle_Roll )/100.0));


	if(deltaAngle<0)
	{
		deltaAngle=-deltaAngle;
	}

   if (deltaAngle >= 0 && deltaAngle <= deltaAngle1)
   {
	 GV.Motor_LeiSai[0].Target_Velcity = GV.Motor_LeiSai[0].Target_Velcity + KP1 * deltaSpeed;
	 GV.Motor_LeiSai[1].Target_Velcity = GV.Motor_LeiSai[1].Target_Velcity - KP1 * deltaSpeed;
   }
   if (deltaAngle > deltaAngle1 && deltaAngle <= deltaAngle2)
   {
	 GV.Motor_LeiSai[0].Target_Velcity = GV.Motor_LeiSai[0].Target_Velcity + KP2 * deltaSpeed;
	 GV.Motor_LeiSai[1].Target_Velcity = GV.Motor_LeiSai[1].Target_Velcity - KP2 * deltaSpeed;
   }
   if (deltaAngle > deltaAngle2 && deltaAngle <= deltaAngle3)
   {
	 GV.Motor_LeiSai[0].Target_Velcity = GV.Motor_LeiSai[0].Target_Velcity + KP3 * deltaSpeed;
	 GV.Motor_LeiSai[1].Target_Velcity = GV.Motor_LeiSai[1].Target_Velcity - KP3 * deltaSpeed;
   }
   if (deltaAngle > deltaAngle3 && deltaAngle <= 90)
   {
	 GV.Motor_LeiSai[0].Target_Velcity = GV.Motor_LeiSai[0].Target_Velcity + KP4 * deltaSpeed;
	 GV.Motor_LeiSai[1].Target_Velcity = GV.Motor_LeiSai[1].Target_Velcity - KP4 * deltaSpeed;
   }
}







void VehicleAutoBackward(double InclinometerAngle, double offsetAngle)//自动后退
{
	double Angle_Error_Vlog=0;
	//水平
	if ((InclinometerAngle >= 45 && InclinometerAngle <= 135))
	{
		ExpectationAngle = 90 + offsetAngle;
	}
	if ((InclinometerAngle >= -135 && InclinometerAngle <= -45))
	{
		ExpectationAngle = - 90 - offsetAngle;
	}

	//垂直
	if ((InclinometerAngle >= 0 && InclinometerAngle < 45) || (InclinometerAngle > -45 && InclinometerAngle <= 0))
	{
		ExpectationAngle = 0 + offsetAngle;
	}
	if ((InclinometerAngle > 135 && InclinometerAngle <= 180) || (InclinometerAngle >= -179 && InclinometerAngle < -135))
	{
		ExpectationAngle = 180 + offsetAngle;
	}

	 deltaAngle=( ((double)GV.RFTL720_D.RF_Angle_Roll )/100.0)-ExpectationAngle;
	 Angle_Error_Vlog=deltaAngle;

	 deltaSpeed=PositionalPID(ExpectationAngle, ( ((double)GV.RFTL720_D.RF_Angle_Roll )/100.0));

	if(deltaAngle<0)
	{
		deltaAngle=-deltaAngle;
	}

   if (deltaAngle >= 0 && deltaAngle <= deltaAngle1)
   {
	 GV.Motor_LeiSai[0].Target_Velcity = GV.Motor_LeiSai[0].Target_Velcity + KP1 * deltaSpeed;//-
	 GV.Motor_LeiSai[1].Target_Velcity = GV.Motor_LeiSai[1].Target_Velcity - KP1 * deltaSpeed;//+
   }
   if (deltaAngle > deltaAngle1 && deltaAngle <= deltaAngle2)
   {
	 GV.Motor_LeiSai[0].Target_Velcity = GV.Motor_LeiSai[0].Target_Velcity + KP2 * deltaSpeed;
	 GV.Motor_LeiSai[1].Target_Velcity = GV.Motor_LeiSai[1].Target_Velcity - KP2 * deltaSpeed;
   }
   if (deltaAngle > deltaAngle2 && deltaAngle <= deltaAngle3)
   {
	 GV.Motor_LeiSai[0].Target_Velcity =  GV.Motor_LeiSai[0].Target_Velcity + KP3 * deltaSpeed;
	 GV.Motor_LeiSai[1].Target_Velcity =  GV.Motor_LeiSai[1].Target_Velcity - KP3 * deltaSpeed;
   }
   if (deltaAngle > deltaAngle3 && deltaAngle <= 90)
   {
	 GV.Motor_LeiSai[0].Target_Velcity = GV.Motor_LeiSai[0].Target_Velcity + KP4 * deltaSpeed;
	 GV.Motor_LeiSai[1].Target_Velcity = GV.Motor_LeiSai[1].Target_Velcity - KP4 * deltaSpeed;
   }
}

void VehicleAutoTurn(double InclinometerAngle, double expectationAngle)//换道自动转弯
{
	deltaSpeed=0.75 * PositionalPID(expectationAngle,(((double)GV.RFTL720_D.RF_Angle_Roll)/100.0));     //水平纠偏
	deltaAngle=(((double)GV.RFTL720_D.RF_Angle_Roll)/100.0)-expectationAngle;

	if(deltaAngle<0)//取角度差值的绝对值
	{
		deltaAngle=-deltaAngle;
	}
	 GV.Motor_LeiSai[0].Target_Velcity =0;
	 GV.Motor_LeiSai[1].Target_Velcity = 0;
   if (deltaAngle >= 0 && deltaAngle <= deltaAngle1)
   {
	 GV.Motor_LeiSai[0].Target_Velcity = GV.Motor_LeiSai[0].Target_Velcity + KP11 * deltaSpeed;
	 GV.Motor_LeiSai[1].Target_Velcity = GV.Motor_LeiSai[1].Target_Velcity - KP11 * deltaSpeed;
   }
   if (deltaAngle > deltaAngle1 && deltaAngle <= deltaAngle2)
   {
	   GV.Motor_LeiSai[0].Target_Velcity = GV.Motor_LeiSai[0].Target_Velcity + KP22 * deltaSpeed;
	   GV.Motor_LeiSai[1].Target_Velcity = GV.Motor_LeiSai[1].Target_Velcity - KP22 * deltaSpeed;
   }
   if (deltaAngle > deltaAngle2 && deltaAngle <= deltaAngle3)
   {
	   GV.Motor_LeiSai[0].Target_Velcity = GV.Motor_LeiSai[0].Target_Velcity + KP33 * deltaSpeed;
	   GV.Motor_LeiSai[1].Target_Velcity = GV.Motor_LeiSai[1].Target_Velcity - KP33 * deltaSpeed;
   }
   if (deltaAngle > deltaAngle3 && deltaAngle <= deltaAngle4)
   {
	   GV.Motor_LeiSai[0].Target_Velcity = GV.Motor_LeiSai[0].Target_Velcity + KP44 * deltaSpeed;
	   GV.Motor_LeiSai[1].Target_Velcity = GV.Motor_LeiSai[1].Target_Velcity - KP44 * deltaSpeed;
   }
   if (deltaAngle > deltaAngle4 && deltaAngle <= 200)
   {
	   GV.Motor_LeiSai[0].Target_Velcity = GV.Motor_LeiSai[0].Target_Velcity + KP55 * deltaSpeed;
	   GV.Motor_LeiSai[1].Target_Velcity = GV.Motor_LeiSai[1].Target_Velcity - KP55 * deltaSpeed;
   }
}


void Manu_No_MENS()
{
	if(GV.DH_WH_HandSet.for_flag == 1)
	{
		GV.Motor_LeiSai[0].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
		GV.Motor_LeiSai[1].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
		GV.Motor_LeiSai[0].Target_Velcity=GV.DH_WH_HandSet.motor_speed_value;
		GV.Motor_LeiSai[1].Target_Velcity=GV.DH_WH_HandSet.motor_speed_value;
	}
	else if(GV.DH_WH_HandSet.back_flag == 1)
	{
		GV.Motor_LeiSai[0].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
		GV.Motor_LeiSai[1].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
		GV.Motor_LeiSai[0].Target_Velcity=-GV.DH_WH_HandSet.motor_speed_value;
		GV.Motor_LeiSai[1].Target_Velcity=-GV.DH_WH_HandSet.motor_speed_value;
	}
	else if(GV.DH_WH_HandSet.left_flag == 1)//左转
	{
		GV.Motor_LeiSai[0].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
		GV.Motor_LeiSai[1].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
		GV.Motor_LeiSai[0].Target_Velcity=-66666;
		GV.Motor_LeiSai[1].Target_Velcity=66666;
	}
	else if(GV.DH_WH_HandSet.right_flag == 1)//右转
	{
		GV.Motor_LeiSai[0].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
		GV.Motor_LeiSai[1].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
		GV.Motor_LeiSai[0].Target_Velcity=66666;
		GV.Motor_LeiSai[1].Target_Velcity=-66666;
	}
	else
	{
		GV.Motor_LeiSai[0].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
		GV.Motor_LeiSai[1].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
		GV.Motor_LeiSai[0].Target_Velcity=0;
		GV.Motor_LeiSai[1].Target_Velcity=0;
	}
}

int Last_Test_Falg=0;
int Test_Count=0;
int Test_Falg_G=2;
void Test_Void()
{
	if(GV.DH_WH_HandSet.working_start1_stop2 == 2)
	{
	if(Last_Test_Falg==GV.DH_WH_HandSet.test_start1_stop0)
	{
	}
	else
	{
		Test_Falg_G=GV.DH_WH_HandSet.test_start1_stop0;
	}
	switch(Test_Falg_G)
	{
	case 0:
		GF_BSP_GPIO_SetIO(0,GPIO_PIN_SET);
		//LeisaiCan_FDCANfunction(ID1,8,DO1_OFF);
		Paint_Gun_Open1_Close0=0;
		Test_Count++;
		if(Test_Count>=2)
		{
			Test_Falg_G=3;Test_Count=0;
		}
		break;
	case 1:
		GF_BSP_GPIO_SetIO(0,GPIO_PIN_RESET);
		//LeisaiCan_FDCANfunction(ID1,8,DO1_ON);
		Paint_Gun_Open1_Close0=1;
		Test_Count++;
		if(Test_Count>=2)
		{
			Test_Falg_G=3;
			Test_Count=0;
		}
		break;
	case 2:
		GF_BSP_GPIO_SetIO(0,GPIO_PIN_SET);
		//LeisaiCan_FDCANfunction(ID1,8,DO1_OFF);
		Paint_Gun_Open1_Close0=0;
		Test_Count++;
		if(Test_Count>=2)
		{
			Test_Falg_G=3;
			Test_Count=0;
		}
		break;
	case 3:
		Test_Count=0;
		break;
	}
	Last_Test_Falg=GV.DH_WH_HandSet.test_start1_stop0;
	}
	else if(GV.DH_WH_HandSet.working_start1_stop2 == 1)
	{
		GF_BSP_GPIO_SetIO(0,GPIO_PIN_SET);
		Paint_Gun_Open1_Close0=0;
	}
}

void Vertical_Auto_Tast()
{
// 	int16_t Fore_Back_Value;					//前进后退
// 	int16_t Left_Right_Value;					//左转右转


	 if(GV.DH_WH_HandSet.for_flag == 1)     //如果遥控器前进位被置位
	{
		Paint_Gun_Delay_Close_Flag=1;
		Dual_Wheel_Posture_Adjust_Homonymous_Forward_Vertical();
		Backward_Vertical_Falg=0;
	}
	else if(GV.DH_WH_HandSet.back_flag == 1)//后退
	{
		Paint_Gun_Delay_Close_Flag=7;
		Dual_Wheel_Posture_Adjust_Homonymous_Backward_Vertical();
		Forward_Vertical_Falg=0;
	}
	else if(GV.DH_WH_HandSet.left_flag == 1)//左转
	{
		GV.Motor_LeiSai[0].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
		GV.Motor_LeiSai[1].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
		GV.Motor_LeiSai[0].Target_Velcity=-66666;
		GV.Motor_LeiSai[1].Target_Velcity=66666;
		Forward_Vertical_Falg=0;
		Backward_Vertical_Falg=0;
	}
	else if(GV.DH_WH_HandSet.right_flag == 1)//右转
	{
		GV.Motor_LeiSai[0].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
		GV.Motor_LeiSai[1].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
		GV.Motor_LeiSai[0].Target_Velcity=66666;
		GV.Motor_LeiSai[1].Target_Velcity=-66666;
		Forward_Vertical_Falg=0;
		Backward_Vertical_Falg=0;
	} //换道，摇杆都没按 停车
	else
	{
		GV.Motor_LeiSai[0].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
		GV.Motor_LeiSai[1].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
		GV.Motor_LeiSai[0].Target_Velcity=0;
		GV.Motor_LeiSai[1].Target_Velcity=0;
		Forward_Vertical_Falg=0;
		Backward_Vertical_Falg=0;
		if(Paint_Gun_Delay_Close_Flag_Error == 65 || Paint_Gun_Delay_Close_Flag == 65)
		{
			Paint_Gun_Delay_Close_Flag_Error=0;
			Paint_Gun_Delay_Close_Flag = 0;
		}
	}
}

int Paint_Gun_Delay_Count=0;
double Paint_Gun_Delay_Time_Ms=0;
//
//void Paint_Gun_Delay_Close_Func()
//{
//	//后退时间计算；；；；Paint_Gun_Delay_Time_Ms
//	switch(Paint_Gun_Delay_Close_Flag)
//	{
//		case 0:
//			break;
//		case 1:
//			Paint_Gun_Delay_Close_Flag++;
//			break;
//		case 2:
//			Paint_Gun_Delay_Close_Flag++;
//			break;
//		case 3: case 4:
//			LeisaiCan_FDCANfunction(ID1,8,DO1_OFF);
//			Paint_Gun_Open1_Close0=0;
//			Paint_Gun_Delay_Close_Flag++;
//			Paint_Gun_Delay_Count=0;
//			break;
//		case 5:
//			Paint_Gun_Delay_Time_Ms_Compu();
//
//			//喷枪延时
//			if(Paint_Gun_Delay_Count>1*Paint_Gun_Delay_Time_Ms)
//			{
//				Paint_Gun_Delay_Close_Flag++;
//			}
//			else
//			{
//				Paint_Gun_Delay_Count++;
//			}
//			GV.Motor_LeiSai[0].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
//			GV.Motor_LeiSai[1].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
//			GV.Motor_LeiSai[0].Target_Velcity=GV.DH_CAN_Paint.Motor_Speed_Level;
//			GV.Motor_LeiSai[1].Target_Velcity=GV.DH_CAN_Paint.Motor_Speed_Level;
//			VehicleAutoForward_1( ( ((double)GV.RFTL720_D.RF_Angle_Roll )/100.0),0 );//自动前进
//			break;
//		case 6:
//			Paint_Gun_Delay_Count=0;
//			GV.Motor_LeiSai[0].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
//			GV.Motor_LeiSai[1].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
//			GV.Motor_LeiSai[0].Target_Velcity=0;
//			GV.Motor_LeiSai[1].Target_Velcity=0;
//			Paint_Gun_Delay_Close_Flag=61;
//			break;
//		case 61:  case 62: case 63: case 64:
//			Paint_Gun_Delay_Count=0;
//			GV.Motor_LeiSai[0].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
//			GV.Motor_LeiSai[1].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
//			GV.Motor_LeiSai[0].Target_Velcity=0;
//			GV.Motor_LeiSai[1].Target_Velcity=0;
//			Paint_Gun_Delay_Close_Flag++;
//			break;
//		case 65:
//			Paint_Gun_Delay_Count=0;
//			GV.Motor_LeiSai[0].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
//			GV.Motor_LeiSai[1].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
//			GV.Motor_LeiSai[0].Target_Velcity=0;
//			GV.Motor_LeiSai[1].Target_Velcity=0;
//			break;
//		case 7:
//			Paint_Gun_Delay_Close_Flag++;
//			break;
//		case 8:
//			Paint_Gun_Delay_Close_Flag++;
//			break;
//		case 9: case 10:
//			LeisaiCan_FDCANfunction(ID1,8,DO1_OFF);
//			Paint_Gun_Open1_Close0=0;
//			Paint_Gun_Delay_Close_Flag++;
//			Paint_Gun_Delay_Count=0;
//			break;
//
//		case 11:
//			Paint_Gun_Delay_Time_Ms_Compu();
//			//喷枪延时
//			if(Paint_Gun_Delay_Count>1*Paint_Gun_Delay_Time_Ms)
//			{
//				Paint_Gun_Delay_Close_Flag++;
//			}
//			else
//			{
//				Paint_Gun_Delay_Count++;
//			}
//			GV.Motor_LeiSai[0].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
//			GV.Motor_LeiSai[1].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
//			GV.Motor_LeiSai[0].Target_Velcity=-GV.DH_CAN_Paint.Motor_Speed_Level;
//			GV.Motor_LeiSai[1].Target_Velcity=-GV.DH_CAN_Paint.Motor_Speed_Level;
//			VehicleAutoBackward_12_18_1((((double)GV.RFTL720_D.RF_Angle_Roll)/100.0),0);//自动前进
//			break;
//		case 12:
//			Paint_Gun_Delay_Count=0;
//			GV.Motor_LeiSai[0].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
//			GV.Motor_LeiSai[1].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
//			GV.Motor_LeiSai[0].Target_Velcity=0;
//			GV.Motor_LeiSai[1].Target_Velcity=0;
//			Paint_Gun_Delay_Close_Flag=71;
//			break;
//		case 71: case 72: case 73: case 74:
//			Paint_Gun_Delay_Count=0;
//			GV.Motor_LeiSai[0].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
//			GV.Motor_LeiSai[1].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
//			GV.Motor_LeiSai[0].Target_Velcity=0;
//			GV.Motor_LeiSai[1].Target_Velcity=0;
//			Paint_Gun_Delay_Close_Flag++;
//			break;
//		case 75:
//			Paint_Gun_Delay_Count=0;
//			GV.Motor_LeiSai[0].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
//			GV.Motor_LeiSai[1].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
//			GV.Motor_LeiSai[0].Target_Velcity=0;
//			GV.Motor_LeiSai[1].Target_Velcity=0;
//			break;
//	    default:
//			GV.Motor_LeiSai[0].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
//			GV.Motor_LeiSai[1].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
//	    	GV.Motor_LeiSai[0].Target_Velcity=0;
//	    	GV.Motor_LeiSai[1].Target_Velcity=0;
//	    	break;
//	}
//	//Paint_Gun_Delay_Time_Ms_Compu();
//}






void Vertical_Left_Right_Change_Road_UP()
{
	switch(change_road_status_flag)
	{
	    case 0:
	    		isTurn=false;//换完置位
	    		Paint_Gun_Close();
	    		change_road_status_flag=0;
	    		VehicleAutoTurn(( ((double)GV.RFTL720_D.RF_Angle_Roll )/100.0),-90);  //没到90右转
	    		continueTurn_up(-90);
	    		change_road_straight_count=0;
	    		if (isTurn)
				{
					change_road_status_flag=1;
					GV.Motor_LeiSai[0].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
					GV.Motor_LeiSai[1].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
					GV.Motor_LeiSai[0].Target_Velcity=0;
					GV.Motor_LeiSai[1].Target_Velcity=0;
					isTurn=false;//换完置位
				}
	    	break;

	    case 1:                                              //到了90°之后就开始水平走
	    	Paint_Gun_Close_Flag=0;
	    	if(change_road_straight_count<change_road_distance_Ms)
	    	{
	    		change_road_status_flag=1;
	    		change_road_straight_count++;
	    		GV.Motor_LeiSai[0].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
	    		GV.Motor_LeiSai[1].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
				GV.Motor_LeiSai[0].Target_Velcity=-185775;
				GV.Motor_LeiSai[1].Target_Velcity=-185775;
	         }
	    	else            //水平运动结束，该转回去了
	    	{
	    		change_road_status_flag=2;
	    		change_road_straight_count=0;
	    		GV.Motor_LeiSai[0].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
	    		GV.Motor_LeiSai[1].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
				GV.Motor_LeiSai[0].Target_Velcity=0;
				GV.Motor_LeiSai[1].Target_Velcity=0;
	    	}
	    	break;
	    case 2:
				VehicleAutoTurn(( ((double)GV.RFTL720_D.RF_Angle_Roll )/100.0),0);  //转回来，没到0左转
				continueTurn_up(0);
				if (isTurn)
				{
					change_road_status_flag++;
					GV.Motor_LeiSai[0].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
					GV.Motor_LeiSai[1].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
					GV.Motor_LeiSai[0].Target_Velcity=0;
					GV.Motor_LeiSai[1].Target_Velcity=0;
					isTurn=false;
				}
		    break;
	    case 3:
			GV.Motor_LeiSai[0].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
			GV.Motor_LeiSai[1].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
			GV.Motor_LeiSai[0].Target_Velcity=0;
			GV.Motor_LeiSai[1].Target_Velcity=0;
	    	break;
	    default:
	    	break;
	}
}

void Vertical_Left_Right_Change_Road_Down()
{
	switch(change_road_status_flag)
	{
	    case 0:
	    		isTurn=false;
	    		Paint_Gun_Close();
	    		change_road_status_flag=0;
	    		VehicleAutoTurn(( ((double)GV.RFTL720_D.RF_Angle_Roll )/100.0),90);  //没到90右转
	    		continueTurn_up(90);
	    		change_road_straight_count=0;
	    		if (isTurn)
				{
					change_road_status_flag=1;
					GV.Motor_LeiSai[0].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
					GV.Motor_LeiSai[1].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
					GV.Motor_LeiSai[0].Target_Velcity=0;
					GV.Motor_LeiSai[1].Target_Velcity=0;
					isTurn=false;//换完置位
				}
	    	break;

	    case 1:                                              //到了90°之后就开始水平走
	    	Paint_Gun_Close_Flag=0;
	    	if(change_road_straight_count<change_road_distance_Ms)
	    	{
	    		change_road_status_flag=1;
	    		change_road_straight_count++;
	    		GV.Motor_LeiSai[0].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
	    		GV.Motor_LeiSai[1].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
				GV.Motor_LeiSai[0].Target_Velcity=185775;
				GV.Motor_LeiSai[1].Target_Velcity=185775;
	         }
	    	else            //水平运动结束，该转回去了
	    	{
	    		change_road_status_flag=2;
	    		change_road_straight_count=0;
	    		GV.Motor_LeiSai[0].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
	    		GV.Motor_LeiSai[1].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
				GV.Motor_LeiSai[0].Target_Velcity=0;
				GV.Motor_LeiSai[1].Target_Velcity=0;
	    	}
	    	break;
	    case 2:
	    		isTurn=false;//换完置位
				VehicleAutoTurn(( ((double)GV.RFTL720_D.RF_Angle_Roll )/100.0),0);  //转回来，没到0左转
				continueTurn_up(0);
				if (isTurn)
				{
					change_road_status_flag++;
					GV.Motor_LeiSai[0].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
					GV.Motor_LeiSai[1].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
					GV.Motor_LeiSai[0].Target_Velcity=0;
					GV.Motor_LeiSai[1].Target_Velcity=0;
					isTurn=false;
				}
		    break;
	    case 3:
			GV.Motor_LeiSai[0].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
			GV.Motor_LeiSai[1].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
			GV.Motor_LeiSai[0].Target_Velcity=0;
			GV.Motor_LeiSai[1].Target_Velcity=0;

	    	break;
	    default:
			GV.Motor_LeiSai[0].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
			GV.Motor_LeiSai[1].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
	    	GV.Motor_LeiSai[0].Target_Velcity=0;
	    	GV.Motor_LeiSai[1].Target_Velcity=0;
	    	break;
	}
}




double Back_Dis_Flag=0;
int Growth_Rate=0;


void Back_Time_Compu()
{
	//double Velocity_CM_S=((double)GV.DH_CAN_Paint.Motor_Speed_Level)/70/500*3*0.942*100/60;

	double Velocity_CM_S=185775.0/70/500*3*0.942*100/60;
	if(Velocity_CM_S!=0)
	{
		change_road_distance_Ms = Back_Dis_CM/Velocity_CM_S*1000/2 ;
		//换到距离存在偏差，修改此处
	}
}
void Back_Time_Compu_Hori()
{
	//经测试水平换道距离偏差为7厘米，在此进行修正（Back_Dis_CM-7）
	double Velocity_CM_S=185775.0/70/500*3*0.942*100/60;
	if(Velocity_CM_S!=0)
	{
		change_road_distance_Ms = (Back_Dis_CM-7)/Velocity_CM_S*1000/2 ;
		//换到距离存在偏差，修改此处
	}
}

double Back_Dis_CM_Input=0;
double change_road_distance_Ms_1=0;
void Back_Time_Compu_Ver()
{
	//经测试竖直换道距离偏差为4厘米，在此进行修正（Back_Dis_CM-4）
	double Velocity_CM_S=185775.0/70/500*3*0.942*100/60;

	Back_Dis_CM_Input=(GV.DH_WH_HandSet.Left_compensation * 9 + 127) / 255 *10+((GV.DH_WH_HandSet.Right_compensation * 9 + 127) / 255) + 2;
	if(Velocity_CM_S!=0)
	{
	//	change_road_distance_Ms = (Back_Dis_CM-4)/Velocity_CM_S*1000/2 ;
		change_road_distance_Ms=(Back_Dis_CM_Input-4)/Velocity_CM_S*1000/2 ;
		//换到距离存在偏差，修改此处
	}
}

void Paint_Gun_Close()
{
	switch(Paint_Gun_Close_Flag)
	{
	case 0:
		LeisaiCan_FDCANfunction(ID1,8,DO1_OFF);
		Paint_Gun_Open1_Close0=0;
		Paint_Gun_Close_Flag++;
		break;
	case 1:
		LeisaiCan_FDCANfunction(ID1,8,DO1_OFF);
		Paint_Gun_Open1_Close0=0;
		Paint_Gun_Close_Flag++;
		break;
	case 2:
		break;

	}
}



void Paint_Gun_Delay_Time_Ms_Compu()
{
	double Velocity_CM_S=((double)GV.DH_WH_HandSet.motor_speed_value)/70/500*3*0.942*100/60;
	if(Velocity_CM_S!=0)
	{
		Paint_Gun_Delay_Time_Ms= 4/Velocity_CM_S*1000/2 ;  //延时时间由原来的7->4
	}
}

void Vertical_Right_Left_Change_Road_UP()
{
	switch(change_road_status_flag)
	{
	    case 0:
	    		isTurn=false;//换完置位
	    		Paint_Gun_Close();
	    		change_road_status_flag=0;
	    		VehicleAutoTurn((((double)GV.RFTL720_D.RF_Angle_Roll )/100.0),90);  //没到90右转
	    		continueTurn_up(90);
	    		change_road_straight_count=0;
	    		if (isTurn)
				{
					change_road_status_flag=1;
					GV.Motor_LeiSai[0].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
					GV.Motor_LeiSai[1].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
					GV.Motor_LeiSai[0].Target_Velcity=0;
					GV.Motor_LeiSai[1].Target_Velcity=0;
					isTurn=false;//换完置位
				}
	    	break;

	    case 1:                                              //到了90°之后就开始水平走
	    	Paint_Gun_Close_Flag=0;
	    	if(change_road_straight_count<change_road_distance_Ms)
	    	{
	    		change_road_status_flag=1;
	    		change_road_straight_count++;
	    		GV.Motor_LeiSai[0].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
	    		GV.Motor_LeiSai[1].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
				GV.Motor_LeiSai[0].Target_Velcity=-185775;
				GV.Motor_LeiSai[1].Target_Velcity=-185775;
	         }
	    	else            //水平运动结束，该转回去了
	    	{
	    		change_road_status_flag=2;
	    		change_road_straight_count=0;
	    		GV.Motor_LeiSai[0].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
	    		GV.Motor_LeiSai[1].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
				GV.Motor_LeiSai[0].Target_Velcity=0;
				GV.Motor_LeiSai[1].Target_Velcity=0;
	    	}
	    	break;
	    case 2:
	    	VehicleAutoTurn_End(( ((double)GV.RFTL720_D.RF_Angle_Roll )/100.0),0);  //转回来，没到0左转
				continueTurn_up(0);
				if (isTurn)
				{
					change_road_status_flag++;
					GV.Motor_LeiSai[0].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
					GV.Motor_LeiSai[1].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
					GV.Motor_LeiSai[0].Target_Velcity=0;
					GV.Motor_LeiSai[1].Target_Velcity=0;
					isTurn=false;
				}
		    break;
	    case 3:
			GV.Motor_LeiSai[0].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
			GV.Motor_LeiSai[1].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
			GV.Motor_LeiSai[0].Target_Velcity=0;
			GV.Motor_LeiSai[1].Target_Velcity=0;
	    	break;
	    default:
	    	break;
	}
}



void Vertical_Right_Left_Change_Road_Down()
{
	switch(change_road_status_flag)
	{
	    case 0:
	    		isTurn=false;
	    		Paint_Gun_Close();
	    		change_road_status_flag=0;
	    		VehicleAutoTurn((((double)GV.RFTL720_D.RF_Angle_Roll )/100.0),-90);  //没到90右转
	    		continueTurn_up(-90);
	    		change_road_straight_count=0;
	    		if (isTurn)
				{
					change_road_status_flag=1;
					GV.Motor_LeiSai[0].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
					GV.Motor_LeiSai[1].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
					GV.Motor_LeiSai[0].Target_Velcity=0;
					GV.Motor_LeiSai[1].Target_Velcity=0;
					isTurn=false;//换完置位
				}
	    	break;

	    case 1:                                              //到了90°之后就开始水平走
	    	Paint_Gun_Close_Flag=0;
	    	if(change_road_straight_count<change_road_distance_Ms)
	    	{
	    		change_road_status_flag=1;
	    		change_road_straight_count++;
	    		GV.Motor_LeiSai[0].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
	    		GV.Motor_LeiSai[1].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
				GV.Motor_LeiSai[0].Target_Velcity=185775;
				GV.Motor_LeiSai[1].Target_Velcity=185775;
	         }
	    	else            //水平运动结束，该转回去了
	    	{
	    		change_road_status_flag=2;
	    		change_road_straight_count=0;
	    		GV.Motor_LeiSai[0].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
	    		GV.Motor_LeiSai[1].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
				GV.Motor_LeiSai[0].Target_Velcity=0;
				GV.Motor_LeiSai[1].Target_Velcity=0;
	    	}
	    	break;
	    case 2:
	    		isTurn=false;//换完置位
	    		VehicleAutoTurn_End(( ((double)GV.RFTL720_D.RF_Angle_Roll )/100.0),0);  //转回来，没到0左转
				continueTurn_up(0);
				if (isTurn)
				{
					change_road_status_flag++;
					GV.Motor_LeiSai[0].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
					GV.Motor_LeiSai[1].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
					GV.Motor_LeiSai[0].Target_Velcity=0;
					GV.Motor_LeiSai[1].Target_Velcity=0;
					isTurn=false;
				}
		    break;
	    case 3:
			GV.Motor_LeiSai[0].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
			GV.Motor_LeiSai[1].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
			GV.Motor_LeiSai[0].Target_Velcity=0;
			GV.Motor_LeiSai[1].Target_Velcity=0;

	    	break;
	    default:
			GV.Motor_LeiSai[0].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
			GV.Motor_LeiSai[1].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
	    	GV.Motor_LeiSai[0].Target_Velcity=0;
	    	GV.Motor_LeiSai[1].Target_Velcity=0;
	    	break;
	}
}





double Vertical_ExpectationAngle=0;
double Angle_Error=0;

void Dual_Wheel_Posture_Adjust_Homonymous_Forward_Vertical()
{
	double InclinometerAngle= ((double)GV.RFTL720_D.RF_Angle_Roll )/100.0;

	switch(Forward_Vertical_Falg)
	{
	case 0:

		if (InclinometerAngle <= 90 && InclinometerAngle >=-90)
		{
			Vertical_ExpectationAngle = 0 ;
		}
		else
		{
			Vertical_ExpectationAngle = 180 ;
		}
		Forward_Vertical_Falg++;


		break;
	case 1: case 2:

		Angle_Error=InclinometerAngle-Vertical_ExpectationAngle;

		if(Angle_Error>180)
		{
			Angle_Error=Angle_Error-360;
		}
		else if(Angle_Error<-180)
		{
			Angle_Error=Angle_Error+360;
		}
		if((Angle_Error>0)&&(fabs(Angle_Error)>2))
		{
			GV.Motor_LeiSai[0].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
			GV.Motor_LeiSai[1].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
			GV.Motor_LeiSai[0].Target_Velcity=20000;
			GV.Motor_LeiSai[1].Target_Velcity=150000;
		}
		else if((Angle_Error<0)&&(fabs(Angle_Error)>2))
		{
			GV.Motor_LeiSai[0].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
			GV.Motor_LeiSai[1].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
			GV.Motor_LeiSai[0].Target_Velcity=150000;
			GV.Motor_LeiSai[1].Target_Velcity=20000;
		}
		Forward_Vertical_Falg++;
		break;
	case 3:
		Angle_Error=InclinometerAngle-Vertical_ExpectationAngle;



		if(Angle_Error>180)
		{
			Angle_Error=Angle_Error-360;
		}
		else if(Angle_Error<-180)
		{
			Angle_Error=Angle_Error+360;
		}
		if((Angle_Error>0)&&(fabs(Angle_Error)>2))
		{
			GV.Motor_LeiSai[0].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
			GV.Motor_LeiSai[1].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
			GV.Motor_LeiSai[0].Target_Velcity=20000/2;
			GV.Motor_LeiSai[1].Target_Velcity=150000/2;
		}
		else if((Angle_Error<0)&&(fabs(Angle_Error)>2))
		{
			GV.Motor_LeiSai[0].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
			GV.Motor_LeiSai[1].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
			GV.Motor_LeiSai[0].Target_Velcity=150000/2;
			GV.Motor_LeiSai[1].Target_Velcity=20000/2;
		}
		if(fabs(Angle_Error)<2)
		{
			Forward_Vertical_Falg++;
		}
		else
		{
			Forward_Vertical_Falg=3;
		}

		break;
	case 4:
		GV.Motor_LeiSai[0].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
		GV.Motor_LeiSai[1].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
		GV.Motor_LeiSai[0].Target_Velcity=GV.DH_WH_HandSet.motor_speed_value;
		GV.Motor_LeiSai[1].Target_Velcity=GV.DH_WH_HandSet.motor_speed_value;
		VehicleAutoForward( ( ((double)GV.RFTL720_D.RF_Angle_Roll )/100.0),0 );//自动前进



		break;
	}
}

void Dual_Wheel_Posture_Adjust_Homonymous_Backward_Vertical()
{
	double InclinometerAngle= ((double)GV.RFTL720_D.RF_Angle_Roll )/100.0;
	double Angle_Error=0;

	switch(Backward_Vertical_Falg)
	{
	case 0:
		if (InclinometerAngle <= 90 && InclinometerAngle >=-90)
		{
			Vertical_ExpectationAngle = 0 ;
		}
		else
		{
			Vertical_ExpectationAngle = 180 ;
		}
		Backward_Vertical_Falg++;

		break;
	case 1: case 2:

		Angle_Error=InclinometerAngle-Vertical_ExpectationAngle;

		if(Angle_Error>180)
		{
			Angle_Error=Angle_Error-360;
		}
		else if(Angle_Error<-180)
		{
			Angle_Error=Angle_Error+360;
		}
		if((Angle_Error>0)&&(fabs(Angle_Error)>2))
		{
			GV.Motor_LeiSai[0].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
			GV.Motor_LeiSai[1].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
			GV.Motor_LeiSai[0].Target_Velcity=-150000;
			GV.Motor_LeiSai[1].Target_Velcity=-20000;
		}
		else if((Angle_Error<0)&&(fabs(Angle_Error)>2))
		{
			GV.Motor_LeiSai[0].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
			GV.Motor_LeiSai[1].Run_Mode=DF_MSP_LSMotor_SpeedValue ;

			GV.Motor_LeiSai[0].Target_Velcity=-20000;
			GV.Motor_LeiSai[1].Target_Velcity=-150000;
		}
		Backward_Vertical_Falg++;
		break;
	case 3:
		Angle_Error=InclinometerAngle-Vertical_ExpectationAngle;
		if(fabs(Angle_Error)<2)
		{
			Backward_Vertical_Falg++;
		}
		break;
	case 4:
		GV.Motor_LeiSai[0].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
		GV.Motor_LeiSai[1].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
		GV.Motor_LeiSai[0].Target_Velcity=-GV.DH_WH_HandSet.motor_speed_value;
		GV.Motor_LeiSai[1].Target_Velcity=-GV.DH_WH_HandSet.motor_speed_value;
		VehicleAutoBackward_12_18( ( ((double)GV.RFTL720_D.RF_Angle_Roll )/100.0),0 );//自动前进

		break;
	}
}




//int Error_Flag=0;
//Paint_Gun_Delay_Close_Flag=1;
//void Paint_Gun_Delay_Close_Func_Modify()
//{
//	double Angle_111111=0;
//	switch(Paint_Gun_Delay_Close_Flag_Modfiy)
//	{
//		case 0:
//			Angle_111111=fabs(Angle_Error_Vlog);
//			Paint_Gun_Delay_Close_Flag_Modfiy=1;
//			if(Angle_111111>4)
//			{
//				Error_Flag++;
//				Paint_Gun_Delay_Close_Flag_Modfiy=0;
//				if(Error_Flag>=2)
//				{
//					Paint_Gun_Delay_Close_Flag_Modfiy=2;
//				}
//			}
//			else
//			{
//				Error_Flag=0;
//			}
//			break;
//		case 1:
//			Paint_Gun_Delay_Close_Flag++;
//			Paint_Gun_Delay_Close_Flag_Modfiy=Paint_Gun_Delay_Close_Flag;
//			break;
//
//		case 8:
//			if(Paint_Gun_Delay_Close_Flag==7)
//			{
//				Paint_Gun_Delay_Close_Flag_Modfiy=0;
//			}
//			else
//			{
//				Paint_Gun_Delay_Close_Flag_Modfiy++;
//			}
//			break;
//
//		case 2:
//			if(Paint_Gun_Delay_Close_Flag==1)
//			{
//				Paint_Gun_Delay_Close_Flag_Modfiy=0;
//			}
//			else
//			{
//				Paint_Gun_Delay_Close_Flag_Modfiy++;
//			}
//			break;
//		case 3: case 4:
//			LeisaiCan_FDCANfunction(ID1,8,DO1_OFF);
//			Paint_Gun_Open1_Close0=0;
//			Paint_Gun_Delay_Close_Flag_Modfiy++;
//			Paint_Gun_Delay_Count=0;
//			break;
//		case 5:
//			Paint_Gun_Delay_Time_Ms_Compu();
//
//			//喷枪延时
//			if(Paint_Gun_Delay_Count>1*Paint_Gun_Delay_Time_Ms)
//			{
//				Paint_Gun_Delay_Close_Flag_Modfiy++;
//			}
//			else
//			{
//				Paint_Gun_Delay_Count++;
//			}
//			GV.Motor_LeiSai[0].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
//			GV.Motor_LeiSai[1].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
//			GV.Motor_LeiSai[0].Target_Velcity=GV.DH_CAN_Paint.Motor_Speed_Level;
//			GV.Motor_LeiSai[1].Target_Velcity=GV.DH_CAN_Paint.Motor_Speed_Level;
//			VehicleAutoForward( ( ((double)GV.RFTL720_D.RF_Angle_Roll )/100.0),0 );//自动前进
//			break;
//		case 6:
//			Paint_Gun_Delay_Count=0;
//			GV.Motor_LeiSai[0].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
//			GV.Motor_LeiSai[1].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
//			GV.Motor_LeiSai[0].Target_Velcity=0;
//			GV.Motor_LeiSai[1].Target_Velcity=0;
//			Paint_Gun_Delay_Close_Flag_Modfiy=61;
//			break;
//		case 61:  case 62: case 63: case 64:
//			Paint_Gun_Delay_Count=0;
//			GV.Motor_LeiSai[0].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
//			GV.Motor_LeiSai[1].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
//			GV.Motor_LeiSai[0].Target_Velcity=0;
//			GV.Motor_LeiSai[1].Target_Velcity=0;
//			Paint_Gun_Delay_Close_Flag_Modfiy++;
//			break;
//		case 65:
//			Error_Flag=0;
//
//			break;
//
//		case 9: case 10:
//			LeisaiCan_FDCANfunction(ID1,8,DO1_OFF);
//			Paint_Gun_Open1_Close0=0;
//			Paint_Gun_Delay_Close_Flag_Modfiy++;
//			Paint_Gun_Delay_Count=0;
//			break;
//
//		case 11:
//			Paint_Gun_Delay_Time_Ms_Compu();
//			//喷枪延时
//			if(Paint_Gun_Delay_Count>1*Paint_Gun_Delay_Time_Ms)
//			{
//				Paint_Gun_Delay_Close_Flag_Modfiy++;
//			}
//			else
//			{
//				Paint_Gun_Delay_Count++;
//			}
//			GV.Motor_LeiSai[0].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
//			GV.Motor_LeiSai[1].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
//			GV.Motor_LeiSai[0].Target_Velcity=-GV.DH_CAN_Paint.Motor_Speed_Level;
//			GV.Motor_LeiSai[1].Target_Velcity=-GV.DH_CAN_Paint.Motor_Speed_Level;
//			VehicleAutoForward((((double)GV.RFTL720_D.RF_Angle_Roll)/100.0),0);//自动前进
//			break;
//		case 12:
//			Paint_Gun_Delay_Count=0;
//			GV.Motor_LeiSai[0].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
//			GV.Motor_LeiSai[1].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
//			GV.Motor_LeiSai[0].Target_Velcity=0;
//			GV.Motor_LeiSai[1].Target_Velcity=0;
//			Paint_Gun_Delay_Close_Flag_Modfiy=71;
//			break;
//		case 71: case 72: case 73: case 74:
//			Paint_Gun_Delay_Count=0;
//			GV.Motor_LeiSai[0].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
//			GV.Motor_LeiSai[1].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
//			GV.Motor_LeiSai[0].Target_Velcity=0;
//			GV.Motor_LeiSai[1].Target_Velcity=0;
//			Paint_Gun_Delay_Close_Flag_Modfiy++;
//			break;
//		case 75:
//			Error_Flag=0;
//			break;
//	    default:
//			GV.Motor_LeiSai[0].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
//			GV.Motor_LeiSai[1].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
//	    	GV.Motor_LeiSai[0].Target_Velcity=0;
//	    	GV.Motor_LeiSai[1].Target_Velcity=0;
//	    	break;
//	}
//
//}




//

void Paint_Gun_Delay_Close_Func()
{
	//后退时间计算；；；；Paint_Gun_Delay_Time_Ms
	switch(Paint_Gun_Delay_Close_Flag)
	{
		case 0:
			break;
		case 1:
			Paint_Gun_Delay_Close_Flag++;
			break;
		case 2:
			Paint_Gun_Delay_Close_Flag++;
			break;
		case 3: case 4:
			GF_BSP_GPIO_SetIO(0,GPIO_PIN_SET);
			//LeisaiCan_FDCANfunction(ID1,8,DO1_OFF);
			Paint_Gun_Open1_Close0=0;
			Paint_Gun_Delay_Close_Flag++;
			Paint_Gun_Delay_Count=0;
			break;
		case 5:
			Paint_Gun_Delay_Time_Ms_Compu();

			//喷枪延时
			if(Paint_Gun_Delay_Count>1*Paint_Gun_Delay_Time_Ms)
			{
				Paint_Gun_Delay_Close_Flag++;
			}
			else
			{
				Paint_Gun_Delay_Count++;
			}
			GV.Motor_LeiSai[0].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
			GV.Motor_LeiSai[1].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
			GV.Motor_LeiSai[0].Target_Velcity=GV.DH_WH_HandSet.motor_speed_value;
			GV.Motor_LeiSai[1].Target_Velcity=GV.DH_WH_HandSet.motor_speed_value;
			VehicleAutoForward( ( ((double)GV.RFTL720_D.RF_Angle_Roll )/100.0),0 );//自动前进
			break;
		case 6:
			Paint_Gun_Delay_Count=0;
			GV.Motor_LeiSai[0].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
			GV.Motor_LeiSai[1].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
			GV.Motor_LeiSai[0].Target_Velcity=0;
			GV.Motor_LeiSai[1].Target_Velcity=0;
			Paint_Gun_Delay_Close_Flag=61;
			break;
		case 61:  case 62: case 63: case 64:
			Paint_Gun_Delay_Count=0;
			GV.Motor_LeiSai[0].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
			GV.Motor_LeiSai[1].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
			GV.Motor_LeiSai[0].Target_Velcity=0;
			GV.Motor_LeiSai[1].Target_Velcity=0;
			Paint_Gun_Delay_Close_Flag++;
			break;
		case 65:
			break;

		case 7:
			Paint_Gun_Delay_Close_Flag++;
			break;
		case 8:
			Paint_Gun_Delay_Close_Flag++;
			break;
		case 9: case 10:
			GF_BSP_GPIO_SetIO(0,GPIO_PIN_SET);
			//LeisaiCan_FDCANfunction(ID1,8,DO1_OFF);
			Paint_Gun_Open1_Close0=0;
			Paint_Gun_Delay_Close_Flag++;
			Paint_Gun_Delay_Count=0;
			break;

		case 11:
			Paint_Gun_Delay_Time_Ms_Compu();
			//喷枪延时
			if(Paint_Gun_Delay_Count>1*Paint_Gun_Delay_Time_Ms)
			{
				Paint_Gun_Delay_Close_Flag++;
			}
			else
			{
				Paint_Gun_Delay_Count++;
			}
			GV.Motor_LeiSai[0].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
			GV.Motor_LeiSai[1].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
			GV.Motor_LeiSai[0].Target_Velcity=-GV.DH_WH_HandSet.motor_speed_value;
			GV.Motor_LeiSai[1].Target_Velcity=-GV.DH_WH_HandSet.motor_speed_value;
			VehicleAutoForward((((double)GV.RFTL720_D.RF_Angle_Roll)/100.0),0);//自动前进
			break;
		case 12:
			Paint_Gun_Delay_Count=0;
			GV.Motor_LeiSai[0].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
			GV.Motor_LeiSai[1].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
			GV.Motor_LeiSai[0].Target_Velcity=0;
			GV.Motor_LeiSai[1].Target_Velcity=0;
			Paint_Gun_Delay_Close_Flag=71;
			break;
		case 71: case 72: case 73: case 74:
			Paint_Gun_Delay_Count=0;
			GV.Motor_LeiSai[0].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
			GV.Motor_LeiSai[1].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
			GV.Motor_LeiSai[0].Target_Velcity=0;
			GV.Motor_LeiSai[1].Target_Velcity=0;
			Paint_Gun_Delay_Close_Flag++;
			break;
		case 75:
			break;
	    default:
			GV.Motor_LeiSai[0].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
			GV.Motor_LeiSai[1].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
	    	GV.Motor_LeiSai[0].Target_Velcity=0;
	    	GV.Motor_LeiSai[1].Target_Velcity=0;
	    	break;
	}
	//Paint_Gun_Delay_Time_Ms_Compu();
}

void VehicleAutoForward_1(double InclinometerAngle, double offsetAngle)//自动前进
{
	//水平
	//需要调整左右补偿
	if ((InclinometerAngle >= 45 && InclinometerAngle <= 135))
	{
		ExpectationAngle = 90;

	}
	if ((InclinometerAngle >= -135 && InclinometerAngle <= -45))
	{
		ExpectationAngle = -90;
	}

	//垂直
	if ((InclinometerAngle >= 0 && InclinometerAngle < 45) || (InclinometerAngle > -45 && InclinometerAngle <= 0))
	{
		ExpectationAngle = 0 + offsetAngle;

	}
	if ((InclinometerAngle > 135 && InclinometerAngle <= 180) || (InclinometerAngle >= -179 && InclinometerAngle < -135))
	{
		ExpectationAngle = 180 + offsetAngle;

	}

	 deltaAngle=( ((double)GV.RFTL720_D.RF_Angle_Roll )/100.0)-ExpectationAngle;

	 deltaSpeed=PositionalPID(ExpectationAngle, ( ((double)GV.RFTL720_D.RF_Angle_Roll )/100.0));


	if(deltaAngle<0)
	{
		deltaAngle=-deltaAngle;
	}

   if (deltaAngle >= 0 && deltaAngle <= deltaAngle1)
   {
	 GV.Motor_LeiSai[0].Target_Velcity = GV.Motor_LeiSai[0].Target_Velcity + KP1 * deltaSpeed;
	 GV.Motor_LeiSai[1].Target_Velcity = GV.Motor_LeiSai[1].Target_Velcity - KP1 * deltaSpeed;
   }
   if (deltaAngle > deltaAngle1 && deltaAngle <= deltaAngle2)
   {
	 GV.Motor_LeiSai[0].Target_Velcity = GV.Motor_LeiSai[0].Target_Velcity + KP2 * deltaSpeed;
	 GV.Motor_LeiSai[1].Target_Velcity = GV.Motor_LeiSai[1].Target_Velcity - KP2 * deltaSpeed;
   }
   if (deltaAngle > deltaAngle2 && deltaAngle <= deltaAngle3)
   {
	 GV.Motor_LeiSai[0].Target_Velcity = GV.Motor_LeiSai[0].Target_Velcity + KP3 * deltaSpeed;
	 GV.Motor_LeiSai[1].Target_Velcity = GV.Motor_LeiSai[1].Target_Velcity - KP3 * deltaSpeed;
   }
   if (deltaAngle > deltaAngle3 && deltaAngle <= 90)
   {
	 GV.Motor_LeiSai[0].Target_Velcity = GV.Motor_LeiSai[0].Target_Velcity + KP4 * deltaSpeed;
	 GV.Motor_LeiSai[1].Target_Velcity = GV.Motor_LeiSai[1].Target_Velcity - KP4 * deltaSpeed;
   }
}






void VehicleAutoBackward_12_18_1(double InclinometerAngle, double offsetAngle)//自动前进
{
	//水平
	//需要调整左右补偿
	if ((InclinometerAngle >= 45 && InclinometerAngle <= 135))
	{
		ExpectationAngle = 90;

	}
	if ((InclinometerAngle >= -135 && InclinometerAngle <= -45))
	{
		ExpectationAngle = -90;
	}

	//垂直
	if ((InclinometerAngle >= 0 && InclinometerAngle < 45) || (InclinometerAngle > -45 && InclinometerAngle <= 0))
	{
		ExpectationAngle = 0 + offsetAngle;

	}
	if ((InclinometerAngle > 135 && InclinometerAngle <= 180) || (InclinometerAngle >= -179 && InclinometerAngle < -135))
	{
		ExpectationAngle = 180 + offsetAngle;

	}

	 deltaAngle=( ((double)GV.RFTL720_D.RF_Angle_Roll )/100.0)-ExpectationAngle;


	 deltaSpeed=PositionalPID(ExpectationAngle, ( ((double)GV.RFTL720_D.RF_Angle_Roll )/100.0));


	if(deltaAngle<0)
	{
		deltaAngle=-deltaAngle;
	}

   if (deltaAngle >= 0 && deltaAngle <= deltaAngle1)
   {
	 GV.Motor_LeiSai[0].Target_Velcity = GV.Motor_LeiSai[0].Target_Velcity + KP1 * deltaSpeed;
	 GV.Motor_LeiSai[1].Target_Velcity = GV.Motor_LeiSai[1].Target_Velcity - KP1 * deltaSpeed;
   }
   if (deltaAngle > deltaAngle1 && deltaAngle <= deltaAngle2)
   {
	 GV.Motor_LeiSai[0].Target_Velcity = GV.Motor_LeiSai[0].Target_Velcity + KP2 * deltaSpeed;
	 GV.Motor_LeiSai[1].Target_Velcity = GV.Motor_LeiSai[1].Target_Velcity - KP2 * deltaSpeed;
   }
   if (deltaAngle > deltaAngle2 && deltaAngle <= deltaAngle3)
   {
	 GV.Motor_LeiSai[0].Target_Velcity = GV.Motor_LeiSai[0].Target_Velcity + KP3 * deltaSpeed;
	 GV.Motor_LeiSai[1].Target_Velcity = GV.Motor_LeiSai[1].Target_Velcity - KP3 * deltaSpeed;
   }
   if (deltaAngle > deltaAngle3 && deltaAngle <= 90)
   {
	 GV.Motor_LeiSai[0].Target_Velcity = GV.Motor_LeiSai[0].Target_Velcity + KP4 * deltaSpeed;
	 GV.Motor_LeiSai[1].Target_Velcity = GV.Motor_LeiSai[1].Target_Velcity - KP4 * deltaSpeed;
   }
}


int Error_Close_Time=250;

void Paint_Gun_Delay_Close_Func_1()
{
	//后退时间计算；；；；Paint_Gun_Delay_Time_Ms
	switch(Paint_Gun_Delay_Close_Flag_Error)
	{
		case 0:
			break;
		case 1:
			Paint_Gun_Delay_Close_Flag_Error++;
			break;
		case 2:
			Paint_Gun_Delay_Close_Flag_Error++;
			break;
		case 3: case 4:
			GF_BSP_GPIO_SetIO(0,GPIO_PIN_SET);
			//LeisaiCan_FDCANfunction(ID1,8,DO1_OFF);
			Paint_Gun_Open1_Close0=0;
			Paint_Gun_Delay_Close_Flag_Error++;
			Paint_Gun_Delay_Count=0;
			break;
		case 5:
		//	Paint_Gun_Delay_Time_Ms_Compu();

			//喷枪延时
			if(Paint_Gun_Delay_Count>1*Error_Close_Time)
			{
				Paint_Gun_Delay_Close_Flag_Error++;
			}
			else
			{
				Paint_Gun_Delay_Count++;
			}
			GV.Motor_LeiSai[0].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
			GV.Motor_LeiSai[1].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
			GV.Motor_LeiSai[0].Target_Velcity=GV.DH_WH_HandSet.motor_speed_value;
			GV.Motor_LeiSai[1].Target_Velcity=GV.DH_WH_HandSet.motor_speed_value;
			VehicleAutoForward( ( ((double)GV.RFTL720_D.RF_Angle_Roll )/100.0),0 );//自动前进
			break;
		case 6:
			Paint_Gun_Delay_Count=0;
			GV.Motor_LeiSai[0].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
			GV.Motor_LeiSai[1].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
			GV.Motor_LeiSai[0].Target_Velcity=0;
			GV.Motor_LeiSai[1].Target_Velcity=0;
			Paint_Gun_Delay_Close_Flag_Error=61;
			break;
		case 61:  case 62: case 63: case 64:
			Paint_Gun_Delay_Count=0;
			GV.Motor_LeiSai[0].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
			GV.Motor_LeiSai[1].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
			GV.Motor_LeiSai[0].Target_Velcity=0;
			GV.Motor_LeiSai[1].Target_Velcity=0;
			Paint_Gun_Delay_Close_Flag_Error++;
			break;
		case 65:
			GV.Motor_LeiSai[0].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
			GV.Motor_LeiSai[1].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
			GV.Motor_LeiSai[0].Target_Velcity=0;
			GV.Motor_LeiSai[1].Target_Velcity=0;
			break;

		case 7:
			Paint_Gun_Delay_Close_Flag_Error++;
			break;
		case 8:
			Paint_Gun_Delay_Close_Flag_Error++;
			break;
		case 9: case 10:
			GF_BSP_GPIO_SetIO(0,GPIO_PIN_SET);
			//LeisaiCan_FDCANfunction(ID1,8,DO1_OFF);
			Paint_Gun_Open1_Close0=0;
			Paint_Gun_Delay_Close_Flag_Error++;
			Paint_Gun_Delay_Count=0;
			break;

		case 11:
		//	Paint_Gun_Delay_Time_Ms_Compu();
			//喷枪延时
			if(Paint_Gun_Delay_Count>1*Error_Close_Time)
			{
				Paint_Gun_Delay_Close_Flag_Error++;
			}
			else
			{
				Paint_Gun_Delay_Count++;
			}
			GV.Motor_LeiSai[0].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
			GV.Motor_LeiSai[1].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
			GV.Motor_LeiSai[0].Target_Velcity=-GV.DH_WH_HandSet.motor_speed_value;
			GV.Motor_LeiSai[1].Target_Velcity=-GV.DH_WH_HandSet.motor_speed_value;
			VehicleAutoForward((((double)GV.RFTL720_D.RF_Angle_Roll)/100.0),0);//自动前进
			break;
		case 12:
			Paint_Gun_Delay_Count=0;
			GV.Motor_LeiSai[0].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
			GV.Motor_LeiSai[1].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
			GV.Motor_LeiSai[0].Target_Velcity=0;
			GV.Motor_LeiSai[1].Target_Velcity=0;
			Paint_Gun_Delay_Close_Flag_Error=71;
			break;
		case 71: case 72: case 73: case 74:
			Paint_Gun_Delay_Count=0;
			GV.Motor_LeiSai[0].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
			GV.Motor_LeiSai[1].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
			GV.Motor_LeiSai[0].Target_Velcity=0;
			GV.Motor_LeiSai[1].Target_Velcity=0;
			Paint_Gun_Delay_Close_Flag_Error++;
			break;
		case 75:
			GV.Motor_LeiSai[0].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
			GV.Motor_LeiSai[1].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
			GV.Motor_LeiSai[0].Target_Velcity=0;
			GV.Motor_LeiSai[1].Target_Velcity=0;
			break;
	    default:
			GV.Motor_LeiSai[0].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
			GV.Motor_LeiSai[1].Run_Mode=DF_MSP_LSMotor_SpeedValue ;
	    	GV.Motor_LeiSai[0].Target_Velcity=0;
	    	GV.Motor_LeiSai[1].Target_Velcity=0;
	    	break;
	}
}


void VehicleAutoTurn_End(double InclinometerAngle, double expectationAngle)//换道自动转弯
{
	deltaSpeed=0.75 * PositionalPID(expectationAngle,(((double)GV.RFTL720_D.RF_Angle_Roll)/100.0));     //水平纠偏
	deltaAngle=(((double)GV.RFTL720_D.RF_Angle_Roll)/100.0)-expectationAngle;

	if(deltaAngle<0)//取角度差值的绝对值
	{
		deltaAngle=-deltaAngle;
	}
	 GV.Motor_LeiSai[0].Target_Velcity =0;
	 GV.Motor_LeiSai[1].Target_Velcity = 0;
   if (deltaAngle >= 0 && deltaAngle <= deltaAngle1)
   {
	 GV.Motor_LeiSai[0].Target_Velcity = GV.Motor_LeiSai[0].Target_Velcity + 0.8*KP11 * deltaSpeed;
	 GV.Motor_LeiSai[1].Target_Velcity = GV.Motor_LeiSai[1].Target_Velcity - 0.8*KP11 * deltaSpeed;
   }
   if (deltaAngle > deltaAngle1 && deltaAngle <= deltaAngle2)
   {
	   GV.Motor_LeiSai[0].Target_Velcity = GV.Motor_LeiSai[0].Target_Velcity + 0.75*KP22 * deltaSpeed;
	   GV.Motor_LeiSai[1].Target_Velcity = GV.Motor_LeiSai[1].Target_Velcity - 0.75*KP22 * deltaSpeed;
   }
   if (deltaAngle > deltaAngle2 && deltaAngle <= deltaAngle3)
   {
	   GV.Motor_LeiSai[0].Target_Velcity = GV.Motor_LeiSai[0].Target_Velcity + 0.75*KP33 * deltaSpeed;
	   GV.Motor_LeiSai[1].Target_Velcity = GV.Motor_LeiSai[1].Target_Velcity - 0.75*KP33 * deltaSpeed;
   }
   if (deltaAngle > deltaAngle3 && deltaAngle <= deltaAngle4)
   {
	   GV.Motor_LeiSai[0].Target_Velcity = GV.Motor_LeiSai[0].Target_Velcity + 0.75*KP44 * deltaSpeed;
	   GV.Motor_LeiSai[1].Target_Velcity = GV.Motor_LeiSai[1].Target_Velcity - 0.75*KP44 * deltaSpeed;
   }
   if (deltaAngle > deltaAngle4 && deltaAngle <= 200)
   {
	   GV.Motor_LeiSai[0].Target_Velcity = GV.Motor_LeiSai[0].Target_Velcity + 0.75*KP55 * deltaSpeed;
	   GV.Motor_LeiSai[1].Target_Velcity = GV.Motor_LeiSai[1].Target_Velcity - 0.75*KP55 * deltaSpeed;
   }
}







