AGV / agv_shanghaisanling · Files

#include "KIVA.h"

#if P_SETUP_PLATFORM_TYPE == 5
void initPlaformParam(void) //参数初始化
{
//    Uart_Printf(COM1, "参数初始化\r\n");
//    Y9(0); // 关闭agv自动充电触点

//    agv.Command.standSiteID = 1;
//    agv.Command.Au_Hand = 0;
//		agv.Public.i_StartFirst = false;
//    agv.Public.i_UpdatePathSig = 1;

//    DriverMotor1.Command.speedSlope = 5;
//    DriverMotor2.Command.speedSlope = 5;
//    DriverLifter1.Command.speedSlope = 50;
//    DriverShifter1.Command.speedSlope = 50;

//		#if P_SETUP_SINGLE_PLAYER == 0 //0不开启单机测试  1开启
//		SetAlarm(0x0040);
//		#endif
}

void lifterRunManul()
{
	if (agv.Command.HandLiftState == 1) //上升
	{
		Lifter1.setSpeed = 2000000;
	}
	else if (agv.Command.HandLiftState == 2) //下降
	{
		Lifter1.setSpeed = -2000000;
	}
	else
	{
		Lifter1.setSpeed = 0;
	}

	//升降限制
	if (agv.Public.i_Lift1DownLimit && Lifter1.setSpeed < 0)
	{
			Lifter1.setSpeed = 0;
	}
	else if(agv.Public.i_Lift1UpLimit && Lifter1.setSpeed > 0)
	{
			Lifter1.setSpeed = 0;
	}
}

u8 liftFirstToZero1 = 1;
void lifterRunAuto() //举升自动运行,运行状态0不动  1上升  2下降 
{		
		switch (Lifter1.runState)
		{
			case 0:
				Lifter1.setSpeed = 0;
				break;
			case 1:
				if(Lifter1.encodePos <= 18)
				{
					Lifter1.setSpeed = 5000000;
				}
				else
				{
					Lifter1.setSpeed = 2000000;
				}
				break;
			case 2:
				if(Lifter1.encodePos >= 6)
				{
					Lifter1.setSpeed = -4000000;
				}
				else
				{
					Lifter1.setSpeed = -1500000;
				}
				
				break;
			case 3:
				break;
			case 4:
				break;
			case 5:
				break;
			case 6:
				break;
			case 7:
				break;
			case 8:
				break;
			default:
				break;
			
		}
		
		if(Lifter1.runState ||  (Lifter1.setSpeed!= 0))
		{
			agv.Command.MusicFlag = Music_Arrivel;
		}
		if(liftFirstToZero1)
		{
			Lifter1.setSpeed = -1500000;
		}

    //升降限制
    if (agv.Public.i_Lift1DownLimit && Lifter1.setSpeed < 0)
    {
        liftFirstToZero1 = 0;
        Lifter1.setSpeed = 0;
				Lifter1.runState = 0;
    }
		else if((agv.Public.i_Lift1UpLimit || (Lifter1.encodePos >= 22)) && Lifter1.setSpeed > 0)
		{
				Lifter1.setSpeed = 0;
				Lifter1.runState = 0;
		}
		//举升状态
		if(agv.Public.i_Lift1DownLimit)
		{
			Lifter1.liftState = -1;
		}
		else if(agv.Public.i_Lift1UpLimit || (Lifter1.encodePos >= 22))
		{
			Lifter1.liftState = 1;
		}
		else 
		{
			Lifter1.liftState = 0;
		}
}

