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

struct CV_struct_define CV;
struct GV_struct_define GV;
struct PV_struct_define PV;

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

double Horzi_Der_Angle_Deg=0;
double System_time_SR=0.1;


double Horiz_Angle_error_Value=0;

double Turn_Angle_Back=TT_One_Deg_Count*5;//5度的回旋量
int Vertical_Flag_Zreo=0;

double Auto_Job_Back_Speed=0*256;//需要距离和时间计算速度
double Auto_Job_Back_Speed_Max=0*360;
double Auto_Job_Back_Time_MS=1500;//后退时间3S

double Back_Angle_Deg=5;
double Back_Comp_Time_MS=250;

double Change_Road_Time=0;
double Change_Road_Speed=750;
double Change_Road_Speed_MAX=1000;
double Last_Swing_Angle=0;
double Last_Swing_Speed=0;
double Horiz_Change_Road_Comp_Dis_MM=60;

double Error_Cont_Thresh=0.5;
//void Change_Road_Time_calculate();
void Change_Road_Time_Calculate_Horiz();
void Change_Road_Time_Calculate_Verti();
void NO_MENS_Auto_Operat();
//Tral_Time_MS
//上前为全新的函数


double Swi_Speed_Auto=0;  double Swi_Range_Auto=0;

void Robot_hult_Fuction();

void Mobile_Robot_Manual_Con_1();
void Robot_Speed_0_Mode();


int32_t Speed_Zero_Falg=0;
int Robot_Size_mm[2]={204,180};
//double Camera_Size[2]={0 ,0};// Camera_Size[0]  The distance between the center point of the camera and the outer wheel;
int Auto_Flag=0;
double Pi=3.141592653589793;

int Start_Flag=0;
double Camera_Size[2]={200, 30};// Camera_Size[0]  The distance between the center point of the camera and the outer wheel;
double Step_0_Dis[1]={0};

int  Lingt_Auto_Flag=1;
void Vert_Auto_Operat();

void Manage_Func();
void Vert_Manual_Task();
void Vert_Auto_Task();

void Vertical_Linear_Auto_Walk();
int Manage_Flag=0;
int Vert_Auto_Flag=0;
int Swi_Stop_Flag=0;
int Stop_Posi_TT=0;
void Swi_Stop();
int CRLU_Flag=0;

void Change_Road_Down_Up();
void Change_Road_Left();
void Change_Road_Right();

void Speed_0_10_All(double Value_1);
void TT_Speed_Left_Right();

void Horiz_Auto_Operat();
void Vert_Auto_Operat_Sub_Fuc();

void Horiz_Auto_Operat_Sub_Fuc();
double Angle_Error_LLL=0;
void Vert_Auto_Task_Speed();
void Horiz_Auto_Task_Speed();


void Robot_Speed_MPU6050_Vertical();
void Horiz_Angle_Add(double Add_Angle_Deg[]);


void Der_Angle_Judge_Vertic(double Present_Angle, double *ideal_Angle_Deg);
void Der_Angle_Judge_Horiz(double Present_Angle,  double Horizontal_Add_Angle, double *ideal_Angle_Deg);

void Speed_0_10_PID_Vertical(double Value_1);
void Speed_0_10_PID_Horiz(double Value_1);int Horiz_Flag_Zreo=0;
void Linear_Auto_Walk_PID(double Deri_Speed, double Deri_MAX_Speed, double Robot_Speed[]);
void Linear_Auto_Walk_Horiz(double Deri_Speed,double Deri_MAX_Speed);
void Robot_Speed_MPU6050_Horiz();
void Linear_Vertic_Auto_Walk_PID(double Deri_Speed, double Deri_MAX_Speed);
//double Sw_Rust_Mid_Posi=-741970;//需检测修改
double Sw_Rust_Mid_Posi=-812181;
void Tis_Dis_Speed_Time(double Dis_CM,double Speed_Value,int *Tral_Time_ms);
void No_MENS_Linear_Auto_Walk(double Deri_Speed,double Deri_MAX_Speed);//开放给自动作业用
void Back_Dis_Adjust();
//double Back_Dis_CM=30;
//int Auto_Job_Back_Time[1]={2310};
//double Auto_Job_Back_Speed=1000;
//double Auto_Job_Back_Speed_Max=2000;

double Back_Dis_CM=5;
double Vertical_Der_Angle_Deg=0;
int NO_MENS_Auto_Flag=0;
void Lift_motor_control_Swing();

void CV_GV_Init()
{
	CV.Speed_1 = 1;
	GV.Motor_P_CANOPEN[1].MotorID=1;
	GV.Motor_P_CANOPEN[2].MotorID=2;
	GV.TT_Motor_P[3].MotorID=3;

//	GV.Motor_P[2].MotorID=3;
}

void GF_Robot_Init()
{
	uint8_t _state=1;
	memset(&GV, 0, sizeof(struct GV_struct_define));
	memset(&PV, 0, sizeof(struct PV_struct_define));
//	HAL_Delay(5000);
	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_TIM8_2ms_PeriodElapsedCallback,GF_Robot_MainLoop);
	_state = _state & GF_BSP_Interrupt_Add_CallBack(DF_BSP_InterCall_TIM8_2ms_PeriodElapsedCallback,GF_MSP_OBD_Run_Timer);

	_state = _state & GF_BSP_Interrupt_Add_CallBack(DF_BSP_InterCall_TIM8_2ms_PeriodElapsedCallback,Manage_Func);

	//由钛虎电机//锋华电机

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


	_state = _state & GF_MSP_Ti5Motor_CANOPEN_Init(1,(CANOPENPMotorarameters*)(&GV.Motor_P_CANOPEN),1,DF_GL_MotorNum);

	_state = _state & GF_MSP_TT_Motor_Init(2, (TT_MotorParameters*) (&GV.TT_Motor_P), 3, 1);

	_state = _state & GF_MSP_DH_CAN_Init(1, (SP_MSP_DH_CAN_Parameters*) (&GV.DH_CAN_HangSet));

	_state = _state & GF_MSP_IMU_6050_Init((SP_BSP_MPU6050*)&GV.Robot_Angle);

	_state = _state & GF_MSP_FS_Init(&huart1,(SP_MSP_Force_Value*)&GV.Force_Value_Bar);

	//_state = _state & GF_MSP_OBD_Init(2);
	_state = _state & GF_BSP_TIMER_Init(); //定时器最后启动
	if(_state == 1)//APP启动成功
	{
		GF_BSP_FDCAN_Senddata(2, 0x1FF, 5, (uint8_t*)GV.NC_ARRAY_u8_0);
	}
}


int32_t Bat_falg=1;
int32_t Bat_falg_1=1;
//100ms执行一次
void GF_WatchDog_Loop()
{
	//硬件看门狗复位
	HAL_GPIO_TogglePin(RST_WDI_GPIO_Port, RST_WDI_Pin);//Hardware Watchdog,kicking the dog

	//监控遥控器有没有持续接收到新数据，有新数据，数据将改写GV.MK32_Online的数据

	switch(Bat_falg)
	{
	case 1:
		GF_BSP_GetAngle();//如，数据有问题，请分开调用
		GF_FS_Inquiry();
		Bat_falg=1;
		break;
	}


	if(GV.DH_CAN_HangSet.EMERGENCY_STOP == 0)
	{
		GF_MainLoop_Jump(1,0); //停止
	}
	else if((GV.DH_CAN_HangSet.Tast_Init1_Close2==1)&&(GV.DH_CAN_HangSet.Vert2_Horiz1==0))
	{
		GF_MainLoop_Jump(1,5);//无倾角仪器作业
	}
	else if(GV.DH_CAN_HangSet.Vert2_Horiz1==0)
	{
		GF_MainLoop_Jump(1,2); //手动控制
	}
	else if(GV.DH_CAN_HangSet.Vert2_Horiz1==2)
	{
		GF_MainLoop_Jump(1,3); //竖直作业
	}
	else if(GV.DH_CAN_HangSet.Vert2_Horiz1==1)
	{
		GF_MainLoop_Jump(1,4);//水平作业
	}
	else
	{
		GF_MainLoop_Jump(1,0); //停止
	}

}

void GF_MainLoop_Jump(int32_t Next_Index_U,int32_t Next_Index_N)
{
	PV.State_Index_U_Last=GV.State_Index_U;
	PV.State_Index_N_Last=GV.State_Index_N;
	GV.State_Index_U=Next_Index_U;
	GV.State_Index_N=Next_Index_N;

	if(PV.State_Index_U_Last != GV.State_Index_U)
	{
		GV.State_Index_U_Elapse=0;
		GV.State_Index_N_Elapse=0;
		Speed_Zero_Falg=0;
	}
	else if(PV.State_Index_N_Last != GV.State_Index_N)
	{
		GV.State_Index_N_Elapse=0;
		Speed_Zero_Falg=0;
	}
}

//2ms 执行一次
//GV.State_Main_Loop_Primary=-1 错误状态
//-1-0 机器人初始步骤，瞬态，初始化机器人，停止
//-1-1 机器人初始完成了，常态
//
//0-0 机器人初始步骤，瞬态，初始化机器人，停止
//0-1 机器人初始完成了，常态
//
//1-0 手动控制模式初始化，瞬态
//1-1 手动控制模式，常态，根据遥控器控制电机
//
//
void GF_Robot_MainLoop()
{

	GV.State_Index_U_Elapse=GV.State_Index_U_Elapse+2;
	GV.State_Index_N_Elapse=GV.State_Index_N_Elapse+2;

	switch(GV.State_Index_U)
	{
	case -1:						//错误状态
		switch(GV.State_Index_N)
		{
			case 0:
				if((GV.DH_CAN_HangSet.EMERGENCY_STOP == 0)||(GV.DH_CAN_HangSet.Wireless_Status==1))
				{
					GF_MainLoop_Jump(1,0);
				}
				break;
			case 1:

				break;
			default:

				break;
		}
		break;
	case 0:							//初始化状态
		switch(GV.State_Index_N)
		{
			case 0:					//初始化进程，瞬态
				GF_MainLoop_Jump(GV.State_Index_U, GV.State_Index_N+1);
				break;
			case 1:

				break;
			default:
				break;
		}
		break;
	case 1:
		switch(GV.State_Index_N)
		{
			case 0:
				Robot_hult_Fuction();
				Lift_motor_control_Swing();
				break;
			case 1:
				//Mobile_Robot_Manual_Con();
				Robot_hult_Fuction();
				NO_MENS_Auto_Flag=0;
				Lift_motor_control_Swing();
				break;
			case 2:
				Mobile_Robot_Manual_Con_1();
				TT_Speed_Left_Right();
				Vert_Auto_Flag=0;
				Vert_Auto_Flag=0;
				NO_MENS_Auto_Flag=0;
				Swi_Range_Auto=((double)GV.DH_CAN_HangSet.Swing_Angle)*TT_One_Deg_Count/2;
				Swi_Speed_Auto=((double)GV.DH_CAN_HangSet.Swing_Speed)*201.7/10;
				Back_Dis_Adjust();


				Lift_motor_control_Swing();
				break;
			case 3:
				Vert_Auto_Operat();
				NO_MENS_Auto_Flag=0;
				Lift_motor_control_Swing();
				break;
			case 4:
				Horiz_Auto_Operat();

				NO_MENS_Auto_Flag=0;
				Lift_motor_control_Swing();
				break;
			case 5:
				NO_MENS_Auto_Operat();
				Lift_motor_control_Swing();
				break;
			default:
				break;
		}
		break;
	default:

		break;
	}
}

int32_t One_Cri_count=5308416;
double Speed_Scale;
double X_Value_2;
double Y_Value_2;
double R_Speed_Con;
double L_Speed_Con;

void Robot_hult_Fuction()
{
	int32_t Rear_Speed=0;
	int32_t Front_Speed=0;

	GV.Motor_P_CANOPEN[2].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
	GV.Motor_P_CANOPEN[1].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;

	GV.Motor_P_CANOPEN[2].Target_Velcity=Front_Speed;
	GV.Motor_P_CANOPEN[1].Target_Velcity=Rear_Speed;

	GV.TT_Motor_P[3].Run_Mode=DF_MSP_TT_Motor_SpeedMode;
	GV.TT_Motor_P[3].Target_Velcity=0;
}


double Deried_Pos_Fisrt[2]={0,0};
double Deried_Pos_Senc[2]={0,0};
double Turn_angle_rad[1]={0};
int time_delay_Flag=0;
int Robot_Speed_0[2]={0,0};
int Robot_Speed[2]={0,0};
int Robot_Speed_1[2]={0,0};

double Present_Angle=0;
double Desire_Angle_Robot=0;

double Angle_Error_1=0;

int Start_Count=0;
double Robot_offset_Angle_DIS[2]={0,0};
void Robot_Automatic_Fuction()
{


}
/**
  * @brief  Weld detect robot bias value function
  * @param  _Bias_Value[]; _Bias_Value[0] bias value;_Bias_Value[1] bias angle;
  * @param  Robot_Size[]; Robot_Size[0] Left wheel and right Wheel distance; Robot_Size[0] wheel Radius;
  * @param  displa_incr;  Robot forward distance
  * @retval_1  *Dsired_Pos;  Dsired_Pos[0] Left Wheel desired position;Dsired_Pos[1] Right Wheel desired position;
  * @retval_2  *Desired_Velocity;  Desired_Velocity[0] Left Wheel desird Velocity; Desired_Velocity[1] Right Wheel desird Velocity;
  */


int Sea_Robot_Flag=0;
void Mobile_Robot_Manual_Con_1()
{
	switch(Sea_Robot_Flag)
	{
		case 0:
			Robot_Speed_0_Mode();
			break;
		default:
			GV.Motor_P_CANOPEN[2].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
			GV.Motor_P_CANOPEN[2].Target_Velcity=0;
			GV.Motor_P_CANOPEN[1].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
			GV.Motor_P_CANOPEN[1].Target_Velcity=0;
			break;
	}
}

