/*
 *
 *后右0号电机；后左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();
void Mobile_Robot_Manual_Con_2();
void Robot_EMERGENCY_STOP_Fuction();
void Robot_Hold_On();
void Water_Cannon_Control();
void Ackerman_Steer_Model_0(double V1,double V2,double Robot_Size[],double *Vol_Rear_L_R);

void Robot_auto_forward_gyro();


int Motor_Stop_Flag=0;
int Hold_On_Flag=20;
int Speed_flag=0;
double Count_Vel_m_S=0.0000933146;//3.1415926*0.3/100/101//pi*2r/100/101
double Vel_Count=10716.43283;//3.1415926*0.3/100/101//pi*2r/100/101
double Robot_size_W_H[2]={0.37,0.35};//Robot_Size[0]  Distance between left and right wheels; Robot_Size[1]  Distance between front and rear wheels
int top_limit=0; //限位是否触发标志位 0未触发 1触发
int bot_limit=0;
int right_limit=0;
int top_flag=0;  // 标志位 限位每每触发发一次速度零
int bot_flag=0;
int right_flag=0;
int auto_move=0;

int16_t Expectation_Angle;// = 0;

uint16_t Rot_expected_position_left = 2354;  //根据情况设定左摆期望位置
uint16_t Rot_expected_position_right = 1954;
uint32_t Rot_current_position;
uint8_t Rot_expected_time_left = 4;  //根据情况设定左摆期望时间
uint8_t Rot_expected_time_right = 8;
uint8_t Rot_dir_flag = 0;
int32_t Left_V = 100;
int32_t Right_V = -100;

uint8_t Pitch_dir_flag = 0;
int32_t Up_V = 100;
int32_t Down_V = -100;
uint32_t Pitch_current_position;
uint16_t Pitch_expected_position_up = 2354;  //根据情况设定上仰期望位置
uint16_t Pitch_expected_position_down = 1954;
uint8_t Pitch_expected_time_up = 4;  //根据情况设定上仰期望时间
uint8_t Pitch_expected_time_down = 4;

void CV_GV_Init()
{
	CV.Speed_1 = 1;
	GV.Motor_P[0].MotorID=1;
	GV.Motor_P[1].MotorID=2;
	GV.Motor_P[2].MotorID=3;
	GV.Motor_P[3].MotorID=4;
	GV.Motor_P[4].MotorID=5;
	GV.Motor_P[5].MotorID=6;
}

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));

	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,GF_MSP_Ti5Motor_Control_Timer);

	_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_Init(1, (MotorParameters*)(&GV.Motor_P),DF_GL_MotorNum);
	_state = _state & GF_MSP_DH_Wash_H_Init(&huart1,(SP_MSP_DH_Handset_Parameters*)&GV.DH_WH_HandSet);

	_state = _state & GF_MSP_RF_TL720D_Init(&huart3,(SP_MSP_RF_TL720D_Parameters*)&GV.EX_RF_TL720D);

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


	_state = _state & GF_MSP_OBD_Init(2);
	_state = _state & GF_BSP_TIMER_Init(); //定时器最后启动

	_state = _state & GF_MSP_Gyro_MFOG40_Init(&huart3,(SP_MSP_Gyro_parameters*)&GV.Gyro_MFOG40);

	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 Wheel_mile=1;
double Robot_Auto_Speed[2];
int _Start=0;

int Mov_RL_Position_Flag = 0;
int Mov_UD_Position_Flag = 0;
int target_Position=0;


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

	//SSTEST
	//GF_BSP_FDCAN_Senddata(2, 0x123, 1, (uint8_t*)GV.NC_ARRAY_u8_0);

	//监控遥控器有没有持续接收到新数据，有新数据，数据将改写GV.MK32_Online的数据
//	double Robot_Roll_angle;
	double Speed_1=fabs(GV.Motor_P[0].Velcity)*3.1415926*0.3/100/101*60;
	int16_t Velocity_Va=(int16_t)(Speed_1);
	int16_t Angle_Va=(int16_t)GV.Robot_Angle.MPU_Roll;
	int16_t Current_Va=(fabs(GV.Motor_P[0].Current)+fabs(GV.Motor_P[1].Current)+fabs(GV.Motor_P[2].Current)+fabs(GV.Motor_P[3].Current))/4/10;
	double b=((double)GV.DH_WH_HandSet.Cleaning_swing/255)*2000;
	int16_t Angle=(int16_t)(b);
	switch(Bat_falg)
	{
	case 1:
		GF_MSP_DH_Stu_Inquiry(Velocity_Va,Angle,Current_Va);//遥控器，每200ms读取一次
		GF_BSP_GetAngle();//如，数据有问题，请分开调用
		Bat_falg=1;
		break;
	}
//	GV.Motor_P[0].Run_Mode = DF_MSP_Ti5Motor_SpeedMode;
//	GV.Motor_P[0].Target_Velcity=200;
////	Tis_Position_control(2,500);  //测试电机线路连接，在测试2号电机发送位置时出现问题，改为发送位置解决问题
//	GV.Motor_P[1].Run_Mode = DF_MSP_Ti5Motor_SpeedMode;
//	GV.Motor_P[1].Target_Velcity=200;
//	GV.Motor_P[2].Run_Mode = DF_MSP_Ti5Motor_SpeedMode;
//	GV.Motor_P[2].Target_Velcity=200;
//	GV.Motor_P[3].Run_Mode = DF_MSP_Ti5Motor_SpeedMode;
//	GV.Motor_P[3].Target_Velcity=200;


//	if((GV.DH_WH_HandSet.EMERGENCY_STOP == 1)||(GV.DH_WH_HandSet.Wireless_Status==0))
//	{
////		GF_MainLoop_Jump(1,3); //机器人急停
//	}
//	else if(GV.DH_WH_HandSet.Mode_A1_B2==1)
//	{
//		GF_MainLoop_Jump(1,1); //手动控制
//	}
//	else if(GV.DH_WH_HandSet.Mode_A1_B2==2)
//	{
//		GF_MainLoop_Jump(1,5); //手动控制_2//四轮转向模型
//	}
//	else if(GV.DH_WH_HandSet.Mode_A1_B2==2)
//	{
//		GF_MainLoop_Jump(1,2); //预期为自动操作(未填写)
//	}
//	else if(GV.DH_WH_HandSet.Mov_Horiz1_vert2==2)
//	{
//		GF_MainLoop_Jump(1,4); //竖直移动
//	}
//	else
//	{
//		GF_MainLoop_Jump(1,0); //无操作
//	}
	if(GV.DH_WH_HandSet.Mov_Horiz1_vert2==0)
	{
		auto_move=0;
	}

	if(GV.DH_WH_HandSet.Mode_A1_B2 == 1)  //旋转电机左转、右转自动作业
	{
		if(Mov_RL_Position_Flag == 0)
		{
			switch(Rot_dir_flag)
			{
			case 0:
				GV.Motor_P[4].Run_Mode = DF_MSP_Ti5Motor_SpeedMode;
				GV.Motor_P[4].Target_Velcity = Left_V;  //旋转电机左转
				Rot_current_position = GV.Motor_P[4].Position;
				if((Rot_current_position == Rot_expected_position_left) || ( Rot_current_time== Rot_expected_time_left))
				{
					Rot_Stop();
					Rot_dir_flag = 1;
					Rot_current_time = 0;
				}
				break;
			case 1:
				if(right_limit==0)
				{
					GV.Motor_P[4].Run_Mode = DF_MSP_Ti5Motor_SpeedMode;
					GV.Motor_P[4].Target_Velcity = Right_V;  //旋转电机右转
					Rot_current_position = GV.Motor_P[4].Position;
					if((Rot_current_position == Rot_expected_position_right) || ( Rot_current_time == Rot_expected_time_right))
					{
						Rot_Stop();
						Rot_dir_flag = 2;
						Rot_current_time = 0;
					}
				}
				else {
					Rot_Stop();
				}
				break;
			case 2:
				GV.Motor_P[4].Run_Mode = DF_MSP_Ti5Motor_SpeedMode;
				GV.Motor_P[4].Target_Velcity = Left_V;
				Rot_current_position = GV.Motor_P[4].Position;
				if((Rot_current_position == Rot_expected_position_left) || ( Rot_current_time== 2*Rot_expected_time_left))
				{
					Rot_Stop();
					Rot_dir_flag = 1;
					Rot_current_time = 0;
				}
				break;
			}
		}
//		int32_t a=((double)GV.DH_WH_HandSet.Cleaning_swing/255)*20*101*65536/360;//旋转电机左转
//		if(Mov_RL_Position_Flag == 0)
//		{
//			GV.Motor_P[4].Target_Position = GV.Motor_P[4].Position + a;
//			GV.Motor_P[4].Run_Mode = DF_MSP_Ti5Motor_PositionMode;
//			Mov_RL_Position_Flag = 2;
//		}
	}
	else if(GV.DH_WH_HandSet.Mode_A1_B2 == 2 && right_limit==0) //旋转电机右转
	{
//		int32_t a=((double)GV.DH_WH_HandSet.Cleaning_swing/255)*20*101*65536/360;
//		if(Mov_RL_Position_Flag == 0)
//		{
//			GV.Motor_P[4].Target_Position = GV.Motor_P[4].Position - a;
//			GV.Motor_P[4].Run_Mode = DF_MSP_Ti5Motor_PositionMode;
//			Mov_RL_Position_Flag = 2;
//		}

	}
	else if(GV.DH_WH_HandSet.Mode_A1_B2 ==0)
	{
		Mov_RL_Position_Flag = 0;
		Rot_Stop();
	}
	if(GV.DH_WH_HandSet.Mode_C1_D2 == 1)  //俯仰电机上仰、下降自动作业
	{
		if(Mov_UD_Position_Flag == 0)
		{
			switch(Pitch_dir_flag)
			{
			case 0:
				if(top_limit==0)
				{
					GV.Motor_P[5].Run_Mode = DF_MSP_Ti5Motor_SpeedMode;
					GV.Motor_P[5].Target_Velcity = Up_V;  //俯仰电机上仰
					Pitch_current_position = GV.Motor_P[5].Position;
					if((Pitch_current_position == Pitch_expected_position_up) || ( Pit_current_time == Pitch_expected_time_up))
					{
						Up_Stop();
						Pitch_dir_flag = 1;
						Pit_current_time = 0;
					}
				}
				else {
					Up_Stop();
				}
				break;
			case 1:
				if(bot_limit==0)
				{
					GV.Motor_P[5].Run_Mode = DF_MSP_Ti5Motor_SpeedMode;
					GV.Motor_P[5].Target_Velcity = Down_V;  //俯仰电机下降
					Pitch_current_position = GV.Motor_P[5].Position;
					if((Pitch_current_position == Pitch_expected_position_down) || ( Pit_current_time == Pitch_expected_time_down))
					{
						Up_Stop();
						Pitch_dir_flag = 0;
						Pit_current_time = 0;
					}
				}
				else {
					Up_Stop();
				}
				break;
			}
		}

//		int32_t a=((double)GV.DH_WH_HandSet.Cleaning_swing/255)*20*101*65536/360;//俯仰电机上仰
//		if(Mov_UD_Position_Flag == 0)
//		{
//			GV.Motor_P[5].Target_Position = GV.Motor_P[5].Position + a;
//			GV.Motor_P[5].Run_Mode = DF_MSP_Ti5Motor_PositionMode;
//			Mov_UD_Position_Flag = 2;
//		}
	}
	else if(GV.DH_WH_HandSet.Mode_C1_D2 == 2 && bot_limit==0) //俯仰电机下降
	{
//		int32_t a=((double)GV.DH_WH_HandSet.Cleaning_swing/255)*20*101*65536/360;
//		if(Mov_UD_Position_Flag == 0)
//		{
//			GV.Motor_P[5].Target_Position = GV.Motor_P[5].Position - a;
//			GV.Motor_P[5].Run_Mode = DF_MSP_Ti5Motor_PositionMode;
//			Mov_UD_Position_Flag = 2;
//		}
	}
	else if(GV.DH_WH_HandSet.Mode_C1_D2 ==0)
	{
		Mov_UD_Position_Flag = 0;
		Up_Stop();
	}

	if(GV.DH_WH_HandSet.Scan_horiz1_vert2==1)
	{
		Init_Position();
	}
	else if (GV.DH_WH_HandSet.Scan_horiz1_vert2==2)
	{

	}

	if(!GF_BSP_GPIO_ReadIO(1))
	{
		top_limit=0;
		//顶部工装
		if(!GF_BSP_GPIO_ReadIO(2))
		{
			Robot_Stop();
			//顶部限位
			top_limit=1;
			if(top_flag==0)
			{
				Up_Stop();
				Rot_Stop();
				top_flag=1;
				auto_move=1;
			}
		}
		else
		{
			top_flag=0;
		}
	}

	if(!GF_BSP_GPIO_ReadIO(0))
	{
		right_limit=0;
		bot_limit=0;
		//侧面工装
		if(!GF_BSP_GPIO_ReadIO(2))
		{
			right_limit=1;
			Robot_Stop();
			//侧面限位动作
			if(right_flag==0)
			{
				Up_Stop();
				Rot_Stop();
				right_flag=1;
				auto_move=1;
			}
		}
		else
		{
			right_flag=0;
		}
		if(!GF_BSP_GPIO_ReadIO(3))
		{
			bot_limit=1;
			Robot_Stop();
			//底面限位动作
			if(bot_flag==0)
			{
				Up_Stop();
				Rot_Stop();
				bot_flag=1;
				auto_move=1;
			}
		}
		else
		{
			bot_flag=0;
		}
	}
	if(!GF_BSP_GPIO_ReadIO(4))
	{
		//底部工装
		right_limit=0;

		if(!GF_BSP_GPIO_ReadIO(2))
		{
			Robot_Stop();
			right_limit=1;
			if(right_flag==0)
			{
				Up_Stop();
				Rot_Stop();
				right_flag=1;
				auto_move=1;
			}
		}
		else
		{
			right_flag=0;
		}
	}

	if((GV.DH_WH_HandSet.EMERGENCY_STOP == 1)||(GV.DH_WH_HandSet.Wireless_Status==0))
	{
		//GF_MainLoop_Jump(1,3); //调试时需要将其隐掉
	}
	else if(GV.DH_WH_HandSet.Mov_Horiz1_vert2==0)
	{
		GF_MainLoop_Jump(1,1); //手动控制
	}
	else
	{
		GF_MainLoop_Jump(1,0); //无操作
	}


	if(GV.DH_WH_HandSet.Mov_Horiz1_vert2==1)
	{
		//Linear_motion_flag = 0;
		switch(Linear_motion_flag)
			{
			case 0:
					GF_MainLoop_Jump(1,0); //无操作
					//GF_MainLoop_Jump(1,7); //设置期望位置
					GF_MainLoop_Jump(1,8); //前进直线运动控制
//					Linear_motion_flag = 1;
				break;
			case 1:
					GF_MainLoop_Jump(1,0); //无操作
					//GF_MainLoop_Jump(1,7); //设置期望位置
					GF_MainLoop_Jump(1,9); //后退直线运动控制
//					Linear_motion_flag = 0;

				break;
			}

//		GF_MainLoop_Jump(1,0); //无操作
//		//GF_MainLoop_Jump(1,7); //设置期望位置
//		GF_MainLoop_Jump(1,8); //前进直线运动控制

//		GF_MainLoop_Jump(1,0); //无操作
//		GF_MainLoop_Jump(1,4); //车头自主前进移动
	}
	else if(GV.DH_WH_HandSet.Mov_Horiz1_vert2==2)
	{

//		GF_MainLoop_Jump(1,0); //无操作
//		//GF_MainLoop_Jump(1,7); //设置期望位置
//		GF_MainLoop_Jump(1,9); //后退直线运动控制

//		GF_MainLoop_Jump(1,0); //无操作
//		GF_MainLoop_Jump(1,6); //车头自主后退移动
	}
//	else
//	{
//		GF_MainLoop_Jump(1,0); //无操+作
//	}




}



void Mobile_Left_Rot_Manual_Con()
{
	GV.Motor_P[4].Run_Mode= DF_MSP_Ti5Motor_SpeedMode;
	GV.Motor_P[4].Target_Velcity=Left_V;
}

void Mobile_Right_Rot_Manual_Con()
{
	GV.Motor_P[4].Run_Mode= DF_MSP_Ti5Motor_SpeedMode;
	GV.Motor_P[4].Target_Velcity=Right_V;
}
void Rot_Stop()
{
	GV.Motor_P[4].Run_Mode= DF_MSP_Ti5Motor_SpeedMode;
	GV.Motor_P[4].Target_Velcity=0;
}
void Mobile_up_Rot_Manual_Con()
{
	GV.Motor_P[5].Run_Mode= DF_MSP_Ti5Motor_SpeedMode;
	GV.Motor_P[5].Target_Velcity=Left_V;
}
void Mobile_down_Rot_Manual_Con()
{
	GV.Motor_P[5].Run_Mode= DF_MSP_Ti5Motor_SpeedMode;
	GV.Motor_P[5].Target_Velcity=Right_V;
}
void Up_Stop()
{
	GV.Motor_P[5].Run_Mode= DF_MSP_Ti5Motor_SpeedMode;
	GV.Motor_P[5].Target_Velcity=0;
}
void Init_Position()
{
	GV.Motor_P[4].Target_Position = 2154;
	GV.Motor_P[4].Run_Mode = DF_MSP_Ti5Motor_PositionMode;
	GV.Motor_P[5].Target_Position = 236614;
	GV.Motor_P[5].Run_Mode = DF_MSP_Ti5Motor_PositionMode;
}


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;
	}
	else if(PV.State_Index_N_Last != GV.State_Index_N)
	{
		GV.State_Index_N_Elapse=0;
	}
}

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

	GV.State_Index_U_Elapse=GV.State_Index_U_Elapse+1;
	GV.State_Index_N_Elapse=GV.State_Index_N_Elapse+1;
	//double Robot_Roll_angle;
//	GV.Motor_P[1].Run_Mode=DF_MSP_Ti5Motor_SpeedMode;
//	GV.Motor_P[1].Target_Velcity=200;

	switch(GV.State_Index_U)
	{
	case -1:						//错误状态
		switch(GV.State_Index_N)
		{
			case 0:
				if((GV.DH_WH_HandSet.EMERGENCY_STOP == 0)||(GV.DH_WH_HandSet.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:
				Motor_Stop_Flag=0;
				Hold_On_Flag=0;
				Robot_hult_Fuction();
				//Water_Cannon_Control();//
				break;
			case 1:
				Motor_Stop_Flag=0;
				Hold_On_Flag=0;
//				Mobile_Robot_Manual_Con_1();
				Mobile_Robot_Manual_Con_2();
				Water_Cannon_Control();
				break;
			case 2:
				Speed_flag=1;
				Motor_Stop_Flag=0;
				Hold_On_Flag=0;
				Robot_hult_Fuction();
				//Water_Cannon_Control();
				break;
			case 3:
				Speed_flag=1;
				Robot_EMERGENCY_STOP_Fuction();
				Hold_On_Flag=0;
				break;
			case 4://自动前进
				Robot_auto_forward();
				break;
			case 5:
				Motor_Stop_Flag=0;
				Hold_On_Flag=0;
//				Mobile_Robot_Manual_Con();
//			    Mobile_Robot_Manual_Con_3();
//				Water_Cannon_Control();
			case 6://自动后退

				Robot_auto_Backward();

			case 7:

				Expectation_Angle = GV.Gyro_MFOG40.Gyro_Angular;  //设置期望位置

				break;

			case 8:

				Robot_auto_forward_gyro();  //陀螺仪矫正方向,前进

				break;
			case 9:

				Robot_auto_Backward_gyro();  //陀螺仪矫正方向，后退

				break;

			default:
				break;
		}
		break;

	default:

		break;
	}
}

double delta_Speed = 0;

void Robot_auto_forward_gyro()
{
	int16_t Gyroscope_Angle = GV.Gyro_MFOG40.Gyro_Angular;
	double Angle_Error = 0;

	uint8_t Rotation_flag = 1;


	int32_t Right_Speed=0;
	int32_t Left_Speed=0;
	double Speed_Scale=((double)GV.DH_WH_HandSet.Mob_Velocity)/255;
	double Pi=1.5707963267948967*2;
	Left_Speed=Speed_Scale*24*10100*6/(Pi*108);
	Right_Speed=Left_Speed;

	Angle_Error = Gyroscope_Angle - Expectation_Angle;

	if(Angle_Error < 0) Rotation_flag = 0;
	if(Angle_Error == 0) Rotation_flag = 1;
	if(Angle_Error > 0) Rotation_flag = 2;

	delta_Speed = PositionalPID(Expectation_Angle,Gyroscope_Angle);
	delta_Speed = fabs(delta_Speed);

	switch(Rotation_flag)
	{
		case 0:
			if(top_limit==0 && bot_limit==0 && right_limit==0 && auto_move==0)
			{
				GV.Motor_P[0].Run_Mode=DF_MSP_Ti5Motor_SpeedMode;
				GV.Motor_P[1].Run_Mode=DF_MSP_Ti5Motor_SpeedMode;
				GV.Motor_P[2].Run_Mode=DF_MSP_Ti5Motor_SpeedMode;
				GV.Motor_P[3].Run_Mode=DF_MSP_Ti5Motor_SpeedMode;

				GV.Motor_P[0].Target_Velcity=Left_Speed - delta_Speed;
				GV.Motor_P[1].Target_Velcity=-(Right_Speed + delta_Speed);
				GV.Motor_P[2].Target_Velcity=Left_Speed - delta_Speed;
				GV.Motor_P[3].Target_Velcity=-(Right_Speed + delta_Speed);
			}
			break;
		case 1:
			if(top_limit==0 && bot_limit==0 && right_limit==0 && auto_move==0)
			{
				GV.Motor_P[0].Run_Mode=DF_MSP_Ti5Motor_SpeedMode;
				GV.Motor_P[1].Run_Mode=DF_MSP_Ti5Motor_SpeedMode;
				GV.Motor_P[2].Run_Mode=DF_MSP_Ti5Motor_SpeedMode;
				GV.Motor_P[3].Run_Mode=DF_MSP_Ti5Motor_SpeedMode;

				GV.Motor_P[0].Target_Velcity=Left_Speed;
				GV.Motor_P[1].Target_Velcity=-Right_Speed;
				GV.Motor_P[2].Target_Velcity=Left_Speed;
				GV.Motor_P[3].Target_Velcity=-Right_Speed;
			}
			break;
		case 2:
			if(top_limit==0 && bot_limit==0 && right_limit==0 && auto_move==0)
			{
				GV.Motor_P[0].Run_Mode=DF_MSP_Ti5Motor_SpeedMode;
				GV.Motor_P[1].Run_Mode=DF_MSP_Ti5Motor_SpeedMode;
				GV.Motor_P[2].Run_Mode=DF_MSP_Ti5Motor_SpeedMode;
				GV.Motor_P[3].Run_Mode=DF_MSP_Ti5Motor_SpeedMode;

				GV.Motor_P[0].Target_Velcity=Left_Speed + delta_Speed;
				GV.Motor_P[1].Target_Velcity=-(Right_Speed - delta_Speed);
				GV.Motor_P[2].Target_Velcity=Left_Speed + delta_Speed;
				GV.Motor_P[3].Target_Velcity=-(Right_Speed - delta_Speed);
			}
			break;
	}
}

void Robot_auto_forward()
{
	int32_t Right_Speed=0;
	int32_t Left_Speed=0;
	double Speed_Scale=((double)GV.DH_WH_HandSet.Mob_Velocity)/255;
	double Pi=1.5707963267948967*2;
	Left_Speed=Speed_Scale*24*10100*6/(Pi*108);
	Right_Speed=Left_Speed;
	if(top_limit==0 && bot_limit==0 && right_limit==0 && auto_move==0)
	{
		GV.Motor_P[0].Run_Mode=DF_MSP_Ti5Motor_SpeedMode;
		GV.Motor_P[1].Run_Mode=DF_MSP_Ti5Motor_SpeedMode;
		GV.Motor_P[2].Run_Mode=DF_MSP_Ti5Motor_SpeedMode;
		GV.Motor_P[3].Run_Mode=DF_MSP_Ti5Motor_SpeedMode;

		GV.Motor_P[0].Target_Velcity=Left_Speed;
		GV.Motor_P[1].Target_Velcity=-Right_Speed;
		GV.Motor_P[2].Target_Velcity=Left_Speed;
		GV.Motor_P[3].Target_Velcity=-Right_Speed;
	}
}

double delta_Speed_1 = 0;

void Robot_auto_Backward_gyro()
{

	int16_t Gyroscope_Angle = GV.Gyro_MFOG40.Gyro_Angular;
	double Angle_Error = 0;

	uint8_t Rotation_flag = 1;



	int32_t Right_Speed=0;
	int32_t Left_Speed=0;
	double Speed_Scale=((double)GV.DH_WH_HandSet.Mob_Velocity)/255;
	double Pi=1.5707963267948967*2;
	Left_Speed=Speed_Scale*24*10100*6/(Pi*108);
	Right_Speed=Left_Speed;

	Angle_Error = Gyroscope_Angle - Expectation_Angle;

	if(Angle_Error < 0) Rotation_flag = 0;
	if(Angle_Error == 0) Rotation_flag = 1;
	if(Angle_Error > 0) Rotation_flag = 2;

	delta_Speed_1 = PositionalPID(Expectation_Angle,Gyroscope_Angle);
	delta_Speed_1 = fabs(delta_Speed_1);

	switch(Rotation_flag)
	{
		case 0:
			if(top_limit==0 && bot_limit==0 && right_limit==0 && auto_move==0)
			{
				GV.Motor_P[0].Run_Mode=DF_MSP_Ti5Motor_SpeedMode;
				GV.Motor_P[1].Run_Mode=DF_MSP_Ti5Motor_SpeedMode;
				GV.Motor_P[2].Run_Mode=DF_MSP_Ti5Motor_SpeedMode;
				GV.Motor_P[3].Run_Mode=DF_MSP_Ti5Motor_SpeedMode;

				GV.Motor_P[0].Target_Velcity=-(Left_Speed + delta_Speed_1);
				GV.Motor_P[1].Target_Velcity=Right_Speed - delta_Speed_1;
				GV.Motor_P[2].Target_Velcity=-(Left_Speed + delta_Speed_1);
				GV.Motor_P[3].Target_Velcity=Right_Speed - delta_Speed_1;
			}
			break;
		case 1:
			if(top_limit==0 && bot_limit==0 && right_limit==0 && auto_move==0)
			{
				GV.Motor_P[0].Run_Mode=DF_MSP_Ti5Motor_SpeedMode;
				GV.Motor_P[1].Run_Mode=DF_MSP_Ti5Motor_SpeedMode;
				GV.Motor_P[2].Run_Mode=DF_MSP_Ti5Motor_SpeedMode;
				GV.Motor_P[3].Run_Mode=DF_MSP_Ti5Motor_SpeedMode;

				GV.Motor_P[0].Target_Velcity=-Left_Speed;
				GV.Motor_P[1].Target_Velcity=Right_Speed;
				GV.Motor_P[2].Target_Velcity=-Left_Speed;
				GV.Motor_P[3].Target_Velcity=Right_Speed;
			}
			break;
		case 2:
			if(top_limit==0 && bot_limit==0 && right_limit==0 && auto_move==0)
			{
				GV.Motor_P[0].Run_Mode=DF_MSP_Ti5Motor_SpeedMode;
				GV.Motor_P[1].Run_Mode=DF_MSP_Ti5Motor_SpeedMode;
				GV.Motor_P[2].Run_Mode=DF_MSP_Ti5Motor_SpeedMode;
				GV.Motor_P[3].Run_Mode=DF_MSP_Ti5Motor_SpeedMode;

				GV.Motor_P[0].Target_Velcity=-(Left_Speed - delta_Speed_1);
				GV.Motor_P[1].Target_Velcity=Right_Speed + delta_Speed_1;
				GV.Motor_P[2].Target_Velcity=-(Left_Speed - delta_Speed_1);
				GV.Motor_P[3].Target_Velcity=Right_Speed + delta_Speed_1;
			}
			break;
	}
}



void Robot_auto_Backward()
{
	int32_t Right_Speed=0;
	int32_t Left_Speed=0;
	double Speed_Scale=((double)GV.DH_WH_HandSet.Mob_Velocity)/255;
	double Pi=1.5707963267948967*2;
	Left_Speed=Speed_Scale*24*10100*6/(Pi*108);
	Right_Speed=Left_Speed;
	if(top_limit==0 && bot_limit==0 && right_limit==0 && auto_move==0)
	{
		GV.Motor_P[0].Run_Mode=DF_MSP_Ti5Motor_SpeedMode;
		GV.Motor_P[1].Run_Mode=DF_MSP_Ti5Motor_SpeedMode;
		GV.Motor_P[2].Run_Mode=DF_MSP_Ti5Motor_SpeedMode;
		GV.Motor_P[3].Run_Mode=DF_MSP_Ti5Motor_SpeedMode;

		GV.Motor_P[0].Target_Velcity=-Left_Speed;
		GV.Motor_P[1].Target_Velcity=Right_Speed;
		GV.Motor_P[2].Target_Velcity=-Left_Speed;
		GV.Motor_P[3].Target_Velcity=Right_Speed;
	}


}

void Robot_hult_Fuction()
{


	int32_t Right_Speed=0;
	int32_t Left_Speed=0;

	GV.Motor_P[0].Run_Mode=DF_MSP_Ti5Motor_SpeedMode;
	GV.Motor_P[1].Run_Mode=DF_MSP_Ti5Motor_SpeedMode;
	GV.Motor_P[2].Run_Mode=DF_MSP_Ti5Motor_SpeedMode;
	GV.Motor_P[3].Run_Mode=DF_MSP_Ti5Motor_SpeedMode;

	GV.Motor_P[0].Target_Velcity=Left_Speed;
	GV.Motor_P[1].Target_Velcity=Right_Speed;
	GV.Motor_P[2].Target_Velcity=Left_Speed;
	GV.Motor_P[3].Target_Velcity=Right_Speed;
//	if(Speed_flag==0)
//	{
//
//	}
//	else if (Speed_flag<=20)
//	{
//		int32_t Right_Speed=0;
//		int32_t Left_Speed=0;
//
//		GV.Motor_P[0].Run_Mode=DF_MSP_Ti5Motor_SpeedMode;
//		GV.Motor_P[1].Run_Mode=DF_MSP_Ti5Motor_SpeedMode;
//		GV.Motor_P[2].Run_Mode=DF_MSP_Ti5Motor_SpeedMode;
//		GV.Motor_P[3].Run_Mode=DF_MSP_Ti5Motor_SpeedMode;
//
//		GV.Motor_P[0].Target_Velcity=Left_Speed;
//		GV.Motor_P[1].Target_Velcity=Right_Speed;
//		GV.Motor_P[2].Target_Velcity=Left_Speed;
//		GV.Motor_P[3].Target_Velcity=Right_Speed;
//		Speed_flag++;
//	}
//	else
//	{
//		GV.Motor_P[0].Run_Mode=0;
//		GV.Motor_P[1].Run_Mode=0;
//		GV.Motor_P[2].Run_Mode=0;
//		GV.Motor_P[3].Run_Mode=0;
//	}

}






void Robot_EMERGENCY_STOP_Fuction()
{
	if(Motor_Stop_Flag<32)
	{
		GV.Motor_P[0].Run_Mode=DF_MSP_Ti5Motor_StopMode;
		GV.Motor_P[1].Run_Mode=DF_MSP_Ti5Motor_StopMode;
		GV.Motor_P[2].Run_Mode=DF_MSP_Ti5Motor_StopMode;
		GV.Motor_P[3].Run_Mode=DF_MSP_Ti5Motor_StopMode;
		Motor_Stop_Flag++;
	}
	else if(Motor_Stop_Flag<200)
	{
		GV.Motor_P[0].Run_Mode=0;
		GV.Motor_P[1].Run_Mode=0;
		GV.Motor_P[2].Run_Mode=0;
		GV.Motor_P[3].Run_Mode=0;
		Motor_Stop_Flag++;
	}
	else if(Motor_Stop_Flag<216)
	{
		GV.Motor_P[0].Run_Mode=DF_MSP_Ti5Motor_Fault_Reset;
		GV.Motor_P[1].Run_Mode=DF_MSP_Ti5Motor_Fault_Reset;
		GV.Motor_P[2].Run_Mode=DF_MSP_Ti5Motor_Fault_Reset;
		GV.Motor_P[3].Run_Mode=DF_MSP_Ti5Motor_Fault_Reset;
		Motor_Stop_Flag++;
	}
	else
	{
		GV.Motor_P[0].Run_Mode=0;
		GV.Motor_P[1].Run_Mode=0;
		GV.Motor_P[2].Run_Mode=0;
		GV.Motor_P[3].Run_Mode=0;
	}

}

void Robot_Hold_On()
{
	if(Hold_On_Flag<16)
	{
		int32_t Right_Speed=0;
		int32_t Left_Speed=0;

		GV.Motor_P[0].Run_Mode=DF_MSP_Ti5Motor_SpeedMode;
		GV.Motor_P[1].Run_Mode=DF_MSP_Ti5Motor_SpeedMode;
		GV.Motor_P[2].Run_Mode=DF_MSP_Ti5Motor_SpeedMode;
		GV.Motor_P[3].Run_Mode=DF_MSP_Ti5Motor_SpeedMode;

		GV.Motor_P[0].Target_Velcity=Left_Speed;
		GV.Motor_P[1].Target_Velcity=Right_Speed;
		GV.Motor_P[2].Target_Velcity=Left_Speed;
		GV.Motor_P[3].Target_Velcity=Right_Speed;
		Hold_On_Flag++;
	}
	else
	{
	GV.Motor_P[0].Run_Mode=0;
	GV.Motor_P[1].Run_Mode=0;
	GV.Motor_P[2].Run_Mode=0;
	GV.Motor_P[3].Run_Mode=0;
	}

}
	double Right_Speed;
	double Left_Speed;
	double Handset_angle;

	double V1_FL=0;
	double V2_FR=0;
	double V3_V4[2]={0,0};

	double V1_V2_V3_V4[4];


int Tar_5_Flag=0;
int32_t Tar_5_Cur_Pos=0;
int32_t Tar_5_Der_Pos=0;
int32_t Motor_5_Velocity=0;
int Speed_1_sca=0;
void Water_Cannon_Control()
{
	int32_t Deg_Count=18386;
	int32_t Actual_Pos=GV.Motor_P[4].Position;
	int Fabs_Y=50;
	Speed_1_sca=(int)((double) GV.DH_WH_HandSet.Mob_Velocity)/25;
	int Fuhao_111= GV.DH_WH_HandSet.Turn_Left_Right_Y;

	GV.Motor_P[3].WC_MotorID=0X10;
	GV.Motor_P[3].WC_Clean_Swi=GV.DH_WH_HandSet.Cleaning_swing;
	GV.Motor_P[3].WC_Scan_Hor1_Ver2=GV.DH_WH_HandSet.Scan_horiz1_vert2;
	GV.Motor_P[3].WC_Rise_down_XX=GV.DH_WH_HandSet.Rise_down_X;
	GV.Motor_P[3].WC_Turn_Left_Right_YY=GV.DH_WH_HandSet.Turn_Left_Right_Y;
	GV.Motor_P[3].WC_Hz_Adjust=60;

	if(GV.DH_WH_HandSet.Scan_horiz1_vert2==0)
	{

		if(Fuhao_111>30)
		{
			GV.Motor_P[4].Target_Velcity=Fabs_Y*Speed_1_sca;
		}
		else if(Fuhao_111<-30)
		{
			GV.Motor_P[4].Target_Velcity=-Fabs_Y*Speed_1_sca;
		}
		else
		{
			GV.Motor_P[4].Target_Velcity=0;
		}

//		if(Fabs_Y>10)
//		{
//			GV.Motor_P[4].Target_Velcity=(int32_t)GV.DH_WH_HandSet.Turn_Left_Right_Y*5*(((double)GV.DH_WH_HandSet.Mob_Velocity)/1000);
//		}

		Tar_5_Flag=0;
	}
	else if (GV.DH_WH_HandSet.Scan_horiz1_vert2==1)
	{
		if(Tar_5_Flag<=2)
		{
			Tar_5_Cur_Pos=GV.Motor_P[4].Position;
			Tar_5_Flag++;
		}
		else
		{
			Tar_5_Der_Pos=Tar_5_Cur_Pos+((double)GV.DH_WH_HandSet.Cleaning_swing)/255*180*Deg_Count;

			if(Actual_Pos>=Tar_5_Der_Pos)
			{
				Motor_5_Velocity=-Fabs_Y*Speed_1_sca;

			}
			else if(Actual_Pos<=Tar_5_Cur_Pos)
			{
				Motor_5_Velocity=Fabs_Y*Speed_1_sca;
			}
			GV.Motor_P[4].Run_Mode=DF_MSP_Ti5Motor_SpeedMode;
			GV.Motor_P[4].Target_Velcity=Motor_5_Velocity;
//			else if((GV.Motor_P[4].Position<Tar_5_Der_Pos))
//			{
//
//			}
//			else
//			{
//				GV.Motor_P[4].Target_Velcity=0;
//			}
		}
	}
	else
	{
		GV.Motor_P[4].Target_Velcity=0;
		Tar_5_Flag=0;
	}
}

double L_R_DIS=0.37;
double F_R_DIS=0.35;


/**
  * @brief  Four wheel rigid body steering model
  * @param  V1 Front left wheel speed per second (m/s)
  * @param  V2   Front right wheel speed per second (m/s)
  * @param 	Robot_Size[0]  Distance between left and right wheels; Robot_Size[1]  Distance between front and rear wheels
  * @retval Vol_Rear_L_R[0] Rear left wheel velocity; Vol_Rear_L_R[1] Rear right wheel velocity;
  */