float palstanceOffset = 0.005,palstanceMin = 0.12,setArriveAngle = 1,setPalstance = 0;
void RotateRunAuto() //举升自动运行,运行状态0不动  1左转90  2左转180  3右转90  4右转180  5左转联动90  6左转联动180  7右转联动90  8右转联动180
{		
	static float lastAngle = 0,diffAngle = 0,alreadyTurnAngle = 0,lastPalstance = 0;
	static u8 TurnFlag = 0;
	float PalstanceMax = (float)agv.Parameter.HandTurnSpeed/360 + palstanceOffset;

	if(!(Rotate1.runState == 1 || Rotate1.runState == 2))
	{
		if(!TurnFlag && agv.Command.SetPlatformTurnAngle != 0)
		{
//			if(agv.Command.HandMotorState == 5)
//			{
//				Rotate1.runState = 5;
//				TurnFlag = 1;
//			}
//			else if(agv.Command.HandMotorState == 6)
//			{
//				Rotate1.runState = 6;
//				TurnFlag = 1;
//			}
//			else if(agv.Command.CurDirection == 3)
//			{
//				Rotate1.runState = 3;
//				TurnFlag = 1;
//			}
//			else if(agv.Command.CurDirection == 4)
//			{
//				Rotate1.runState = 4;
//				TurnFlag = 1;
//			}
		}
		else if(Rotate1.runState == 0 && agv.Command.CurDirection != 4 && agv.Command.CurDirection != 3
			&& agv.Command.HandMotorState!=5 && agv.Command.HandMotorState!=6 )
		{
			TurnFlag = 0;
		}
	}
	
	  alreadyTurnAngle = Rotate1.encodeAngle - lastAngle;

		if(alreadyTurnAngle > 180)
			alreadyTurnAngle -= 360;
		if(alreadyTurnAngle < -180)
			alreadyTurnAngle += 360;
				
		diffAngle = fabs(fabs(alreadyTurnAngle) - agv.Command.SetPlatformTurnAngle);

		if(Rotate1.runState == 2 || Rotate1.runState == 3 || Rotate1.runState == 4 || Rotate1.runState == 5 || Rotate1.runState == 6)
		{
			if(diffAngle > 30)//平台旋转角度
			{
				setPalstance = PalstanceMax;
			}
			else if(diffAngle > 3)
			{
				setPalstance = mapping(diffAngle,3,30,palstanceMin,PalstanceMax);
			}
			else
			{
				setPalstance = palstanceMin;
			}
		}
		switch (Rotate1.runState)
		{
			case 0:
				Rotate1.setSpeed = 0;
				lastAngle = Rotate1.encodeAngle;
				break;
			case 1://右转90度
				Rotate1.setSpeed = -500000;//PalstanceMax*6666666.7;//-500000;
				break;
			case 2://左转
				Rotate1.setSpeed = setPalstance*6666666.7/2;
				if(diffAngle < 5 && (Rotate1.encodeAngle > -10 && Rotate1.encodeAngle < 10))//回零点且转盘快到位
					{
						if(X1 & X2)
						{
							Rotate1.setSpeed = 0;
							Rotate1.runState = 0;
							Rotate1.encodeAngle = 0;
							lastAngle = 0;
						}
					}
					else if(diffAngle < setArriveAngle)
					{
						if(X1 ||  X2)
						{
						  Rotate1.setSpeed = 0;
					  	Rotate1.runState = 0;
						}
					}			
				break;
			case 3://右转
					Rotate1.setSpeed = -setPalstance*6666666.7/2;//IMU.palstance
					
					if(diffAngle < 5 && (Rotate1.encodeAngle > -10 && Rotate1.encodeAngle < 10))//回零点且转盘快到位
					{
						if(X1 & X2)
						{
							Rotate1.setSpeed = 0;
							Rotate1.runState = 0;
							Rotate1.encodeAngle = 0;
							lastAngle = 0;
						}
					}
					else if(diffAngle < setArriveAngle)
					{
							if(X1 ||  X2)
						{
						  Rotate1.setSpeed = 0;
					  	Rotate1.runState = 0;
						}
					}			
				break;
			case 4://
					Rotate1.setSpeed = setPalstance*6666666.7/2;
					if(diffAngle < 5 && (Rotate1.encodeAngle > -10 && Rotate1.encodeAngle < 10))//回零点且转盘快到位
					{
						if(X1 & X2)//零点触发
						{
							Rotate1.setSpeed = 0;
							Rotate1.runState = 0;
							Rotate1.encodeAngle = 0;
							lastAngle = 0;
						}
					}
					else if(diffAngle < setArriveAngle)
					{
						Rotate1.setSpeed = 0;
						Rotate1.runState = 0;
					}		
				break;
			case 5://左转联动	
				Rotate1.setSpeed = -setPalstance*6666666.7/2;//IMU.palstance
			
				if(diffAngle < 10 && (Rotate1.encodeAngle > -10 && Rotate1.encodeAngle < 10))//回零点且转盘快到位
				{
					if(X1 & X2)//零点触发
					{
						Rotate1.setSpeed = 0;
						Rotate1.runState = 0;
						Rotate1.encodeAngle = 0;
						lastAngle = 0;
					}
				}
				else if(diffAngle < setArriveAngle)
				{
					Rotate1.setSpeed = 0;
					Rotate1.runState = 0;
				}	
				break;
			case 6://右转联动
				Rotate1.setSpeed = setPalstance*6666666.7/2;
				if(diffAngle < 10 && (Rotate1.encodeAngle > -10 && Rotate1.encodeAngle < 10))//回零点且转盘快到位
				{
					if(X1 & X2)//零点触发
					{
						Rotate1.setSpeed = 0;
						Rotate1.runState = 0;
						Rotate1.encodeAngle = 0;
						lastAngle = 0;
					}
				}
				else if(diffAngle < setArriveAngle)
				{
					Rotate1.setSpeed = 0;
					Rotate1.runState = 0;
				}		
				break;
			case 7:
				Rotate1.setSpeed = 800000;
				break;
			case 8:
				break;
			default:
				break;		
		}
		//旋转状态设置
		if(agv.Public.i_UpdatePathSig)//停车时才做判断
		{
			if(Rotate1.runState == 0)
				Rotate1.rotateState = 0;
			else
				Rotate1.rotateState = 1;
		}
		
}



void checkMaterialState() //物料检测
{
    //下层物料检测
    if (agv.Public.i_Shift1ArrivalLeft && agv.Public.i_Shift1ArrivalRight)
    {
        agv.Public.i_MaterialFlagFirst = 1;
    }
    else if (!agv.Public.i_Shift1ArrivalLeft && !agv.Public.i_Shift1ArrivalRight && !agv.Public.i_Shift1_LimitLeft && !agv.Public.i_Shift1_LimitRight)
    {
        agv.Public.i_MaterialFlagFirst = 0;
    }
    else
        agv.Public.i_MaterialFlagFirst = 2;
}

void platformControlManual() //平台手动运行
{
//    lifterRunManu(&DriverLifter1);
//    shifterRunManu(&DriverShifter1);
		lifterRunManul();
	  Rotate1.setSpeed = 0;
		Rotate1.runState = 0;
//		lifterRunAuto();
//    RotateRunAuto();

}

void platformControlAuto() //平台自动对接
{
	  if(Lifter1.runState ||Rotate1.runState)
		{
			if(!Camera.UpdataFlag )
			{
				SetAlarm(0x0004);
				Camera.UpdataFlag1 = 1;
			}
			else
			{
				Reset_Alarm(0x0004);	
			}
		}
    lifterRunAuto();
    RotateRunAuto();
}

