AGV / agv_gelinmei · 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
}

//在静止下计算，在运动中补偿
float CalculateAngleOffset(float curAngle)
{
	static float allowOffset = 20;
	int dir = 1;//
	float angle = 0;
	if(fabs(curAngle) < allowOffset)
	{
		if(curAngle > 0)//左偏
		{
			dir = 1;
			angle = fabs(curAngle);
		}
		else//右偏
		{	
			dir = -1;
			angle = fabs(curAngle);
		}
	}
	else if(fabs(curAngle - 90) < allowOffset)
	{
		if(curAngle > 90)//左偏
		{
			dir = 1;
			angle = fabs(curAngle - 90);
		}
		else//右偏
		{	
			dir = -1;
			angle = fabs(curAngle - 90);
		}
	}
	else if(fabs(fabs(curAngle) - 180) < allowOffset)
	{
		if(curAngle > 0)//右偏
		{
			dir = -1;
			angle = fabs(fabs(curAngle) - 180);
		}
		else//左偏
		{	
			dir = 1;
			angle = fabs(fabs(curAngle) - 180);
		}
	}
	else if(fabs(curAngle + 90) < allowOffset)
	{
		if(curAngle > -90)//左偏
		{
			dir = 1;
			angle = fabs(curAngle + 90);
		}
		else//右偏
		{	
			dir = -1;
			angle = fabs(curAngle + 90);
		}
	}
	return dir*angle;
}

u8 liftFirstToZero1 = 1;
void lifterRunAuto() //举升自动运行,运行状态0不动  1上升  2下降 
{		
		switch (Lifter1.runState)
		{
			case 0:
				Lifter1.setSpeed = 0;
				break;
			case 1:
				Lifter1.setSpeed = 2000000;
				break;
			case 2:
				Lifter1.setSpeed = -2000000;
				break;
			case 3:
				break;
			case 4:
				break;
			case 5:
				break;
			case 6:
				break;
			case 7:
				break;
			case 8:
				break;
			default:
				break;
			
		}
		
		if(liftFirstToZero1)
		{
			Lifter1.setSpeed = -2000000;
		}

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

//int turnCount = 0;
float palstanceOffset = 0.005,palstanceMin = 0.12,setArriveAngle = 1,setPalstance = 0,upSlopeSpeed = 0.00035,downSlopeSpeed = 0.003;
float angleOffset = 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)
	{
		angleOffset = CalculateAngleOffset(Rotate1.encodeAngle);
	}
	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);//fabs(Rotate1.encodeAngle - lastAngle);

		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;
			}
//			if(diffAngle > 30)
//			{
////				setPalstance = fabs(KincoStruct1.setSpeed)/360;
//				if(fabs(KincoStruct1.setSpeed)/360 >= palstanceMin)
//				{
//					setPalstance = fabs(KincoStruct1.setSpeed)/360;
//				}
//				else 
//				{
//					setPalstance = palstanceMin;
//				}
//			}
//			else 
//			{
//				if(fabs(KincoStruct1.setSpeed)/360 >= palstanceMin)
//				{
//					setPalstance = fabs(KincoStruct1.setSpeed)/360;
//				}
//				else 
//				{
//					setPalstance = palstanceMin;
//				}
//			}
		}
		