void Robot_Speed_0_Mode()
{
//钛虎电机1/m每分钟转速为，输出值为255

	int Speed_Value_DH=(int)GV.DH_CAN_HangSet.Car_Speed;
	double Speed_Value_Big_Small=0;
	switch (Speed_Value_DH)
	{
	case 0:case 1:case 2:case 3:case 4:case 5:case 6:case 7:case 8:case 10:case 14:
		Speed_Value_Big_Small=(double)Speed_Value_DH;
		Speed_0_10_All(Speed_Value_Big_Small);
		break;
	}
}

void Speed_0_10_All(double Value_1)
{
	double Speed_Base=255;

	double V1,V2;
	if(GV.DH_CAN_HangSet.For_Back_Value_X>=50)
	{
		V1=-((double)GV.DH_CAN_HangSet.For_Back_Value_X)/128*Speed_Base*Value_1;
		V2=V1;
	}
	else if(GV.DH_CAN_HangSet.For_Back_Value_X<=-50)
	{
		V1=-((double)GV.DH_CAN_HangSet.For_Back_Value_X)/128*Speed_Base*Value_1;
		V2=V1;

	}
	else if(GV.DH_CAN_HangSet.Left_Right_Value_Y>=50)
	{
		V1=2*((double)GV.DH_CAN_HangSet.Left_Right_Value_Y)/128*Speed_Base*Value_1/3;
		V2=-V1;
	}
	else if(GV.DH_CAN_HangSet.Left_Right_Value_Y<=-50)
	{
		V2=-2*((double)GV.DH_CAN_HangSet.Left_Right_Value_Y)/128*Speed_Base*Value_1/3;
		V1=-V2;
	}
	else
	{
		V1=0;
		V2=0;
	}

	GV.Motor_P_CANOPEN[2].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
	GV.Motor_P_CANOPEN[1].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;

	GV.Motor_P_CANOPEN[2].Target_Velcity=V1;
	GV.Motor_P_CANOPEN[1].Target_Velcity=-V2;
}



void Manage_Func()
{
	double Swing_Arm_Radius=1;
	switch(Manage_Flag)
	{
//	case 0:
//		GF_MSP_Ti5Motor_Control_Timer_CANOPEN();
//		GF_MSP_DH_40022_CAN_Inquiry(Swing_Arm_Radius,Back_Dis_CM,GV.Force_Value_Bar.Force_Value);
//		Manage_Flag++;
//		break;
//	case 1:
//		GF_MSP_Ti5Motor_Control_Timer_CANOPEN();
//		GF_MSP_DH_40022_CAN_Inquiry(Swing_Arm_Radius,Back_Dis_CM,GV.Force_Value_Bar.Force_Value);
//		Manage_Flag++;
//		break;
//	case 2:
//		GF_MSP_TT_Motor_Control_Timer();
//		GF_MSP_DH_40022_CAN_Inquiry(Swing_Arm_Radius,Back_Dis_CM,GV.Force_Value_Bar.Force_Value);
//		Manage_Flag=0;
//		break;
//
//	case 3:
//		GF_MSP_DH_40022_CAN_Inquiry(Swing_Arm_Radius,Back_Dis_CM,GV.Force_Value_Bar.Force_Value);
//		Manage_Flag=0;
//		break;


	case 0:
		GF_MSP_Ti5Motor_Control_Timer_CANOPEN();
		GF_MSP_DH_40022_CAN_Inquiry(Swing_Arm_Radius,Back_Dis_CM,GV.Force_Value_Bar.Force_Value);
		Manage_Flag++;
		break;

	case 25:
		GF_MSP_TT_Motor_Control_Timer();
		GF_MSP_DH_40022_CAN_Inquiry(Swing_Arm_Radius,Back_Dis_CM,GV.Force_Value_Bar.Force_Value);
		Manage_Flag=0;
		break;

	default:
		GF_MSP_Ti5Motor_Control_Timer_CANOPEN();
		GF_MSP_DH_40022_CAN_Inquiry(Swing_Arm_Radius,Back_Dis_CM,GV.Force_Value_Bar.Force_Value);
		Manage_Flag++;
		break;

	}

}

double TT_Speed_Flag_1=1;
void Vert_Auto_Operat()
{
	int Task_Inti=GV.DH_CAN_HangSet.Tast_Init1_Close2;

	switch(Task_Inti)
	{
	case 1:
	//	Vert_Auto_Task_Speed();
	//	Vert_Auto_Task_Speed_1();
		Vert_Auto_Task_Speed_2();
		Swi_Stop_Flag=0;
		break;
	case 2:
		Swi_Range_Auto=((double)GV.DH_CAN_HangSet.Swing_Angle)*TT_One_Deg_Count/2;
		//Swi_Speed=((double)GV.DH_CAN_HangSet.Swing_Speed)*500;
		Swi_Speed_Auto=((double)GV.DH_CAN_HangSet.Swing_Speed)*201.7*1.5;
		Vert_Auto_Flag=0;

		Vert_Auto_Operat_Sub_Fuc();
		TT_Speed_Flag_1=1;
		break;
	default:
		break;
	}
}

void Vert_Auto_Operat_Sub_Fuc()
{
	int Task_Inti_Sub=GV.DH_CAN_HangSet.Left_Up1_Right_Down2;
	GV.TT_Motor_P[3].Target_Velcity=0;
	TT_Speed_Left_Right();
	switch(Task_Inti_Sub)
	{
	case 0:
		//Vert_Manual_Task();
		CRLU_Flag=0;
		Robot_Speed_MPU6050_Vertical();
		Back_Dis_Adjust();
		Change_Road_Time_Calculate_Verti();
		break;
	case 1: case 2:
		Change_Road_Down_Up();
		break;
//
//		Change_Road_Down_Up();
//		break;
	}

}



int Mid_switch=0;
int Vert_Count=0;

void Vert_Auto_Task()
{

	double Swi_Position=GV.TT_Motor_P[3].Real_Position;

	double Turn_Angle_Back=50000;
	double Left_Threshold_Pos=Sw_Rust_Mid_Posi+Swi_Range_Auto-Turn_Angle_Back;
	double Right_Threshold_Pos=Sw_Rust_Mid_Posi-Swi_Range_Auto+Turn_Angle_Back;



	switch(Vert_Auto_Flag)
	{
	case 0://先找到中位开关量

		if(Mid_switch==0)
		{
			Vert_Auto_Flag=1;
		}
		break;

	case 1://给出向左摆动目标位置
		GV.TT_Motor_P[3].Run_Mode=DF_MSP_TT_Motor_PositionMode;
		GV.TT_Motor_P[3].Target_Position=Sw_Rust_Mid_Posi+Swi_Range_Auto;
		GV.TT_Motor_P[3].Target_Velcity=(int)Swi_Speed_Auto;
		Vert_Auto_Flag++;
		break;
	case 2://给出向右摆动目标位置
		Vert_Auto_Flag++;
		break;
	case 3://到达目标位置1开始后退

		if(Swi_Position>Left_Threshold_Pos)
		{
			Vert_Auto_Flag++;

			GV.TT_Motor_P[3].Run_Mode=DF_MSP_TT_Motor_PositionMode;
			GV.TT_Motor_P[3].Target_Position=Sw_Rust_Mid_Posi-Swi_Range_Auto;
			GV.TT_Motor_P[3].Target_Velcity=(int)Swi_Speed_Auto;
		}

		break;
	case 4:// 机器人后退指定距离

		GV.TT_Motor_P[3].Run_Mode=DF_MSP_TT_Motor_PositionMode;
		GV.TT_Motor_P[3].Target_Position=Sw_Rust_Mid_Posi-Swi_Range_Auto;
		GV.TT_Motor_P[3].Target_Velcity=(int)Swi_Speed_Auto;

		Vert_Count++;
//		GV.Motor_P_CANOPEN[2].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
//		GV.Motor_P_CANOPEN[1].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
//
//		GV.Motor_P_CANOPEN[2].Target_Velcity=100;
//		GV.Motor_P_CANOPEN[1].Target_Velcity=-100;

		Linear_Auto_Walk( Auto_Job_Back_Speed, Auto_Job_Back_Speed_Max);

		if(Vert_Count>500)
		{
			GV.Motor_P_CANOPEN[2].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
			GV.Motor_P_CANOPEN[1].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;

			GV.Motor_P_CANOPEN[2].Target_Velcity=0;
			GV.Motor_P_CANOPEN[1].Target_Velcity=0;
			Vert_Count=0;
			Vert_Auto_Flag=5;
		}
		break;
	case 5://给出向左摆动目标位置
		GV.Motor_P_CANOPEN[2].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
		GV.Motor_P_CANOPEN[1].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;

		GV.Motor_P_CANOPEN[2].Target_Velcity=0;
		GV.Motor_P_CANOPEN[1].Target_Velcity=0;


		Vert_Auto_Flag++;
		break;
	case 6:
		GV.Motor_P_CANOPEN[2].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
		GV.Motor_P_CANOPEN[1].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;

		GV.Motor_P_CANOPEN[2].Target_Velcity=0;
		GV.Motor_P_CANOPEN[1].Target_Velcity=0;

		if(Swi_Position<Right_Threshold_Pos)
		{
			Vert_Auto_Flag++;
			GV.TT_Motor_P[3].Run_Mode=DF_MSP_TT_Motor_PositionMode;
			GV.TT_Motor_P[3].Target_Position=Sw_Rust_Mid_Posi+Swi_Range_Auto;
			GV.TT_Motor_P[3].Target_Velcity=(int)Swi_Speed_Auto;
		}
		break;
	case 7:// 机器人后退指定距离

		GV.TT_Motor_P[3].Run_Mode=DF_MSP_TT_Motor_PositionMode;
		GV.TT_Motor_P[3].Target_Position=Sw_Rust_Mid_Posi+Swi_Range_Auto;
		GV.TT_Motor_P[3].Target_Velcity=(int)Swi_Speed_Auto;

		Vert_Count++;

		Linear_Auto_Walk( Auto_Job_Back_Speed, Auto_Job_Back_Speed_Max);

		if(Vert_Count>500)
		{
			GV.Motor_P_CANOPEN[2].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
			GV.Motor_P_CANOPEN[1].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;

			GV.Motor_P_CANOPEN[2].Target_Velcity=0;
			GV.Motor_P_CANOPEN[1].Target_Velcity=0;
			Vert_Count=0;
			Vert_Auto_Flag=0;
		}
		break;

	default:

		GV.Motor_P_CANOPEN[2].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
		GV.Motor_P_CANOPEN[1].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;

		GV.Motor_P_CANOPEN[2].Target_Velcity=0;
		GV.Motor_P_CANOPEN[1].Target_Velcity=0;
		break;
	}
}




void Vert_Manual_Task()
{
	switch(Sea_Robot_Flag)
	{
		case 0:
			Robot_Speed_0_Mode();
			break;
		default:
			GV.Motor_P_CANOPEN[2].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
			GV.Motor_P_CANOPEN[2].Target_Velcity=0;
			GV.Motor_P_CANOPEN[1].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
			GV.Motor_P_CANOPEN[1].Target_Velcity=0;
			break;
	}
}


void Vertical_Linear_Auto_Walk(double Deri_Speed,double Deri_MAX_Speed)//开放给自动作业用
{
	double Present_Angle=0;
	Present_Angle=((double)GV.Robot_Angle.MPU_Pitch)/100;
	double Robot_Speed[4];
//	GF_MSP_Auto_Motion_adj_Com(Present_Angle,0, Deri_Speed, Deri_MAX_Speed, System_time_SR , Robot_Speed);


	GF_MSP_PID_Now_Der_adj_Com(Present_Angle,Vertical_Der_Angle_Deg, Deri_Speed, Deri_MAX_Speed, System_time_SR , Robot_Speed);

	GV.Motor_P_CANOPEN[2].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
	GV.Motor_P_CANOPEN[2].Target_Velcity=Robot_Speed[1];
	GV.Motor_P_CANOPEN[1].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
	GV.Motor_P_CANOPEN[1].Target_Velcity=- Robot_Speed[0];

}

void Swi_Stop()
{
//	int Swi_Real_Position;
//	double  Swi_Real_Velocity;
	switch(Swi_Stop_Flag)
	{
	case 0:
		GV.TT_Motor_P[3].Run_Mode=DF_MSP_TT_Motor_Posi_Now;
		GV.TT_Motor_P[3].Target_Velcity=0;

	//	GV.TT_Motor_P[3].Target_Position=Stop_Posi_TT;




//		Swi_Real_Position=GV.TT_Motor_P[3].Real_Position;
//		Swi_Real_Velocity=GV.TT_Motor_P[3].Real_Velcity;
//		Swi_Real_Velocity=Swi_Real_Velocity*0.3;
//		Stop_Posi_TT=Swi_Real_Position+(int)Swi_Real_Velocity;
		Swi_Stop_Flag=1;
		break;
	case 1:

		break;
	}
}

//换道函数
   int Change_R_L_Count=0;
//void Change_Road_Left(double Der_Angle[2],int Ver_Speed, int Distance)



//double Der_Angle_Down_Up[2]={0,-90};
double Der_Angle_Down_Up[2]={180,-90};
void Change_Road_Down_Up()
{

	double Present_Angle_1=0;
	Present_Angle_1=((double)GV.Robot_Angle.MPU_Pitch)/100;
	double Robot_Speed_1[4];

	switch(CRLU_Flag)
	{
	case 0://两轮速度相同转向//先转向期望角度（水平角度）

		Horzi_Der_Angle_Deg=Der_Angle_Down_Up[0];
		Horiz_First_Adjus();
		if(Angle_Error_LLL<5)
		{
			CRLU_Flag++;
		}
		break;
	case 1://两轮速度相同转向//先转向期望角度（水平角度）//PID转向期望角度1

		Horzi_Der_Angle_Confirm();
		Horiz_First_Adjus();

		if(Angle_Error_LLL<Error_Cont_Thresh)
		{
			CRLU_Flag++;
		}
		Change_R_L_Count=0;
		break;
	case 2://后退指定距离
		Change_R_L_Count++;
		GF_MSP_PID_Now_Der_adj_Com(Present_Angle_1,Horzi_Der_Angle_Deg, Change_Road_Speed, Change_Road_Speed_MAX, System_time_SR , Robot_Speed_1);

		GV.Motor_P_CANOPEN[2].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
		GV.Motor_P_CANOPEN[2].Target_Velcity=Robot_Speed_1[1];
		GV.Motor_P_CANOPEN[1].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
		GV.Motor_P_CANOPEN[1].Target_Velcity=-Robot_Speed_1[0];

		if(Change_R_L_Count>Change_Road_Time)
		{
			CRLU_Flag++;
		}
		break;

	case 3://到达位置
		Change_R_L_Count=0;

		Vertical_Der_Angle_Deg=Der_Angle_Down_Up[1];
		Vertical_First_Adjus();


		if(Angle_Error_LLL<Error_Cont_Thresh)
		{
			CRLU_Flag++;
		}
		break;
	case 4://PID转向期望角度2

		GV.Motor_P_CANOPEN[2].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
		GV.Motor_P_CANOPEN[2].Target_Velcity=0;
		GV.Motor_P_CANOPEN[1].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
		GV.Motor_P_CANOPEN[1].Target_Velcity=0;
		break;

	default:
		break;
	}
}




