From e35dee853f62eac89a6e00c031301eb62b6dada0 Mon Sep 17 00:00:00 2001 From: backuppc Date: Tue, 3 Feb 2026 17:15:15 +0900 Subject: [PATCH] =?UTF-8?q?=EC=B5=9C=EC=A2=85=EC=9A=B0=EC=B9=98=EC=97=90.?= =?UTF-8?q?=20=EC=9D=B4=EC=A0=84=EB=85=B8=EB=93=9C=EC=A0=95=EB=B3=B4?= =?UTF-8?q?=EC=99=80=20=EB=8B=A4=EC=9D=8C=20=EC=8B=9C=EC=9E=91=20=EB=85=B8?= =?UTF-8?q?=EB=93=9C=20=EC=A0=95=EB=B3=B4=EB=A5=BC=20=EB=AA=A8=EB=91=90=20?= =?UTF-8?q?=EC=B6=94=EA=B0=80=ED=95=A8.=20=EC=B5=9C=EC=B4=88=20=EC=8B=A4?= =?UTF-8?q?=ED=96=89=EC=8B=9C=20=EC=84=A0=ED=83=9D=ED=95=98=EB=8A=94=20?= =?UTF-8?q?=EC=9C=84=EC=B9=98=20=EB=8B=A4=EC=9D=B4=EC=96=BC=EB=A1=9C?= =?UTF-8?q?=EA=B7=B8=20=EC=A0=9C=EA=B1=B0=20=ED=9B=84=20=EB=B0=94=EB=A1=9C?= =?UTF-8?q?=20=EC=9E=90=EB=8F=99=20=EB=A1=9C=EB=94=A9=ED=95=98=EB=8F=84?= =?UTF-8?q?=EB=A1=9D=20=ED=95=A8?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- HMI/Project/AGV4.csproj | 1 + HMI/Project/Class/LastPositionData.cs | 27 ++++++++ HMI/Project/PUB.cs | 30 +++++---- HMI/Project/StateMachine/Step/_Util.cs | 91 ++++++++++++++------------ HMI/Project/StateMachine/_Xbee.cs | 9 +++ HMI/Project/fMain.cs | 76 ++++++++++++++++----- 6 files changed, 164 insertions(+), 70 deletions(-) create mode 100644 HMI/Project/Class/LastPositionData.cs diff --git a/HMI/Project/AGV4.csproj b/HMI/Project/AGV4.csproj index 7b17160..34668ba 100644 --- a/HMI/Project/AGV4.csproj +++ b/HMI/Project/AGV4.csproj @@ -245,6 +245,7 @@ fVolume.cs + Form diff --git a/HMI/Project/Class/LastPositionData.cs b/HMI/Project/Class/LastPositionData.cs new file mode 100644 index 0000000..c5da97f --- /dev/null +++ b/HMI/Project/Class/LastPositionData.cs @@ -0,0 +1,27 @@ +using System; +using AGVNavigationCore.Models; + +namespace Project +{ + public class LastPositionData + { + /// + /// 현재노드id + /// + public string NodeId { get; set; } + public AgvDirection Direction { get; set; } + + + public AGVTurn Turn { get; set; } + public DateTime SaveTime { get; set; } + public string PrevNode { get; set; } + public AgvDirection PrevDirection { get; set; } + public string StartNode { get; set; } + public string TargetNode { get; set; } + + public arDev.NarumiTurnInfo AGV_Turn { get; set; } + public char AGV_Motor { get; set; } + public char AGV_Speed { get; set; } + public string AGV_TagString { get; set; } + } +} diff --git a/HMI/Project/PUB.cs b/HMI/Project/PUB.cs index a99912e..119f475 100644 --- a/HMI/Project/PUB.cs +++ b/HMI/Project/PUB.cs @@ -25,13 +25,6 @@ using System.Drawing; namespace Project { - public class LastPositionData - { - public string NodeId { get; set; } - public AgvDirection Direction { get; set; } - public AGVTurn Turn { get; set; } - public DateTime SaveTime { get; set; } - } public static class PUB { @@ -121,7 +114,7 @@ namespace Project /// /// /// 로그에도 출력 합니다 - public static void Speak(string m, Boolean force = false, bool addlog = true,string logcate="") + public static void Speak(string m, Boolean force = false, bool addlog = true, string logcate = "") { if (force == false && PUB.setting.Enable_Speak == false) { @@ -643,7 +636,7 @@ namespace Project if (rfidValue.isEmpty()) return null; return _mapNodes.Where(t => t.RfidId.Equals(rfidValue)).FirstOrDefault(); } - + /// @@ -666,7 +659,7 @@ namespace Project { if (_virtualAGV == null) return; - if(_virtualAGV.CurrentDirection != direction) + if (_virtualAGV.CurrentDirection != direction) { PUB.log.Add($"[PUB] AGV Direction Change {_virtualAGV.CurrentDirection}->{direction}"); _virtualAGV.CurrentDirection = direction; @@ -682,13 +675,13 @@ namespace Project { if (_virtualAGV == null) return; - if(_virtualAGV.CurrentState != state) + if (_virtualAGV.CurrentState != state) { PUB.log.Add($"[PUB] AGV State Change {_virtualAGV.CurrentState}->{state}"); _virtualAGV.CurrentState = state; RefreshAGVCanvas(); } - + } /// @@ -714,7 +707,7 @@ namespace Project } } - + #region "Starting Position Persistence" private static string LastPosFilePath => Path.Combine(UTIL.CurrentPath, "Data", "last_pos.json"); @@ -723,6 +716,7 @@ namespace Project { if (_virtualAGV == null || _virtualAGV.CurrentNode == null) return; + try { var data = new LastPositionData @@ -730,7 +724,15 @@ namespace Project NodeId = _virtualAGV.CurrentNode.Id, Direction = _virtualAGV.CurrentDirection, Turn = _virtualAGV.Turn, - SaveTime = DateTime.Now + SaveTime = DateTime.Now, + AGV_Motor = PUB.AGV.data.Direction, + AGV_Speed = PUB.AGV.data.Speed, + AGV_TagString = PUB.AGV.data.TagString, + PrevNode = _virtualAGV.PrevNode?.Id ?? string.Empty, + PrevDirection = _virtualAGV.PrevDirection, + StartNode = _virtualAGV.StartNode?.Id ?? string.Empty, + TargetNode = _virtualAGV.TargetNode?.Id ?? string.Empty, + AGV_Turn = PUB.AGV.TurnInformation, }; var fi = new System.IO.FileInfo(LastPosFilePath); diff --git a/HMI/Project/StateMachine/Step/_Util.cs b/HMI/Project/StateMachine/Step/_Util.cs index 6aa7a18..79add7d 100644 --- a/HMI/Project/StateMachine/Step/_Util.cs +++ b/HMI/Project/StateMachine/Step/_Util.cs @@ -202,7 +202,7 @@ namespace Project { //절반이상 경로가 짧을때에는 재계산을 하게한다 var halfcnt = (int)(PUB._virtualAGV.CurrentPath.DetailedPath.Count / 2.0); - if (PathResult2.DetailedPath.Count < halfcnt) + if (PathResult2.DetailedPath.Count > 2 && PathResult2.DetailedPath.Count < halfcnt) { var msg = $"단축경로가 확인되었습니다. 경로를 삭제 합니다"; PUB.log.AddE(msg); @@ -234,12 +234,23 @@ namespace Project return false; } + bool Moveforce = false; + if(nextAction.Motor == MotorCommand.Stop && nextAction.Reason == eAGVCommandReason.MarkStop) + { + //마크스탑을 해야하는데 움직이지 않는다면 움직이도록 해야한다 + if(PUB.AGV.system1.agv_run==false) + { + PUB.log.Add("마크스탑을 해야하는데 중지상태라서 강제 이동하도록 합니다"); + Moveforce = true; + } + } + + //모터에서 정지를 요청했다 - if (nextAction.Motor == AGVNavigationCore.Models.MotorCommand.Stop) + if (Moveforce == false && nextAction.Motor == AGVNavigationCore.Models.MotorCommand.Stop) { if (PUB.AGV.system1.agv_run) { - // 완료(Complete) 상태라면 MarkStop 전송 if (nextAction.Reason == AGVNavigationCore.Models.eAGVCommandReason.MarkStop) { @@ -277,57 +288,55 @@ namespace Project } } - - + //이동중이라면 멈추는 명령을 전송하거나, 마크스탑을 진행해야하낟. + return false; } - // 목적지 도착 여부 확인 - // .. (생략) .. - if (PUB._virtualAGV.IsPositionConfirmed) + if (nextAction.Reason == eAGVCommandReason.Complete) { - if (PUB.AGV.system1.agv_run == false) + // 목적지 도착 여부 확인 + if (PUB.AGV.signal1.mark_sensor == false) { - //마크센서확인 - if(PUB.AGV.signal1.mark_sensor==false) - { - PUB.log.AddI($"목표도착되었으나 마크센서가 감지되지 않아 경로를 삭제 합니다"); - PUB._virtualAGV.SetPath(null); - return false; - } - // 경로가 존재한다면... - if (PUB._virtualAGV.CurrentPath != null && PUB._virtualAGV.CurrentPath.DetailedPath.Any()) - { - var lastInfo = PUB._virtualAGV.CurrentPath.DetailedPath.Last(); + PUB.log.AddI($"목표도착되었으나 마크센서가 감지되지 않아 완료처리 하지않습니다"); + return false; - // 위치와 방향이 모두 일치해야 완료된 것으로 본다. - if (PUB._virtualAGV.CurrentNode.Id == lastInfo.NodeId && - PUB._virtualAGV.CurrentDirection == lastInfo.MotorDirection) - { - var node = PUB._mapCanvas.Nodes.Where(t => t.Id == PUB._virtualAGV.CurrentNodeId).FirstOrDefault(); - var rfid = node?.ID2 ?? "(X)"; - PUB.log.AddI($"목표 도착 및 정지 확인됨(MarkStop 완료) Node:{rfid}, Dir:{PUB._virtualAGV.CurrentDirection}"); - return true; - } - else - { - // [DEBUG] 도착했으나 조건 불일치 - // PUB.log.Add($"[DEBUG] Arrived but condition mismatch. CurNode:{PUB._virtualAGV.CurrentNode.Id}, Target:{lastInfo.NodeId}, CurDir:{PUB._virtualAGV.CurrentDirection}, TargetDir:{lastInfo.MotorDirection}"); - } + } + + // 경로가 존재한다면... + if (PUB._virtualAGV.CurrentPath != null && PUB._virtualAGV.CurrentPath.DetailedPath.Any()) + { + var lastInfo = PUB._virtualAGV.CurrentPath.DetailedPath.Last(); + + // 위치와 방향이 모두 일치해야 완료된 것으로 본다. + if (PUB._virtualAGV.CurrentNode.Id == lastInfo.NodeId && + PUB._virtualAGV.CurrentDirection == lastInfo.MotorDirection) + { + var node = PUB._mapCanvas.Nodes.Where(t => t.Id == PUB._virtualAGV.CurrentNodeId).FirstOrDefault(); + var rfid = node?.ID2 ?? "(X)"; + PUB.log.AddI($"목표 도착 및 정지 확인됨(MarkStop 완료) Node:{rfid}, Dir:{PUB._virtualAGV.CurrentDirection}"); + return true; } else + { + // [DEBUG] 도착했으나 조건 불일치, 그러면.. predict 가 stop을 반환하면 안된다. + PUB.log.Add($"[DEBUG] Arrived but condition mismatch. CurNode:{PUB._virtualAGV.CurrentNode.Id}, Target:{lastInfo.NodeId}, CurDir:{PUB._virtualAGV.CurrentDirection}, TargetDir:{lastInfo.MotorDirection}"); + } + } + else + { + // ... + if (PUB._virtualAGV.CurrentNode.Id == PUB._virtualAGV.TargetNode.Id) { // ... - if (PUB._virtualAGV.CurrentNode.Id == PUB._virtualAGV.TargetNode.Id) - { - // ... - PUB.log.AddI($"목표 도착 및 정지 확인됨(MarkStop 완료, No Path Info) Node:..."); - return true; - } + PUB.log.AddI($"목표 도착 및 정지 확인됨(MarkStop 완료, No Path Info) Node:..."); + return true; } } } + + return false; } else @@ -391,7 +400,7 @@ namespace Project } // AGV가 정지 상태라면 구동 시작 (라이다가켜져있을때에만 사용한다) - if (PUB.AGV.system1.agv_run == false && PUB.AGV.PBSSensor == arDev.eNarmiPBSSensor.on) + if (PUB.AGV.system1.agv_run == false && PUB.AGV.PBSSensor == arDev.eNarmiPBSSensor.on) { // 2초 쿨타임 적용 (AGVMoveSet과 동일한 타이머 사용) var tsCmd = DateTime.Now - LastCommandTime; diff --git a/HMI/Project/StateMachine/_Xbee.cs b/HMI/Project/StateMachine/_Xbee.cs index b2faf04..35f9ebe 100644 --- a/HMI/Project/StateMachine/_Xbee.cs +++ b/HMI/Project/StateMachine/_Xbee.cs @@ -210,6 +210,14 @@ namespace Project return; } + //s/w턴이 걸려있다면 이동 불가로한다.(버퍼 작업중이다) + if(PUB._virtualAGV.Turn != AGVTurn.None) + { + SetRunStepError(ENIGProtocol.AGVErrorCode.BUFFER_NOT_COMPLETE, $"[{logPrefix}-Goto] 버퍼작업이 완료되지 않았습니다"); + return; + } + + //목적지 PUB._virtualAGV.TargetNode = targetNode; if (targetNode == null) @@ -233,6 +241,7 @@ namespace Project PUB.sm.ResetRunStepSeq(); } + PUB.SaveLastPosition(); //Move to PUB.log.Add($"[{logPrefix}-{cmd}] {startNode.RfidId} -> {targetNode.RfidId}"); diff --git a/HMI/Project/fMain.cs b/HMI/Project/fMain.cs index bed37db..790bcea 100644 --- a/HMI/Project/fMain.cs +++ b/HMI/Project/fMain.cs @@ -310,24 +310,70 @@ namespace Project } PUB.log.Add($"맵 파일 로드 완료: {filePath.Name}, 노드 수: {result.Nodes.Count}"); - // 🔥 초기 위치 설정 및 확인 화면 표시 - this.BeginInvoke(new Action(() => + //마지막위치복원 + // 마지막 위치 로드 + var lastPos = PUB.LoadLastPosition(); + if (lastPos != null) { - using (var f = new Dialog.fSetCurrentPosition()) + var sb = new System.Text.StringBuilder(); + var curNode = PUB.FindByNodeID(lastPos.NodeId); + var prevNode = PUB.FindByNodeID(lastPos.PrevNode); + var targNode = PUB.FindByNodeID(lastPos.TargetNode); + var staNode = PUB.FindByNodeID(lastPos.StartNode); + if(prevNode != null) { - if (f.ShowDialog() == DialogResult.OK) - { - if (f.SelectedNode != null) - { - PUB._virtualAGV.Turn = f.SelectedTurn; - PUB._virtualAGV.SetPosition(f.SelectedNode, f.SelectedDirection); - PUB._mapCanvas.SetAGVPosition(PUB.setting.MCID, f.SelectedNode, f.SelectedDirection); - PUB.SaveLastPosition(); - PUB.log.Add($"[초기위치] 설정 완료: {f.SelectedNode.Id}, {f.SelectedDirection}, Turn:{f.SelectedTurn}"); - } - } + PUB._virtualAGV.SetPosition(prevNode, lastPos.PrevDirection); + PUB._mapCanvas.SetAGVPosition(PUB.setting.MCID, prevNode, lastPos.PrevDirection); + sb.AppendLine($"이전: {prevNode.ID2}, {lastPos.PrevDirection}"); } - })); + if (curNode != null) + { + PUB._virtualAGV.SetPosition(curNode, lastPos.Direction); + PUB._mapCanvas.SetAGVPosition(PUB.setting.MCID, curNode, lastPos.Direction); + sb.AppendLine($"현재: {curNode.ID2}, {lastPos.Direction}"); + } + if (staNode != null) + { + PUB._virtualAGV.StartNode = staNode; + sb.AppendLine($"시직:{staNode.ID2}"); + } + if (targNode != null) + { + PUB._virtualAGV.TargetNode = targNode; + sb.AppendLine($"대상:{targNode.ID2}"); + } + + PUB._virtualAGV.Turn = lastPos.Turn; + sb.AppendLine($"S/W회전:{lastPos.Turn}"); + + if(lastPos.AGV_Turn != null) + { + PUB.AGV.TurnInformation = lastPos.AGV_Turn; + sb.AppendLine($"H/W회전:{lastPos.AGV_Turn}"); + } + + PUB.log.Add($"위치복원\n{sb}"); + } + + + //// 🔥 초기 위치 설정 및 확인 화면 표시 + //this.BeginInvoke(new Action(() => + //{ + // using (var f = new Dialog.fSetCurrentPosition()) + // { + // if (f.ShowDialog() == DialogResult.OK) + // { + // if (f.SelectedNode != null) + // { + // PUB._virtualAGV.Turn = f.SelectedTurn; + // PUB._virtualAGV.SetPosition(f.SelectedNode, f.SelectedDirection); + // PUB._mapCanvas.SetAGVPosition(PUB.setting.MCID, f.SelectedNode, f.SelectedDirection); + // PUB.SaveLastPosition(); + // PUB.log.Add($"[초기위치] 설정 완료: {f.SelectedNode.Id}, {f.SelectedDirection}, Turn:{f.SelectedTurn}"); + // } + // } + // } + //})); } else {