void Ackerman_Steer_Model_0(double V1,double V2,double Robot_Size[],double *Vol_Rear_L_R)
{
	double Angle_W1=0;
	double Rotation_radius=1;

   if(fabs(V1)>fabs(V2))
   {
	Angle_W1=(fabs(V1)-fabs(V2))/Robot_Size[0];
	Rotation_radius=V2/Angle_W1;

	Vol_Rear_L_R[0]=V1*sqrt((Rotation_radius+Robot_Size[0])*(Rotation_radius+Robot_Size[0])+Robot_Size[1]*Robot_Size[1])/(Rotation_radius+Robot_Size[0]);
	Vol_Rear_L_R[1]=V2*sqrt(Rotation_radius*Rotation_radius+Robot_Size[1]*Robot_Size[1])/Rotation_radius;
   }
   else if(fabs(V1)<fabs(V2))
   {
	Angle_W1=(fabs(V2)-fabs(V1))/Robot_Size[0];
	Rotation_radius=V1/Angle_W1;

	Vol_Rear_L_R[0]=V1*sqrt(Rotation_radius*Rotation_radius+Robot_Size[1]*Robot_Size[1])/Rotation_radius;
	Vol_Rear_L_R[1]=V2*sqrt((Rotation_radius+Robot_Size[0])*(Rotation_radius+Robot_Size[0])+Robot_Size[1]*Robot_Size[1])/(Rotation_radius+Robot_Size[0]);
   }
   else
   {
	   Vol_Rear_L_R[0]=V1;
	   Vol_Rear_L_R[1]=V2;
   }

}