//			if(setPalstance > lastPalstance)
//			{
//				setPalstance = lastPalstance + upSlopeSpeed;
//			}
//			else if(setPalstance < lastPalstance)
//			{
//				setPalstance = lastPalstance - downSlopeSpeed;
//			}
//			if(setPalstance < 0)
//			{
//				setPalstance = 0;
//			}
//			lastPalstance = setPalstance;

		switch (Rotate1.runState)
		{
			case 0:
				Rotate1.setSpeed = 0;
				lastAngle = Rotate1.encodeAngle;
				break;
			case 1://右转90度
				Rotate1.setSpeed = -500000;//PalstanceMax*6666666.7;//-500000;
//				if(fabs(diffAngle) <=setArriveAngle)
//				{
//					Rotate1.setSpeed = 0;
//					Rotate1.runState = 0;
//				}	
				break;
			case 2://左转
				Rotate1.setSpeed = setPalstance*6666666.7;
				if(fabs(diffAngle) <=setArriveAngle)
				{
					Rotate1.setSpeed = 0;
					Rotate1.runState = 0;
				}
				break;
			case 3://右转
					Rotate1.setSpeed = -setPalstance*6666666.7;//IMU.palstance
					
					if(diffAngle < 5 && (Rotate1.encodeAngle > -10 && Rotate1.encodeAngle < 10))//回零点且转盘快到位
					{
						if(X1)
						{
							Rotate1.setSpeed = 0;
							Rotate1.runState = 0;
							Rotate1.encodeAngle = 0;
							lastAngle = 0;
						}
					}
					else if(diffAngle < setArriveAngle)
					{
						Rotate1.setSpeed = 0;
						Rotate1.runState = 0;
					}			
				break;
			case 4://
					Rotate1.setSpeed = setPalstance*6666666.7;
					if(diffAngle < 5 && (Rotate1.encodeAngle > -10 && Rotate1.encodeAngle < 10))//回零点且转盘快到位
					{
						if(X1)//零点触发
						{
							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;//IMU.palstance
			
//			Rotate1.setSpeed = -fabs(KincoStruct1.encodeSpeed/360)*6666666.7;
				if(diffAngle < 5 && (Rotate1.encodeAngle > -10 && Rotate1.encodeAngle < 10))//回零点且转盘快到位
				{
					if(X1)//零点触发
					{
						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;
				if(diffAngle < 5 && (Rotate1.encodeAngle > -10 && Rotate1.encodeAngle < 10))//回零点且转盘快到位
				{
					if(X1)//零点触发
					{
						Rotate1.setSpeed = 0;
						Rotate1.runState = 0;
						Rotate1.encodeAngle = 0;
						lastAngle = 0;
					}
				}
				else if(diffAngle < setArriveAngle)
				{
					Rotate1.setSpeed = 0;
					Rotate1.runState = 0;
				}		
				break;
			case 7:
				break;
			case 8:
				break;
			default:
				break;
			
		}
		
}



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);
		lifterRunAuto();
    RotateRunAuto();
}

void platformControlAuto() //平台自动对接
{
    lifterRunAuto();
    RotateRunAuto();
}

void X_Input()
{
	static int lastTime = 0;
//	agv.Public.i_RadarSigFront_1 			= MasterInput.IN1 = !X12;//前壁障减速1
//	agv.Public.i_RadarSigFront_2 			= MasterInput.IN2 = !X13;//前壁障减速2
//	agv.Public.i_RadarSigFront_3 			= MasterInput.IN3 = !X14;//前壁障减速3
	
//	agv.Public.i_RadarSigBack_1 			= MasterInput.IN4 = !X4;//后壁障减速1
//	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_BumperSig 						= MasterInput.IN8 = X8;//防撞条
//	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;//右到位对射
	if(agv.Public.i_StartSig)//第一次上电按启动复位举升
	{
		agv.Public.i_StartFirst = true;
	}
	if(agv.Public.i_Lift1DownLimit)//下限位触发  位置清零
	{
		Lifter1.encodePos = 0;
	}
//	if(agv.Public.i_StopSig)
//	{
//		if(agv.Public.SystemTime - lastTime >= 3000)
//		{
//			resetLiftFlag = 1;
//		}
//	}
//	else
//		lastTime = agv.Public.SystemTime;
}

void platformDataProcess(void)
{
	X_Input();//IO输入信号
	//自动模式且开启红外，雷达避障信号
	if(agv.Command.Au_Hand&&agv.Command.Barrier_ONOFF)
	{
		if(agv.Command.CurDirection||agv.Command.HandMotorState)//前进方向
		{
			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//不开启避障
	{
		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)
	{	
		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)
	{
		//前进后退，弧线左右转做脱轨判断
		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) > 100)//位置偏差大于10CM，脱轨
			{
				SetAlarm(0x0008);//脱轨报警
			}
			else
				Reset_Alarm(0x0008);//取消脱轨报警
		}
		else
			Reset_Alarm(0x0008);//取消脱轨报警
	}
}


/*SETVMOT动作指令解析*/
void commandActionAnalysis(int dataOne, int dataTwo)
{
    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); //关闭充电器触点
            break;

        case 1:    
            Y8(1); //打开充电器触点
            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;
        }
    }
}
#endif