/*
 *
 *后右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;
int Tar_5_Flag = 0;
int32_t Actual_Pos;
int Tar_5_Flag1 = 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;

int speed_max = 5000;

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 Downward_flag = 0;
uint8_t Forward_flag = 0;

int dir = 0;
double Speed_Scale;
double dir1 = 0;
double dir2 = 0;
double dir3 = 0;
double dir4 = 0;

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(2, (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_MK32_Init(&huart5,(SP_MSP_MK32_Button*)&GV.MK32);
	_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_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)
	{


	}
	else if(GV.DH_WH_HandSet.Mov_Horiz1_vert2==2)
	{
		GF_MainLoop_Jump(1,8);                   //竖直直线运动
	}
	else
	{

	}
}

void UpwardAutomaticWorking()
{

}



void DownwardAutomaticWorking()
{
	switch(Downward_flag)
	{
		case 0:
			int32_t Deg_Count=18386;
			int Fabs_Y = 100;
			Actual_Pos=GV.Motor_P[4].Position;
			Speed_1_sca=(int)((double) GV.DH_WH_HandSet.Mob_Velocity)/25;

			if(Tar_5_Flag1 <= 2)
			{
				Tar_5_Cur_Pos = GV.Motor_P[4].Position;
				Tar_5_Flag1 ++;
			}

			Tar_5_Der_Pos=Tar_5_Cur_Pos+((double)GV.DH_WH_HandSet.Cleaning_swing)/255*180*Deg_Count;
			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;

			if((Actual_Pos >= (Tar_5_Der_Pos - 5000)) && (Actual_Pos <= (Tar_5_Der_Pos + 5000)))
			{
				GV.Motor_P[4].Run_Mode = DF_MSP_Ti5Motor_SpeedMode;
				GV.Motor_P[4].Target_Velcity = 0;

				AutomaticBackward1();

			}
			break;
		case 1:

			GV.Motor_P[4].Run_Mode = DF_MSP_Ti5Motor_SpeedMode;
			GV.Motor_P[4].Target_Velcity = -Motor_5_Velocity;

			if((Actual_Pos <= (Tar_5_Cur_Pos + 5000)) && (Actual_Pos >= (Tar_5_Cur_Pos - 5000)))
			{
				GV.Motor_P[4].Run_Mode = DF_MSP_Ti5Motor_SpeedMode;
				GV.Motor_P[4].Target_Velcity = 0;

				AutomaticBackward2();
			}


			break;
	}
}

double Robot_back_distance = 20;                //单位厘米
uint32_t back_time = 0;
double robot_back_time = 0;

void AutomaticBackward1()
{
	back_time ++;
	robot_back_time = (Robot_back_distance / 100 / 2) * 60000 / 2;             //后退速度2米/分钟

//	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 = -360;
//	GV.Motor_P[1].Target_Velcity = 360;
//	GV.Motor_P[2].Target_Velcity = -360;
//	GV.Motor_P[3].Target_Velcity = 360;

	Linear_motion_function_back1(GV.EX_RF_TL720D.RF_Angle_Roll / 100.0 , 0);

	if(back_time >= robot_back_time)
	{
		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 = 0;
		GV.Motor_P[1].Target_Velcity = 0;
		GV.Motor_P[2].Target_Velcity = 0;
		GV.Motor_P[3].Target_Velcity = 0;
		Downward_flag = 1;
		back_time = 0;
	}

}
uint32_t for_time = 0;
double robot_forward_time = 0;
double Robot_forward_distance = 20;                //单位厘米

void AutomaticForward1()
{
	for_time ++;
	robot_forward_time = (Robot_forward_distance / 100 / 2) * 60000 / 2;             //后退速度2米/分钟

	Linear_motion_function_forward1(GV.EX_RF_TL720D.RF_Angle_Roll / 100.0 , 0);

	if(for_time >= robot_forward_time)
	{
		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 = 0;
		GV.Motor_P[1].Target_Velcity = 0;
		GV.Motor_P[2].Target_Velcity = 0;
		GV.Motor_P[3].Target_Velcity = 0;
		Forward_flag = 1;
		for_time = 0;
	}

}

void AutomaticForward2()
{
	for_time ++;
	robot_forward_time = (Robot_forward_distance / 100 / 2) * 60000 / 2;             //后退速度2米/分钟

	Linear_motion_function_forward1(GV.EX_RF_TL720D.RF_Angle_Roll / 100.0 , 0);

	if(for_time >= robot_forward_time)
	{
		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 = 0;
		GV.Motor_P[1].Target_Velcity = 0;
		GV.Motor_P[2].Target_Velcity = 0;
		GV.Motor_P[3].Target_Velcity = 0;
		Forward_flag = 0;
		for_time = 0;
	}

}



void AutomaticBackward2()
{
	back_time ++;
	robot_back_time = (Robot_back_distance / 100 / 2) * 60000 / 2;            //后退速度2米/分钟

//	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 = -360;
//	GV.Motor_P[1].Target_Velcity = 360;
//	GV.Motor_P[2].Target_Velcity = -360;
//	GV.Motor_P[3].Target_Velcity = 360;

	Linear_motion_function_back1(GV.EX_RF_TL720D.RF_Angle_Roll / 100.0 , 0);

	if(back_time >= robot_back_time)
	{
		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 = 0;
		GV.Motor_P[1].Target_Velcity = 0;
		GV.Motor_P[2].Target_Velcity = 0;
		GV.Motor_P[3].Target_Velcity = 0;
		Downward_flag = 0;
		back_time = 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:

				Linear_motion_function();        //竖直直线运动

				break;
			case 9:

				//DownwardAutomaticWorking();      //后退自动作业

				break;

			default:
				break;
		}
		break;

	default:

		break;
	}
}

double ExpectationAngle=0;
double deltaAngle = 0;
double deltaSpeed = 0;

int32_t deltaAngle1 = 2;
int32_t deltaAngle2 = 5;
int32_t deltaAngle3 = 30;
int32_t deltaAngle4 = 100;

int dir_ = 0;
double Speed_Scale1;
double Handset_angle1;
double dir_1 = 0;
double dir_2 = 0;
double dir_3 = 0;
double dir_4 = 0;
int speed_max1 = 5000;

void Linear_motion_function()
{
	Speed_Scale1=((double)GV.DH_WH_HandSet.Mob_Velocity)/255;
	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_angle1=atan2(X_Value,Y_Value);


	if((X_Value==0)&&(Y_Value==0))
	{
		dir_ = 0;
		dir_1 = 0;
		dir_2 = 0;
		dir_3 = 0;
		dir_4 = 0;
	}
	else if(Y_Value > 100)//1.angle in first quadrant,and X>=0 左转
	{
		if(Handset_angle1 > -0.8 && (Handset_angle1 < 0.8))
		{
			dir_ = 3;
			dir_1 = 0.25;
			dir_2 = -0.5;
			dir_3 = 0.25;
			dir_4 = -0.5;
			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=dir_1*speed_max1*Speed_Scale1;
			GV.Motor_P[1].Target_Velcity=dir_2*speed_max1*Speed_Scale1;
			GV.Motor_P[2].Target_Velcity=dir_3*speed_max1*Speed_Scale1;
			GV.Motor_P[3].Target_Velcity=dir_4*speed_max1*Speed_Scale1;
		}
	}
	else if(Y_Value < -100)  //右转
	{
		if(((Handset_angle1>2.4) && (Handset_angle1 < 3.2))
				|| ((Handset_angle1 <-2.4) && (Handset_angle1 > -3.14)))
		{
			dir_ = 1;
			dir_1 = 0.5;
			dir_2 = -0.25;
			dir_3 = 0.5;
			dir_4 = -0.25;

			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=dir_1*speed_max1*Speed_Scale1;
			GV.Motor_P[1].Target_Velcity=dir_2*speed_max1*Speed_Scale1;
			GV.Motor_P[2].Target_Velcity=dir_3*speed_max1*Speed_Scale1;
			GV.Motor_P[3].Target_Velcity=dir_4*speed_max1*Speed_Scale1;
		}

	}
	else if(X_Value > 100)//前进
	{
		if((Handset_angle1>0.9) && (Handset_angle1<2.3))
		{
			dir_ =4;
			dir_1 = 1;
			dir_2 = -1;
			dir_3 = 1;
			dir_4 = -1;
			Linear_motion_function_forward(GV.EX_RF_TL720D.RF_Angle_Roll / 100.0 , 0);              //注意倾角仪数据是否需要除以100
		}
	}
	else if(X_Value < 10)//后退
	{
		if((Handset_angle1<-0.9) && (Handset_angle1 > -2.3))
		{
			dir_ = 2;
			dir_1 = -1;
			dir_2 = 1;
			dir_3 = -1;
			dir_4 = 1;
			Linear_motion_function_back(GV.EX_RF_TL720D.RF_Angle_Roll / 100.0 , 0);
		}

	}
	else
	{
		dir_1 = 0;
		dir_2 = 0;
		dir_3 = 0;
		dir_4 = 0;
	}

}
int32_t KP1 = 1;/**直线纠偏*/
int32_t KP2 = 1;
double KP3 = 0.5;
double KP4 = 0.2;