double Der_Angle_Left[2]={-90,0};

void Change_Road_Left()
{
	double Present_Angle_1=0;
	double Robot_Speed_1[4];

	switch(CRLU_Flag)
	{
	case 0://两轮速度相同转向
		Horzi_Der_Angle_Deg=Der_Angle_Left[0];

		Horiz_First_Adjus();
		CRLU_Flag++;
		break;

	case 1://PID转向期望角度1
		Horiz_First_Adjus();


		if(Angle_Error_LLL<Error_Cont_Thresh)
		{
			CRLU_Flag++;
		}
		Change_R_L_Count=0;
		break;

	case 2://后退指定距离
		Change_R_L_Count++;

		//GF_MSP_PID_Now_Der_adj_Com(Present_Angle_1,Horzi_Der_Angle_Deg, Change_Road_Speed, Change_Road_Speed_MAX, System_time_SR , Robot_Speed_1);

		GF_MSP_Auto_Motion_adj_Com(Present_Angle_1, 0,Change_Road_Speed, Change_Road_Speed_MAX, System_time_SR , Robot_Speed_1);

		GV.Motor_P_CANOPEN[2].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
		GV.Motor_P_CANOPEN[2].Target_Velcity=Robot_Speed_1[1];
		GV.Motor_P_CANOPEN[1].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
		GV.Motor_P_CANOPEN[1].Target_Velcity=-Robot_Speed_1[0];

		if(Change_R_L_Count>Change_Road_Time)
		{
			CRLU_Flag++;
		}
		break;
	case 3://到达位置
		Change_R_L_Count=0;

		Horzi_Der_Angle_Deg=Der_Angle_Left[1];

		Horiz_First_Adjus();

//		Angle_Error_LLL= fabs(Present_Angle_1-Der_Angle_Left[1]);
//
		if(Angle_Error_LLL<5)
		{
			CRLU_Flag++;
		}
		break;
	case 4://两轮相同速度转向

//		GF_MSP_Auto_Motion_adj_Com(Present_Angle_1,0, Ver_Speed, MAX_Speed_Value, 0.1 , Robot_Speed_1);
//		Horzi_Der_Angle_Confirm();

		Horzi_Der_Angle_Confirm();
		Horiz_First_Adjus();
//		GF_MSP_PID_Now_Der_adj_Com(Present_Angle_1,Horzi_Der_Angle_Deg, Change_Road_Speed, Change_Road_Speed_MAX, System_time_SR , Robot_Speed_1);
//		GV.Motor_P_CANOPEN[2].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
//		GV.Motor_P_CANOPEN[2].Target_Velcity=-Robot_Speed_1[0];
//		GV.Motor_P_CANOPEN[1].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
//		GV.Motor_P_CANOPEN[1].Target_Velcity=-Robot_Speed_1[1];

//		Present_Angle_1=GV.Robot_Angle.MPU_Pitch/100;
//		Angle_Error_LLL= fabs(Present_Angle_1-Der_Angle_Left[1]);

		if(Angle_Error_LLL<Error_Cont_Thresh)
		{
			CRLU_Flag++;
		}
		break;
	case 5://PID转向期望角度2

		GV.Motor_P_CANOPEN[2].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
		GV.Motor_P_CANOPEN[2].Target_Velcity=0;
		GV.Motor_P_CANOPEN[1].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
		GV.Motor_P_CANOPEN[1].Target_Velcity=0;
		break;

	default:
		break;
	}
}


double Der_Angle_Right[2]={-90,180};
void Change_Road_Right()
{
	double Present_Angle_1=0;

	double Robot_Speed_1[4];

	switch(CRLU_Flag)
	{
	case 0://两轮速度相同转向
		Horzi_Der_Angle_Deg=Der_Angle_Right[0];
		Horiz_First_Adjus();


		CRLU_Flag++;
		break;

	case 1://PID转向期望角度1

		Horiz_First_Adjus();


		if(Angle_Error_LLL<Error_Cont_Thresh)
		{
			CRLU_Flag++;
		}
		Change_R_L_Count=0;
		break;

	case 2://后退指定距离
		Change_R_L_Count++;
			//GF_MSP_PID_Now_Der_adj_Com(Present_Angle_1,Horzi_Der_Angle_Deg, Change_Road_Speed, Change_Road_Speed_MAX, System_time_SR , Robot_Speed_1);
		//GF_MSP_Auto_Motion_adj_Com(Present_Angle_1,0, Ver_Speed, MAX_Speed_Value, 0.1 , Robot_Speed_1);


		GF_MSP_Auto_Motion_adj_Com(Present_Angle_1,0, Change_Road_Speed, Change_Road_Speed_MAX, System_time_SR , Robot_Speed_1);

		GV.Motor_P_CANOPEN[2].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
		GV.Motor_P_CANOPEN[2].Target_Velcity=Robot_Speed_1[1];
		GV.Motor_P_CANOPEN[1].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
		GV.Motor_P_CANOPEN[1].Target_Velcity=-Robot_Speed_1[0];

		if(Change_R_L_Count>Change_Road_Time)
		{
			CRLU_Flag++;
		}
		break;
	case 3://到达位置
		Change_R_L_Count=0;
		Horzi_Der_Angle_Deg=Der_Angle_Right[1];

		Horiz_First_Adjus();

//		Present_Angle_1=GV.Robot_Angle.MPU_Pitch/100;
//		Angle_Error_LLL= fabs(Present_Angle_1-Der_Angle_Right[1]);

		if(Angle_Error_LLL<5)
		{
			CRLU_Flag++;
		}

		break;
	case 4://两轮相同速度转向

//		GF_MSP_Auto_Motion_adj_Com(Present_Angle_1,0, Ver_Speed, MAX_Speed_Value, 0.1 , Robot_Speed_1);

		Horzi_Der_Angle_Confirm();

		Horiz_First_Adjus();


//		Present_Angle_1=GV.Robot_Angle.MPU_Pitch/100;
//		Angle_Error_LLL= fabs(Present_Angle_1-Der_Angle_Right[1]);

		if(Angle_Error_LLL<Error_Cont_Thresh)
		{
			CRLU_Flag++;
		}
		break;
	case 5://PID转向期望角度2

		GV.Motor_P_CANOPEN[2].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
		GV.Motor_P_CANOPEN[2].Target_Velcity=0;
		GV.Motor_P_CANOPEN[1].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
		GV.Motor_P_CANOPEN[1].Target_Velcity=0;
		break;

	default:
		break;
	}
}


//车轮运行距离//速度//计算时间
//给定距离和速度计算时间
void Tis_Dis_Speed_Time(double Dis_CM,double Speed_Value,int *Tral_Time_ms)
{
	double Wheel_Diameter=250; double PI_Value=3.1415926;
	double Motor_Ratio=121;
	double MM_Senct=Dis_CM*10;//CM to MM
	double Deg_Senct=Speed_Value*360/100/Motor_Ratio;
	double Wheel_Speed_MM_Senc=Deg_Senct/360*Wheel_Diameter*PI_Value;
	double Tral_Time_Double=MM_Senct/Wheel_Speed_MM_Senc*1000/2;


	Tral_Time_ms[0]=(int)(Tral_Time_Double);
}

//给定距离时间计算速度
void TI5_Dis_Speed_Compute_3S(double Dis_CM, double Tral_Time_MS,double *Speed_Value )
{
	double Wheel_Diameter=250; double PI_Value=3.1415926;	double Motor_Ratio=121;
	double MM_Senct=Dis_CM*10;//CM to MM
	double Deg_Sencend_Compute=MM_Senct/Wheel_Diameter/PI_Value*360/Tral_Time_MS/2*1000;
	Speed_Value[0]=Deg_Sencend_Compute*Motor_Ratio*100/360;
}







void Vert_Auto_Task_Speed()
{

	double Now_Swi_Position=GV.TT_Motor_P[3].Real_Position;
//	double Turn_Angle_Back=TT_One_Deg_Count*5;//5度的回旋量

//	double Turn_Angle_Back=TT_One_Deg_Count*((double)GV.DH_CAN_HangSet.Swing_Speed)*3*1.5;	//总后退3S，1.5S在左边，1.5S在右边
	double Turn_Angle_Back=TT_One_Deg_Count*Back_Angle_Deg;	//总后退3S，1.5S在左边，1.5S在右边
	double Left_Threshold_Pos=Sw_Rust_Mid_Posi+Swi_Range_Auto-Turn_Angle_Back;
	double Right_Threshold_Pos=Sw_Rust_Mid_Posi-Swi_Range_Auto+Turn_Angle_Back;
	double Left_Target_Pos=Sw_Rust_Mid_Posi+Swi_Range_Auto;
	double Right_Target_Pos=Sw_Rust_Mid_Posi-Swi_Range_Auto;


	switch(Vert_Auto_Flag)
	{
	case 0://先找到中位开关量   //先调整车体到-90，然后再运动

		Vertical_Der_Angle_Deg=-90;

		Vertical_First_Adjus();

		if(Angle_Error_LLL<Error_Cont_Thresh)
		{
			Vert_Auto_Flag++;
			GV.Motor_P_CANOPEN[2].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
			GV.Motor_P_CANOPEN[2].Target_Velcity=0;
			GV.Motor_P_CANOPEN[1].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
			GV.Motor_P_CANOPEN[1].Target_Velcity=0;
		}
		break;

	case 1:

		GV.Motor_P_CANOPEN[2].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
		GV.Motor_P_CANOPEN[2].Target_Velcity=0;
		GV.Motor_P_CANOPEN[1].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
		GV.Motor_P_CANOPEN[1].Target_Velcity=0;
		GV.TT_Motor_P[3].Run_Mode=DF_MSP_TT_Motor_SpeedMode;

		GV.TT_Motor_P[3].Target_Velcity=(int)Swi_Speed_Auto*1;
		Vert_Auto_Flag++;
		break;
	case 2:
//		GV.Motor_P_CANOPEN[2].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
//		GV.Motor_P_CANOPEN[2].Target_Velcity=0;
//		GV.Motor_P_CANOPEN[1].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
//		GV.Motor_P_CANOPEN[1].Target_Velcity=0;

		if(Now_Swi_Position>Left_Target_Pos)
		{
			Vert_Auto_Flag=8;
			GV.TT_Motor_P[3].Run_Mode=DF_MSP_TT_Motor_SpeedMode;
			GV.TT_Motor_P[3].Target_Velcity=(int)-Swi_Speed_Auto;
		}
		Auto_Mode_LR_Manu_turn();
		break;

	case 3://给出向左摆动目标位置
		GV.TT_Motor_P[3].Run_Mode=DF_MSP_TT_Motor_SpeedMode;

		GV.TT_Motor_P[3].Target_Velcity=(int)Swi_Speed_Auto*1;
		Vert_Auto_Flag++;
		break;
	case 4://给出向右摆动目标位置
		Vert_Auto_Flag++;
		break;
	case 5://到达目标位置1开始后退
		if(Now_Swi_Position>Left_Threshold_Pos)
		{
			Vert_Auto_Flag++;
			GV.TT_Motor_P[3].Run_Mode=DF_MSP_TT_Motor_SpeedMode;
			GV.TT_Motor_P[3].Target_Velcity=(int)Swi_Speed_Auto;
		}
		Auto_Mode_LR_Manu_turn();
		Vert_Count=0;
		break;
	case 6:// 机器人后退指定距离
		Vert_Count++;
		Vertical_Der_Angle_Deg=-90;
		Vertical_Linear_Auto_Walk( Auto_Job_Back_Speed, Auto_Job_Back_Speed_Max);

		if(Vert_Count>Auto_Job_Back_Time_MS)
		{
			GV.Motor_P_CANOPEN[2].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
			GV.Motor_P_CANOPEN[1].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;

			GV.Motor_P_CANOPEN[2].Target_Velcity=0;
			GV.Motor_P_CANOPEN[1].Target_Velcity=0;
			Vert_Count=0;
			Vert_Auto_Flag++;
		}

		if(Now_Swi_Position>Left_Target_Pos)
		{
			GV.TT_Motor_P[3].Run_Mode=DF_MSP_TT_Motor_SpeedMode;
			GV.TT_Motor_P[3].Target_Velcity=(int)-1*Swi_Speed_Auto;
		}
		break;
	case 7://给出向左摆动目标位置
//		GV.Motor_P_CANOPEN[2].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
//		GV.Motor_P_CANOPEN[1].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
//
//		GV.Motor_P_CANOPEN[2].Target_Velcity=0;
//		GV.Motor_P_CANOPEN[1].Target_Velcity=0;

		if(Now_Swi_Position>Left_Target_Pos)
		{
			GV.TT_Motor_P[3].Run_Mode=DF_MSP_TT_Motor_SpeedMode;
			GV.TT_Motor_P[3].Target_Velcity=(int)-1*Swi_Speed_Auto;
			Vert_Auto_Flag++;
		}
		else if(GV.TT_Motor_P[3].Target_Velcity<0)
		{
			Vert_Auto_Flag++;
		}
		Auto_Mode_LR_Manu_turn();
		break;
	case 8:
		if(Now_Swi_Position>Left_Target_Pos)
		{
			GV.TT_Motor_P[3].Run_Mode=DF_MSP_TT_Motor_SpeedMode;
			GV.TT_Motor_P[3].Target_Velcity=(int)-1*Swi_Speed_Auto;
		}
//		GV.Motor_P_CANOPEN[2].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
//		GV.Motor_P_CANOPEN[1].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
//
//		GV.Motor_P_CANOPEN[2].Target_Velcity=0;
//		GV.Motor_P_CANOPEN[1].Target_Velcity=0;

		if(Now_Swi_Position<Right_Threshold_Pos)
		{
			Vert_Auto_Flag++;
			GV.TT_Motor_P[3].Run_Mode=DF_MSP_TT_Motor_SpeedMode;
			GV.TT_Motor_P[3].Target_Velcity=(int)-1*Swi_Speed_Auto;
		}
		Vert_Count=0;
		Auto_Mode_LR_Manu_turn();
		break;
	case 9:// 机器人后退指定距离

		Vert_Count++;
		Vertical_Linear_Auto_Walk( Auto_Job_Back_Speed, Auto_Job_Back_Speed_Max);

		if(Vert_Count>Auto_Job_Back_Time_MS)
		{
			GV.Motor_P_CANOPEN[2].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
			GV.Motor_P_CANOPEN[1].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;

			GV.Motor_P_CANOPEN[2].Target_Velcity=0;
			GV.Motor_P_CANOPEN[1].Target_Velcity=0;
			Vert_Count=0;
			Vert_Auto_Flag++;
		}
		if(Now_Swi_Position<Right_Target_Pos)
		{
			GV.TT_Motor_P[3].Run_Mode=DF_MSP_TT_Motor_SpeedMode;
			GV.TT_Motor_P[3].Target_Velcity=(int)Swi_Speed_Auto;
		}

		break;
	case 10:
		if(Now_Swi_Position<Right_Target_Pos)
		{
			GV.TT_Motor_P[3].Target_Velcity=(int)Swi_Speed_Auto;
			Vert_Auto_Flag=3;
		}
		else if(GV.TT_Motor_P[3].Target_Velcity>0)
		{
			Vert_Auto_Flag=3;
		}

//		GV.Motor_P_CANOPEN[2].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
//		GV.Motor_P_CANOPEN[1].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
//
//		GV.Motor_P_CANOPEN[2].Target_Velcity=0;
//		GV.Motor_P_CANOPEN[1].Target_Velcity=0;
		Auto_Mode_LR_Manu_turn();
		break;



	default:

		GV.Motor_P_CANOPEN[2].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
		GV.Motor_P_CANOPEN[1].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;

		GV.Motor_P_CANOPEN[2].Target_Velcity=0;
		GV.Motor_P_CANOPEN[1].Target_Velcity=0;
		break;
	}
}

