diff --git a/AGVLogic/EnigProtocol/enigprotocol/Commands.cs b/AGVLogic/EnigProtocol/enigprotocol/Commands.cs index a4ac3fb..b846ce9 100644 --- a/AGVLogic/EnigProtocol/enigprotocol/Commands.cs +++ b/AGVLogic/EnigProtocol/enigprotocol/Commands.cs @@ -74,6 +74,7 @@ namespace ENIGProtocol MARK_STOP_FAIL, LIDAR_STOP, NOT_BUFFERPOINT, + NOT_EQUIPMENTPOINT, } public static class AGVUtility @@ -111,6 +112,7 @@ namespace ENIGProtocol case AGVErrorCode.ALREADY_CHARGE: return "이미 충전 중 상태임"; case AGVErrorCode.CHARGE_RETRY_OVER: return $"충전명령 재전송 횟수 초과"; case AGVErrorCode.NOT_BUFFERPOINT: return "현재 위치가 버퍼가 아닙니다"; + case AGVErrorCode.NOT_EQUIPMENTPOINT: return "현재 위치가 장비 노드가 아닙니다"; default: return ecode.ToString(); } } diff --git a/HMI/Project/StateMachine/Step/_SM_RUN_CHARGE_ON.cs b/HMI/Project/StateMachine/Step/_SM_RUN_CHARGE_ON.cs index 0ab3b00..93b28af 100644 --- a/HMI/Project/StateMachine/Step/_SM_RUN_CHARGE_ON.cs +++ b/HMI/Project/StateMachine/Step/_SM_RUN_CHARGE_ON.cs @@ -54,7 +54,6 @@ namespace Project return false; } - var idx = 1; if (PUB.sm.RunStepSeq == idx++) { diff --git a/HMI/Project/StateMachine/Step/_SM_RUN_ENTER.cs b/HMI/Project/StateMachine/Step/_SM_RUN_ENTER.cs index ff8aa1e..e585682 100644 --- a/HMI/Project/StateMachine/Step/_SM_RUN_ENTER.cs +++ b/HMI/Project/StateMachine/Step/_SM_RUN_ENTER.cs @@ -36,6 +36,19 @@ namespace Project return false; } else if (PUB.sm.RunStepSeq == idx++) + { + //장비 노드 여부 확인 (버퍼가 아닌 도킹 가능 노드여야 함) + if (PUB._virtualAGV.CurrentNode.StationType != StationType.Loader && + PUB._virtualAGV.CurrentNode.StationType != StationType.Clearner && + PUB._virtualAGV.CurrentNode.StationType != StationType.UnLoader) + { + 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) @@ -126,7 +139,7 @@ namespace Project { if (seqTime.TotalSeconds > 5) { - if(VAR.I32[eVarInt32.RetryManget] < 3) + if (VAR.I32[eVarInt32.RetryManget] < 3) { PUB.sm.UpdateRunStepSeq(-1); } @@ -136,7 +149,7 @@ namespace Project SetRunStepError(ENIGProtocol.AGVErrorCode.MAGNET_ON_ERROR, errmsg); VAR.I32[eVarInt32.RetryManget] = 0; } - + } return false; } diff --git a/HMI/Project/StateMachine/Step/_SM_RUN_EXIT.cs b/HMI/Project/StateMachine/Step/_SM_RUN_EXIT.cs index 0480bbd..83a173b 100644 --- a/HMI/Project/StateMachine/Step/_SM_RUN_EXIT.cs +++ b/HMI/Project/StateMachine/Step/_SM_RUN_EXIT.cs @@ -37,6 +37,19 @@ namespace Project return false; } else if (PUB.sm.RunStepSeq == idx++) + { + //장비 노드 여부 확인 (버퍼가 아닌 도킹 가능 노드여야 함) + if (PUB._virtualAGV.CurrentNode.StationType != StationType.Loader && + PUB._virtualAGV.CurrentNode.StationType != StationType.Clearner && + PUB._virtualAGV.CurrentNode.StationType != StationType.UnLoader) + { + 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++) { //작업형태에 따라서. 리프트를 제어한다. var liftCmd = LiftCommand.DN;