void Linear_motion_function_forward(double InclinometerAngle, double offsetAngle)
{
	Speed_Scale1=((double)GV.DH_WH_HandSet.Mob_Velocity)/255;
	//垂直方向
	if ((InclinometerAngle >= 0 && InclinometerAngle < 45) || (InclinometerAngle > -45 && InclinometerAngle <= 0))
	{
		ExpectationAngle = 0 + offsetAngle;

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

	}

	deltaAngle=GV.EX_RF_TL720D.RF_Angle_Roll / 100.0 - ExpectationAngle;     //注意倾角仪的数据是否需要除以100
	deltaSpeed=PositionalPID(ExpectationAngle, GV.EX_RF_TL720D.RF_Angle_Roll / 100.0);  //下面纠偏的前提是右转deltaSpeed为正

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

	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;

	 if (deltaAngle >= 0 && deltaAngle <= deltaAngle1)
	 {
		GV.Motor_P[0].Target_Velcity=dir_1*(speed_max1*Speed_Scale1 + KP1 * deltaSpeed);
		GV.Motor_P[1].Target_Velcity=dir_2*(speed_max1*Speed_Scale1 - KP1 * deltaSpeed);
		GV.Motor_P[2].Target_Velcity=dir_3*(speed_max1*Speed_Scale1 + KP1 * deltaSpeed);
		GV.Motor_P[3].Target_Velcity=dir_4*(speed_max1*Speed_Scale1 - KP1 * deltaSpeed);
//		 GV.LeftFrontMotor.Target_Velcity= GV.Move_Speed + KP1 * deltaSpeed;
//		 GV.RightFrontMotor.Target_Velcity = -(GV.Move_Speed - KP1 * deltaSpeed);
	 }
	 if (deltaAngle > deltaAngle1 && deltaAngle <= deltaAngle2)
	 {
		GV.Motor_P[0].Target_Velcity=dir_1*(speed_max1*Speed_Scale1 + KP2 * deltaSpeed);
		GV.Motor_P[1].Target_Velcity=dir_2*(speed_max1*Speed_Scale1 - KP2 * deltaSpeed);
		GV.Motor_P[2].Target_Velcity=dir_3*(speed_max1*Speed_Scale1 + KP2 * deltaSpeed);
		GV.Motor_P[3].Target_Velcity=dir_4*(speed_max1*Speed_Scale1 - KP2 * deltaSpeed);
	 }
	 if (deltaAngle > deltaAngle2 && deltaAngle <= deltaAngle3)
	 {
		GV.Motor_P[0].Target_Velcity=dir_1*(speed_max1*Speed_Scale1 + KP3 * deltaSpeed);
		GV.Motor_P[1].Target_Velcity=dir_2*(speed_max1*Speed_Scale1 - KP3 * deltaSpeed);
		GV.Motor_P[2].Target_Velcity=dir_3*(speed_max1*Speed_Scale1 + KP3 * deltaSpeed);
		GV.Motor_P[3].Target_Velcity=dir_4*(speed_max1*Speed_Scale1 - KP3 * deltaSpeed);
	 }
	 if (deltaAngle > deltaAngle3 && deltaAngle <= 90)
	 {
		GV.Motor_P[0].Target_Velcity=dir_1*(speed_max1*Speed_Scale1 + KP4 * deltaSpeed);
		GV.Motor_P[1].Target_Velcity=dir_2*(speed_max1*Speed_Scale1 - KP4 * deltaSpeed);
		GV.Motor_P[2].Target_Velcity=dir_3*(speed_max1*Speed_Scale1 + KP4 * deltaSpeed);
		GV.Motor_P[3].Target_Velcity=dir_4*(speed_max1*Speed_Scale1 - KP4 * deltaSpeed);
	 }
}