double TT_Speed_Flag=0;

void Vert_Auto_Task_Speed_1()
{

	double Now_Swi_Position=GV.TT_Motor_P[3].Real_Position;
//	double Turn_Angle_Back=TT_One_Deg_Count*5;//5度的回旋量

//	double Turn_Angle_Back=TT_One_Deg_Count*((double)GV.DH_CAN_HangSet.Swing_Speed)*3*1.5;	//总后退3S，1.5S在左边，1.5S在右边
	double Turn_Angle_Back=TT_One_Deg_Count*Back_Angle_Deg;	//总后退3S，1.5S在左边，1.5S在右边
	double Left_Threshold_Pos=Sw_Rust_Mid_Posi+Swi_Range_Auto-Turn_Angle_Back;
	double Right_Threshold_Pos=Sw_Rust_Mid_Posi-Swi_Range_Auto+Turn_Angle_Back;
	double Left_Target_Pos=Sw_Rust_Mid_Posi+Swi_Range_Auto;
	double Right_Target_Pos=Sw_Rust_Mid_Posi-Swi_Range_Auto;


	switch(Vert_Auto_Flag)
	{
	case 0://先找到中位开关量   //先调整车体到-90，然后再运动

		Vertical_Der_Angle_Deg=-90;

		Vertical_First_Adjus();

		if(Angle_Error_LLL<Error_Cont_Thresh)
		{
			Vert_Auto_Flag++;
			GV.Motor_P_CANOPEN[2].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
			GV.Motor_P_CANOPEN[2].Target_Velcity=0;
			GV.Motor_P_CANOPEN[1].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
			GV.Motor_P_CANOPEN[1].Target_Velcity=0;
		}

		TT_Speed_Flag=1;
		GV.TT_Motor_P[3].Run_Mode=DF_MSP_TT_Motor_SpeedMode;
		GV.TT_Motor_P[3].Target_Velcity=(int)Swi_Speed_Auto*TT_Speed_Flag;

		break;

	case 1:

		GV.Motor_P_CANOPEN[2].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
		GV.Motor_P_CANOPEN[2].Target_Velcity=0;
		GV.Motor_P_CANOPEN[1].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
		GV.Motor_P_CANOPEN[1].Target_Velcity=0;


		TT_Speed_Flag=1;
		GV.TT_Motor_P[3].Run_Mode=DF_MSP_TT_Motor_SpeedMode;
		GV.TT_Motor_P[3].Target_Velcity=(int)Swi_Speed_Auto*TT_Speed_Flag;

		Vert_Auto_Flag++;
		break;
	case 2:

		if(Now_Swi_Position>Left_Target_Pos)
		{
			Vert_Auto_Flag=8;

		}
		else
		{
			TT_Speed_Flag=1;
		}
		GV.TT_Motor_P[3].Run_Mode=DF_MSP_TT_Motor_SpeedMode;
		GV.TT_Motor_P[3].Target_Velcity=(int)TT_Speed_Flag*Swi_Speed_Auto;

		Auto_Mode_LR_Manu_turn();
		break;

	case 3://给出向左摆动目标位置
		GV.TT_Motor_P[3].Run_Mode=DF_MSP_TT_Motor_SpeedMode;

		GV.TT_Motor_P[3].Target_Velcity=(int)Swi_Speed_Auto*TT_Speed_Flag;
		Vert_Auto_Flag++;
		break;
	case 4://给出向右摆动目标位置
		Vert_Auto_Flag++;
		break;

	case 5://到达目标位置1开始后退
		if(Now_Swi_Position>Left_Target_Pos)
		{
			Vert_Auto_Flag=51;
			TT_Speed_Flag=0;
		}
		else
		{
			TT_Speed_Flag=1;
		}
		GV.TT_Motor_P[3].Run_Mode=DF_MSP_TT_Motor_SpeedMode;
		GV.TT_Motor_P[3].Target_Velcity=(int)TT_Speed_Flag*Swi_Speed_Auto;
		Auto_Mode_LR_Manu_turn();
		Vert_Count=0;
		break;

	case 51:
		if(Now_Swi_Position>Left_Target_Pos)
		{
			Vert_Auto_Flag=6;
			TT_Speed_Flag=0;
		}
		else
		{
			TT_Speed_Flag=1;
		}
		GV.TT_Motor_P[3].Run_Mode=DF_MSP_TT_Motor_SpeedMode;
		GV.TT_Motor_P[3].Target_Velcity=(int)TT_Speed_Flag*Swi_Speed_Auto;
		Auto_Mode_LR_Manu_turn();
		break;

	case 6:// 机器人后退指定距离
		Vert_Count++;
		Vertical_Der_Angle_Deg=-90;
		Vertical_Linear_Auto_Walk( Auto_Job_Back_Speed, Auto_Job_Back_Speed_Max);

		if(Vert_Count>Auto_Job_Back_Time_MS)
		{
			GV.Motor_P_CANOPEN[2].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
			GV.Motor_P_CANOPEN[1].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;

			GV.Motor_P_CANOPEN[2].Target_Velcity=0;
			GV.Motor_P_CANOPEN[1].Target_Velcity=0;
			Vert_Count=0;
			Vert_Auto_Flag++;
		}

		TT_Speed_Flag=0;
		GV.TT_Motor_P[3].Run_Mode=DF_MSP_TT_Motor_SpeedMode;
		GV.TT_Motor_P[3].Target_Velcity=(int)TT_Speed_Flag*Swi_Speed_Auto;

		break;
	case 7://给出向左摆动目标位置
//		GV.Motor_P_CANOPEN[2].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
//		GV.Motor_P_CANOPEN[1].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
//
//		GV.Motor_P_CANOPEN[2].Target_Velcity=0;
//		GV.Motor_P_CANOPEN[1].Target_Velcity=0;


		if(Now_Swi_Position>Right_Target_Pos)
		{
			TT_Speed_Flag=-1;
		}
		else
		{
			Vert_Auto_Flag++;
			TT_Speed_Flag=0;
		}

		GV.TT_Motor_P[3].Run_Mode=DF_MSP_TT_Motor_SpeedMode;
		GV.TT_Motor_P[3].Target_Velcity=(int)TT_Speed_Flag*Swi_Speed_Auto;
		Auto_Mode_LR_Manu_turn();
		break;
	case 8:
		if(Now_Swi_Position>Right_Target_Pos)
		{
			TT_Speed_Flag=-1;
		}
		else
		{
			Vert_Auto_Flag=81;
			TT_Speed_Flag=0;
		}

		GV.TT_Motor_P[3].Run_Mode=DF_MSP_TT_Motor_SpeedMode;
		GV.TT_Motor_P[3].Target_Velcity=(int)TT_Speed_Flag*Swi_Speed_Auto;
		Vert_Count=0;
		Auto_Mode_LR_Manu_turn();
		break;
	case 81:
		if(Now_Swi_Position>Right_Target_Pos)
		{
			TT_Speed_Flag=-1;
		}
		else
		{
			Vert_Auto_Flag=9;
			TT_Speed_Flag=0;
		}
		GV.TT_Motor_P[3].Run_Mode=DF_MSP_TT_Motor_SpeedMode;
		GV.TT_Motor_P[3].Target_Velcity=(int)TT_Speed_Flag*Swi_Speed_Auto;
		break;

	case 9:// 机器人后退指定距离

		Vert_Count++;
		Vertical_Der_Angle_Deg=-90;
		Vertical_Linear_Auto_Walk( Auto_Job_Back_Speed, Auto_Job_Back_Speed_Max);

		if(Vert_Count>Auto_Job_Back_Time_MS)
		{
			GV.TT_Motor_P[2].Run_Mode=DF_MSP_TT_Motor_SpeedMode;
			GV.TT_Motor_P[1].Run_Mode=DF_MSP_TT_Motor_SpeedMode;

			GV.TT_Motor_P[2].Target_Velcity=0;
			GV.TT_Motor_P[1].Target_Velcity=0;
			Vert_Count=0;
			Vert_Auto_Flag++;
		}

		TT_Speed_Flag=0;
		GV.TT_Motor_P[3].Run_Mode=DF_MSP_TT_Motor_SpeedMode;
		GV.TT_Motor_P[3].Target_Velcity=(int)TT_Speed_Flag*Swi_Speed_Auto;
		break;
	case 10:

		if(Now_Swi_Position>Left_Target_Pos)
		{
			TT_Speed_Flag=0;
			Vert_Auto_Flag=3;
		}
		else
		{
			Vert_Auto_Flag=3;
			TT_Speed_Flag=1;
		}
		GV.TT_Motor_P[3].Run_Mode=DF_MSP_TT_Motor_SpeedMode;
		GV.TT_Motor_P[3].Target_Velcity=(int)TT_Speed_Flag*Swi_Speed_Auto;

		Auto_Mode_LR_Manu_turn();
		break;



	default:

		GV.Motor_P_CANOPEN[2].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
		GV.Motor_P_CANOPEN[1].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;

		GV.Motor_P_CANOPEN[2].Target_Velcity=0;
		GV.Motor_P_CANOPEN[1].Target_Velcity=0;
		break;
	}
}



void Swing_Arm_Limit()
{
	double Now_Swi_Position=GV.TT_Motor_P[3].Real_Position;
	double Left_Target_Pos=Sw_Rust_Mid_Posi+Swi_Range_Auto;
	double Right_Target_Pos=Sw_Rust_Mid_Posi-Swi_Range_Auto;

	if(Now_Swi_Position>Left_Target_Pos)
	{
		TT_Speed_Flag_1=-1;
	}
	else if(Now_Swi_Position<Right_Target_Pos)
	{
		TT_Speed_Flag_1=1;
	}
	GV.TT_Motor_P[3].Run_Mode=DF_MSP_TT_Motor_SpeedMode;
	GV.TT_Motor_P[3].Target_Velcity=(int)TT_Speed_Flag_1*Swi_Speed_Auto;
}

void Vert_Auto_Task_Speed_2()
{
	double Speed_Base=255;

	switch(Vert_Auto_Flag)
	{
	case 0://先找到中位开关量   //先调整车体到-90，然后再运动

		Vertical_Der_Angle_Deg=-90;

		Vertical_First_Adjus();

		if(Angle_Error_LLL<Error_Cont_Thresh)
		{
			Vert_Auto_Flag++;
			GV.Motor_P_CANOPEN[2].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
			GV.Motor_P_CANOPEN[2].Target_Velcity=0;
			GV.Motor_P_CANOPEN[1].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
			GV.Motor_P_CANOPEN[1].Target_Velcity=0;
		}
		Swing_Arm_Limit();
		Auto_Job_Back_Speed=((double)GV.DH_CAN_HangSet.Car_Speed)*Speed_Base/12;
		Auto_Job_Back_Speed_Max=1.5*Auto_Job_Back_Speed;
		break;

	case 1:
		Auto_Job_Back_Speed=((double)GV.DH_CAN_HangSet.Car_Speed)*Speed_Base/12;
		Auto_Job_Back_Speed_Max=1.5*Auto_Job_Back_Speed;
		Vertical_Der_Angle_Deg=-90;
		Vertical_Linear_Auto_Walk( Auto_Job_Back_Speed, Auto_Job_Back_Speed_Max);
		Swing_Arm_Limit();
		break;

	default:

		GV.Motor_P_CANOPEN[2].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
		GV.Motor_P_CANOPEN[1].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;

		GV.Motor_P_CANOPEN[2].Target_Velcity=0;
		GV.Motor_P_CANOPEN[1].Target_Velcity=0;
		break;
	}
}