void Mobile_Robot_Manual_Con_2()
{

	double Speed_Scale=((double)GV.DH_WH_HandSet.Mob_Velocity)/255;

	double Pi=1.5707963267948967*2;

	double X_Value=GV.DH_WH_HandSet.For_Back_Value_X;
	double Y_Value=GV.DH_WH_HandSet.Left_Right_Value_Y;

	double X_Value_1=X_Value;

	if(fabs(X_Value)<50)
	{
		X_Value=0;
	}
	if(fabs(Y_Value)<50)
	{
		Y_Value=0;
	}

	double Hand_Scale=sqrt(X_Value*X_Value+Y_Value*Y_Value)/255;
	if (Hand_Scale>1)
	{
		Hand_Scale=1;
	}
	 Handset_angle=atan2(X_Value,Y_Value);


	if((X_Value==0)&&(Y_Value==0))
	{
		Right_Speed=0;
		Left_Speed=0;
	}
	else if((Handset_angle>=0)&&(Handset_angle<=Pi/2) &&(X_Value_1>=0)    )//1.angle in first quadrant,and X>=0 左转
	{
		Left_Speed=Handset_angle/Pi*2*3200+1800;
		Right_Speed=Left_Speed*cos((Pi/2-Handset_angle)*0.7889);
	}
	else if((Handset_angle>Pi/2)&&(Handset_angle<=Pi) &&(X_Value_1>=0))  //右转
	{

		Right_Speed =(Pi-Handset_angle)/Pi*2*3200+1800;
		Left_Speed=Right_Speed*cos((Pi/2-Handset_angle)*0.7889);
	}
	else if((Handset_angle<=-0.19)&(Handset_angle>=-Pi/2))
	{

		Left_Speed=(Handset_angle)/Pi*2*3640-1360;
		Right_Speed=Left_Speed*cos((-Pi/2-Handset_angle)*0.8915);
	}
	else if((Handset_angle<-Pi/2)&&(Handset_angle>=-Pi+0.19))
	{
		Right_Speed=-(Handset_angle+Pi)/Pi*2*3640-1360;
		Left_Speed=Right_Speed*cos((-Pi/2-Handset_angle)*0.8915);
	}
	else
	{
		Right_Speed=0;
		Left_Speed=0;
	}

	GV.Motor_P[0].Run_Mode=DF_MSP_Ti5Motor_SpeedMode;
	GV.Motor_P[1].Run_Mode=DF_MSP_Ti5Motor_SpeedMode;
	GV.Motor_P[2].Run_Mode=DF_MSP_Ti5Motor_SpeedMode;
	GV.Motor_P[3].Run_Mode=DF_MSP_Ti5Motor_SpeedMode;

	// four turn
	V1_FL=fabs(Right_Speed*Hand_Scale*Speed_Scale)*Count_Vel_m_S;
	V2_FR=fabs(Left_Speed*Hand_Scale*Speed_Scale)*Count_Vel_m_S;

	Ackerman_Steer_Model_0(V1_FL,V2_FR,Robot_size_W_H,V3_V4);

	if(Right_Speed*Hand_Scale*Speed_Scale>=0)
	{
		V3_V4[0]=V3_V4[0]*Vel_Count;
		V3_V4[1]=-V3_V4[1]*Vel_Count;
	}
	else
	{
		V3_V4[0]=-V3_V4[0]*Vel_Count;
		V3_V4[1]=V3_V4[1]*Vel_Count;
	}

	GV.Motor_P[0].Target_Velcity=-V3_V4[1];
	GV.Motor_P[1].Target_Velcity=-V3_V4[0];
	GV.Motor_P[2].Target_Velcity=Right_Speed*Hand_Scale*Speed_Scale;
	GV.Motor_P[3].Target_Velcity=-Left_Speed*Hand_Scale*Speed_Scale;

}