void X_Input()
{
	static u8 Hand_AutoSelect = 0,LastHand_AutoSelect = 0,Hand_CentringUp = 0,Hand_CentringDown = 0,LastHand_CentringUp = 0,LastHand_CentringDown = 0;
	static int lastTime = 0;
//	if(agv.Parameter.AGVID == 0xA010)
//	{
//		agv.Public.i_RadarSigFront_1 			= MasterInput.IN1 = !X22;//前壁障减速1
//		agv.Public.i_RadarSigFront_2 			= MasterInput.IN2 = !X23;//前壁障减速2
//		agv.Public.i_RadarSigFront_3 			= MasterInput.IN3 = !X24;//前壁障减速3
//	}
//	else
//	{
			agv.Public.i_RadarSigFront_1 			= MasterInput.IN1 = !X24;//前壁障减速1
			if(agv.Parameter.AGVID == 0xA007)
			{
				agv.Public.i_RadarSigFront_2 = 0;
			}
			else
			{
				agv.Public.i_RadarSigFront_2 		= MasterInput.IN2 = !X23;//前壁障减速2
			}
			
			agv.Public.i_RadarSigFront_3 			= MasterInput.IN3 = !X22;//前壁障减速3
//	}
//	agv.Public.i_RadarSigBack_1 			= MasterInput.IN4 = !X24;//前壁障减速4
	
//	agv.Public.i_RadarSigBack_2 			= MasterInput.IN5 = !X5;//后壁障减速2
//	agv.Public.i_RadarSigBack_3 			= MasterInput.IN6 = !X6;//后壁障减速3
	
	agv.Public.i_QuickStopSig 				= MasterInput.IN7 = X6;// 急停 常开
	
//	if(!agv.Command.Au_Hand)
//	{
//		agv.Public.i_HandLiftSig					= X7;
//		agv.Public.i_HandDownSig					= X8;
//		if(agv.Public.i_HandLiftSig)
//			Lifter1.runState = 1;
//		else if(agv.Public.i_HandDownSig)
//			Lifter1.runState = 2;
//		
//		agv.Public.i_HandFrontSig					= X9;
//		agv.Public.i_HandBackSig					= X10;
//		agv.Public.i_HandTurnLeftSig					= X11;
//		agv.Public.i_HandTurnRightSig					= X12;
//		if(agv.Public.i_HandFrontSig)
//			agv.Command.HandMotorState = 1;
//		else if(agv.Public.i_HandBackSig)
//			agv.Command.HandMotorState = 2;
//		else if(agv.Public.i_HandTurnLeftSig)
//			agv.Command.HandMotorState = 3;
//		else if(agv.Public.i_HandTurnRightSig)
//			agv.Command.HandMotorState = 4;
//		else
//			agv.Command.HandMotorState = 0;
//						;
//			
//	}
	agv.Public.i_RotateOkSig	 				= X2;
  agv.Public.i_RotateZeroSig				= X1;
	agv.Public.i_BumperSig 						= MasterInput.IN5 = X5;//防撞条
//	agv.Public.i_StartSig 						= MasterInput.IN9 = X9;// 启动 
//	agv.Public.i_StopSig 							= MasterInput.IN10 = X10;//暂停
//	agv.Public.i_LightStopFlagL 			= MasterInput.IN11 = X11;//左停靠
//	agv.Public.i_LightStopFlagR 			= MasterInput.IN12 = X12;//右停靠
//	
	agv.Public.i_Lift1UpLimit					= MasterInput.IN13 = X4;//升降平台上限位
	agv.Public.i_Lift1DownLimit				= MasterInput.IN14 = X3;//升降平台下限位
//	agv.Public.i_Shift1_LimitLeft			= MasterInput.IN15 = X15;//左极限
//	agv.Public.i_Shift1_LimitRight 		= MasterInput.IN18 = X18;//右极限
//	agv.Public.i_Shift1ArrivalLeft 		= MasterInput.IN16 = X16;//左到位对射
//	agv.Public.i_Shift1ArrivalRight 	= MasterInput.IN17 = !X17;//右到位对射
	Hand_AutoSelect 										= X14;
	Hand_CentringUp 										= X15;
	Hand_CentringDown 									= X16;
	if(LastHand_AutoSelect == 0 && Hand_AutoSelect == 1)
	{
		agv.Command.Au_Hand = !agv.Command.Au_Hand;
		if(agv.Command.Au_Hand)
		{
			Y3(0);
			if(agv.Public.i_RotateOkSig & agv.Public.i_RotateZeroSig) 
			{
				Reset_Alarm(0x0400);//复位报警
			}
			else 	
			{
				SetAlarm(0x0400);//平台报警
			
			}		
		}
		else
		{
			Y3(1);
		}
	}
	if(agv.Command.Au_Hand)
	{ 
		agv.Parameter.angleTurnSpeed = 100;
	}
	else
	{
		agv.Parameter.angleTurnSpeed = 150;
	}
	LastHand_AutoSelect = Hand_AutoSelect;
if(!agv.Command.Au_Hand)
	{
		agv.Parameter.lastlandIDSig = 99999;
		agv.Public.i_HandLiftSig					= X12;
		
		agv.Public.i_HandDownSig					= X13;
		
		//对中指令控制
		if(Hand_CentringUp == 1 && LastHand_CentringUp == 0 && agv.Public.i_HandCentringDown == 0)//遥控信号切换，下对中为0
			agv.Public.i_HandCentringUp = !agv.Public.i_HandCentringUp;
		if(Hand_CentringDown == 1 && LastHand_CentringDown == 0 && agv.Public.i_HandCentringUp == 0)//遥控信号切换，上对中为0
			agv.Public.i_HandCentringDown = !agv.Public.i_HandCentringDown;
		
		LastHand_CentringUp = Hand_CentringUp;		
		LastHand_CentringDown = Hand_CentringDown;
		
		if(agv.Public.i_HandLiftSig) 	
			agv.Command.HandLiftState = 1;
		else if(agv.Public.i_HandDownSig && !agv.Public.i_Lift1DownLimit) 	
			agv.Command.HandLiftState = 2;
		else 
			agv.Command.HandLiftState = 0;
		
		agv.Public.i_HandFrontSig					= X8;
		agv.Public.i_HandBackSig					= X9;
		agv.Public.i_HandTurnLeftSig					= X10;		
		agv.Public.i_HandTurnRightSig					= X11;
		

				
		if(agv.Public.i_HandFrontSig)
			agv.Command.HandMotorState = 1;//前进
		else if(agv.Public.i_HandBackSig)
			agv.Command.HandMotorState = 2;//后退
		else if(agv.Public.i_HandTurnLeftSig)
			agv.Command.HandMotorState = 3;//左转
		else if(agv.Public.i_HandTurnRightSig)
			agv.Command.HandMotorState = 4;//右转
		else
			agv.Command.HandMotorState = 0;
	}
	if(agv.Public.i_StartSig)//第一次上电按启动复位举升
	{
		agv.Public.i_StartFirst = true;
	}
	if(agv.Public.i_Lift1DownLimit)//下限位触发  位置清零
	{
		Lifter1.encodePos = 0;
	}
}

