AGV / agv_gelinmei · Files

#include "RunCore.h"
#include "DisplacementSensor.h"

//雷达避障减速处理
int laserSlowDownProcess(int baseSpeed)
{
    if (agv.Public.i_DoubleReduction && baseSpeed > 50) //先执行二级避障，如果速度大于50减速到50
    {
        baseSpeed = 50;
    }
    else if (agv.Public.i_Reduction && baseSpeed > 100) //没有二级避障就执行一级避障,减速到100
    {
        baseSpeed = 100;
    }
    return baseSpeed;
}

void setMotorSpeedSlope(float speedSlope)
{
    KincoStruct1.speedSlope = speedSlope;

    KincoStruct2.speedSlope = speedSlope;
}

void StopChassis(float speedSlope)
{
//    KincoStruct1.setSpeed = 0;
//		KincoStruct2.setSpeed = 0;
	DriverSteering1.Command.speed = 0; 
}

void StopAgv(int speedSlope)
{
	DriverSteering1.Command.speed = 0;
	DriverLifter1.Command.speed = 0;
//		KincoStruct1.setSpeed = 0;
//		KincoStruct2.setSpeed = 0;
//		Lifter1.setSpeed = 0;
//		Rotate1.setSpeed = 0;
		
}

//报警代码处理，返回1代表没有报警，返回0代表报警
u8 alarmCodeProcess(u32 ErrorFlag)
{
    static int i = 0, i2 = 0;
    if (ErrorFlag & 0x9b9a) //碰撞/急停/脱轨/系统预停车停车PLC急停
    {
        if (ErrorFlag & 0x1000) //定位跳点，报脱轨
        {
            LED_color(LED_red, 1000);

            agv.Command.MusicFlag = Music_Derail; //语音提示
        }
				else if (ErrorFlag & 0x0100) //急停停车
        {
            LED_color(LED_red, 0);

            agv.Command.MusicFlag = Music_Emg; //语音提示
        }
        else if (ErrorFlag & 0x0200) //目标停车
        {
            LED_color(LED_green, 1000);
            agv.Command.MusicFlag = Music_Arrivel; //语音提示
            StopChassis(10);
//            return 1;
        }
        else if (ErrorFlag & 0x0008) //脱轨
        {
            LED_color(LED_yellow, 100);

            agv.Command.MusicFlag = Music_Derail;
        }
				else if (ErrorFlag & 0x8000) //距离偏差脱轨
        {
            LED_color(LED_yellow, 100);

            agv.Command.MusicFlag = Music_Derail;
        }
        else if (ErrorFlag & 0x0080) //
        {
            LED_color(LED_yellow, 1000);
						agv.Command.MusicFlag = Music_Derail;
        }
        else if (ErrorFlag & 0x0800) //驱动器报警
        {
            LED_color(LED_red, 1000);

            agv.Command.MusicFlag = Music_Driver;
        }
        else if (ErrorFlag & 0x0002) //碰撞停车
        {
            LED_color(LED_red, 100);

            agv.Command.MusicFlag = Music_Et; //语音提示
        }
        else
        {
            LED_color(LED_red, 1000);
        }
        StopAgv(50);

        return 0;
    }
    else if (ErrorFlag & 0x6067) //红外/暂停/单机停车/系统停车
    {
        if (ErrorFlag & 0x4000) //低电量
        {
            LED_color(LED_yellow, 1000);

            agv.Command.MusicFlag = Music_Power; //语音提示
        }
        else if (ErrorFlag & 0x0020) //货架纠偏
        {
            LED_color(LED_green, 1000);
        }
        else if (ErrorFlag & 0x0040) //系统停车
        {
            LED_color(LED_green, 1000);
						
						agv.Command.MusicFlag = Music_close;
											
            StopChassis(10);
            return 1;
        }
        else if (ErrorFlag & 0x0004) //暂停
        {
					if(Camera.UpdataFlag)
					{
						LED_color(LED_yellow, 100);
					}
					else
						LED_color(LED_yellow, 1000);

						agv.Command.MusicFlag = Music_Pause;
        }
        else if (ErrorFlag & 0x0001) //红外
        {
            LED_color(LED_yellow, 0);

            agv.Command.MusicFlag = Music_Laser; //语音提示
        }
				else if (ErrorFlag & 0x2000) //定位失败
        {
            LED_color(LED_yellow, 1000);

            agv.Command.MusicFlag = Music_Derail;
        }
        else
        {
            LED_color(LED_red, 500);
        }
        StopAgv(5);

        return 0;
    }
    else
    {
				LED_color(LED_green, 1000);

        agv.Command.MusicFlag = Music; //正常音乐播放
							
        return 1;
    }
}