void Linear_motion_function_forward1(double InclinometerAngle, double offsetAngle)
{
	Speed_Scale1=((double)GV.DH_WH_HandSet.Mob_Velocity)/255;
	//垂直方向
	if ((InclinometerAngle >= 0 && InclinometerAngle < 45) || (InclinometerAngle > -45 && InclinometerAngle <= 0))
	{
		ExpectationAngle = 0 + offsetAngle;

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

	}

	deltaAngle=GV.EX_RF_TL720D.RF_Angle_Roll / 100.0 - ExpectationAngle;     //注意倾角仪的数据是否需要除以100
	deltaSpeed=PositionalPID(ExpectationAngle, GV.EX_RF_TL720D.RF_Angle_Roll / 100.0);  //下面纠偏的前提是右转deltaSpeed为正

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

	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;

	 if (deltaAngle >= 0 && deltaAngle <= deltaAngle1)
	 {
		GV.Motor_P[0].Target_Velcity=(360 + KP1 * deltaSpeed);
		GV.Motor_P[1].Target_Velcity=-(360 - KP1 * deltaSpeed);
		GV.Motor_P[2].Target_Velcity=(360 + KP1 * deltaSpeed);
		GV.Motor_P[3].Target_Velcity=-(360 - KP1 * deltaSpeed);
//		 GV.LeftFrontMotor.Target_Velcity= GV.Move_Speed + KP1 * deltaSpeed;
//		 GV.RightFrontMotor.Target_Velcity = -(GV.Move_Speed - KP1 * deltaSpeed);
	 }
	 if (deltaAngle > deltaAngle1 && deltaAngle <= deltaAngle2)
	 {
		GV.Motor_P[0].Target_Velcity=(360 + KP2 * deltaSpeed);
		GV.Motor_P[1].Target_Velcity=-(360 - KP2 * deltaSpeed);
		GV.Motor_P[2].Target_Velcity=(360 + KP2 * deltaSpeed);
		GV.Motor_P[3].Target_Velcity=-(360 - KP2 * deltaSpeed);
	 }
	 if (deltaAngle > deltaAngle2 && deltaAngle <= deltaAngle3)
	 {
		GV.Motor_P[0].Target_Velcity=(360 + KP3 * deltaSpeed);
		GV.Motor_P[1].Target_Velcity=-(360 - KP3 * deltaSpeed);
		GV.Motor_P[2].Target_Velcity=(360 + KP3 * deltaSpeed);
		GV.Motor_P[3].Target_Velcity=-(360 - KP3 * deltaSpeed);
	 }
	 if (deltaAngle > deltaAngle3 && deltaAngle <= 90)
	 {
		GV.Motor_P[0].Target_Velcity=(360 + KP4 * deltaSpeed);
		GV.Motor_P[1].Target_Velcity=-(360 - KP4 * deltaSpeed);
		GV.Motor_P[2].Target_Velcity=(360 + KP4 * deltaSpeed);
		GV.Motor_P[3].Target_Velcity=-(360 - KP4 * deltaSpeed);
	 }
}




