/*
 * robot_state.c
 *
 *  Created on: Aug 2, 2024
 *      Author: akeguo
 */
#include "robot_state.h"
#include "bsp_GPIO.h"
#include <stdio.h>
#include <math.h>
//Manual_State_Entry, Manual_State_Do, Manual_State_Exit
int Swing_Count=0;
int Home_Flag=0;



ChgRoad_Parameters Chg_Pa =
{
	.KP11 = 2,
	.KP22 = 1.75,
	.KP33 = 1.75,
	.KP44 = 1.25,
	.KPCR1 = 16,
	.KDCR1 = 13,
	.change_road_status_flag=0,     //换道标志位
	.change_road_straight_count=0,
	.change_road_distance=12,   //换道时的距离（执行换道前进程序的次数，可以理解为执行次数越多，机器人走的越远）
	.speed = 3,
	/**转弯*/
	.isTurn = false,
	.Ini_Postition_L = 0,
	.Final_Postition_L = 0,
	.Ini_Postition_R = 0,
	.Final_Postition_R = 0,
	.ChgWidth = 0,
	.change_road_finish_flag = 0
};

int32_t compensate_angle=0;            //纠偏补偿角，客户端提供的
int32_t change_road_speed=200;       //换道时的速度（RPM）   200/70*0.942=2.69m/min

AutoMove_Parameters AM_Pa ={
	/**直线纠偏*/
	.KP1 = 2,
	.KP2 = 1,
	.KP3 = 0.5,
	.KP4 = 0.25,
	.KPFB1 = 50,
	.KDFB1 = 50,
	.deltaAngle1 = 5,
	.deltaAngle2 = 10,
	.deltaAngle3 = 30,
	.ExpectationAngle=0
};
int* AuTo_Flag;

double speed[11] ={ 0, 0.1, 0.3, 0.5, 1, 3, 5, 10, 15, 20, 22 };


double speed_Turn = 2.5;


// <summary>
/// 驱动轮减速比
/// </summary>
double DrivingWheelRatio = 80;//mm

/// <summary>
/// 驱动轮轮径 mm
/// </summary>
double DrivingWheelDiameter = 230;//mm

/// <summary>
/// 驱动轮速比
/// </summary>
double DrivingWheelSpeedRatio = 0;
/// <summary>
/// 位置系数
/// </summary>
double DrivingWheelPositionRatio = 0;

/// <summary>
/// 电机转速r/min与轮上线速度m/min的速比
/// </summary>
void WheelMotorSpeedInit()
{
    DrivingWheelSpeedRatio = 6 * 100 * (DrivingWheelRatio + 1) / 360 / 3.14 / DrivingWheelDiameter / 0.001;
    DrivingWheelPositionRatio = (int)(((65536 * DrivingWheelRatio / (DrivingWheelDiameter * 3.14159)) / 0.00001) * 0.00001);
}


/// <summary>
/// 轮上线速度m/min求电机转速r/min
/// </summary>
/// <param name="speed">轮上线速度 m/min</param>
/// <returns>电机转速 r/min</returns>
int32_t WheelMotorSpeedGet(double speed)
{

	int32_t motorSpeed = (int32_t)(speed * DrivingWheelSpeedRatio);
    return motorSpeed;
}

/// <summary>
/// 下发参数根据实际输出距离求得
/// </summary>
/// <param name="Position">下发参数pulse</param>
/// <returns>电机转速 r/min</returns>
int32_t WheelMotorPositionGet(double Length)
{

	int32_t motorPosition = (int32_t)(Length*DrivingWheelPositionRatio);
	return motorPosition;
}





void Manual_State_Do(void)
{

}

void Auto_State_Do(void)
{

}

void Abnormal_State_Do(void)
{
	LOG("Abnormal_State_Do");

}

void Forwards_State_Do(void)
{
	GV.LeftFrontMotor.Target_Velcity = GV.Robot_ManualSpeed;

	GV.RightFrontMotor.Target_Velcity = -GV.Robot_ManualSpeed;

}