//void Ackerman_Steer_Model_1(double V1,double V2,double Robot_Size[],double *Vol_Rear_L_R)
//{
//	double Angle_W1=0;
//	double Rotation_radius=1;
//	double pi=3.141592653589793;
//	double Angle_wheel_4;
//	double Angle_wheel_3;
//   if(fabs(V1)>fabs(V2))
//   {
//	Angle_W1=(fabs(V1)-fabs(V2))/Robot_Size[0];
//	Rotation_radius=V2/Angle_W1;
//
//	Angle_wheel_4=pi/2-atan(Rotation_radius/Robot_Size[1]);
//    Angle_wheel_3=pi/2-atan((Rotation_radius+Robot_Size[0])/Robot_Size[1]);
//
//	Vol_Rear_L_R[0]=V1*sqrt((Rotation_radius+Robot_Size[0])*(Rotation_radius+Robot_Size[0])+Robot_Size[1]*Robot_Size[1])/(Rotation_radius+Robot_Size[0])/cos(Angle_wheel_3);
//	Vol_Rear_L_R[1]=V2*sqrt(Rotation_radius*Rotation_radius+Robot_Size[1]*Robot_Size[1])/Rotation_radius/cos(Angle_wheel_4);
//   }
//   else if(fabs(V1)<fabs(V2))
//   {
//	Angle_W1=(fabs(V2)-fabs(V1))/Robot_Size[0];
//	Rotation_radius=V1/Angle_W1;
//
//    Angle_wheel_3=pi/2-atan(Rotation_radius/Robot_Size[1]);
//    Angle_wheel_4=pi/2-atan((Rotation_radius+Robot_Size[0])/Robot_Size[1]);
//
//	Vol_Rear_L_R[0]=V1*sqrt(Rotation_radius*Rotation_radius+Robot_Size[1]*Robot_Size[1])/Rotation_radius/cos(Angle_wheel_3);
//	Vol_Rear_L_R[1]=V2*sqrt((Rotation_radius+Robot_Size[0])*(Rotation_radius+Robot_Size[0])+Robot_Size[1]*Robot_Size[1])/(Rotation_radius+Robot_Size[0])/cos(Angle_wheel_4);
//   }
//   else
//   {
//	   Vol_Rear_L_R[0]=V1;
//	   Vol_Rear_L_R[1]=V2;
//   }
//
//}