void Linear_motion_function_back(double InclinometerAngle, double offsetAngle)
{
	Speed_Scale1=((double)GV.DH_WH_HandSet.Mob_Velocity)/255;
	//垂直方向
	if ((InclinometerAngle >= 0 && InclinometerAngle < 45) || (InclinometerAngle > -45 && InclinometerAngle <= 0))
	{
		ExpectationAngle = 0 + offsetAngle;

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

	}

	deltaAngle=GV.EX_RF_TL720D.RF_Angle_Roll / 100.0 - ExpectationAngle;     //注意倾角仪的数据是否需要除以100
	deltaSpeed=PositionalPID(ExpectationAngle, GV.EX_RF_TL720D.RF_Angle_Roll / 100.0);

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

	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;

	 if (deltaAngle >= 0 && deltaAngle <= deltaAngle1)
	 {
		GV.Motor_P[0].Target_Velcity=dir_1*(speed_max1*Speed_Scale1 - KP1 * deltaSpeed);
		GV.Motor_P[1].Target_Velcity=dir_2*(speed_max1*Speed_Scale1 + KP1 * deltaSpeed);
		GV.Motor_P[2].Target_Velcity=dir_3*(speed_max1*Speed_Scale1 - KP1 * deltaSpeed);
		GV.Motor_P[3].Target_Velcity=dir_4*(speed_max1*Speed_Scale1 + KP1 * deltaSpeed);
//		 GV.LeftFrontMotor.Target_Velcity= GV.Move_Speed + KP1 * deltaSpeed;
//		 GV.RightFrontMotor.Target_Velcity = -(GV.Move_Speed - KP1 * deltaSpeed);
	 }
	 if (deltaAngle > deltaAngle1 && deltaAngle <= deltaAngle2)
	 {
		GV.Motor_P[0].Target_Velcity=dir_1*(speed_max1*Speed_Scale1 - KP2 * deltaSpeed);
		GV.Motor_P[1].Target_Velcity=dir_2*(speed_max1*Speed_Scale1 + KP2 * deltaSpeed);
		GV.Motor_P[2].Target_Velcity=dir_3*(speed_max1*Speed_Scale1 - KP2 * deltaSpeed);
		GV.Motor_P[3].Target_Velcity=dir_4*(speed_max1*Speed_Scale1 + KP2 * deltaSpeed);
	 }
	 if (deltaAngle > deltaAngle2 && deltaAngle <= deltaAngle3)
	 {
		GV.Motor_P[0].Target_Velcity=dir_1*(speed_max1*Speed_Scale1 - KP3 * deltaSpeed);
		GV.Motor_P[1].Target_Velcity=dir_2*(speed_max1*Speed_Scale1 + KP3 * deltaSpeed);
		GV.Motor_P[2].Target_Velcity=dir_3*(speed_max1*Speed_Scale1 - KP3 * deltaSpeed);
		GV.Motor_P[3].Target_Velcity=dir_4*(speed_max1*Speed_Scale1 + KP3 * deltaSpeed);
	 }
	 if (deltaAngle > deltaAngle3 && deltaAngle <= 90)
	 {
		GV.Motor_P[0].Target_Velcity=dir_1*(speed_max1*Speed_Scale1 - KP4 * deltaSpeed);
		GV.Motor_P[1].Target_Velcity=dir_2*(speed_max1*Speed_Scale1 + KP4 * deltaSpeed);
		GV.Motor_P[2].Target_Velcity=dir_3*(speed_max1*Speed_Scale1 - KP4 * deltaSpeed);
		GV.Motor_P[3].Target_Velcity=dir_4*(speed_max1*Speed_Scale1 + KP4 * deltaSpeed);
	 }

}