void Backwards_State_Do(void)
{
	GV.LeftFrontMotor.Target_Velcity = -GV.Robot_ManualSpeed;

	GV.RightFrontMotor.Target_Velcity = GV.Robot_ManualSpeed;
}
void TurnLeft_State_Do(void)
{
	GV.LeftFrontMotor.Target_Velcity = -CV.TurnLeftRightSpeed;

	GV.RightFrontMotor.Target_Velcity = -CV.TurnLeftRightSpeed;
}
void TurnRight_State_Do(void)
{
	GV.LeftFrontMotor.Target_Velcity =CV.TurnLeftRightSpeed;

	GV.RightFrontMotor.Target_Velcity = CV.TurnLeftRightSpeed;
}
void HALT_State_Do(void)
{


	Chg_Pa.change_road_status_flag = 0;

	GV.LeftFrontMotor.Target_Velcity = 0;

	GV.RightFrontMotor.Target_Velcity = 0;
}


void continueTurn_up(double Angle, double CurrentAngle)
{
	double offset = 0.5;

	if (CurrentAngle >= Angle - offset && CurrentAngle <= Angle + offset)
	{
		Chg_Pa.isTurn = true;
	}
}


void Auto()
{
	double angle = GV.Robot_Angle.RF_Angle_Roll / 100.0;
	switch(*AuTo_Flag)
	{
		case 1:
			 //水平
			if ((angle >= 45 && angle <= 135))
			{
				AM_Pa.ExpectationAngle = 90 -  GV.Right_Compensation;
			}
			else if ((angle >= -135 && angle <= -45))
			{
				AM_Pa.ExpectationAngle = -90 +  GV.Left_Compensation;
			}

			//垂直
			else if ((angle > -45 && angle < 45))
			{
				AM_Pa.ExpectationAngle = 0;
			}

				VehicleAutoTurn((angle), AM_Pa.ExpectationAngle, Chg_Pa.KPCR1, Chg_Pa.KDCR1);  //没到-90左转
				continueTurn_up(AM_Pa.ExpectationAngle, (angle));
				if (Chg_Pa.isTurn)
				{
					*AuTo_Flag=2;
					GV.LeftFrontMotor.Target_Velcity=0;
					GV.RightFrontMotor.Target_Velcity=0;
					Chg_Pa.isTurn=false;//换完置位
				}
			break;
		case 2:
			   VehicleAutoForward(angle,
					   GV.PV.Robot_LeftCompensation, GV.PV.Robot_RightCompensation);//自动前进
			break;

		case 3:
			 //水平
			if ((angle >= 45 && angle <= 135))
			{
				AM_Pa.ExpectationAngle = 90 + GV.Left_Compensation;
			}
			if ((angle >= -135 && angle <= -45))
			{
				AM_Pa.ExpectationAngle = - 90 - GV.Right_Compensation;
			}

			//垂直
			if ((angle > -45 && angle < 45))
			{
				AM_Pa.ExpectationAngle = 0;
			}

			VehicleAutoTurn((angle), AM_Pa.ExpectationAngle, Chg_Pa.KPCR1, Chg_Pa.KDCR1);  //没到-90左转
			continueTurn_up(AM_Pa.ExpectationAngle, (angle));
			if (Chg_Pa.isTurn)
			{
				*AuTo_Flag=4;
				GV.LeftFrontMotor.Target_Velcity=0;
				GV.RightFrontMotor.Target_Velcity=0;
				Chg_Pa.isTurn=false;//换完置位
			}
			break;
		case 4:
			   VehicleAutoBackward(angle,
					   GV.PV.Robot_LeftCompensation, GV.PV.Robot_RightCompensation);//自动后退
			break;


	}
}


