diff --git a/Document/통신프로토콜/.~lock.통신 프로토콜_AGV_V350_LF_25.01.10_r2.xlsx# b/Document/통신프로토콜/.~lock.통신 프로토콜_AGV_V350_LF_25.01.10_r2.xlsx# new file mode 100644 index 0000000..64e8f9a --- /dev/null +++ b/Document/통신프로토콜/.~lock.통신 프로토콜_AGV_V350_LF_25.01.10_r2.xlsx# @@ -0,0 +1 @@ +,BACKUPPC/1,backuppc,26.02.2026 11:12,file:///C:/Users/1/AppData/Roaming/LibreOffice/4; \ No newline at end of file diff --git a/HMI/Project/StateMachine/Step/_SM_RUN_BUFFER_IN.cs b/HMI/Project/StateMachine/Step/_SM_RUN_BUFFER_IN.cs index 473de25..922af81 100644 --- a/HMI/Project/StateMachine/Step/_SM_RUN_BUFFER_IN.cs +++ b/HMI/Project/StateMachine/Step/_SM_RUN_BUFFER_IN.cs @@ -15,6 +15,7 @@ namespace Project { public partial class fMain { + DateTime _lastLidarOffTime_IN = DateTime.MinValue; public Boolean _SM_RUN_BUFFER_IN(bool isFirst, TimeSpan seqtime) { var funcname = "_SM_RUN_BUFFER_IN"; @@ -26,6 +27,27 @@ namespace Project //라이더멈춤이 설정되어있다면 음성으로 알려준다 if (CheckLiderStop() == false) return false; + // [Global Safety] 버퍼 작업 중에는 무조건 라이다를 끄도록 강제한다 (재시작 시 멈춤 방지) + if (PUB.AGV.PBSSensor != arDev.eNarmiPBSSensor.off || PUB.AGV.data.Distance > 0) + { + if ((DateTime.Now - _lastLidarOffTime_IN).TotalSeconds > 2) + { + PUB.log.Add($"[{funcname}] 버퍼 작업 중 라이다 강제 OFF 시도"); + PUB.AGV.AGVMoveSet(new arDev.Narumi.BunkiData + { + Bunki = arDev.Narumi.eBunki.Strate, + Direction = arDev.Narumi.eMoveDir.Backward, + PBSSensor = 0, + Speed = arDev.Narumi.eMoveSpd.Low, + }); + _lastLidarOffTime_IN = DateTime.Now; + } + return false; // 라이다가 꺼질 때까지 진행 스톱 (명령이 적용될 때까지 대기) + } + + // [State Save] 현재 진행 단계를 저장한다 + SaveBufferStep(PUB._virtualAGV.CurrentNode, PUB.sm.RunStepSeq, PUB.NextWorkCmd); + /* * 버퍼IN시퀀스 * 1. 회전이 진행되지 않았다면 회전을 진행한다. @@ -428,6 +450,9 @@ namespace Project // 작업을 마치고 설비 안에 멈춰있는 상태. // ACS가 이 상태를 확인하고 NextWorkCmd로 퇴출(Out) 명령을 보내야 함. //PUB.AddEEDB($"[{funcname}] 버퍼작업완료({PUB.Result.TargetPos})"); + + // [State Clear] 작업 완료 시 저장된 상태를 지운다 + ClearBufferStep(PUB._virtualAGV.CurrentNode); return true; } } diff --git a/HMI/Project/StateMachine/Step/_SM_RUN_BUFFER_OUT.cs b/HMI/Project/StateMachine/Step/_SM_RUN_BUFFER_OUT.cs index 605820f..5380d8e 100644 --- a/HMI/Project/StateMachine/Step/_SM_RUN_BUFFER_OUT.cs +++ b/HMI/Project/StateMachine/Step/_SM_RUN_BUFFER_OUT.cs @@ -14,6 +14,7 @@ namespace Project public partial class fMain { bool SpeedSetRetry = false; + DateTime _lastLidarOffTime_OUT = DateTime.MinValue; public Boolean _SM_RUN_BUFFER_OUT(bool isFirst, TimeSpan seqtime) { var funcname = "_SM_RUN_BUFFER_OUT"; @@ -25,6 +26,27 @@ namespace Project //라이더멈춤이 설정되어있다면 음성으로 알려준다 if (CheckLiderStop() == false) return false; + // [Global Safety] 버퍼 작업 중에는 무조건 라이다를 끄도록 강제한다 (재시작 시 멈춤 방지) + if (PUB.AGV.PBSSensor != arDev.eNarmiPBSSensor.off || PUB.AGV.data.Distance > 0) + { + if ((DateTime.Now - _lastLidarOffTime_OUT).TotalSeconds > 2) + { + PUB.log.Add($"[{funcname}] 버퍼 작업 중 라이다 강제 OFF 시도"); + PUB.AGV.AGVMoveSet(new arDev.Narumi.BunkiData + { + Bunki = arDev.Narumi.eBunki.Strate, + Direction = arDev.Narumi.eMoveDir.Forward, + PBSSensor = 0, + Speed = arDev.Narumi.eMoveSpd.Low, + }); + _lastLidarOffTime_OUT = DateTime.Now; + } + return false; // 라이다가 꺼질 때까지 진행 스톱 (명령이 적용될 때까지 대기) + } + + // [State Save] 현재 진행 단계를 저장한다 + SaveBufferStep(PUB._virtualAGV.CurrentNode, PUB.sm.RunStepSeq, PUB.NextWorkCmd); + /* * 버퍼 OUT 시퀀스 * 1-1. PickOn 라면 리프트를 Up 한다 @@ -100,35 +122,42 @@ namespace Project else if (PUB.sm.RunStepSeq == idx++) { - //if (PUB.AGV.signal1.mark_sensor == false) + // [Smart Restart] 재시작 시 안전 검사 + // 이미 마크센서가 켜져서 도착한 상태라면 전진 구동을 생략하고 턴으로 넘어간다. + if (PUB.AGV.signal1.mark_sensor == true && PUB._virtualAGV.Turn == AGVNavigationCore.Models.AGVTurn.L90) { - //빈 상태로 아웃해야한다. - if (SpeedSetRetry) - { - if (seqtime.TotalSeconds < 2) - return false; - } - var ret = PUB.AGV.AGVMoveSet(new arDev.Narumi.BunkiData - { - Bunki = arDev.Narumi.eBunki.Strate, - Direction = arDev.Narumi.eMoveDir.Forward, - PBSSensor = 0, - Speed = arDev.Narumi.eMoveSpd.Low, - }); - //명령이 실패되었다면 재시도를 한다 - if (ret != arDev.eNarumiCommandResult.Success) - { - if (SpeedSetRetry == true || ret >= arDev.eNarumiCommandResult.Error) - { - SetRunStepError(ENIGProtocol.AGVErrorCode.AGV_SPEED_SET_FAIL); - } - else SpeedSetRetry = true; - return false; - } - - PUB.log.Add($"[{funcname}-{PUB.sm.RunStepSeq}] AGV 이동 설정 완료 (Dir:Forward, Spd:Low)"); + PUB.log.Add($"[{funcname}-{PUB.sm.RunStepSeq}] 이미 마크센서 감지됨. 전진 이동을 생략합니다."); + PUB.sm.SetStepSeq((byte)(idx + 4)); // Turn 스텝으로 직행 (현재 idx부터 4단계 뒤) + return false; } + //빈 상태로 아웃해야한다. + if (SpeedSetRetry) + { + if (seqtime.TotalSeconds < 2) + return false; + } + var ret = PUB.AGV.AGVMoveSet(new arDev.Narumi.BunkiData + { + Bunki = arDev.Narumi.eBunki.Strate, + Direction = arDev.Narumi.eMoveDir.Forward, + PBSSensor = 0, + Speed = arDev.Narumi.eMoveSpd.Low, + }); + //명령이 실패되었다면 재시도를 한다 + if (ret != arDev.eNarumiCommandResult.Success) + { + if (SpeedSetRetry == true || ret >= arDev.eNarumiCommandResult.Error) + { + SetRunStepError(ENIGProtocol.AGVErrorCode.AGV_SPEED_SET_FAIL); + } + else SpeedSetRetry = true; + return false; + } + + PUB.log.Add($"[{funcname}-{PUB.sm.RunStepSeq}] AGV 이동 설정 완료 (Dir:Forward, Spd:Low)"); + + VAR.I32[eVarInt32.RetryMove] = 0; PUB.sm.UpdateRunStepSeq(); return false; @@ -225,6 +254,17 @@ namespace Project } return false; } + + // [Resume Safety] 재시작 직후 여기에 진입했는데 마크 센서가 꺼져있다면, + // 중간에 멈췄을 가능성이 크므로 다시 구동 스텝으로 되돌아간다. + // (단, seqtime이 매우 짧은 경우 = 방금 Resume된 경우를 필터링) + if (PUB.AGV.signal1.mark_sensor == false && seqtime.TotalSeconds < 1.0) + { + PUB.log.Add($"[{funcname}-{PUB.sm.RunStepSeq}] 재시작 감지됨. 하지만 목적지(마크) 도착 불확실. 이동을 재시도합니다."); + PUB.sm.SetStepSeq(6); // Speed Set 스텝(이동 시작 단계)으로 복귀 + return false; + } + PUB.log.Add($"[{funcname}-{PUB.sm.RunStepSeq}] AGV 멈춤 확인 완료"); PUB.sm.UpdateRunStepSeq(); return false; @@ -443,6 +483,9 @@ namespace Project } PUB.AddEEDB($"[{funcname}] bufferout 완료({PUB.Result.TargetPos})"); + + // [State Clear] 작업 완료 시 저장된 상태를 지운다 + ClearBufferStep(PUB._virtualAGV.CurrentNode); return true; } } diff --git a/HMI/Project/StateMachine/_AGV.cs b/HMI/Project/StateMachine/_AGV.cs index 73168bc..3a9c8b5 100644 --- a/HMI/Project/StateMachine/_AGV.cs +++ b/HMI/Project/StateMachine/_AGV.cs @@ -16,6 +16,56 @@ namespace Project public partial class fMain { + public int GetBufferIndex(MapNode node) + { + if (node == null) return 0; + string name = node.AliasName; + if (string.IsNullOrEmpty(name)) name = node.Id; + + if (name.Contains("Buffer") || name.ToUpper().StartsWith("B")) + { + var match = System.Text.RegularExpressions.Regex.Match(name, @"\d+"); + if (match.Success) + { + int.TryParse(match.Value, out int idx); + if (idx >= 1 && idx <= 6) return idx; + } + } + return 0; + } + + public void SaveBufferStep(MapNode node, int step, ENIGProtocol.AGVCommandHE cmd) + { + int bufIdx = GetBufferIndex(node); + if (bufIdx > 0) + { + try + { + var stepVar = (eVarInt32)Enum.Parse(typeof(eVarInt32), $"Buffer{bufIdx}Step"); + var cmdVar = (eVarString)Enum.Parse(typeof(eVarString), $"Buffer{bufIdx}LastCmd"); + VAR.I32[stepVar] = step; + VAR.STR[cmdVar] = cmd.ToString(); + } + catch { } + } + } + + public void ClearBufferStep(MapNode node) + { + int bufIdx = GetBufferIndex(node); + if (bufIdx > 0) + { + try + { + var stepVar = (eVarInt32)Enum.Parse(typeof(eVarInt32), $"Buffer{bufIdx}Step"); + var cmdVar = (eVarString)Enum.Parse(typeof(eVarString), $"Buffer{bufIdx}LastCmd"); + VAR.I32[stepVar] = 0; + VAR.STR[cmdVar] = ""; + } + catch { } + } + } + private void AGV_TurnComplete(object sender, arDev.Narumi.TurnEventArgs e) { //턴작업이완료되었을때 발생된다. diff --git a/HMI/Project/StateMachine/_Xbee.cs b/HMI/Project/StateMachine/_Xbee.cs index f784d0f..9049069 100644 --- a/HMI/Project/StateMachine/_Xbee.cs +++ b/HMI/Project/StateMachine/_Xbee.cs @@ -125,6 +125,40 @@ namespace Project //다음명령처리 PUB.NextWorkCmd = cmd; PUB.log.AddI($"작업 시작: {nextStep} (Type: {cmd})"); + + // [Resume Logic] + if (nextStep == ERunStep.BUFFER_IN || nextStep == ERunStep.BUFFER_OUT) + { + int bufIdx = GetBufferIndex(targetNode); + if (bufIdx > 0) + { + var stepVar = (eVarInt32)Enum.Parse(typeof(eVarInt32), $"Buffer{bufIdx}Step"); + var cmdVar = (eVarString)Enum.Parse(typeof(eVarString), $"Buffer{bufIdx}LastCmd"); + + int savedStep = VAR.I32[stepVar]; + string savedCmd = VAR.STR[cmdVar]; + + // Resume Condition: Saved step exists AND command matches AND node matches + if (savedStep > 1 && savedCmd == cmd.ToString() && currNode.Id == targetNode.Id) + { + PUB.log.AddI($"[RESUME] 버퍼 {bufIdx} 작업 재개: Step {savedStep} (Cmd: {cmd})"); + VAR.I32[eVarInt32.RetryMove] = 0; + VAR.I32[eVarInt32.RetryLift] = 0; + VAR.I32[eVarInt32.RetryManget] = 0; + VAR.I32[eVarInt32.RetryMarkStop] = 0; + PUB.sm.SetNewRunStep(nextStep); + PUB.sm.SetStepSeq((byte)savedStep); + return; // SetNewRunStep + SetStepSeq 로직으로 바로 시작 + } + else if (savedStep > 0) + { + PUB.log.AddI($"[RESET] 버퍼 {bufIdx} 작업 초기화 (SavedStep:{savedStep}, SavedCmd:{savedCmd}, NodeMatch:{currNode.Id == targetNode.Id})"); + VAR.I32[stepVar] = 0; + VAR.STR[cmdVar] = ""; + } + } + } + PUB.sm.SetNewRunStep(nextStep); } break; @@ -170,7 +204,33 @@ namespace Project //다음명령처리 PUB.NextWorkCmd = cmd; - PUB.log.AddI($"작업 시작: {nextStep} (Type: {cmd})"); + // [Resume Logic] + if (nextStep == ERunStep.BUFFER_IN || nextStep == ERunStep.BUFFER_OUT) + { + int bufIdx = GetBufferIndex(targetNode); + if (bufIdx > 0) + { + var stepVar = (eVarInt32)Enum.Parse(typeof(eVarInt32), $"Buffer{bufIdx}Step"); + var cmdVar = (eVarString)Enum.Parse(typeof(eVarString), $"Buffer{bufIdx}LastCmd"); + + int savedStep = VAR.I32[stepVar]; + string savedCmd = cmd.ToString(); + + // Resume Condition (Exit): Exit commands can resume if the AGV is still at the buffer + if (savedStep > 1 && VAR.STR[cmdVar] == savedCmd && currNode.Id == targetNode.Id) + { + PUB.log.AddI($"[RESUME] 버퍼 {bufIdx} 퇴출 재개: Step {savedStep}"); + VAR.I32[eVarInt32.RetryMove] = 0; + VAR.I32[eVarInt32.RetryLift] = 0; + VAR.I32[eVarInt32.RetryManget] = 0; + VAR.I32[eVarInt32.RetryMarkStop] = 0; + PUB.sm.SetNewRunStep(nextStep); + PUB.sm.SetStepSeq((byte)savedStep); + return; + } + } + } + PUB.sm.SetNewRunStep(nextStep); } break; @@ -435,7 +495,6 @@ namespace Project else PUB.log.Add(e.Message); } - } } diff --git a/HMI/SubProject/AGV/Narumi.cs b/HMI/SubProject/AGV/Narumi.cs index 4c9cc3b..21f27af 100644 --- a/HMI/SubProject/AGV/Narumi.cs +++ b/HMI/SubProject/AGV/Narumi.cs @@ -166,6 +166,7 @@ namespace arDev TurnInformation.State = eNarumiTurn.Left; TurnInformation.End = DateTime.Now; } + TurnComplete?.Invoke(this, new TurnEventArgs(eTurnEvent.Left)); } else if (frame.DataString.Contains("RIGHT-TURN OK")) { @@ -174,11 +175,13 @@ namespace arDev TurnInformation.State = eNarumiTurn.Right; TurnInformation.End = DateTime.Now; } + TurnComplete?.Invoke(this, new TurnEventArgs(eTurnEvent.Right)); } // $로 시작되는 AGV 상태 표시 //var text_Sts_Etc = Encoding.Default.GetString(bRcvData, 3, bRcvData.Length - 2).TrimStart(' '); //20210311 김정만 - SmartX FrameWork 사용 안함으로 주석처리 //var sMessageOther = Encoding.Default.GetString(bRcvData, 3, bRcvData.Length - 2).TrimStart(' '); //AGV RIGHT-TURN OK + RaiseMessage(MessageType.Normal, "$메세지수신:" + frame.DataString); } else @@ -305,10 +308,20 @@ namespace arDev // } //} - if (idx <= rcvdNow.Length - 2) + if (idx <= rcvdNow.Length - 5) { nDataTemp = Convert.ToByte(rcvdNow.Substring(idx, 2), 16); - signal2.SetValue(nDataTemp); + signal2.SetValue(nDataTemp); idx += 2; + } + + if (idx <= rcvdNow.Length - 3) + { + var str_dist= rcvdNow.Substring(idx, 3); //260226 + if (int.TryParse(str_dist, out int vDist)) + data.Distance = vDist; + else + data.Distance = -1; + idx += 3; } diff --git a/HMI/SubProject/AGV/Structure/AgvData.cs b/HMI/SubProject/AGV/Structure/AgvData.cs index 3902eaf..dabeab8 100644 --- a/HMI/SubProject/AGV/Structure/AgvData.cs +++ b/HMI/SubProject/AGV/Structure/AgvData.cs @@ -30,6 +30,7 @@ namespace arDev public char Direction { get; set; } public int guidesensor { get; set; } + public int Distance { get; set; } public string TagString { get; set; } = string.Empty; @@ -46,17 +47,13 @@ namespace arDev { //모든사태값을 탭으로 구분하여 문자를 생성한다 var sb = new System.Text.StringBuilder(); - sb.AppendLine($"[Sts] : {Sts}"); sb.AppendLine($"[Speed] : {Speed}"); sb.AppendLine($"[Direction] : {Direction}"); sb.AppendLine($"[guidesensor] : {guidesensor}"); - sb.AppendLine($"[TagNo] : {TagNo}"); sb.AppendLine($"[CallNo] : {CallNo}"); sb.AppendLine($"[CCANo] : {CCANo}"); - - return sb.ToString(); } diff --git a/HMI/SubProject/CommData/Enum.cs b/HMI/SubProject/CommData/Enum.cs index b2030cc..8e98a9c 100644 --- a/HMI/SubProject/CommData/Enum.cs +++ b/HMI/SubProject/CommData/Enum.cs @@ -19,6 +19,12 @@ namespace COMM RetryMove, RetryMoveset, RetryMarkStop, + Buffer1Step, + Buffer2Step, + Buffer3Step, + Buffer4Step, + Buffer5Step, + Buffer6Step, } public enum eVarUInt32 { @@ -136,6 +142,12 @@ namespace COMM JOBCustCode, SWVersion, ChargeCheckMsg, + Buffer1LastCmd, + Buffer2LastCmd, + Buffer3LastCmd, + Buffer4LastCmd, + Buffer5LastCmd, + Buffer6LastCmd, } public enum eVarTime