void TT_Speed_Left_Right()
{
//	int Value_2=0;
//	double Swi_Speed_Manu=((double)GV.DH_CAN_HangSet.Swing_Speed)*201.7;
//
//	if((GV.DH_CAN_HangSet.LEFT3==1)&&(GV.TT_Motor_P[3].Real_Position<TT_Left_limt))
//	{
//		Value_2=1;
//	}
//	else if((GV.DH_CAN_HangSet.RIGHT4==1)&&(GV.TT_Motor_P[3].Real_Position>TT_Right_limt))
//	{
//		Value_2=-1;
//	}
//	else
//	{
//		Value_2=0;
//	}
//
//	GV.TT_Motor_P[3].Run_Mode=DF_MSP_TT_Motor_SpeedMode;
//	GV.TT_Motor_P[3].Target_Velcity=(int)Swi_Speed_Manu*Value_2;
//



	int Value_2=0;
	double Swi_Speed_Manu=((double)GV.DH_CAN_HangSet.Swing_Speed)*201.7/10;

	if((GV.DH_CAN_HangSet.LEFT3==1))//&&(GV.TT_Motor_P[3].Real_Position<TT_Left_limt)
	{
		Value_2=1;
	}
	else if((GV.DH_CAN_HangSet.RIGHT4==1))//&&(GV.TT_Motor_P[3].Real_Position>TT_Right_limt)
	{
		Value_2=-1;
	}
	else
	{
		Value_2=0;
	}

	GV.TT_Motor_P[3].Run_Mode=DF_MSP_TT_Motor_SpeedMode;
	GV.TT_Motor_P[3].Target_Velcity=(int)Swi_Speed_Manu*Value_2;
}



void Horiz_Auto_Operat()
{
	int Task_Inti=GV.DH_CAN_HangSet.Tast_Init1_Close2;
	switch(Task_Inti)
	{
	case 1:
		//Horiz_Auto_Task_Speed();
		Horiz_Auto_Task_Speed_1();

		Swi_Stop_Flag=0;
		break;
	case 2:
		Swi_Range_Auto=((double)GV.DH_CAN_HangSet.Swing_Angle)*TT_One_Deg_Count/2;
		Swi_Speed_Auto=((double)GV.DH_CAN_HangSet.Swing_Speed)*201.7/10;
		Vert_Auto_Flag=0;
		Horiz_Auto_Operat_Sub_Fuc();
		break;
	default:
		//Task_Inti=1;
		break;
	}
}
int Change_Road_Jude_Flag=0;

void Change_Road_Left_Jude()
{
	double Robot_Angle_DEG=((double)GV.Robot_Angle.MPU_Pitch)/100;
	int Robot_Angle_Int=fabs((int)Robot_Angle_DEG);
	switch(Change_Road_Jude_Flag)
	{
	case 0:
		if(Robot_Angle_Int>91)
		{
			Change_Road_Jude_Flag++;
		}
		break;
	case 1:
		Change_Road_Left();
		break;
	}
}

void Change_Road_Right_Jude()
{
	double Robot_Angle_DEG=((double)GV.Robot_Angle.MPU_Pitch)/100;
	int Robot_Angle_Int=fabs((int)Robot_Angle_DEG);
	switch(Change_Road_Jude_Flag)
	{
	case 0:
		if(Robot_Angle_Int<89)
		{
			Change_Road_Jude_Flag++;
		}
		break;
	case 1:
		Change_Road_Right();
		break;
	}
}


void Horiz_Auto_Operat_Sub_Fuc()
{
	int Task_Inti_Sub=GV.DH_CAN_HangSet.Left_Up1_Right_Down2;
	GV.TT_Motor_P[3].Target_Velcity=0;
	TT_Speed_Left_Right();
	switch(Task_Inti_Sub)
	{
	case 0:
		CRLU_Flag=0;
		Robot_Speed_MPU6050_Horiz();
		Change_Road_Jude_Flag=0;
		Back_Dis_Adjust();
		Change_Road_Time_Calculate_Horiz();

		break;
	case 1:
		Change_Road_Left_Jude();
		Horiz_Flag_Zreo=0;
		break;
	case 2:
		Change_Road_Right_Jude();
		Horiz_Flag_Zreo=0;
		break;
	default:

		break;
	}

}

void Horiz_Auto_Task_Speed()
{
	double Now_Swi_Position=GV.TT_Motor_P[3].Real_Position;

//	double Turn_Angle_Back=TT_One_Deg_Count*5;	//5度的回旋量

//	double Turn_Angle_Back=TT_One_Deg_Count*((double)GV.DH_CAN_HangSet.Swing_Speed)*1.5;	//总后退3S，1.5S在左边，1.5S在右边
	double Turn_Angle_Back=TT_One_Deg_Count*Back_Angle_Deg;	//5度的回旋量

	double Left_Threshold_Pos=Sw_Rust_Mid_Posi+Swi_Range_Auto-Turn_Angle_Back;
	double Right_Threshold_Pos=Sw_Rust_Mid_Posi-Swi_Range_Auto+Turn_Angle_Back;
	double Left_Target_Pos=Sw_Rust_Mid_Posi+Swi_Range_Auto;
	double Right_Target_Pos=Sw_Rust_Mid_Posi-Swi_Range_Auto;

	switch(Vert_Auto_Flag)
	{
	case 0://先调整车体到期望姿态，然后再运动

		Horzi_Der_Angle_Confirm();//确认期望角度
		Horiz_First_Adjus();     //双轮反向调整

		if(Angle_Error_LLL<Error_Cont_Thresh)
		{
			Vert_Auto_Flag++;
			GV.Motor_P_CANOPEN[2].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
			GV.Motor_P_CANOPEN[2].Target_Velcity=0;
			GV.Motor_P_CANOPEN[1].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
			GV.Motor_P_CANOPEN[1].Target_Velcity=0;
		}
		break;
	case 1:
		GV.Motor_P_CANOPEN[2].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
		GV.Motor_P_CANOPEN[2].Target_Velcity=0;
		GV.Motor_P_CANOPEN[1].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
		GV.Motor_P_CANOPEN[1].Target_Velcity=0;
		GV.TT_Motor_P[3].Run_Mode=DF_MSP_TT_Motor_SpeedMode;
		GV.TT_Motor_P[3].Target_Velcity=(int)Swi_Speed_Auto*1;

		Vert_Auto_Flag++;
		break;
	case 2:
//		GV.Motor_P_CANOPEN[2].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
//		GV.Motor_P_CANOPEN[2].Target_Velcity=0;
//		GV.Motor_P_CANOPEN[1].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
//		GV.Motor_P_CANOPEN[1].Target_Velcity=0;
		if(Now_Swi_Position>Left_Target_Pos)
		{
			Vert_Auto_Flag=8;
			GV.TT_Motor_P[3].Run_Mode=DF_MSP_TT_Motor_SpeedMode;
			GV.TT_Motor_P[3].Target_Velcity=(int)-Swi_Speed_Auto;
		}
		Auto_Mode_LR_Manu_turn();
		break;
	case 3://给出向左摆动目标位置
		GV.TT_Motor_P[3].Run_Mode=DF_MSP_TT_Motor_SpeedMode;
		GV.TT_Motor_P[3].Target_Velcity=(int)Swi_Speed_Auto*1;
		Vert_Auto_Flag++;
		break;
	case 4://给出向右摆动目标位置
		Vert_Auto_Flag++;
		break;
	case 5://到达目标位置1开始后退
		if(Now_Swi_Position>Left_Threshold_Pos)
		{
			Vert_Auto_Flag++;
			GV.TT_Motor_P[3].Run_Mode=DF_MSP_TT_Motor_SpeedMode;
			GV.TT_Motor_P[3].Target_Velcity=(int)Swi_Speed_Auto;
		}
		Auto_Mode_LR_Manu_turn();
		break;
	case 6:// 机器人后退指定距离
		Vert_Count++;
		Linear_Auto_Walk_Horiz( Auto_Job_Back_Speed, Auto_Job_Back_Speed_Max);
		if(Vert_Count>Auto_Job_Back_Time_MS)
		{
			GV.Motor_P_CANOPEN[2].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
			GV.Motor_P_CANOPEN[1].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
			GV.Motor_P_CANOPEN[2].Target_Velcity=0;
			GV.Motor_P_CANOPEN[1].Target_Velcity=0;
			Vert_Count=0;
			Vert_Auto_Flag++;
		}
		if(Now_Swi_Position>Left_Target_Pos)
		{
			GV.TT_Motor_P[3].Run_Mode=DF_MSP_TT_Motor_SpeedMode;
			GV.TT_Motor_P[3].Target_Velcity=(int)-1*Swi_Speed_Auto;
		}
		break;
	case 7://给出向左摆动目标位置
//		GV.Motor_P_CANOPEN[2].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
//		GV.Motor_P_CANOPEN[1].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
//		GV.Motor_P_CANOPEN[2].Target_Velcity=0;
//		GV.Motor_P_CANOPEN[1].Target_Velcity=0;
		if(Now_Swi_Position>Left_Target_Pos)
		{
			GV.TT_Motor_P[3].Run_Mode=DF_MSP_TT_Motor_SpeedMode;
			GV.TT_Motor_P[3].Target_Velcity=(int)-1*Swi_Speed_Auto;
			Vert_Auto_Flag++;
		}
		else if(GV.TT_Motor_P[3].Target_Velcity<0)
		{
			Vert_Auto_Flag++;
		}
		Auto_Mode_LR_Manu_turn();
		break;
	case 8:
		if(Now_Swi_Position>Left_Target_Pos)
		{
			GV.TT_Motor_P[3].Target_Velcity=(int)-1*Swi_Speed_Auto;
		}
//		GV.Motor_P_CANOPEN[2].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
//		GV.Motor_P_CANOPEN[1].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
//		GV.Motor_P_CANOPEN[2].Target_Velcity=0;
//		GV.Motor_P_CANOPEN[1].Target_Velcity=0;
		if(Now_Swi_Position<Right_Threshold_Pos)
		{
			Vert_Auto_Flag++;
			GV.TT_Motor_P[3].Run_Mode=DF_MSP_TT_Motor_SpeedMode;
			GV.TT_Motor_P[3].Target_Velcity=(int)-1*Swi_Speed_Auto;
		}
		Vert_Count=0;
		Auto_Mode_LR_Manu_turn();
		break;
	case 9:// 机器人后退指定距离
		Vert_Count++;
		Linear_Auto_Walk_Horiz( Auto_Job_Back_Speed, Auto_Job_Back_Speed_Max);
		if(Vert_Count>Auto_Job_Back_Time_MS)
		{
			GV.Motor_P_CANOPEN[2].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
			GV.Motor_P_CANOPEN[1].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;

			GV.Motor_P_CANOPEN[2].Target_Velcity=0;
			GV.Motor_P_CANOPEN[1].Target_Velcity=0;
			Vert_Count=0;
			Vert_Auto_Flag++;
		}
		if(Now_Swi_Position<Right_Target_Pos)
		{
			GV.TT_Motor_P[3].Run_Mode=DF_MSP_TT_Motor_SpeedMode;
			GV.TT_Motor_P[3].Target_Velcity=(int)Swi_Speed_Auto;
		}
		break;
	case 10:
		if(Now_Swi_Position<Right_Target_Pos)
		{
			GV.TT_Motor_P[3].Run_Mode=DF_MSP_TT_Motor_SpeedMode;
			GV.TT_Motor_P[3].Target_Velcity=(int)Swi_Speed_Auto;
			Vert_Auto_Flag=3;
		}
		else if(GV.TT_Motor_P[3].Target_Velcity>0)
		{
			Vert_Auto_Flag=3;
		}
//		GV.Motor_P_CANOPEN[2].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
//		GV.Motor_P_CANOPEN[1].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
//
//		GV.Motor_P_CANOPEN[2].Target_Velcity=0;
//		GV.Motor_P_CANOPEN[1].Target_Velcity=0;
		Auto_Mode_LR_Manu_turn();
		break;
	default:
		GV.Motor_P_CANOPEN[2].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
		GV.Motor_P_CANOPEN[1].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;

		GV.Motor_P_CANOPEN[2].Target_Velcity=0;
		GV.Motor_P_CANOPEN[1].Target_Velcity=0;
		break;
	}
}