//void Mobile_Robot_Manual_Con()
//{
//
//	double Right_Speed;
//	double Left_Speed;
//
//	double Speed_Scale;
//
//
//	double Basic_speed=200;double  Turn_speed=70;
//	double  Turn_Scale=0.3;
//	Speed_Scale=(GV.DH_WH_HandSet.Mob_Velocity)/10;
//
//	double SPeed_Scale_1;
//
//
//	if(GV.DH_WH_HandSet.>=50)
//	{
//		SPeed_Scale_1=fabs(GV.DH_WH_HandSet.For_Back_Value_X)/255;
//		Right_Speed=Basic_speed*Speed_Scale*SPeed_Scale_1;//机器人前进
//		Left_Speed=-Right_Speed;
//	}
//	else if (GV.DH_WH_HandSet.For_Back_Value_X<=-50)
//	{
//		SPeed_Scale_1=fabs(GV.DH_WH_HandSet.For_Back_Value_X)/255;
//		Right_Speed=-1*Basic_speed*Speed_Scale*SPeed_Scale_1;//机器人前进
//		Left_Speed=-1*Right_Speed;
//	}
//	else if(GV.DH_WH_HandSet.Left_Right_Value_Y>=50)
//	{
//		SPeed_Scale_1=fabs(GV.DH_WH_HandSet.Left_Right_Value_Y)/255;
//
//		Left_Speed=-1*Turn_speed*Speed_Scale*SPeed_Scale_1;
//		Right_Speed=-Left_Speed*Turn_Scale;
//
////		Left_Speed=-1*Right_Speed*Turn_Scale;
////
////		Right_Speed=-1*Turn_speed*Speed_Scale*SPeed_Scale_1;//机器人前进
//
//	}
//	else if(GV.DH_WH_HandSet.Left_Right_Value_Y<=-50)
//	{
//		SPeed_Scale_1=fabs(GV.DH_WH_HandSet.Left_Right_Value_Y)/255;
//		Right_Speed=1*Turn_speed*Speed_Scale*SPeed_Scale_1;//机器人前进
//		Left_Speed=-1*Right_Speed*Turn_Scale;
//	}
//	else
//	{
//		Left_Speed=0;
//		Right_Speed=0;
//	}
//
//	GV.Motor_P[0].Run_Mode=DF_MSP_Ti5Motor_SpeedMode;
//	GV.Motor_P[1].Run_Mode=DF_MSP_Ti5Motor_SpeedMode;
//	GV.Motor_P[2].Run_Mode=DF_MSP_Ti5Motor_SpeedMode;
//	GV.Motor_P[3].Run_Mode=DF_MSP_Ti5Motor_SpeedMode;
//
//	GV.Motor_P[0].Target_Velcity=Left_Speed;
//	GV.Motor_P[1].Target_Velcity=Right_Speed;
//	GV.Motor_P[2].Target_Velcity=Right_Speed;
//	GV.Motor_P[3].Target_Velcity=Left_Speed;
//}