void OpenOrCloseWheelPower(u8 *OpenWheelPowerFlag)
{
	static u8 iFirstFlag = 1;
	static int lastTime = 0;
	//第一次上电三秒后车体才上电
	if(iFirstFlag)
	{
		if(agv.Public.SystemTime > 30000)
		{
			*OpenWheelPowerFlag = 1;
			
			iFirstFlag = 0;
		}
	}
	else
	{
		if(*OpenWheelPowerFlag == 0)
		{
			if(agv.Public.SystemTime - lastTime > 2000)
			*OpenWheelPowerFlag = 1;
		}
		else
			lastTime = agv.Public.SystemTime;
	}
	if(*OpenWheelPowerFlag)
		Y5(1);//上电
	else
		Y5(0);//下电
}

extern u8 chargingFlag;
//agv主要任务处理
void AGVRunCore()
{
	static bool colorFlag = false;
	static u8 firstFlag = 0;
	OpenOrCloseWheelPower(&agv.Public.i_OpenWheelPowerFlag);


    if (!agv.Command.Au_Hand)//手动模式
    {
			firstFlag = 1;
			if(chargingFlag)
			{
				Y9(1);//打开自动充电
				sendCommandToCharging(1);//打开充电
			}
			else
			{
				Y9(0);//打开自动充电
				sendCommandToCharging(0);//打开充电
			}
		
        LED_color(LED_yellow, 1000);//手动模式黄色

        agv.Command.MusicFlag = Music_close;//关闭音乐播放

				chassisControlManual();		//底盘点动控制

				platformControlManual(); 	//平台点动控制

					clearPathInfomation();	//清除路径信息
    }
    else //自动模式
    {
			static u8 i = 0;
			if(firstFlag)
			{
				if(i++ > 200)
				{
					firstFlag = 0;
					i = 0;
				}
				chargingFlag = 0;
				Y9(0);//关闭自动充电
				sendCommandToCharging(0);//关闭自动充电
			}
			
				slamNavigation();//无轨导航，计算路径更新，直行、转弯、旋转偏移量，以及通过偏移计算pid值。

			if (alarmCodeProcess(agv.Public.Error_Flag))//报警代码处理返回1（待完成超距、通讯超时报警），证明没有报警，进入运动控制部分
			{
				InitFlag = 0;
					chassisGetAutoSpeed(&agv.Command.CurDirection);															//****行走计算部分****根据不同运动方向 平移 直行 旋转 转弯以及距离目的地距离 确定agv速度

					platformControlAuto();																											//****平台计算和执行部分****待完成不同底盘上部结构运动处理 平台状态可能会要求agv减速或停车已完成对接

					agv.Command.SetBaseSpeed = laserSlowDownProcess(agv.Command.SetBaseSpeed); 	//****雷达判断部分****雷达避障减速处理 agv整体速度减速

					chassisControlAuto();																												//****行走执行部分****根据不同底盘 agv整体速度 通过pid值计算差速agv的速度差或者舵轮agv的角度等 设置驱动器速度,避障减速处理
			}
			if(agv.Public.Error_Flag == 0x200 || agv.Public.Error_Flag == 0x040)//路径走完有停稳信号可以旋转
			{
				platformControlAuto();
				chassisControlManual();//充电旋转
			}
			if(agv.Public.i_ForkliftAngleAdjustment)
			{
				ForkliftAngleAdjustment();//叉车角度调整
			}
			
		}
				
}

void _AppTaskRunCoreUpdate(void)
{
			SlamDataProcess(); 	//工控机SLAM通讯
	
		platformDataProcess();//根据不同平台IO数据处理


    HMIDataUpdate(); 			//触摸屏数据更新
		if(agv.Command.CurDirection ==  7|| agv.Command.CurDirection == 8||agv.Command.CurDirection == 9||agv.Command.CurDirection == 10)
		{
			if(agv.Command.LaserArea!=0)
			agv.Command.LaserArea = 1;//旋转区域设置
		}

    Laser_Run(agv.Command.LaserArea); 	//激光区域选择


    AGVRunCore(); 											//agv运动控制和逻辑处理
		
		//获取举升高度
		getLiftHeight();	
		
		liftEncoderDataProcess();
		
		receiveCommandFromCharging();
}