CoordinatePoints curCamera;
void platformDataProcess(void)
{
	X_Input();//IO输入信号
	//自动模式且开启红外，雷达避障信号
	if(agv.Command.Au_Hand&&agv.Command.Barrier_ONOFF&& (agv.Command.CurDirection == 1))
	{
//		if(agv.Command.CurDirection == 1)//前进方向
		{
			if(agv.Command.LaserArea!=3)
			{
				if(agv.Public.i_RadarSigFront_3)//最里层雷达避障
				{
					SetAlarm(0x0001);//停车
				}
				else
				{
					Reset_Alarm(0x0001);//取消停车避障
				}
				if(agv.Public.i_RadarSigFront_2)//二级减速
				{
					agv.Public.i_DoubleReduction = 1;//开启二级减速
				}
				else
				{
					agv.Public.i_DoubleReduction = 0;//取消二级减速
				}
				if(agv.Public.i_RadarSigFront_1)//一级减速
				{
					agv.Public.i_Reduction = 1;//开启一级减速
				}
				else
				{
					agv.Public.i_Reduction = 0;//取消一级减速
				}
			}
			else if(agv.Command.LaserArea==3)
			{
				Reset_Alarm(0x0001);
				
				agv.Public.i_DoubleReduction = 0;
				
				agv.Public.i_Reduction = 0;
			}
		}
		
	}
	else//不开启避障
	{
		Reset_Alarm(0x0001);//取消停车避障
		
		agv.Public.i_Reduction = 0;//取消一级减速
		
		agv.Public.i_DoubleReduction = 0;//取消二级减速
	}
	//防撞条
	if(agv.Public.i_BumperSig)
	{
		SetAlarm(0x0006);//停车，防撞条报警和暂停触发
	}
//	else
//		Reset_Alarm(0x0002);//取消防撞条报警
	
	//急停触发
	if (agv.Public.i_QuickStopSig)
	{	
		 if(agv.Public.Error_Flag & 0x02)
		 {
			 Reset_Alarm(0x0006);//取消防撞条报警
		 }
		SetAlarm(0x0100);//急停和暂停触发，防止再启动
	}
	else
	{
		Reset_Alarm(0x0100);//取消急停报警
	}
	if(agv.Public.i_StartSig)//启动
	{	
		Reset_Alarm(0x0004);
	}
//	if(agv.Public.i_StopSig)
//	{
//		SetAlarm(0x0004);//暂停	
//	}
//	if(agv.Public.i_Lift1DownLimit)//拉线编码器清零
//	{
//		TIM_SetCounter(TIM1, 0);
//	}

	//启动和暂停指示灯
//	if(agv.Command.Au_Hand && agv.Public.Error_Flag==0)//自动模式无异常显示启动灯
//	{
//		Y8(1);
//		Y9(0);
//	}
//	else
//	{
//		Y8(0);
//		Y9(1);
//	}
	
	agv.Public.i_quadEncoder1 = TIM_GetCounter(TIM1);
	agv.Public.i_quadEncoder2 = TIM_GetCounter(TIM2);
	agv.Public.i_quadEncoder3 = TIM_GetCounter(TIM3);
	agv.Public.i_quadEncoder4 = TIM_GetCounter(TIM4);
	agv.Public.i_LiftHeightFirst  = agv.Public.i_quadEncoder1/4;
	
  checkMaterialState(); //物料检测

    
	//自动模式脱轨判断
	if(agv.Command.Au_Hand)
	{
		static float CurDis = 0;
		static int lastTimeOffLine = 0;
		
		curCamera.TarX = Camera.XCoordingData;
		curCamera.TarY = Camera.YCoordingData;
		
		
		
		//前进后退，弧线左右转做脱轨判断
		if(agv.Command.CurDirection == 1||agv.Command.CurDirection == 2||agv.Command.CurDirection == 7
			||agv.Command.CurDirection == 8||agv.Command.CurDirection == 9||agv.Command.CurDirection == 10)
		{
			if(fabs(navi.Public.CenterOffset) > 200)//位置偏差大于10CM，脱轨
			{
				SetAlarm(0x0008);//脱轨报警
			}
			else
				Reset_Alarm(0x0008);//取消脱轨报警
			
			CurDis = TwoPointDistance(TargetPoint,StartPoint);
			
			if(navi.Public.DistanceAgvToSTART - CurDis > 300)
			{
				if(agv.Public.SystemTime - lastTimeOffLine > 1000)
				{
					SetAlarm(0x8000);//超距脱轨报警
					if(agv.Public.i_QuickStopSig && Camera.LocationSig)
					{
						navi.Public.DistanceAgvToSTART = TwoPointDistance(StartPoint,curCamera);
					}
				}
			}
			else
			{
				lastTimeOffLine = agv.Public.SystemTime;
				Reset_Alarm(0x8000);//取消脱轨报警
			}
		}
		
//		if(agv.Command.CurDirection == 2)
//		{
//			if(navi.Public.DistanceAgvToSTART - navi.Public.DistanceSTARTtoTARGET > 50)
//			{
//				SetAlarm(0x0008);//脱轨报警
//			}
//		}
	}
}