void Mobile_Robot_Manual_Con_1()  //全向转弯，左侧两轮速度相同，右侧两轮速度相同
{
	if(top_limit==0 && bot_limit==0 && right_limit==0)
	{
		double Speed_Scale=((double)GV.DH_WH_HandSet.Mob_Velocity)/255;

		double Pi=1.5707963267948967*2;

		double X_Value=GV.DH_WH_HandSet.For_Back_Value_X;
		double Y_Value=GV.DH_WH_HandSet.Left_Right_Value_Y;

		double X_Value_1=X_Value;

		if(fabs(X_Value)<50)
		{
			X_Value=0;
		}
		if(fabs(Y_Value)<50)
		{
			Y_Value=0;
		}

		double Hand_Scale=sqrt(X_Value*X_Value+Y_Value*Y_Value)/255;
		if (Hand_Scale>1)
		{
			Hand_Scale=1;
		}
		 Handset_angle=atan2(X_Value,Y_Value);


		if((X_Value==0)&&(Y_Value==0))
		{
			Right_Speed=0;
			Left_Speed=0;
		}
		else if((Handset_angle>=0)&&(Handset_angle<=Pi/2) &&(X_Value_1>=0)    )//1.angle in first quadrant,and X>=0
		{
	//		Left_Speed=Handset_angle/Pi*2*3200+1800;
	//		Right_Speed=Left_Speed*cos((Pi/2-Handset_angle)*0.7889);

			Right_Speed=Handset_angle/Pi*2*3200+1800;
			Left_Speed=Right_Speed*cos((Pi/2-Handset_angle)*0.7889);
		}
		else if((Handset_angle>Pi/2)&&(Handset_angle<=Pi) &&(X_Value_1>=0))
		{

	//		Right_Speed = (Pi-Handset_angle)/Pi*2*3200+1800;
	//		Left_Speed = Right_Speed*cos((Pi/2-Handset_angle)*0.7889);
			Left_Speed = (Pi-Handset_angle)/Pi*2*3200+1800;
			Right_Speed = Left_Speed*cos((Pi/2-Handset_angle)*0.7889);

		}
		else if((Handset_angle<=-0.19)&(Handset_angle>=-Pi/2))
		{

			Right_Speed=(Handset_angle)/Pi*2*3640-1360;
			Left_Speed=Right_Speed*cos((-Pi/2-Handset_angle)*0.8915);
		}
		else if((Handset_angle<-Pi/2)&&(Handset_angle>=-Pi+0.19))
		{
			Left_Speed=(Handset_angle+Pi/2)/Pi*2*3640-1360;
			Right_Speed=Left_Speed*cos((-Pi/2-Handset_angle)*0.8915);
		}

		else
		{
			Right_Speed=0;
			Left_Speed=0;
		}

		GV.Motor_P[0].Run_Mode=DF_MSP_Ti5Motor_SpeedMode;
		GV.Motor_P[1].Run_Mode=DF_MSP_Ti5Motor_SpeedMode;
		GV.Motor_P[2].Run_Mode=DF_MSP_Ti5Motor_SpeedMode;
		GV.Motor_P[3].Run_Mode=DF_MSP_Ti5Motor_SpeedMode;

		GV.Motor_P[0].Target_Velcity=Left_Speed*Hand_Scale*Speed_Scale;
		GV.Motor_P[1].Target_Velcity=-Right_Speed*Hand_Scale*Speed_Scale;
		GV.Motor_P[2].Target_Velcity=Left_Speed*Hand_Scale*Speed_Scale;
		GV.Motor_P[3].Target_Velcity=-Right_Speed*Hand_Scale*Speed_Scale;
		// four turn
	}

}

void Robot_Stop()
{
	int32_t Right_Speed=0;
	int32_t Left_Speed=0;

	GV.Motor_P[0].Run_Mode=DF_MSP_Ti5Motor_SpeedMode;
	GV.Motor_P[1].Run_Mode=DF_MSP_Ti5Motor_SpeedMode;
	GV.Motor_P[2].Run_Mode=DF_MSP_Ti5Motor_SpeedMode;
	GV.Motor_P[3].Run_Mode=DF_MSP_Ti5Motor_SpeedMode;

	GV.Motor_P[0].Target_Velcity=Left_Speed;
	GV.Motor_P[1].Target_Velcity=Right_Speed;
	GV.Motor_P[2].Target_Velcity=Left_Speed;
	GV.Motor_P[3].Target_Velcity=Right_Speed;
}