void Linear_motion_function_back1(double InclinometerAngle, double offsetAngle)
{
	Speed_Scale1=((double)GV.DH_WH_HandSet.Mob_Velocity)/255;
	//垂直方向
	if ((InclinometerAngle >= 0 && InclinometerAngle < 45) || (InclinometerAngle > -45 && InclinometerAngle <= 0))
	{
		ExpectationAngle = 0 + offsetAngle;

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

	}

	deltaAngle=GV.EX_RF_TL720D.RF_Angle_Roll / 100.0 - ExpectationAngle;     //注意倾角仪的数据是否需要除以100
	deltaSpeed=PositionalPID(ExpectationAngle, GV.EX_RF_TL720D.RF_Angle_Roll / 100.0);

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

	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;

	 if (deltaAngle >= 0 && deltaAngle <= deltaAngle1)
	 {
		GV.Motor_P[0].Target_Velcity=-(360 - KP1 * deltaSpeed);
		GV.Motor_P[1].Target_Velcity=(360 + KP1 * deltaSpeed);
		GV.Motor_P[2].Target_Velcity=-(360 - KP1 * deltaSpeed);
		GV.Motor_P[3].Target_Velcity=(360 + KP1 * deltaSpeed);
//		 GV.LeftFrontMotor.Target_Velcity= GV.Move_Speed + KP1 * deltaSpeed;
//		 GV.RightFrontMotor.Target_Velcity = -(GV.Move_Speed - KP1 * deltaSpeed);
	 }
	 if (deltaAngle > deltaAngle1 && deltaAngle <= deltaAngle2)
	 {
		GV.Motor_P[0].Target_Velcity=-(360 - KP2 * deltaSpeed);
		GV.Motor_P[1].Target_Velcity=(360 + KP2 * deltaSpeed);
		GV.Motor_P[2].Target_Velcity=-(360 - KP2 * deltaSpeed);
		GV.Motor_P[3].Target_Velcity=(360 + KP2 * deltaSpeed);
	 }
	 if (deltaAngle > deltaAngle2 && deltaAngle <= deltaAngle3)
	 {
		GV.Motor_P[0].Target_Velcity=-(360 - KP3 * deltaSpeed);
		GV.Motor_P[1].Target_Velcity=(360 + KP3 * deltaSpeed);
		GV.Motor_P[2].Target_Velcity=-(360 - KP3 * deltaSpeed);
		GV.Motor_P[3].Target_Velcity=(360 + KP3 * deltaSpeed);
	 }
	 if (deltaAngle > deltaAngle3 && deltaAngle <= 90)
	 {
		GV.Motor_P[0].Target_Velcity=-(360 - KP4 * deltaSpeed);
		GV.Motor_P[1].Target_Velcity=(360 + KP4 * deltaSpeed);
		GV.Motor_P[2].Target_Velcity=-(360 - KP4 * deltaSpeed);
		GV.Motor_P[3].Target_Velcity=(360 + KP4 * deltaSpeed);
	 }

}

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