float CalculateDiffAngle1(float A,float B)
{
	static float angle = 0;
	if(fabs(A - B) > 180)
	{
//		if(A > B)
//			angle = fabs(A - B - 360);
//		else if(A < B)
			angle = fabs(fabs(A - B) - 360);
	}
	else
		angle = fabs(A - B);
	
	return angle;
}

float CalculateCurAngle1(float A)//计算当前处于多少度
{
	static float angle = 0;
	
	if(A > 180)
			A -= 360;
		if(A < -180)
			A += 360;
		
	if(A < 10 && A > -10)
	{
		angle = 0;
	}
	else if(A < 100 && A > 80)
	{
		angle = 90;
	}
	else if((A < -170 && A >= -180) || (A > 170 && A <= 180))
	{
		angle = 180;
	}
	else if(A < -80 && A > -100)
	{
		angle = -90;
	}
	return angle;
}

u8 FloorStepFlag2 = 1,JudgementFlag2 = 1,angleCorrectionFlag = 0;
u8 FloorStepFlag3 = 1,JudgementFlag3 = 1; 
float judgeOffset2 = 5,controlOffset2 = 4;//一个是判断误差，一个是控制误差
void LookforQRCodeCenterEnd2()
{
	static u8 lastTurnDir = 0;
	static float backOffset = 0,lastXpos = 0,lastYpos = 0,curAngleQuadrant = 0,angleDif = 0;
	
	if(angleCorrectionFlag)//角度校准
	{
		curAngleQuadrant = CalculateCurAngle1(CurrentCenterPoint.CurAngle*57.3)/57.3;//求出当前所在象限
		
		angleDif = CalculatingCurrentAndTargetAngle(CurrentCenterPoint.CurAngle,curAngleQuadrant);
		
		if(angleDif > 0.005)//偏左大于0，右转
		{
			KincoStruct1.setSpeed = 20;
				
			KincoStruct2.setSpeed = -20;
		}
		else if(angleDif < -0.005)//偏右小于0，左转
		{
			KincoStruct1.setSpeed = -20;
				
			KincoStruct2.setSpeed = 20;
		}
		else
		{
			KincoStruct1.setSpeed = 0;
				
			KincoStruct2.setSpeed = 0;
			
			Reset_Alarm(0x080);
			SetAlarm(0x200);
			agv.Public.i_UpdatePathSig = 1;
			agv.Public.i_QRCodeCenterStopFlag = 0;
			agv.Public.i_HandCentringDown = 0;
			angleCorrectionFlag = 0;
		}
	}
	else//位置校准
	{
		if((fabs(Camera.X_pixel) < judgeOffset2 && fabs(Camera.Y_pixel + lastYpos) < judgeOffset2) && JudgementFlag3 == 1)//误差允许范围之内
		{
			angleCorrectionFlag = 1;
		}
		else
		{
			JudgementFlag3 = 0;//关闭判断到位标志，开始纠偏
			
			Camera.lastCurAngle = CalculateCurAngle1(CurrentCenterPoint.CurAngle*57.3);//求当前朝向象限
			
			if(FloorStepFlag3== 1)//第一步判断旋转方向，旋转
			{
				if(Camera.Y_pixel >= controlOffset2)//左偏左转后退
				{
					agv.Command.AGVCenterControlStateDown = 3;
					
					lastTurnDir = 3;
				}
				else if(Camera.Y_pixel < -controlOffset2)//右偏右转后退
				{
					agv.Command.AGVCenterControlStateDown = 4;
					
					lastTurnDir = 4;
				}
				else if(fabs(Camera.X_pixel) >= judgeOffset2)
				{
					FloorStepFlag3 = 4;
				}
				
				if(fabs(CalculateDiffAngle1(CurrentCenterPoint.CurAngle*57.3,Camera.lastCurAngle) - 7) < 0.5)//转到位
				{
					agv.Command.AGVCenterControlStateDown = 0;
									
					FloorStepFlag3 = 2;
					
					backOffset = fabs(Camera.Y_pixel)*5;
					
					if(backOffset > 35)
						backOffset = 35;
					
					lastXpos = Camera.X_pixel;
				}
			}
			else if(FloorStepFlag3 == 2)//第二步后退判断横向到位,后退多少距离
			{
				agv.Command.AGVCenterControlStateDown = 2;//后退
				if(fabs(fabs(Camera.X_pixel) - lastXpos) > backOffset)
				{
					agv.Command.AGVCenterControlStateDown = 0;
					
					FloorStepFlag3 = 3;
				}
	//			else if(fabs(Camera.Y_pixel) > 20)//超过一定限制停车重新对中
	//			{
	//				agv.Command.AGVCenterControlStateDown = 0;
	//				
	//				FloorStepFlag = 3;
	//			}
			}
			else if(FloorStepFlag3 == 3)//第三步回正
			{
				if(lastTurnDir == 3)
				{
					agv.Command.AGVCenterControlStateDown = 4;
				}
				else if(lastTurnDir == 4)
				{
					agv.Command.AGVCenterControlStateDown = 3;
				}
				if(fabs(CalculateDiffAngle1(CurrentCenterPoint.CurAngle*57.3,Camera.lastCurAngle) - 0) < 0.5)//转到位
				{
					agv.Command.AGVCenterControlStateDown = 0;
									
					FloorStepFlag3 = 4;
				}	
			}
			else if(FloorStepFlag3 == 4)//第四步前进
			{
				if(Camera.X_pixel > controlOffset2)
				{
					agv.Command.AGVCenterControlStateDown = 2;//后退
				}
				else
				{
					agv.Command.AGVCenterControlStateDown = 1;//前进
				}
							
				if(fabs(Camera.X_pixel) < controlOffset2)
				{
					agv.Command.AGVCenterControlStateDown = 0;
					
					FloorStepFlag3 = 1;
					
					JudgementFlag3 = 1;
				}
				
			}
		}
	}
}