void Horiz_Auto_Task_Speed_1()
{

	double Now_Swi_Position=GV.TT_Motor_P[3].Real_Position;
//	double Turn_Angle_Back=TT_One_Deg_Count*5;//5度的回旋量

//	double Turn_Angle_Back=TT_One_Deg_Count*((double)GV.DH_CAN_HangSet.Swing_Speed)*3*1.5;	//总后退3S，1.5S在左边，1.5S在右边
	double Turn_Angle_Back=TT_One_Deg_Count*Back_Angle_Deg;	//总后退3S，1.5S在左边，1.5S在右边
	double Left_Threshold_Pos=Sw_Rust_Mid_Posi+Swi_Range_Auto-Turn_Angle_Back;
	double Right_Threshold_Pos=Sw_Rust_Mid_Posi-Swi_Range_Auto+Turn_Angle_Back;
	double Left_Target_Pos=Sw_Rust_Mid_Posi+Swi_Range_Auto;
	double Right_Target_Pos=Sw_Rust_Mid_Posi-Swi_Range_Auto;


	switch(Vert_Auto_Flag)
	{
	case 0://先找到中位开关量   //先调整车体到-90，然后再运动

		Horzi_Der_Angle_Confirm();//确认期望角度
		Horiz_First_Adjus();     //双轮反向调整

		if(Angle_Error_LLL<Error_Cont_Thresh)
		{
			Vert_Auto_Flag++;
			GV.Motor_P_CANOPEN[2].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
			GV.Motor_P_CANOPEN[2].Target_Velcity=0;
			GV.Motor_P_CANOPEN[1].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
			GV.Motor_P_CANOPEN[1].Target_Velcity=0;
		}

		TT_Speed_Flag=1;
		GV.TT_Motor_P[3].Run_Mode=DF_MSP_TT_Motor_SpeedMode;
		GV.TT_Motor_P[3].Target_Velcity=(int)Swi_Speed_Auto*TT_Speed_Flag;

		break;

	case 1:

		GV.Motor_P_CANOPEN[2].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
		GV.Motor_P_CANOPEN[2].Target_Velcity=0;
		GV.Motor_P_CANOPEN[1].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
		GV.Motor_P_CANOPEN[1].Target_Velcity=0;


		TT_Speed_Flag=1;
		GV.TT_Motor_P[3].Run_Mode=DF_MSP_TT_Motor_SpeedMode;
		GV.TT_Motor_P[3].Target_Velcity=(int)Swi_Speed_Auto*TT_Speed_Flag;

		Vert_Auto_Flag++;
		break;
	case 2:

		if(Now_Swi_Position>Left_Target_Pos)
		{
			Vert_Auto_Flag=8;

		}
		else
		{
			TT_Speed_Flag=1;
		}
		GV.TT_Motor_P[3].Run_Mode=DF_MSP_TT_Motor_SpeedMode;
		GV.TT_Motor_P[3].Target_Velcity=(int)TT_Speed_Flag*Swi_Speed_Auto;

		Auto_Mode_LR_Manu_turn();
		break;

	case 3://给出向左摆动目标位置
		GV.TT_Motor_P[3].Run_Mode=DF_MSP_TT_Motor_SpeedMode;

		GV.TT_Motor_P[3].Target_Velcity=(int)Swi_Speed_Auto*TT_Speed_Flag;
		Vert_Auto_Flag++;
		break;
	case 4://给出向右摆动目标位置
		Vert_Auto_Flag++;
		break;

	case 5://到达目标位置1开始后退
		if(Now_Swi_Position>Left_Target_Pos)
		{
			Vert_Auto_Flag=51;
			TT_Speed_Flag=0;
		}
		else
		{
			TT_Speed_Flag=1;
		}
		GV.TT_Motor_P[3].Run_Mode=DF_MSP_TT_Motor_SpeedMode;
		GV.TT_Motor_P[3].Target_Velcity=(int)TT_Speed_Flag*Swi_Speed_Auto;
		Auto_Mode_LR_Manu_turn();
		Vert_Count=0;
		break;

	case 51:
		if(Now_Swi_Position>Left_Target_Pos)
		{
			Vert_Auto_Flag=6;
			TT_Speed_Flag=0;
		}
		else
		{
			TT_Speed_Flag=1;
		}
		GV.TT_Motor_P[3].Run_Mode=DF_MSP_TT_Motor_SpeedMode;
		GV.TT_Motor_P[3].Target_Velcity=(int)TT_Speed_Flag*Swi_Speed_Auto;
		Auto_Mode_LR_Manu_turn();
		break;

	case 6:// 机器人后退指定距离
		Vert_Count++;

		Linear_Auto_Walk_Horiz( Auto_Job_Back_Speed, Auto_Job_Back_Speed_Max);
		if(Vert_Count>Auto_Job_Back_Time_MS)
		{
			GV.Motor_P_CANOPEN[2].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
			GV.Motor_P_CANOPEN[1].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;

			GV.Motor_P_CANOPEN[2].Target_Velcity=0;
			GV.Motor_P_CANOPEN[1].Target_Velcity=0;
			Vert_Count=0;
			Vert_Auto_Flag++;
		}

		TT_Speed_Flag=0;
		GV.TT_Motor_P[3].Run_Mode=DF_MSP_TT_Motor_SpeedMode;
		GV.TT_Motor_P[3].Target_Velcity=(int)TT_Speed_Flag*Swi_Speed_Auto;

		break;
	case 7://给出向左摆动目标位置

		if(Now_Swi_Position>Right_Target_Pos)
		{
			TT_Speed_Flag=-1;
		}
		else
		{
			Vert_Auto_Flag++;
			TT_Speed_Flag=0;
		}

		GV.TT_Motor_P[3].Run_Mode=DF_MSP_TT_Motor_SpeedMode;
		GV.TT_Motor_P[3].Target_Velcity=(int)TT_Speed_Flag*Swi_Speed_Auto;
		Auto_Mode_LR_Manu_turn();
		break;
	case 8:
		if(Now_Swi_Position>Right_Target_Pos)
		{
			TT_Speed_Flag=-1;
		}
		else
		{
			Vert_Auto_Flag=81;
			TT_Speed_Flag=0;
		}

		GV.TT_Motor_P[3].Run_Mode=DF_MSP_TT_Motor_SpeedMode;
		GV.TT_Motor_P[3].Target_Velcity=(int)TT_Speed_Flag*Swi_Speed_Auto;
		Vert_Count=0;
		Auto_Mode_LR_Manu_turn();
		break;
	case 81:
		if(Now_Swi_Position>Right_Target_Pos)
		{
			TT_Speed_Flag=-1;
		}
		else
		{
			NO_MENS_Auto_Flag=9;
			TT_Speed_Flag=0;
		}
		GV.TT_Motor_P[3].Run_Mode=DF_MSP_TT_Motor_SpeedMode;
		GV.TT_Motor_P[3].Target_Velcity=(int)TT_Speed_Flag*Swi_Speed_Auto;
		break;

	case 9:// 机器人后退指定距离

		Vert_Count++;
		Vertical_Der_Angle_Deg=-90;
		Linear_Auto_Walk_Horiz( Auto_Job_Back_Speed, Auto_Job_Back_Speed_Max);
		if(Vert_Count>Auto_Job_Back_Time_MS)
		{
			GV.TT_Motor_P[2].Run_Mode=DF_MSP_TT_Motor_SpeedMode;
			GV.TT_Motor_P[1].Run_Mode=DF_MSP_TT_Motor_SpeedMode;

			GV.TT_Motor_P[2].Target_Velcity=0;
			GV.TT_Motor_P[1].Target_Velcity=0;
			Vert_Count=0;
			Vert_Auto_Flag++;
		}

		TT_Speed_Flag=0;
		GV.TT_Motor_P[3].Run_Mode=DF_MSP_TT_Motor_SpeedMode;
		GV.TT_Motor_P[3].Target_Velcity=(int)TT_Speed_Flag*Swi_Speed_Auto;
		break;
	case 10:

		if(Now_Swi_Position>Left_Target_Pos)
		{
			TT_Speed_Flag=0;
			Vert_Auto_Flag=3;
		}
		else
		{
			Vert_Auto_Flag=3;
			TT_Speed_Flag=1;
		}
		GV.TT_Motor_P[3].Run_Mode=DF_MSP_TT_Motor_SpeedMode;
		GV.TT_Motor_P[3].Target_Velcity=(int)TT_Speed_Flag*Swi_Speed_Auto;

		Auto_Mode_LR_Manu_turn();
		break;



	default:

		GV.Motor_P_CANOPEN[2].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
		GV.Motor_P_CANOPEN[1].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;

		GV.Motor_P_CANOPEN[2].Target_Velcity=0;
		GV.Motor_P_CANOPEN[1].Target_Velcity=0;
		break;
	}
}







void Robot_Speed_MPU6050_Vertical()
{
//钛虎电机1/m每分钟转速为，输出值为255

	int Speed_Value_DH=(int)GV.DH_CAN_HangSet.Car_Speed;
	double Speed_Value_Big=0;
	switch (Speed_Value_DH)
	{
	case 0:case 1:case 2:case 3:case 4:case 5:case 6:case 7:case 8:case 10:case 14:
		Speed_Value_Big=(double)Speed_Value_DH;
		Speed_0_10_PID_Vertical(Speed_Value_Big);
		//Speed_0_10_PID_Horiz(Speed_Value_Big);
		break;

	}
}

void Robot_Speed_MPU6050_Horiz()
{
//钛虎电机1/m每分钟转速为，输出值为255

	int Speed_Value_DH=(int)GV.DH_CAN_HangSet.Car_Speed;
	double Speed_Value_Big=0;
	switch (Speed_Value_DH)
	{
	case 0:case 1:case 2:case 3:case 4:case 5:case 6:case 7:case 8:case 10:case 14:
		Speed_Value_Big=(double)Speed_Value_DH;
		Speed_0_10_PID_Horiz(Speed_Value_Big);
		break;

	}
}



void Speed_0_10_PID_Vertical(double Value_1)
{
	double Speed_Base=255;

	double V1,V2;
	double Deri_MAX_Speed;
	double Robot_Speed_PID[4]={0,0,0,0};
	if(GV.DH_CAN_HangSet.For_Back_Value_X>=50)
	{
		V1=((double)GV.DH_CAN_HangSet.For_Back_Value_X)/128*Speed_Base*Value_1;

		Deri_MAX_Speed=fabs(V1)*1.5;
                                           //开放给自动作业用
		Linear_Vertic_Auto_Walk_PID(V1,Deri_MAX_Speed);

	}
	else if(GV.DH_CAN_HangSet.For_Back_Value_X<=-50)
	{
		V1=-((double)GV.DH_CAN_HangSet.For_Back_Value_X)/128*Speed_Base*Value_1;
		Deri_MAX_Speed=fabs(V1)*1.5;
                                           //开放给自动作业用
		Linear_Vertic_Auto_Walk_PID(V1,Deri_MAX_Speed);

	}
	else if(GV.DH_CAN_HangSet.Left_Right_Value_Y>=50)
	{
		V1=2*((double)GV.DH_CAN_HangSet.Left_Right_Value_Y)/128*Speed_Base*Value_1/3;
		V2=V1;
		GV.Motor_P_CANOPEN[2].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
		GV.Motor_P_CANOPEN[1].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;

		GV.Motor_P_CANOPEN[2].Target_Velcity=V1;
		GV.Motor_P_CANOPEN[1].Target_Velcity=V2;
		Vertical_Flag_Zreo=0;
	}
	else if(GV.DH_CAN_HangSet.Left_Right_Value_Y<=-50)
	{
		V2=2*((double)GV.DH_CAN_HangSet.Left_Right_Value_Y)/128*Speed_Base*Value_1/3;
		V1=V2;

		GV.Motor_P_CANOPEN[2].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
		GV.Motor_P_CANOPEN[1].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;

		GV.Motor_P_CANOPEN[2].Target_Velcity=V1;
		GV.Motor_P_CANOPEN[1].Target_Velcity=V2;
		Vertical_Flag_Zreo=0;
	}
	else
	{
		V1=0;
		V2=0;
		GV.Motor_P_CANOPEN[2].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
		GV.Motor_P_CANOPEN[1].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;

		GV.Motor_P_CANOPEN[2].Target_Velcity=V1;
		GV.Motor_P_CANOPEN[1].Target_Velcity=V2;
		Vertical_Flag_Zreo=0;
	}


}


int Linear_Move_Flag=0;

void Linear_Auto_Walk_PID(double Deri_Speed, double Deri_MAX_Speed, double Robot_Speed[])//开放给自动作业用
{
	double Present_Angle=0;
	Present_Angle=((double)GV.Robot_Angle.MPU_Pitch)/100;
	double Angle_Add_Deg_12[2]={0,0};

	Horiz_Angle_Add(Angle_Add_Deg_12);

	double Angle_Deg_01;
	Angle_Deg_01=Angle_Add_Deg_12[0];

	GF_MSP_Auto_Motion_adj_Com(Present_Angle,Angle_Deg_01, Deri_Speed, Deri_MAX_Speed, System_time_SR , Robot_Speed);

}



void Linear_Vertic_Auto_Walk_PID(double Deri_Speed, double Deri_MAX_Speed )//开放给自动作业用
{
	double Present_Angle=0;
	Present_Angle=((double)GV.Robot_Angle.MPU_Pitch)/100;
	double Robot_Speed[4]={0,0,0,0};


	switch(Vertical_Flag_Zreo)
	{

	case 0:
		Vertical_Der_Angle_Confirm();//计算竖直期望角度；
		Vertical_Flag_Zreo++;
		break;

	case 1:
		Vertical_First_Adjus();
		if(fabs(Angle_Error_LLL)<Error_Cont_Thresh)
		{
			Vertical_Flag_Zreo++;
		}
		break;
	case 2:
		if(GV.DH_CAN_HangSet.For_Back_Value_X>=50)
		{
		Present_Angle=((double)GV.Robot_Angle.MPU_Pitch)/100;
		GF_MSP_PID_Now_Der_adj_Com(Present_Angle,Vertical_Der_Angle_Deg, Deri_Speed, Deri_MAX_Speed, System_time_SR , Robot_Speed);


		//		V1=-Robot_Speed_PID[0];
		//		V2=Robot_Speed_PID[1];
		GV.Motor_P_CANOPEN[2].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
		GV.Motor_P_CANOPEN[1].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;

		GV.Motor_P_CANOPEN[2].Target_Velcity=-Robot_Speed[0];
		GV.Motor_P_CANOPEN[1].Target_Velcity=Robot_Speed[1];
		}
		else
		{
			Vertical_Flag_Zreo=3;
		}
		break;

	case 3:
		if(GV.DH_CAN_HangSet.For_Back_Value_X<=-50)
		{
		Present_Angle=((double)GV.Robot_Angle.MPU_Pitch)/100;
		GF_MSP_PID_Now_Der_adj_Com(Present_Angle,Vertical_Der_Angle_Deg, Deri_Speed, Deri_MAX_Speed, System_time_SR , Robot_Speed);
		GV.Motor_P_CANOPEN[2].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
		GV.Motor_P_CANOPEN[1].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;


		GV.Motor_P_CANOPEN[2].Target_Velcity=Robot_Speed[1];
		GV.Motor_P_CANOPEN[1].Target_Velcity=-Robot_Speed[0];

		}
		else
		{
			Vertical_Flag_Zreo=2;
		}


		break;
	}


}





void Linear_Horiz_Auto_Walk_PID(double Deri_Speed, double Deri_MAX_Speed, double Robot_Speed[])//开放给自动作业用
{
	double Present_Angle=0;
	Present_Angle=((double)GV.Robot_Angle.MPU_Pitch)/100;
	double Angle_Add_Deg_12[2]={0,0};
	Horiz_Angle_Add(Angle_Add_Deg_12);
	double Angle_Deg_01;
	Angle_Deg_01=Angle_Add_Deg_12[0];
	GF_MSP_Auto_Motion_adj_Com(Present_Angle,Angle_Deg_01, Deri_Speed, Deri_MAX_Speed, System_time_SR , Robot_Speed);

}