void VehicleAutoForward(double InclinometerAngle,  double offsetAngle_left,  double offsetAngle_right)
{
	double deltaAngle=InclinometerAngle-AM_Pa.ExpectationAngle;
	int32_t deltaSpeed=PositionalPID(AM_Pa.ExpectationAngle, InclinometerAngle, AM_Pa.KPFB1, AM_Pa.KDFB1);

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

	GV.LeftFrontMotor.Target_Velcity = GV.Robot_AutoSpeed;
	GV.RightFrontMotor.Target_Velcity = -GV.Robot_AutoSpeed;

	GV.LeftFrontMotor.Target_Velcity=GV.LeftFrontMotor.Target_Velcity+AM_Pa.KP1 * deltaSpeed;
	GV.RightFrontMotor.Target_Velcity=GV.RightFrontMotor.Target_Velcity+AM_Pa.KP1 * deltaSpeed;



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

void VehicleAutoBackward(double InclinometerAngle, double offsetAngle_left,  double offsetAngle_right)
{

	double deltaAngle=InclinometerAngle-AM_Pa.ExpectationAngle;
	double deltaSpeed=PositionalPID(AM_Pa.ExpectationAngle, InclinometerAngle, AM_Pa.KPFB1, AM_Pa.KDFB1);

	if(deltaAngle<0)
	{
		deltaAngle=-deltaAngle;
	}
	GV.LeftFrontMotor.Target_Velcity = -GV.Robot_AutoSpeed;
	GV.RightFrontMotor.Target_Velcity = GV.Robot_AutoSpeed;
//
	GV.LeftFrontMotor.Target_Velcity=GV.LeftFrontMotor.Target_Velcity+AM_Pa.KP1 * deltaSpeed;
	GV.RightFrontMotor.Target_Velcity=GV.RightFrontMotor.Target_Velcity+AM_Pa.KP1 * deltaSpeed;

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

}
void VehicleAutoTurn(double InclinometerAngle, double expectationAngle, double Kp, double Kd)//换道自动转弯
{
		double deltaSpeed = 0;
		deltaSpeed = PositionalPID(expectationAngle, InclinometerAngle, Kp, Kd);     //水平纠偏
		double deltaAngle = 0;
		deltaAngle = InclinometerAngle - expectationAngle;

		if(deltaAngle<0)//取角度差值的绝对值
		{
			deltaAngle=-deltaAngle;
		}

		if(deltaSpeed >= 400)
		{
			deltaSpeed = 400;
		}

		GV.LeftFrontMotor.Target_Velcity=0;
		GV.RightFrontMotor.Target_Velcity=0;

		GV.LeftFrontMotor.Target_Velcity= GV.LeftFrontMotor.Target_Velcity + deltaSpeed;
		GV.RightFrontMotor.Target_Velcity= GV.RightFrontMotor.Target_Velcity + deltaSpeed;

}



void ChgUp()
{
	//上换道
	double angle;
	angle = GV.Robot_Angle.RF_Angle_Roll / 100.0;
	if(Chg_Pa.change_road_finish_flag==0)
	{
		switch(Chg_Pa.change_road_status_flag)
		{
			case 0:

				if(angle >= -45
						&& (angle <= 45))
				{
		    		Chg_Pa.change_road_status_flag=1;
		    		GV.Chg_Flag = 0;
		    		Chg_Pa.ChgWidth = 200-10;
		    		WheelMotorSpeedInit();
				}
				break;
		    case 1:
		    		VehicleAutoTurn((angle), 90, Chg_Pa.KPCR1, Chg_Pa.KDCR1);  //没到-90左转
		    		continueTurn_up(90, (angle));
		    		if (Chg_Pa.isTurn)
					{
		    			Chg_Pa.change_road_status_flag=2;
		    			GV.LeftFrontMotor.Target_Velcity=0;
		    			GV.RightFrontMotor.Target_Velcity=0;
						Chg_Pa.isTurn=false;//换完置位

						//记录电机换道位置
			    		Chg_Pa.Ini_Postition_L = GV.LeftFrontMotor.Position;
			    		Chg_Pa.Final_Postition_L = GV.LeftFrontMotor.Position - WheelMotorPositionGet(Chg_Pa.ChgWidth);
			    		Chg_Pa.Ini_Postition_L = GV.RightFrontMotor.Position;
			    		Chg_Pa.Final_Postition_R = GV.RightFrontMotor.Position + WheelMotorPositionGet(Chg_Pa.ChgWidth);
					}
		    	break;
		    case 2:             //到了90°之后就开始水平后退
		    	GV.LeftFrontMotor.Target_Velcity= -WheelMotorSpeedGet(Chg_Pa.speed);
		    	GV.RightFrontMotor.Target_Velcity= WheelMotorSpeedGet(Chg_Pa.speed);
                int m = Chg_Pa.Final_Postition_L - GV.LeftFrontMotor.Position;
                int n = Chg_Pa.Final_Postition_R - GV.RightFrontMotor.Position;
                if (m > 0 && n < 0)
                {
		    		Chg_Pa.change_road_status_flag=3;
		    		Chg_Pa.change_road_straight_count=0;
		    		GV.LeftFrontMotor.Target_Velcity=0;
		    		GV.RightFrontMotor.Target_Velcity=0;
		    	}
		    	break;
		    case 3:
		    		VehicleAutoTurn((angle),0,Chg_Pa.KPCR1,Chg_Pa.KDCR1); //转回来，没到0右转
		    		continueTurn_up(0, (angle));
					if (Chg_Pa.isTurn)
					{
						Chg_Pa.change_road_status_flag=0;
		    			GV.LeftFrontMotor.Target_Velcity=0;
		    			GV.RightFrontMotor.Target_Velcity=0;
						Chg_Pa.change_road_finish_flag=1;
			    		GV.Chg_Flag = 1;
						Chg_Pa.isTurn=false;
					}
			    break;
		    default:
		    	break;
		}
	}

}

void ChgDown()
{
	//上换道
	double angle;
	angle = GV.Robot_Angle.RF_Angle_Roll / 100.0;
	if(Chg_Pa.change_road_finish_flag==0)
	{
		switch(Chg_Pa.change_road_status_flag)
		{
			case 0:

				if(angle >= -45
						&& (angle <= 45))
				{
		    		Chg_Pa.change_road_status_flag=1;
		    		GV.Chg_Flag = 0;
		    		Chg_Pa.ChgWidth = 200-10;
		    		WheelMotorSpeedInit();
				}
				break;
		    case 1:
		    		VehicleAutoTurn((angle), -90, Chg_Pa.KPCR1, Chg_Pa.KDCR1);  //没到-90左转
		    		continueTurn_up(-90, (angle));
		    		if (Chg_Pa.isTurn)
					{
		    			Chg_Pa.change_road_status_flag=2;
		    			GV.LeftFrontMotor.Target_Velcity=0;
		    			GV.RightFrontMotor.Target_Velcity=0;
						Chg_Pa.isTurn=false;//换完置位

						//记录电机换道位置
			    		Chg_Pa.Ini_Postition_L = GV.LeftFrontMotor.Position;
			    		Chg_Pa.Final_Postition_L = GV.LeftFrontMotor.Position + WheelMotorPositionGet(Chg_Pa.ChgWidth);
			    		Chg_Pa.Ini_Postition_L = GV.RightFrontMotor.Position;
			    		Chg_Pa.Final_Postition_R = GV.RightFrontMotor.Position - WheelMotorPositionGet(Chg_Pa.ChgWidth);
					}
		    	break;
		    case 2:             //到了-90°之后就开始水平前进
		    	GV.LeftFrontMotor.Target_Velcity= WheelMotorSpeedGet(Chg_Pa.speed);
		    	GV.RightFrontMotor.Target_Velcity= -WheelMotorSpeedGet(Chg_Pa.speed);
                int m = Chg_Pa.Final_Postition_L - GV.LeftFrontMotor.Position;
                int n = Chg_Pa.Final_Postition_R - GV.RightFrontMotor.Position;
                if (m < 0 && n > 0)
                {
		    		Chg_Pa.change_road_status_flag=3;
		    		Chg_Pa.change_road_straight_count=0;
		    		GV.LeftFrontMotor.Target_Velcity=0;
		    		GV.RightFrontMotor.Target_Velcity=0;
		    	}
		    	break;
		    case 3:
		    		VehicleAutoTurn((angle),0,Chg_Pa.KPCR1,Chg_Pa.KDCR1); //转回来，没到0右转
		    		continueTurn_up(0, (angle));
					if (Chg_Pa.isTurn)
					{
						Chg_Pa.change_road_status_flag=0;
		    			GV.LeftFrontMotor.Target_Velcity=0;
		    			GV.RightFrontMotor.Target_Velcity=0;
						Chg_Pa.change_road_finish_flag=1;
						GV.Chg_Flag = 1;
						Chg_Pa.isTurn=false;
					}
			    break;
		    default:
		    	break;
		}
	}
}


void ChgLeft()
{
	//上换道
	double angle;
	angle = GV.Robot_Angle.RF_Angle_Roll / 100.0;

	if(Chg_Pa.change_road_finish_flag==0)
	{
		switch(Chg_Pa.change_road_status_flag)
		{
			case 0:
	    		angle = GV.Robot_Angle.RF_Angle_Roll / 100.0;
				if((angle >= -135 && angle <= -45)
						|| (angle >= 45 && angle <= 135))
				{
		    		Chg_Pa.change_road_status_flag=1;
		    		GV.Chg_Flag = 0;
		    		Chg_Pa.ChgWidth = 200-60;
		    		WheelMotorSpeedInit();
				}
				break;
		    case 1:
		    		VehicleAutoTurn((angle), 0, Chg_Pa.KPCR1, Chg_Pa.KDCR1);  //没到-90左转
		    		continueTurn_up(0, (angle));
		    		if (Chg_Pa.isTurn)
					{
		    			Chg_Pa.change_road_status_flag=2;
		    			GV.LeftFrontMotor.Target_Velcity=0;
		    			GV.RightFrontMotor.Target_Velcity=0;
						Chg_Pa.isTurn=false;//换完置位

						//记录电机换道位置
			    		Chg_Pa.Ini_Postition_L = GV.LeftFrontMotor.Position;
			    		Chg_Pa.Final_Postition_L = GV.LeftFrontMotor.Position - WheelMotorPositionGet(Chg_Pa.ChgWidth - 50);
			    		Chg_Pa.Ini_Postition_L = GV.RightFrontMotor.Position;
			    		Chg_Pa.Final_Postition_R = GV.RightFrontMotor.Position + WheelMotorPositionGet(Chg_Pa.ChgWidth - 50);
					}
		    	break;
		    case 2:             //到了90°之后就开始水平后退
		    	GV.LeftFrontMotor.Target_Velcity= -WheelMotorSpeedGet(Chg_Pa.speed);
		    	GV.RightFrontMotor.Target_Velcity= WheelMotorSpeedGet(Chg_Pa.speed);
                int m = Chg_Pa.Final_Postition_L - GV.LeftFrontMotor.Position;
                int n = Chg_Pa.Final_Postition_R - GV.RightFrontMotor.Position;
                if (m > 0 && n < 0)
                {
		    		Chg_Pa.change_road_status_flag=3;
		    		Chg_Pa.change_road_straight_count=0;
		    		GV.LeftFrontMotor.Target_Velcity=0;
		    		GV.RightFrontMotor.Target_Velcity=0;
		    	}
		    	break;
		    case 3:
		    		VehicleAutoTurn((angle),-90,Chg_Pa.KPCR1,Chg_Pa.KDCR1); //转回来，没到0右转
		    		continueTurn_up(-90, (angle));
					if (Chg_Pa.isTurn)
					{
						Chg_Pa.change_road_status_flag=0;
		    			GV.LeftFrontMotor.Target_Velcity=0;
		    			GV.RightFrontMotor.Target_Velcity=0;
						Chg_Pa.change_road_finish_flag=1;
			    		GV.Chg_Flag = 1;
						Chg_Pa.isTurn=false;
					}
			    break;
		    default:
		    	break;
		}
	}

}

void ChgRight()
{
	//上换道
	double angle;
	angle = GV.Robot_Angle.RF_Angle_Roll / 100.0;

	if(Chg_Pa.change_road_finish_flag==0)
	{
		switch(Chg_Pa.change_road_status_flag)
		{
			case 0:
	    		angle = GV.Robot_Angle.RF_Angle_Roll / 100.0;
				if((angle >= -135 && angle <= -45)
						|| (angle >= 45 && angle <= 135))
				{
		    		Chg_Pa.change_road_status_flag=1;
		    		GV.Chg_Flag = 0;
		    		Chg_Pa.ChgWidth = 200-60;
		    		WheelMotorSpeedInit();
				}
				break;
		    case 1:
		    		VehicleAutoTurn((angle), 0, Chg_Pa.KPCR1, Chg_Pa.KDCR1);  //没到-90左转
		    		continueTurn_up(0, (angle));
		    		if (Chg_Pa.isTurn)
					{
		    			Chg_Pa.change_road_status_flag=2;
		    			GV.LeftFrontMotor.Target_Velcity=0;
		    			GV.RightFrontMotor.Target_Velcity=0;
						Chg_Pa.isTurn=false;//换完置位

						//记录电机换道位置
			    		Chg_Pa.Ini_Postition_L = GV.LeftFrontMotor.Position;
			    		Chg_Pa.Final_Postition_L = GV.LeftFrontMotor.Position - WheelMotorPositionGet(Chg_Pa.ChgWidth - 50);
			    		Chg_Pa.Ini_Postition_L = GV.RightFrontMotor.Position;
			    		Chg_Pa.Final_Postition_R = GV.RightFrontMotor.Position + WheelMotorPositionGet(Chg_Pa.ChgWidth - 50);
					}
		    	break;
		    case 2:             //到了90°之后就开始水平后退
		    	GV.LeftFrontMotor.Target_Velcity= -WheelMotorSpeedGet(Chg_Pa.speed);
		    	GV.RightFrontMotor.Target_Velcity= WheelMotorSpeedGet(Chg_Pa.speed);
                int m = Chg_Pa.Final_Postition_L - GV.LeftFrontMotor.Position;
                int n = Chg_Pa.Final_Postition_R - GV.RightFrontMotor.Position;
                if (m > 0 && n < 0)
                {
		    		Chg_Pa.change_road_status_flag=3;
		    		Chg_Pa.change_road_straight_count=0;
		    		GV.LeftFrontMotor.Target_Velcity=0;
		    		GV.RightFrontMotor.Target_Velcity=0;
		    	}
		    	break;
		    case 3:
		    		VehicleAutoTurn((angle),90,Chg_Pa.KPCR1,Chg_Pa.KDCR1); //转回来，没到0右转
		    		continueTurn_up(90, (angle));
					if (Chg_Pa.isTurn)
					{
						Chg_Pa.change_road_status_flag=0;
		    			GV.LeftFrontMotor.Target_Velcity=0;
		    			GV.RightFrontMotor.Target_Velcity=0;
						Chg_Pa.change_road_finish_flag=1;
			    		GV.Chg_Flag = 1;
						Chg_Pa.isTurn=false;
					}
			    break;
		    default:
		    	break;
		}
	}
}

void ChgFinish()
{
	GV.LeftFrontMotor.Target_Velcity = 0;

	GV.RightFrontMotor.Target_Velcity = 0;
}