void Water_Cannon_Control()
{
	int32_t Deg_Count=18386;
	Actual_Pos=GV.Motor_P[4].Position;
	int Fabs_Y = 100;
	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;
		}

		Tar_5_Flag=0;
		Downward_flag = 0;
		Forward_flag = 0;
	}
	else if (GV.DH_WH_HandSet.Scan_horiz1_vert2==2)        //后退自动作业
	{
		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;

			switch(Downward_flag)
			{
				case 0:
					GV.Motor_P[4].Run_Mode = DF_MSP_Ti5Motor_SpeedMode;
					GV.Motor_P[4].Target_Velcity = Fabs_Y*Speed_1_sca;

					if((Actual_Pos >= (Tar_5_Der_Pos - 40000)) && (Actual_Pos <= (Tar_5_Der_Pos + 40000)))
					{
						GV.Motor_P[4].Run_Mode = DF_MSP_Ti5Motor_SpeedMode;
						GV.Motor_P[4].Target_Velcity = 0;
						AutomaticBackward1();
					}
					break;
				case 1:
					GV.Motor_P[4].Run_Mode = DF_MSP_Ti5Motor_SpeedMode;
					GV.Motor_P[4].Target_Velcity = -Fabs_Y*Speed_1_sca;
					if((Actual_Pos<=(Tar_5_Cur_Pos + 40000)) && (Actual_Pos>=(Tar_5_Cur_Pos - 40000)))
					{
						GV.Motor_P[4].Run_Mode = DF_MSP_Ti5Motor_SpeedMode;
						GV.Motor_P[4].Target_Velcity = 0;
						AutomaticBackward2();
					}
					break;
			}
		}
	}
	else                                                   //前进自动作业
	{
		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;

			switch(Forward_flag)
			{
				case 0:
					GV.Motor_P[4].Run_Mode = DF_MSP_Ti5Motor_SpeedMode;
					GV.Motor_P[4].Target_Velcity = Fabs_Y*Speed_1_sca;

					if((Actual_Pos >= (Tar_5_Der_Pos - 40000)) && (Actual_Pos <= (Tar_5_Der_Pos + 40000)))
					{
						GV.Motor_P[4].Run_Mode = DF_MSP_Ti5Motor_SpeedMode;
						GV.Motor_P[4].Target_Velcity = 0;
						AutomaticForward1();
					}
					break;
				case 1:
					GV.Motor_P[4].Run_Mode = DF_MSP_Ti5Motor_SpeedMode;
					GV.Motor_P[4].Target_Velcity = -Fabs_Y*Speed_1_sca;
					if((Actual_Pos<=(Tar_5_Cur_Pos + 40000)) && (Actual_Pos>=(Tar_5_Cur_Pos - 40000)))
					{
						GV.Motor_P[4].Run_Mode = DF_MSP_Ti5Motor_SpeedMode;
						GV.Motor_P[4].Target_Velcity = 0;
						AutomaticForward2();
					}
					break;
			}
		}
	}
}

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()
{

	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))
	{
		dir = 0;
		dir1 = 0;
		dir2 = 0;
		dir3 = 0;
		dir4 = 0;
//		Right_Speed=0;
//		Left_Speed=0;
	}
	else if(Y_Value > 100)//1.angle in first quadrant,and X>=0 左转
	{
		if(Handset_angle > -0.8 && (Handset_angle < 0.8))
		{
			dir = 3;
			dir1 = 0.25;
			dir2 = -0.5;
			dir3 = 0.25;
			dir4 = -0.5;
		}

//		Left_Speed=Handset_angle/Pi*2*3200+1800;
//		Right_Speed=Left_Speed * 2;
//		Right_Speed=Left_Speed*cos((Pi/2-Handset_angle)*0.7889);
	}
	else if(Y_Value < -100)  //右转
	{
		if(((Handset_angle>2.4) && (Handset_angle < 3.2))
				|| ((Handset_angle <-2.4) && (Handset_angle > -3.14)))
		{
			dir = 1;
			dir1 = 0.5;
			dir2 = -0.25;
			dir3 = 0.5;
			dir4 = -0.25;
		}

//		Left_Speed=(Handset_angle)/Pi*2*3640-1360;
//		Right_Speed=Left_Speed*cos((-Pi/2-Handset_angle)*0.8915);
	}
	else if(X_Value > 100)//前进
	{
		if((Handset_angle>0.9) && (Handset_angle<2.3))
		{
			dir =4;
			dir1 = 1;
			dir2 = -1;
			dir3 = 1;
			dir4 = -1;
		}
//		Right_Speed =(Pi-Handset_angle)/Pi*2*3200+1800;
//		Left_Speed=Right_Speed*cos((Pi/2-Handset_angle)*0.7889);

	}
	else if(X_Value < 10)//后退
	{
		if((Handset_angle<-0.9) && (Handset_angle > -2.3))
		{
			dir = 2;
			dir1 = -1;
			dir2 = 1;
			dir3 = -1;
			dir4 = 1;
		}

//		Right_Speed=-(Handset_angle+Pi)/Pi*2*3640-1360;
//		Left_Speed=Right_Speed*cos((-Pi/2-Handset_angle)*0.8915);
	}
	else
	{
		dir1 = 0;
		dir2 = 0;
		dir3 = 0;
		dir4 = 0;
//		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=dir1*speed_max*Speed_Scale;
	GV.Motor_P[1].Target_Velcity=dir2*speed_max*Speed_Scale;
	GV.Motor_P[2].Target_Velcity=dir3*speed_max*Speed_Scale;
	GV.Motor_P[3].Target_Velcity=dir4*speed_max*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;
}