void Horiz_Angle_Add(double Add_Angle_Deg[])
{
	int Horize_Value=(int)GV.DH_CAN_HangSet.Vert2_Horiz1;
	double Present_Angle_Add=0;
	Present_Angle_Add=fabs(((double)GV.Robot_Angle.MPU_Pitch))/100;
	if(Horize_Value==1)
	{
		if(GV.DH_CAN_HangSet.For_Back_Value_X<=-50)
		{
			if(Present_Angle_Add>145)
			{
				Add_Angle_Deg[0]=-((double)GV.DH_CAN_HangSet.Left_Comp )/35;
			}
			else
			{
				Add_Angle_Deg[0]=((double)GV.DH_CAN_HangSet.Right_Comp)/35;
			}
		}
	}
	else
	{
		Add_Angle_Deg[0]=0;
	}
}



void Linear_Auto_Walk_Horiz(double Deri_Speed,double Deri_MAX_Speed)//开放给自动作业用
{
	double Present_Angle=0;
	Present_Angle=((double)GV.Robot_Angle.MPU_Pitch)/100;
	double Robot_Speed[4];
	Horzi_Der_Angle_Confirm();

	//GF_MSP_Auto_Motion_adj_Com(Present_Angle,Add_Angle_Deg_21, Deri_Speed, Deri_MAX_Speed, System_time_SR , Robot_Speed);

	GF_MSP_PID_Now_Der_adj_Com(Present_Angle,Horzi_Der_Angle_Deg, Deri_Speed, Deri_MAX_Speed, System_time_SR , Robot_Speed);

	GV.Motor_P_CANOPEN[2].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
	GV.Motor_P_CANOPEN[2].Target_Velcity=Robot_Speed[1];
	GV.Motor_P_CANOPEN[1].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
	GV.Motor_P_CANOPEN[1].Target_Velcity=- Robot_Speed[0];

}



void Der_Angle_Judge_Vertic(double Present_Angle, double *ideal_Angle_Deg)
{
	 if((Present_Angle>=45)&(Present_Angle<135))
	 {
		 ideal_Angle_Deg[0] = 90 ;
	 }
	 else if ((Present_Angle<=-45)&(Present_Angle>=-135))
	 {
		 ideal_Angle_Deg[0] = -90 ;
	 }
	 else if ((Present_Angle>-45)&(Present_Angle<45))
	 {
		 ideal_Angle_Deg[0] = 0 ;
	 }
	 else
	 {
		 ideal_Angle_Deg[0] = 180 ;
	 }
}



void Der_Angle_Judge_Horiz(double Present_Angle,  double Horizontal_Add_Angle, double *ideal_Angle_Deg)
{

	 if(fabs(Present_Angle)<90)
	 {
		 ideal_Angle_Deg[0] = 0 + Horizontal_Add_Angle;
	 }
	 else
	 {
		 ideal_Angle_Deg[0] = 180 + Horizontal_Add_Angle;
		 if( ideal_Angle_Deg[0]>180)
		 {
			 ideal_Angle_Deg[0] =ideal_Angle_Deg[0] -360;
		 }
	 }
}



void Horiz_Linear_Auto_Walk_PID(double Deri_Speed, double Deri_MAX_Speed )//开放给自动作业用
{
	double Present_Angle=0;

	double Angle_Add_Deg_12[2]={0,0};
	double ideal_Angle_1[1]={0};
	double Robot_Speed[4]={0,0,0,0};
	switch(Horiz_Flag_Zreo)
	{

	case 0:
		Horiz_Angle_Add(Angle_Add_Deg_12);  //1.计算水平派生角度
		Present_Angle=((double)GV.Robot_Angle.MPU_Pitch)/100;
		Der_Angle_Judge_Horiz(Present_Angle, Angle_Add_Deg_12[0], ideal_Angle_1);//计算期望角度+派生角度
		Horzi_Der_Angle_Deg=ideal_Angle_1[0];
		Horiz_Flag_Zreo++;
		break;
	case 1:
		Horiz_First_Adjus();
		if(fabs(Angle_Error_LLL)<Error_Cont_Thresh)
		{
			Horiz_Flag_Zreo=2;
		}
		break;
	case 2:
		if(GV.DH_CAN_HangSet.For_Back_Value_X>=50)
		{
		Present_Angle=((double)GV.Robot_Angle.MPU_Pitch)/100;
		GF_MSP_PID_Now_Der_adj_Com(Present_Angle,Horzi_Der_Angle_Deg, Deri_Speed, Deri_MAX_Speed, System_time_SR , Robot_Speed);
		GV.Motor_P_CANOPEN[2].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
		GV.Motor_P_CANOPEN[1].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;

		GV.Motor_P_CANOPEN[1].Target_Velcity=Robot_Speed[1];
		GV.Motor_P_CANOPEN[2].Target_Velcity=-Robot_Speed[0];
		}
		else
		{
			Horiz_Flag_Zreo=3;
		}
		break;
	case 3:
		if(GV.DH_CAN_HangSet.For_Back_Value_X<=-50)
		{
		Present_Angle=((double)GV.Robot_Angle.MPU_Pitch)/100;
		GF_MSP_PID_Now_Der_adj_Com(Present_Angle,Horzi_Der_Angle_Deg, Deri_Speed, Deri_MAX_Speed, System_time_SR , Robot_Speed);
		GV.Motor_P_CANOPEN[2].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
		GV.Motor_P_CANOPEN[1].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;



		GV.Motor_P_CANOPEN[1].Target_Velcity=-Robot_Speed[0];
		GV.Motor_P_CANOPEN[2].Target_Velcity=Robot_Speed[1];
		}
		else
		{
			Horiz_Flag_Zreo=2;
		}
		break;
	}
}


void Speed_0_10_PID_Horiz(double Value_1)
{
	double Speed_Base=255;

	double V1,V2;
	double Deri_MAX_Speed;

	if(GV.DH_CAN_HangSet.For_Back_Value_X>=50)
	{
		V1=((double)GV.DH_CAN_HangSet.For_Back_Value_X)/128*Speed_Base*Value_1;

		Deri_MAX_Speed=fabs(V1)*1.5;
		Horiz_Linear_Auto_Walk_PID(V1,Deri_MAX_Speed);                                //开放给自动作业用
	}
	else if(GV.DH_CAN_HangSet.For_Back_Value_X<=-50)
	{
		V1=-((double)GV.DH_CAN_HangSet.For_Back_Value_X)/128*Speed_Base*Value_1;
		Deri_MAX_Speed=fabs(V1)*1.5;
		Horiz_Linear_Auto_Walk_PID(V1,Deri_MAX_Speed);                             //开放给自动作业用
	}
	else if(GV.DH_CAN_HangSet.Left_Right_Value_Y>=50)
	{
		V1=2*((double)GV.DH_CAN_HangSet.Left_Right_Value_Y)/128*Speed_Base*Value_1/3;
		V2=V1;
		Horiz_Flag_Zreo=0;

		GV.Motor_P_CANOPEN[2].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
		GV.Motor_P_CANOPEN[1].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;

		GV.Motor_P_CANOPEN[2].Target_Velcity=V1;
		GV.Motor_P_CANOPEN[1].Target_Velcity=V2;
	}
	else if(GV.DH_CAN_HangSet.Left_Right_Value_Y<=-50)
	{
		V2=2*((double)GV.DH_CAN_HangSet.Left_Right_Value_Y)/128*Speed_Base*Value_1/3;
		V1=V2;
		Horiz_Flag_Zreo=0;
		GV.Motor_P_CANOPEN[2].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
		GV.Motor_P_CANOPEN[1].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;

		GV.Motor_P_CANOPEN[2].Target_Velcity=V1;
		GV.Motor_P_CANOPEN[1].Target_Velcity=V2;
	}
	else
	{
		V1=0;
		V2=0;
		Horiz_Flag_Zreo=0;
		GV.Motor_P_CANOPEN[2].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
		GV.Motor_P_CANOPEN[1].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;

		GV.Motor_P_CANOPEN[2].Target_Velcity=V1;
		GV.Motor_P_CANOPEN[1].Target_Velcity=V2;
	}
}


int Back_Dis_Flag=0;

void Back_Dis_Adjust()
{
 double Auto_Job_Back_Speed_11[1]={0};
  if((GV.DH_CAN_HangSet.For_Back_Value_X>=50)&&(GV.DH_CAN_HangSet.Swing_Speed==11))
  {
	  	Back_Dis_Flag++;
		GV.Motor_P_CANOPEN[2].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
		GV.Motor_P_CANOPEN[1].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
		GV.Motor_P_CANOPEN[2].Target_Velcity=0;
		GV.Motor_P_CANOPEN[1].Target_Velcity=0;
		GV.TT_Motor_P[3].Run_Mode=DF_MSP_TT_Motor_SpeedMode;
		GV.TT_Motor_P[3].Target_Velcity=0;
  }
  else if((GV.DH_CAN_HangSet.For_Back_Value_X<-50)&&(GV.DH_CAN_HangSet.Swing_Speed==11))
  {
	  	Back_Dis_Flag--;
		GV.Motor_P_CANOPEN[2].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
		GV.Motor_P_CANOPEN[1].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
		GV.Motor_P_CANOPEN[2].Target_Velcity=0;
		GV.Motor_P_CANOPEN[1].Target_Velcity=0;
		GV.TT_Motor_P[3].Run_Mode=DF_MSP_TT_Motor_SpeedMode;
		GV.TT_Motor_P[3].Target_Velcity=0;
  }
  else
  {
	  Back_Dis_Flag=0;
  }
  if(Back_Dis_Flag>500)
  {
	  Back_Dis_Flag=0;
	  Back_Dis_CM=Back_Dis_CM+0.5;
	//  Tis_Dis_Speed_Time(Back_Dis_CM, Auto_Job_Back_Speed, Auto_Job_Back_Time);

	  Auto_Job_Back_Time_MS=Back_Angle_Deg/(((double)GV.DH_CAN_HangSet.Swing_Speed)/10)*1000+Back_Comp_Time_MS;
	  TI5_Dis_Speed_Compute_3S(Back_Dis_CM,Auto_Job_Back_Time_MS , Auto_Job_Back_Speed_11 );
	  Auto_Job_Back_Speed=Auto_Job_Back_Speed_11[0];
	  Auto_Job_Back_Speed_Max=Auto_Job_Back_Speed*1.4;
  }
  else if(Back_Dis_Flag<-500)
  {
	  Back_Dis_Flag=0;
	  Back_Dis_CM=Back_Dis_CM-0.5;
	  if(Back_Dis_CM<=1)
	  {
		  Back_Dis_CM=1;
	  }
	  Auto_Job_Back_Time_MS=Back_Angle_Deg/(((double)GV.DH_CAN_HangSet.Swing_Speed)/10)*1000+Back_Comp_Time_MS;
	  TI5_Dis_Speed_Compute_3S(Back_Dis_CM,Auto_Job_Back_Time_MS , Auto_Job_Back_Speed_11 );
	  Auto_Job_Back_Speed=Auto_Job_Back_Speed_11[0];
	  Auto_Job_Back_Speed_Max=Auto_Job_Back_Speed*1.4;
  }
  else
  {
	  if(Last_Swing_Speed!=GV.DH_CAN_HangSet.Swing_Speed)
	  {
		  Auto_Job_Back_Time_MS=Back_Angle_Deg/(((double)GV.DH_CAN_HangSet.Swing_Speed)/10)*1000+Back_Comp_Time_MS;
		  TI5_Dis_Speed_Compute_3S(Back_Dis_CM,Auto_Job_Back_Time_MS , Auto_Job_Back_Speed_11 );
		  Auto_Job_Back_Speed=Auto_Job_Back_Speed_11[0];
		  Auto_Job_Back_Speed_Max=Auto_Job_Back_Speed*1.4;
	  }
	  else
	  {
		  Last_Swing_Speed=GV.DH_CAN_HangSet.Swing_Speed;
	  }
  }
}

void Change_Road_Time_Calculate()
{
	if(Last_Swing_Angle!=GV.DH_CAN_HangSet.Swing_Angle)
	{
		double Wheel_Diameter=250; double PI_Value=3.1415926;	double Motor_Ratio=121;
		double MM_Senct=((double)GV.DH_CAN_HangSet.Turn_Road_Back_Dis)/10-Horiz_Change_Road_Comp_Dis_MM;//Convert to mm
		double Deg_Senct=Change_Road_Speed*360/100/Motor_Ratio;
		double Wheel_Speed_MM_Senc=Deg_Senct/360*Wheel_Diameter*PI_Value;
		Change_Road_Time=MM_Senct/Wheel_Speed_MM_Senc*1000/2;
	}
	else
	{
		Last_Swing_Angle=GV.DH_CAN_HangSet.Swing_Angle;
	}
}


void Change_Road_Time_Calculate_Horiz()
{
	if(Last_Swing_Angle!=GV.DH_CAN_HangSet.Swing_Angle)
	{
		double Wheel_Diameter=250; double PI_Value=3.1415926;	double Motor_Ratio=121;
		double MM_Senct=((double)GV.DH_CAN_HangSet.Turn_Road_Back_Dis)/10-Horiz_Change_Road_Comp_Dis_MM;//Convert to mm
		double Deg_Senct=Change_Road_Speed*360/100/Motor_Ratio;
		double Wheel_Speed_MM_Senc=Deg_Senct/360*Wheel_Diameter*PI_Value;
		Change_Road_Time=MM_Senct/Wheel_Speed_MM_Senc*1000/2;
	}
	else
	{
		Last_Swing_Angle=GV.DH_CAN_HangSet.Swing_Angle;
	}
}

void Change_Road_Time_Calculate_Verti()
{
	if(Last_Swing_Angle!=GV.DH_CAN_HangSet.Swing_Angle)
	{
		double Wheel_Diameter=250; double PI_Value=3.1415926;	double Motor_Ratio=121;
		double MM_Senct=((double)GV.DH_CAN_HangSet.Vertic_Turn_Road_Back_Dis)/10-Horiz_Change_Road_Comp_Dis_MM;//Convert to mm
		double Deg_Senct=Change_Road_Speed*360/100/Motor_Ratio;
		double Wheel_Speed_MM_Senc=Deg_Senct/360*Wheel_Diameter*PI_Value;
		Change_Road_Time=MM_Senct/Wheel_Speed_MM_Senc*1000/2;
	}
	else
	{
		Last_Swing_Angle=GV.DH_CAN_HangSet.Swing_Angle;
	}
}