u8 angleCorrectionFlag2;
void LookforQRCodeCenterEnd()
{
	static u8 lastTurnDir = 0,lastTurnDir2 = 0;
	static float thirdYoffset = 0;
	static u8 ifirst = 0;
	static float backOffset = 0,backDIS = 0;
	static float frontOffset = 0,frontDIS = 0,curAngleQuadrant = 0,angleDif = 0;
	
	if(angleCorrectionFlag2)//角度校准
	{
		curAngleQuadrant = CalculateCurAngle1(CurrentCenterPoint.CurAngle*57.3)/57.3;//求出当前所在象限
		
		angleDif = CalculatingCurrentAndTargetAngle(CurrentCenterPoint.CurAngle,curAngleQuadrant);
		
		if(angleDif > 0.01)//偏左大于0，右转
		{
			KincoStruct1.setSpeed = 20;
				
			KincoStruct2.setSpeed = -20;
		}
		else if(angleDif < -0.01)//偏右小于0，左转
		{
			KincoStruct1.setSpeed = -20;
				
			KincoStruct2.setSpeed = 20;
		}
		else
		{
			KincoStruct1.setSpeed = 0;
				
			KincoStruct2.setSpeed = 0;
			
			Reset_Alarm(0x080);
			SetAlarm(0x200);
			agv.Public.i_UpdatePathSig = 1;
			agv.Public.i_QRCodeCenterStopFlag = 0;
			agv.Public.i_HandCentringDown = 0;
			angleCorrectionFlag2 = 0;
		}
	}
	else//位置校准
	{
		if((fabs(Camera.X_pixel) < judgeOffset2 && fabs(Camera.Y_pixel) < judgeOffset2) && JudgementFlag2 == 1)//误差允许范围之内
		{
			angleCorrectionFlag2 =1;
		}
		else
		{
			JudgementFlag2 = 0;//关闭判断到位标志，开始纠偏
			
			Camera.lastCurAngle = CalculateCurAngle1(CurrentCenterPoint.CurAngle*57.3);//求当前朝向象限
			
			if(FloorStepFlag2 == 0)
			{
				if(Camera.X_pixel > controlOffset2)
				{
					agv.Command.AGVCenterControlStateDown = 2;//后退
				}
				else
				{
					agv.Command.AGVCenterControlStateDown = 1;//前进
				}
				if(fabs(Camera.X_pixel) < controlOffset2)
				{
					agv.Command.AGVCenterControlStateDown = 0;
					
					FloorStepFlag2 = 1;	
				}
			}
			else if(FloorStepFlag2 == 1)//第一步判断旋转方向，旋转
			{
				if(Camera.Y_pixel >= controlOffset2)//左偏左转后退
				{
					agv.Command.AGVCenterControlStateDown = 3;
					
					lastTurnDir = 3;
				}
				else if(Camera.Y_pixel < -controlOffset2)//右偏右转后退
				{
					agv.Command.AGVCenterControlStateDown = 4;
					
					lastTurnDir = 4;
				}
				else if(agv.Command.AGVCenterControlStateDown == 0)
				{
					agv.Command.AGVCenterControlStateDown = 0;
					
					FloorStepFlag2 = 0;
					
					JudgementFlag2 = 1;
				}
				
				if(fabs(CalculateDiffAngle1(CurrentCenterPoint.CurAngle*57.3,Camera.lastCurAngle) - 7) < 0.5)//转到位
				{
					agv.Command.AGVCenterControlStateDown = 0;
									
					FloorStepFlag2 = 2;
				}
			}
			else if(FloorStepFlag2 == 2)//第二步后退判断横向到位,后退多少距离
			{		
				backOffset = fabs(fabs(Camera.Y_pixel) - 3)*6;
				
				if(backOffset < 45)
					backDIS = backOffset;
				else
					backDIS = 45;
				
				agv.Command.AGVCenterControlStateDown = 2;//后退

				if(fabs(Camera.X_pixel) < controlOffset2 && fabs(Camera.Y_pixel) < controlOffset2)
				{
					agv.Command.AGVCenterControlStateDown = 0;
					
					FloorStepFlag2 = 3;
					
				}
				else if(fabs(Camera.Y_pixel) < controlOffset2)
				{
					agv.Command.AGVCenterControlStateDown = 0;
									
					FloorStepFlag2 = 3;
				}
				else if(Camera.X_pixel < -backDIS)
				{
					agv.Command.AGVCenterControlStateDown = 0;
					
					FloorStepFlag2 = 3;	
				}	
			}
			else if(FloorStepFlag2 == 3)//第三步回正
			{
				if(lastTurnDir == 3)
				{
					agv.Command.AGVCenterControlStateDown = 4;
					
					lastTurnDir2 = agv.Command.AGVCenterControlStateDown;
				}
				else if(lastTurnDir == 4)
				{
					agv.Command.AGVCenterControlStateDown = 3;
					
					lastTurnDir2 = agv.Command.AGVCenterControlStateDown;
				}
				if(fabs(CalculateDiffAngle1(CurrentCenterPoint.CurAngle*57.3,Camera.lastCurAngle) - 0) < 1 && ifirst == 0)
				{
					ifirst = 1;
				}
				if(fabs(CalculateDiffAngle1(CurrentCenterPoint.CurAngle*57.3,Camera.lastCurAngle) - 7) < 1 && ifirst == 1)
				{
					ifirst = 0;
					
					agv.Command.AGVCenterControlStateDown = 0;
									
					FloorStepFlag2 = 4;
					
					thirdYoffset = fabs(Camera.Y_pixel);
					
				}
			}
			else if(FloorStepFlag2 == 4)//第四步前进
			{
				if(thirdYoffset > 10)//偏差过大，需要往前多走一点
				{
					agv.Command.AGVCenterControlStateDown = 1;//前进
					
					if(Camera.X_pixel > 45)
					{
						agv.Command.AGVCenterControlStateDown = 0;
						
						FloorStepFlag2 = 5;	
					}
				}
				else
				{			
					frontOffset = fabs(fabs(Camera.Y_pixel) - 3)*6;
					
					if(frontOffset < 45)
						frontDIS = frontOffset;
					else
						frontDIS = 45;
	//				if(Camera.X_pixel > controlOffset2)
	//				{
	//					agv.Command.AGVCenterControlStateDown = 2;//后退
	//				}
	//				else
					{
						agv.Command.AGVCenterControlStateDown = 1;//前进
					}

					if(fabs(Camera.X_pixel) < controlOffset2 && fabs(Camera.Y_pixel) < controlOffset2)
					{
						agv.Command.AGVCenterControlStateDown = 0;
						
						FloorStepFlag2 = 5;
						
					}
					else if(fabs(Camera.Y_pixel) < controlOffset2)
					{
						agv.Command.AGVCenterControlStateDown = 0;
										
						FloorStepFlag2 = 5;
					}
					else if(fabs(Camera.X_pixel) < controlOffset2)
					{
						agv.Command.AGVCenterControlStateDown = 0;
						
						FloorStepFlag2 = 5;	
					}
					else if(Camera.X_pixel > frontDIS)
					{
						agv.Command.AGVCenterControlStateDown = 0;
						
						FloorStepFlag2 = 5;	
					}	
				}			
			}
			else if(FloorStepFlag2 == 5)//旋转回正
			{
				if(lastTurnDir2 == 3)
				{
					agv.Command.AGVCenterControlStateDown = 4;
					
				}
				else if(lastTurnDir2 == 4)
				{
					agv.Command.AGVCenterControlStateDown = 3;
					
				}
				if(fabs(CalculateDiffAngle1(CurrentCenterPoint.CurAngle*57.3,Camera.lastCurAngle) - 0) < 2)
				{	
					agv.Command.AGVCenterControlStateDown = 0;
					if((fabs(KincoStruct1.setSpeed) < 5)  &&  (fabs(KincoStruct2.setSpeed) < 5 )  )		
					{											
							FloorStepFlag2 = 0;
							
							JudgementFlag2 = 1;
					}
					
				}
			}
		}
	}
}

