305 lines
12 KiB
C#
305 lines
12 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.Drawing;
|
|
using System.Linq;
|
|
using System.Text;
|
|
using Project.StateMachine;
|
|
using COMM;
|
|
using AR;
|
|
using AGVNavigationCore.Models;
|
|
using static arDev.Narumi;
|
|
|
|
namespace Project
|
|
{
|
|
public partial class fMain
|
|
{
|
|
/// <summary>
|
|
/// 장비로 진입한다. -
|
|
/// </summary>
|
|
public Boolean _SM_RUN_ENTER(bool isFirst, TimeSpan seqTime)
|
|
{
|
|
var idx = 1;
|
|
var funcname = PUB.sm.RunStep.ToString();
|
|
|
|
//충전 상태가 OFF되어야 동작하게한다
|
|
if (_SM_RUN_CHARGE_OFF(isFirst, seqTime) == false) return false;
|
|
|
|
//라이더멈춤이 설정되어있다면 음성으로 알려준다
|
|
if (CheckLiderStop() == false) return false;
|
|
|
|
if (PUB.sm.RunStepSeq == idx++)
|
|
{
|
|
PUB.log.Add("IN작업-시작");
|
|
PUB.Speak("작업을 시작합니다");
|
|
PUB.AGV.AGVMoveStop(funcname);
|
|
PUB.sm.UpdateRunStepSeq();
|
|
return false;
|
|
}
|
|
else if (PUB.sm.RunStepSeq == idx++)
|
|
{
|
|
//장비 노드 여부 확인 (버퍼가 아닌 도킹 가능 노드여야 함)
|
|
if (PUB._virtualAGV.CurrentNode.StationType != StationType.Loader &&
|
|
PUB._virtualAGV.CurrentNode.StationType != StationType.Plating &&
|
|
PUB._virtualAGV.CurrentNode.StationType != StationType.Cleaner)
|
|
{
|
|
SetRunStepError(ENIGProtocol.AGVErrorCode.NOT_EQUIPMENTPOINT, $"[{funcname}-{PUB.sm.RunStepSeq}] 현재 위치가 장비 노드가 아닙니다({PUB._virtualAGV.CurrentNode.StationType})");
|
|
return false;
|
|
}
|
|
PUB.sm.UpdateRunStepSeq();
|
|
return false;
|
|
}
|
|
else if (PUB.sm.RunStepSeq == idx++)
|
|
{
|
|
//마크센서가 감지된상태여야 완전한위치로 가정한다
|
|
if (PUB.AGV.signal1.mark_sensor == false)
|
|
{
|
|
SetRunStepError(ENIGProtocol.AGVErrorCode.MARK_SENSOR_FAIL, $"[{funcname}] 마크센서가 감지되지 않습니다");
|
|
}
|
|
PUB.sm.UpdateRunStepSeq();
|
|
return false;
|
|
}
|
|
else if (PUB.sm.RunStepSeq == idx++)
|
|
{
|
|
//작업상태에 따라서 카트감지여부를 확인한ㄷ.
|
|
if (PUB.NextWorkCmd == ENIGProtocol.AGVCommandHE.PickOnEnter)
|
|
{
|
|
//가지러가야하는 경우이므로 카트가 없어야한다.
|
|
if (PUB.AGV.signal2.cart_detect1 || PUB.AGV.signal2.cart_detect2)
|
|
{
|
|
SetRunStepError(ENIGProtocol.AGVErrorCode.CART_EXIST, $"[{funcname}] 카트가 존재하여 진입을 할 수 없습니다");
|
|
return false;
|
|
}
|
|
}
|
|
else if (PUB.NextWorkCmd == ENIGProtocol.AGVCommandHE.PickOffEnter)
|
|
{
|
|
//가져다 놓아야하므로 카트가 존재해야 한다
|
|
if (PUB.AGV.signal2.cart_detect1 == false || PUB.AGV.signal2.cart_detect2 == false)
|
|
{
|
|
var errmsg = $"[{funcname}] 카트감지센서가 인식되지 않았습니다";
|
|
SetRunStepError(ENIGProtocol.AGVErrorCode.CART_EXIST, errmsg);
|
|
return false;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
var errmsg = $"[{funcname}] 알수없는 작업형태입니다({PUB.NextWorkCmd})";
|
|
SetRunStepError(ENIGProtocol.AGVErrorCode.UnknownCommand, errmsg);
|
|
return false;
|
|
}
|
|
|
|
PUB.sm.UpdateRunStepSeq();
|
|
return false;
|
|
}
|
|
else if (PUB.sm.RunStepSeq == idx++)
|
|
{
|
|
//pick off/on 상관없이 리프트는 내려서 이동한다
|
|
var liftCmd = LiftCommand.DN;
|
|
PUB.AGV.LiftControl(liftCmd);
|
|
VAR.TIME.Update(eVarTime.LastTurnCommandTime);
|
|
PUB.sm.UpdateRunStepSeq();
|
|
return false;
|
|
}
|
|
else if (PUB.sm.RunStepSeq == idx++)
|
|
{
|
|
//리프트 센서 확인
|
|
if ((PUB.AGV.signal1.lift_down == true && PUB.AGV.signal1.lift_up == false) == false)
|
|
{
|
|
var ts = VAR.TIME.RUN(eVarTime.LastTurnCommandTime);
|
|
if (ts.TotalSeconds > 10)
|
|
{
|
|
var errmsg = $"[{funcname}] 리프트다운이 확인되지 않습니다";
|
|
SetRunStepError(ENIGProtocol.AGVErrorCode.LIFT_ERROR, errmsg);
|
|
}
|
|
return false;
|
|
}
|
|
|
|
VAR.I32[eVarInt32.RetryManget] = 0;
|
|
PUB.log.Add("리프트 동작 확인 완료");
|
|
PUB.sm.UpdateRunStepSeq();
|
|
return false;
|
|
}
|
|
else if (PUB.sm.RunStepSeq == idx++)
|
|
{
|
|
//pick off 조건이라면. 마그넷을 on 한후에 민다.
|
|
if (PUB.NextWorkCmd == ENIGProtocol.AGVCommandHE.PickOffEnter)
|
|
{
|
|
VAR.I32[eVarInt32.RetryManget] += 1;
|
|
PUB.AGV.LiftControl(LiftCommand.ON);
|
|
}
|
|
PUB.sm.UpdateRunStepSeq();
|
|
return false;
|
|
}
|
|
else if (PUB.sm.RunStepSeq == idx++)
|
|
{
|
|
//pick off 조건이라면. 마그넷을 on 한후에 민다.
|
|
if (PUB.NextWorkCmd == ENIGProtocol.AGVCommandHE.PickOffEnter)
|
|
{
|
|
//마그넷센서가 들어오는지 5초간 확인한다.
|
|
if (PUB.AGV.signal1.magnet_on == false)
|
|
{
|
|
if (seqTime.TotalSeconds > 5)
|
|
{
|
|
if (VAR.I32[eVarInt32.RetryManget] < 3)
|
|
{
|
|
PUB.sm.UpdateRunStepSeq(-1);
|
|
}
|
|
else
|
|
{
|
|
var errmsg = $"[{funcname}] 마그넷이 ON 되지 않았습니다";
|
|
SetRunStepError(ENIGProtocol.AGVErrorCode.MAGNET_ON_ERROR, errmsg);
|
|
VAR.I32[eVarInt32.RetryManget] = 0;
|
|
}
|
|
|
|
}
|
|
return false;
|
|
}
|
|
}
|
|
PUB.sm.UpdateRunStepSeq();
|
|
return false;
|
|
}
|
|
|
|
else if (PUB.sm.RunStepSeq == idx++)
|
|
{
|
|
//저속이동 (후진 진입)
|
|
var ret = PUB.AGV.AGVMoveSet(new arDev.Narumi.BunkiData
|
|
{
|
|
Bunki = arDev.Narumi.eBunki.Strate,
|
|
Direction = arDev.Narumi.eMoveDir.Backward,
|
|
PBSSensor = 0,
|
|
Speed = arDev.Narumi.eMoveSpd.Low,
|
|
});
|
|
|
|
//명령이 실패되었다면 재시도를 한다
|
|
if (ret != arDev.eNarumiCommandResult.Success)
|
|
{
|
|
if (ret >= arDev.eNarumiCommandResult.Error)
|
|
{
|
|
SetRunStepError(ENIGProtocol.AGVErrorCode.AGV_SPEED_SET_FAIL);
|
|
}
|
|
return false;
|
|
}
|
|
|
|
VAR.TIME.Update(eVarTime.LastTurnCommandTime);
|
|
PUB.sm.UpdateRunStepSeq();
|
|
return false;
|
|
}
|
|
else if (PUB.sm.RunStepSeq == idx++)
|
|
{
|
|
//전진이동
|
|
PUB.AGV.AGVMoveRun(arDev.Narumi.eRunOpt.Backward);
|
|
PUB.sm.UpdateRunStepSeq();
|
|
return false;
|
|
}
|
|
else if (PUB.sm.RunStepSeq == idx++)
|
|
{
|
|
//AGV구동을 확인하고 마크스탑을 설정한다.
|
|
if (PUB.AGV.system1.agv_run == false)
|
|
{
|
|
if (seqTime.TotalMilliseconds > 1000)
|
|
{
|
|
//구동이확인되지 않으면 오류처리를 한다.
|
|
SetRunStepError(ENIGProtocol.AGVErrorCode.AGV_RUN_FAIL);
|
|
}
|
|
return false;
|
|
}
|
|
//마크스탑설정
|
|
PUB.AGV.AGVMoveStop(funcname, arDev.Narumi.eStopOpt.MarkStop);
|
|
PUB.sm.UpdateRunStepSeq();
|
|
return false;
|
|
}
|
|
else if (PUB.sm.RunStepSeq == idx++)
|
|
{
|
|
//마크스탑신호가 3초이내로 들어와야 한다
|
|
if (PUB.AGV.data.Speed != 'S')
|
|
{
|
|
if (seqTime.TotalMilliseconds > 3000)
|
|
{
|
|
SetRunStepError(ENIGProtocol.AGVErrorCode.MARK_TIMEOUT);
|
|
}
|
|
return false;
|
|
}
|
|
PUB.sm.UpdateRunStepSeq();
|
|
return false;
|
|
}
|
|
else if (PUB.sm.RunStepSeq == idx++)
|
|
{
|
|
//AGV가 멈출때까지 기다린다.
|
|
if (PUB.AGV.system1.agv_run == true)
|
|
{
|
|
if (seqTime.TotalSeconds > 10)
|
|
{
|
|
SetRunStepError(ENIGProtocol.AGVErrorCode.AGV_STOP_FAIL);
|
|
}
|
|
return false;
|
|
}
|
|
PUB.sm.UpdateRunStepSeq();
|
|
return false;
|
|
}
|
|
|
|
else if (PUB.sm.RunStepSeq == idx++)
|
|
{
|
|
//마크센서입력을 확인한다.
|
|
if (PUB.AGV.signal1.mark_sensor == false)
|
|
{
|
|
if (seqTime.TotalSeconds > 5)
|
|
{
|
|
SetRunStepError(ENIGProtocol.AGVErrorCode.MARK_SENSOR_FAIL);
|
|
}
|
|
return false;
|
|
}
|
|
PUB.sm.UpdateRunStepSeq();
|
|
return false;
|
|
}
|
|
else if (PUB.sm.RunStepSeq == idx++)
|
|
{
|
|
//완료되었다.
|
|
PUB.log.Add("진입 완료 마그넷을 off합니다");
|
|
VAR.I32[eVarInt32.RetryManget] = 0;
|
|
PUB.sm.UpdateRunStepSeq();
|
|
return false;
|
|
}
|
|
else if (PUB.sm.RunStepSeq == idx++)
|
|
{
|
|
//pick off 조건이라면. 마그넷을 on 한후에 민다.
|
|
if (PUB.NextWorkCmd == ENIGProtocol.AGVCommandHE.PickOffEnter)
|
|
{
|
|
VAR.I32[eVarInt32.RetryManget] += 1;
|
|
PUB.AGV.LiftControl(LiftCommand.OFF);
|
|
}
|
|
PUB.sm.UpdateRunStepSeq();
|
|
return false;
|
|
}
|
|
else if (PUB.sm.RunStepSeq == idx++)
|
|
{
|
|
//pick off 조건이라면. 마그넷을 on 한후에 민다.
|
|
if (PUB.NextWorkCmd == ENIGProtocol.AGVCommandHE.PickOffEnter)
|
|
{
|
|
//마그넷센서가 들어오는지 5초간 확인한다.
|
|
if (PUB.AGV.signal1.magnet_on == true)
|
|
{
|
|
if (seqTime.TotalSeconds > 5)
|
|
{
|
|
if (VAR.I32[eVarInt32.RetryManget] < 3)
|
|
{
|
|
PUB.sm.UpdateRunStepSeq(-1);
|
|
}
|
|
else
|
|
{
|
|
VAR.I32[eVarInt32.RetryManget] = 0;
|
|
var errmsg = $"[{funcname}] 마그넷이 OFF 되지 않았습니다";
|
|
SetRunStepError(ENIGProtocol.AGVErrorCode.MAGNET_OF_ERROR, errmsg);
|
|
}
|
|
}
|
|
return false;
|
|
}
|
|
}
|
|
PUB.sm.UpdateRunStepSeq();
|
|
return false;
|
|
}
|
|
|
|
|
|
return true;
|
|
}
|
|
}
|
|
}
|