void Horzi_Der_Angle_Confirm()
{
	double Now_Angle_DEG_1=((double)GV.Robot_Angle.MPU_Pitch)/100;
	if((Now_Angle_DEG_1<90)&(Now_Angle_DEG_1>-90))
	{
		Horzi_Der_Angle_Deg=((double)GV.DH_CAN_HangSet.Right_Comp )/35;
		//Der_Angle_Right[1]=Horzi_Der_Angle_Deg;
	}
	else
	{
		Horzi_Der_Angle_Deg=180-((double)GV.DH_CAN_HangSet.Left_Comp )/35;
		//Der_Angle_Left[1]=Horzi_Der_Angle_Deg;
	}
}


void Horiz_First_Adjus()
{
	double Now_Angle_DEG=((double)((double)GV.Robot_Angle.MPU_Pitch))/100;
	double Sign_Flag=0;
	Angle_Error_LLL= Now_Angle_DEG-Horzi_Der_Angle_Deg;
	if(Angle_Error_LLL>=180)
	{
		Angle_Error_LLL=Angle_Error_LLL-360;
	}
	else if(Angle_Error_LLL<=-180)
	{
		Angle_Error_LLL=Angle_Error_LLL+360;
	}
	if(Angle_Error_LLL>0)
	{
		Sign_Flag=1;
	}
	else if(Angle_Error_LLL<0)
	{
		Sign_Flag=-1;
	}
	else
	{
		Sign_Flag=0;
	}

	Angle_Error_LLL=fabs(Angle_Error_LLL);
	double  Speed_Factor=0;
	if(Angle_Error_LLL>5)
	{
		Speed_Factor=7.5;
	}
	else
	{
		Speed_Factor=0.4;
	}

	GV.Motor_P_CANOPEN[2].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
	GV.Motor_P_CANOPEN[2].Target_Velcity=Speed_Factor*Sign_Flag*100;
	GV.Motor_P_CANOPEN[1].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
	GV.Motor_P_CANOPEN[1].Target_Velcity=Speed_Factor*Sign_Flag*100;

}

void Horiz_Angle_error_Compute()
{
	double Present_Angle_2=((double)GV.Robot_Angle.MPU_Pitch)/100;
	Horiz_Angle_error_Value=fabs(Present_Angle_2-Horzi_Der_Angle_Deg);

	if(Horiz_Angle_error_Value>180)
	{
		Horiz_Angle_error_Value=Horiz_Angle_error_Value-360;
	}
	else if(Horiz_Angle_error_Value<-180)
	{
		Horiz_Angle_error_Value=Horiz_Angle_error_Value+360;
	}
}




void Vertical_Der_Angle_Confirm()
{
	double Now_Angle_DEG_1=((double)GV.Robot_Angle.MPU_Pitch)/100;
	if((Now_Angle_DEG_1<=0)&(Now_Angle_DEG_1>-179.9))
	{
		Vertical_Der_Angle_Deg=-90;
	}
	else
	{
		Vertical_Der_Angle_Deg=90;
	}
}


void Vertical_First_Adjus()
{
	double Now_Angle_DEG=((double)GV.Robot_Angle.MPU_Pitch)/100;
	double Sign_Flag=0;
	Angle_Error_LLL= Now_Angle_DEG-Vertical_Der_Angle_Deg;

	if(Angle_Error_LLL>=180)
	{
		Angle_Error_LLL=Angle_Error_LLL-360;
	}
	else if(Angle_Error_LLL<=-180)
	{
		Angle_Error_LLL=Angle_Error_LLL+360;
	}


	if(Angle_Error_LLL>0)
	{
		Sign_Flag=1;
	}
	else if(Angle_Error_LLL<0)
	{
		Sign_Flag=-1;
	}
	else
	{
		Sign_Flag=0;
	}
	Angle_Error_LLL=fabs(Angle_Error_LLL);

	double  Speed_Factor=0;
	if(Angle_Error_LLL>5)
	{
		Speed_Factor=7.5;
	}
	else
	{
		Speed_Factor=0.4;
	}
	GV.Motor_P_CANOPEN[2].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
	GV.Motor_P_CANOPEN[2].Target_Velcity=Speed_Factor*Sign_Flag*100;
	GV.Motor_P_CANOPEN[1].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
	GV.Motor_P_CANOPEN[1].Target_Velcity=Speed_Factor*Sign_Flag*100;

}


void Auto_Mode_LR_Manu_turn()
{
	double Speed_Base_A=201.7*7.64;

	double V1,V2;

	if(GV.DH_CAN_HangSet.Left_Right_Value_Y>=50)
	{
		V1=2*((double)GV.DH_CAN_HangSet.Left_Right_Value_Y)/128*Speed_Base_A;
		V2=V1;
		Horiz_Flag_Zreo=0;

		GV.Motor_P_CANOPEN[2].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
		GV.Motor_P_CANOPEN[1].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;

		GV.Motor_P_CANOPEN[2].Target_Velcity=V1;
		GV.Motor_P_CANOPEN[1].Target_Velcity=V2;
	}
	else if(GV.DH_CAN_HangSet.Left_Right_Value_Y<=-50)
	{
		V2=2*((double)GV.DH_CAN_HangSet.Left_Right_Value_Y)/128*Speed_Base_A;
		V1=V2;
		Horiz_Flag_Zreo=0;
		GV.Motor_P_CANOPEN[2].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
		GV.Motor_P_CANOPEN[1].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;

		GV.Motor_P_CANOPEN[2].Target_Velcity=V1;
		GV.Motor_P_CANOPEN[1].Target_Velcity=V2;
	}
	else
	{
		V1=0;
		V2=0;
		Horiz_Flag_Zreo=0;
		GV.Motor_P_CANOPEN[2].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
		GV.Motor_P_CANOPEN[1].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;

		GV.Motor_P_CANOPEN[2].Target_Velcity=V1;
		GV.Motor_P_CANOPEN[1].Target_Velcity=V2;
	}
}

void NO_MENS_Auto_Operat()
{
	double Now_Swi_Position=GV.TT_Motor_P[3].Real_Position;
	double Turn_Angle_Back=TT_One_Deg_Count*Back_Angle_Deg;	//总后退3S，1.5S在左边，1.5S在右边
	double Left_Threshold_Pos=Sw_Rust_Mid_Posi+Swi_Range_Auto-Turn_Angle_Back;
	double Right_Threshold_Pos=Sw_Rust_Mid_Posi-Swi_Range_Auto+Turn_Angle_Back;
	double Left_Target_Pos=Sw_Rust_Mid_Posi+Swi_Range_Auto;
	double Right_Target_Pos=Sw_Rust_Mid_Posi-Swi_Range_Auto;


	switch(NO_MENS_Auto_Flag)
	{
	case 0://先找到中位开关量   //先调整车体到-90，然后再运动
		NO_MENS_Auto_Flag++;
		break;

	case 1:
		GV.Motor_P_CANOPEN[2].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
		GV.Motor_P_CANOPEN[2].Target_Velcity=0;
		GV.Motor_P_CANOPEN[1].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
		GV.Motor_P_CANOPEN[1].Target_Velcity=0;
		GV.TT_Motor_P[3].Run_Mode=DF_MSP_TT_Motor_SpeedMode;

		GV.TT_Motor_P[3].Target_Velcity=(int)Swi_Speed_Auto*1;
		NO_MENS_Auto_Flag++;
		break;
	case 2:
		if(Now_Swi_Position>Left_Target_Pos)
		{
			NO_MENS_Auto_Flag=8;
			GV.TT_Motor_P[3].Run_Mode=DF_MSP_TT_Motor_SpeedMode;
			GV.TT_Motor_P[3].Target_Velcity=(int)-Swi_Speed_Auto;
		}
		Auto_Mode_LR_Manu_turn();
		break;

	case 3://给出向左摆动目标位置
		GV.TT_Motor_P[3].Run_Mode=DF_MSP_TT_Motor_SpeedMode;

		GV.TT_Motor_P[3].Target_Velcity=(int)Swi_Speed_Auto*1;
		NO_MENS_Auto_Flag++;
		break;
	case 4://给出向右摆动目标位置
		NO_MENS_Auto_Flag++;
		break;
	case 5://到达目标位置1开始后退
		if(Now_Swi_Position>Left_Threshold_Pos)
		{
			NO_MENS_Auto_Flag++;
			GV.TT_Motor_P[3].Run_Mode=DF_MSP_TT_Motor_SpeedMode;
			GV.TT_Motor_P[3].Target_Velcity=(int)Swi_Speed_Auto;
		}
		Auto_Mode_LR_Manu_turn();
		Vert_Count=0;
		break;
	case 6:// 机器人后退指定距离
		Vert_Count++;

		No_MENS_Linear_Auto_Walk(Auto_Job_Back_Speed, Auto_Job_Back_Speed_Max);

		if(Vert_Count>Auto_Job_Back_Time_MS)
		{
			GV.Motor_P_CANOPEN[2].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
			GV.Motor_P_CANOPEN[1].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;

			GV.Motor_P_CANOPEN[2].Target_Velcity=0;
			GV.Motor_P_CANOPEN[1].Target_Velcity=0;
			Vert_Count=0;
			NO_MENS_Auto_Flag++;
		}

		if(Now_Swi_Position>Left_Target_Pos)
		{
			GV.TT_Motor_P[3].Run_Mode=DF_MSP_TT_Motor_SpeedMode;
			GV.TT_Motor_P[3].Target_Velcity=(int)-1*Swi_Speed_Auto;
		}
		break;
	case 7://给出向左摆动目标位置

		if(Now_Swi_Position>Left_Target_Pos)
		{
			GV.TT_Motor_P[3].Run_Mode=DF_MSP_TT_Motor_SpeedMode;
			GV.TT_Motor_P[3].Target_Velcity=(int)-1*Swi_Speed_Auto;
			NO_MENS_Auto_Flag++;
		}
		else if(GV.TT_Motor_P[3].Target_Velcity<0)
		{
			NO_MENS_Auto_Flag++;
		}
		Auto_Mode_LR_Manu_turn();
		break;
	case 8:
		if(Now_Swi_Position>Left_Target_Pos)
		{
			GV.TT_Motor_P[3].Run_Mode=DF_MSP_TT_Motor_SpeedMode;
			GV.TT_Motor_P[3].Target_Velcity=(int)-1*Swi_Speed_Auto;
		}

		if(Now_Swi_Position<Right_Threshold_Pos)
		{
			NO_MENS_Auto_Flag++;
			GV.TT_Motor_P[3].Run_Mode=DF_MSP_TT_Motor_SpeedMode;
			GV.TT_Motor_P[3].Target_Velcity=(int)-1*Swi_Speed_Auto;
		}
		Vert_Count=0;
		Auto_Mode_LR_Manu_turn();
		break;
	case 9:// 机器人后退指定距离

		Vert_Count++;
		No_MENS_Linear_Auto_Walk( Auto_Job_Back_Speed, Auto_Job_Back_Speed_Max);

		if(Vert_Count>Auto_Job_Back_Time_MS)
		{
			GV.Motor_P_CANOPEN[2].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
			GV.Motor_P_CANOPEN[1].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;

			GV.Motor_P_CANOPEN[2].Target_Velcity=0;
			GV.Motor_P_CANOPEN[1].Target_Velcity=0;
			Vert_Count=0;
			NO_MENS_Auto_Flag++;
		}
		if(Now_Swi_Position<Right_Target_Pos)
		{
			GV.TT_Motor_P[3].Run_Mode=DF_MSP_TT_Motor_SpeedMode;
			GV.TT_Motor_P[3].Target_Velcity=(int)Swi_Speed_Auto;
		}

		break;
	case 10:
		if(Now_Swi_Position<Right_Target_Pos)
		{
			GV.TT_Motor_P[3].Target_Velcity=(int)Swi_Speed_Auto;
			NO_MENS_Auto_Flag=3;
		}
		else if(GV.TT_Motor_P[3].Target_Velcity>0)
		{
			NO_MENS_Auto_Flag=3;
		}

//		GV.Motor_P_CANOPEN[2].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
//		GV.Motor_P_CANOPEN[1].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
//
//		GV.Motor_P_CANOPEN[2].Target_Velcity=0;
//		GV.Motor_P_CANOPEN[1].Target_Velcity=0;
		Auto_Mode_LR_Manu_turn();
		break;



	default:

		GV.Motor_P_CANOPEN[2].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
		GV.Motor_P_CANOPEN[1].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;

		GV.Motor_P_CANOPEN[2].Target_Velcity=0;
		GV.Motor_P_CANOPEN[1].Target_Velcity=0;
		break;
	}
}






void No_MENS_Linear_Auto_Walk(double Deri_Speed,double Deri_MAX_Speed)//开放给自动作业用
{
	GV.Motor_P_CANOPEN[2].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
	GV.Motor_P_CANOPEN[2].Target_Velcity=Deri_Speed;
	GV.Motor_P_CANOPEN[1].Run_Mode=DF_MSP_Ti5Motor_Speed_CANOPEN;
	GV.Motor_P_CANOPEN[1].Target_Velcity=- Deri_Speed;

}





void Lift_motor_control_Swing()
{
	//UP Down motor contronl
	if(GV.DH_CAN_HangSet.UP1==1)
	{
		HAL_GPIO_WritePin(OUT_2_GPIO_Port,OUT_2_Pin,GPIO_PIN_RESET  );
		HAL_GPIO_WritePin(OUT_3_GPIO_Port,OUT_3_Pin,GPIO_PIN_SET );
	}
	else if(GV.DH_CAN_HangSet.DOWN2==1)
	{
		HAL_GPIO_WritePin(OUT_2_GPIO_Port,OUT_2_Pin,GPIO_PIN_SET  );
		HAL_GPIO_WritePin(OUT_3_GPIO_Port,OUT_3_Pin,GPIO_PIN_RESET );
	}
	else
	{
		HAL_GPIO_WritePin(OUT_3_GPIO_Port,OUT_3_Pin,GPIO_PIN_SET );
		HAL_GPIO_WritePin(OUT_2_GPIO_Port,OUT_2_Pin,GPIO_PIN_SET );
	}
}