u8 FloorStepFlag = 1,JudgementFlag = 1;
void LookforQRCodeCenter()
{
//	static u8 StepFlag = 1,SelectFlag = 1;
	if((fabs(Camera.X_pixel) < (judgeOffset2 + 2) && fabs(Camera.Y_pixel) < (judgeOffset2 +2)) && JudgementFlag == 1)//误差允许范围之内
	{
		if(X1 & X2)
		{
			Rotate1.runState = 0;  //再复位旋转
			Reset_Alarm(0x0400);//复位报警
			Reset_Alarm(0x080);
		  SetAlarm(0x200);
		  agv.Public.i_UpdatePathSig = 1;
		  agv.Public.i_QRCodeCenterStopFlag = 0;
		  agv.Public.i_HandCentringDown = 0;
						
		}
		else
		{
			Rotate1.runState = 7; //再复位旋转
		}
		
	}
	else
	{
		JudgementFlag = 0;//关闭判断到位标志，开始纠偏
		
		if(FloorStepFlag == 1)//第一步纠正前后方向
		{
			if(Camera.X_pixel > controlOffset2)//超过允许误差
			{			
				agv.Command.AGVCenterControlStateDown = 2;//后退
			}
			else if(Camera.X_pixel < - controlOffset2)
			{			
				agv.Command.AGVCenterControlStateDown = 1;//前进
			}
			else
			{
				agv.Command.AGVCenterControlStateDown = 0;
				
				FloorStepFlag = 2;
			}
			Camera.lastCurAngle = CalculateCurAngle1(CurrentCenterPoint.CurAngle*57.3);//当前处于哪个轴
		}
		else if(FloorStepFlag == 2)//第二步左转
		{
			agv.Command.AGVCenterControlStateDown = 3;//开始转弯
			
			agv.Command.SetPlatformTurnAngle = 90;
			
			if(fabs(CalculateDiffAngle1(CurrentCenterPoint.CurAngle*57.3,Camera.lastCurAngle) - 90) < 0.5)//转到位
			{
				agv.Command.AGVCenterControlStateDown = 0;
				
				Camera.lastCurAngle = CalculateCurAngle1(CurrentCenterPoint.CurAngle*57.3);
				
				FloorStepFlag = 3;
			}
		}
		else if(FloorStepFlag == 3)//第三步纠正前后方向
		{
			if(Camera.X_pixel > controlOffset2)//超过允许误差
			{
				//后退
				agv.Command.AGVCenterControlStateDown = 2;
			}
			else if(Camera.X_pixel < -controlOffset2)
			{
				//前进
				agv.Command.AGVCenterControlStateDown = 1;
			}
			else
			{
				agv.Command.AGVCenterControlStateDown = 0;
				
				FloorStepFlag = 4;
			}
		}
		else if(FloorStepFlag == 4)//第四步回正
		{
			agv.Command.AGVCenterControlStateDown = 4;//开始转弯
			
			agv.Command.SetPlatformTurnAngle = 90;
			
			if(fabs(CalculateDiffAngle1(CurrentCenterPoint.CurAngle*57.3,Camera.lastCurAngle) - 90) < 0.5)//转到位
			{
				agv.Command.AGVCenterControlStateDown = 0;
				
				Camera.lastCurAngle = CalculateCurAngle1(CurrentCenterPoint.CurAngle*57.3);
				FloorStepFlag = 1;
				JudgementFlag = 1;
				
			}
		}
		
					
		
	}
}

void LookforQRCodeCenterSetSpeed()
{
	static float diffAngle = 0,SpeedValue = 0;
	
	diffAngle = fabs(CalculateDiffAngle1(CurrentCenterPoint.CurAngle*57.3,Camera.lastCurAngle) - 90);
	
	if(!angleCorrectionFlag && !angleCorrectionFlag2)
	{
	switch (agv.Command.AGVCenterControlStateDown)
	{
	case 0x00:
			KincoStruct1.setSpeed = 0;
			KincoStruct2.setSpeed = 0;
			break;
	case 0x01: //前进
			KincoStruct1.setSpeed = 30;
			KincoStruct2.setSpeed = 30;

			break;
	case 0x02: //后退
			KincoStruct1.setSpeed = -30;
			KincoStruct2.setSpeed = -30;
			break;
	case 0x03: //左转
			if(diffAngle > 45)
			{
				KincoStruct1.setSpeed = -agv.Parameter.angleTurnSpeed;
				
				KincoStruct2.setSpeed = agv.Parameter.angleTurnSpeed;
			}
			else if(diffAngle > 0.5)
			{
				SpeedValue = mapping(diffAngle,0.5,30,20,agv.Parameter.angleTurnSpeed);
				
				KincoStruct1.setSpeed = -SpeedValue;
				
				KincoStruct2.setSpeed = SpeedValue;
			}
			else
			{
				KincoStruct1.setSpeed = 0;
				
				KincoStruct2.setSpeed = 0;
			}
			break;
	case 0x04: //右转
			if(diffAngle > 45)
			{
				KincoStruct1.setSpeed = agv.Parameter.angleTurnSpeed;
				
				KincoStruct2.setSpeed = -agv.Parameter.angleTurnSpeed;
			}
			else if(diffAngle > 0.5)
			{
				SpeedValue = mapping(diffAngle,0.5,30,20,agv.Parameter.angleTurnSpeed);
				
				KincoStruct1.setSpeed = SpeedValue;
				
				KincoStruct2.setSpeed = -SpeedValue;
			}
			else
			{
				KincoStruct1.setSpeed = 0;
				
				KincoStruct2.setSpeed = 0;
			}
			break;
				
	}
}
}

extern u8 restRotateOkFlag;
/*SETVMOT动作指令解析*/
void commandActionAnalysis(int dataOne, int dataTwo)
{
	if(restRotateOkFlag)//复位完成再执行调度动作
	{
    switch (dataOne)
    {
    case 1: //举升1 下降2
        Lifter1.runState = dataTwo;
        break;

    case 2: //左旋转
				Rotate1.runState = dataOne;
        agv.Command.SetPlatformTurnAngle = dataTwo;
        break;

    case 3: //右旋转
				Rotate1.runState = dataOne;
        agv.Command.SetPlatformTurnAngle = dataTwo;
        break;

    case 4:              //打开充电状态
        switch (dataTwo) //小车运动方式设置       //
        {
        case 0:
            Y8(0); //关闭充电器触点
						traffic_land_marks.ChargingState = 0;
						agv.Public.StartChargingSig = 0;
            break;

        case 1:    
            Y8(1); //打开充电器触点
						traffic_land_marks.ChargingState = 1;
						agv.Public.StartChargingSig = 1;
            break;
        }
				break;

    case 5: //AGV原地旋转左转
				agv.Command.HandMotorState = dataOne;
        agv.Command.SetChassisTurnAngle = (float)dataTwo/57.3;
				agv.Command.SetPlatformTurnAngle = dataTwo;
				break;
		case 6: //AGV原地旋转左转
				agv.Command.HandMotorState = dataOne;
        agv.Command.SetChassisTurnAngle = (float)dataTwo/57.3;
				agv.Command.SetPlatformTurnAngle = dataTwo;
				break;
		case 7:              //打开充电状态
        switch (dataTwo) //小车运动方式设置       //
        {
        case 0:
            
						Reset_Alarm(0x0004);//启动
            break;

        case 1:    
            
						SetAlarm(0x0004);//停车
            break;
        }
				break;
    }
	}
}

int RecodeHandTime;
void commandActionAnalysisRunHand(int dataOne, int dataTwo)
{
//	if(restRotateOkFlag)//复位完成再执行调度动作
	{
		if(agv.Command.Au_Hand == 0)
			RecodeHandTime = agv.Public.SystemTime;
    switch (dataOne)
    {
			case 1: 
					switch (dataTwo) 
					{
						case 0:		
							agv.Command.Au_Hand = 0;	
              agv.Command.LandMarkID = 0;	
               						
						break;		//切手动

						case 1:		agv.Command.Au_Hand = 1;	break;		//切自动    								
					}
					break;
		if(agv.Command.Au_Hand == 0)//手动情况下才接收遥控指令
		{
			case 2: 
					switch (dataTwo)
					{
						case 0:		agv.Command.HandMotorState = 0;		break;  	//前进停止
								
						case 1:		agv.Command.HandMotorState = 1;		break;		//前进																						
					}
					break;

			case 3:
					switch (dataTwo)
					{
						case 0:		agv.Command.HandMotorState = 0;		break;		//后退停止		

						case 1:   agv.Command.HandMotorState = 2;		break;		//后退														
					}
					break;

			case 4:
					switch (dataTwo)
					{
						case 0:		agv.Command.HandMotorState = 0;		break;		//左转停止
								
						case 1:		agv.Command.HandMotorState = 3;		break;		//左转    								
					}
					break;

			case 5: 
					switch (dataTwo) 
					{
						case 0:		agv.Command.HandMotorState = 0;		break;		//右转停止
								
						case 1:		agv.Command.HandMotorState = 4;		break;		//右转						
					}
					break;
			case 6: 
					switch (dataTwo) 
					{
						case 0:		Lifter1.runState = 0;		break;		//上升停止

						case 1:		Lifter1.runState = 1;		break;		//上升   								
					}
					break;
			case 7:              
					switch (dataTwo) 
					{
						case 0:		Lifter1.runState = 0;		break;		//下降停止

						case 1:		Lifter1.runState = 2;		break;		//下降						
					}
					break;
		}
    }
	}
}
#endif