From e99edbe04d4457fedf14fd0bf2fcd4cb7109e018 Mon Sep 17 00:00:00 2001 From: backuppc Date: Thu, 29 Jan 2026 14:51:57 +0900 Subject: [PATCH] Fix oscillation in AGV movement, add command cooldown, and enhance debug logs --- HMI/Project/StateMachine/Step/_SM_RUN_GOTO.cs | 3 + HMI/Project/StateMachine/Step/_Util.cs | 89 +++++++++++++------ gitpull.bat | 2 + 3 files changed, 68 insertions(+), 26 deletions(-) create mode 100644 gitpull.bat diff --git a/HMI/Project/StateMachine/Step/_SM_RUN_GOTO.cs b/HMI/Project/StateMachine/Step/_SM_RUN_GOTO.cs index e746264..c30f194 100644 --- a/HMI/Project/StateMachine/Step/_SM_RUN_GOTO.cs +++ b/HMI/Project/StateMachine/Step/_SM_RUN_GOTO.cs @@ -51,6 +51,7 @@ namespace Project PUB.sm.SetNewRunStep(ERunStep.READY); return false; } + PUB.log.Add($"[GOTO] Step {idx-1}: TargetNode Checked ({PUB._virtualAGV.TargetNode.ID2})"); PUB.sm.UpdateRunStepSeq(); return false; } @@ -59,6 +60,7 @@ namespace Project //모션 전후진 제어 if (UpdateMotionPositionForMark(funcName)) { + PUB.log.Add($"[GOTO] Step {idx-1}: UpdateMotionPositionForMark Completed. Stopping AGV."); PUB.AGV.AGVMoveStop(funcName); PUB.sm.UpdateRunStepSeq(); } @@ -67,6 +69,7 @@ namespace Project else if (PUB.sm.RunStepSeq == idx++) { //QC까지 모두 완료되었다.(완전히 정차할때까지 기다린다) + PUB.log.Add($"[GOTO] Step {idx-1}: Movement Finished. Waiting for full stop."); PUB.Speak(Lang.이동완료, true); PUB.AddEEDB($"이동완료({PUB._virtualAGV.TargetNode.ID2})"); PUB.sm.UpdateRunStepSeq(); diff --git a/HMI/Project/StateMachine/Step/_Util.cs b/HMI/Project/StateMachine/Step/_Util.cs index 51f7db4..aa31744 100644 --- a/HMI/Project/StateMachine/Step/_Util.cs +++ b/HMI/Project/StateMachine/Step/_Util.cs @@ -214,6 +214,13 @@ namespace Project //predict 를 이용하여 다음 이동을 모두 확인한다. var nextAction = PUB._virtualAGV.Predict(); + + // [DEBUG] 예측 결과 로그 추가 + // 너무 빈번하게 찍히지 않도록 변화가 있을 때만 찍거나, 특정 조건에서 찍는 것이 좋으나 + // 디버깅 요청이므로 일단 주요 정보 출력 + // (실제 운용시에는 Verbose 레벨로 조정 필요) + // PUB.log.Add($"[DEBUG] Predict: Reason={nextAction.Reason}, Motor={nextAction.Motor}, Magnet={nextAction.Magnet}, Speed={nextAction.Speed}"); + if (nextAction.Reason == AGVNavigationCore.Models.eAGVCommandReason.PathOut) { //경로이탈 @@ -245,14 +252,13 @@ namespace Project } // 목적지 도착 여부 확인 - // 현재 노드가 타겟 노드와 같고, 위치가 확정된 상태라면 도착으로 간주 - // 단, AGV가 실제로 멈췄는지 확인 (agv_run == false) + // .. (생략) .. if (PUB._virtualAGV.IsPositionConfirmed) { if (PUB.AGV.system1.agv_run == false) { - // 경로가 존재한다면, 경로의 마지막 노드에 도착했는지 확인한다. - if (PUB._virtualAGV.CurrentPath != null && PUB._virtualAGV.CurrentPath.DetailedPath.Any()) + // 경로가 존재한다면... + if (PUB._virtualAGV.CurrentPath != null && PUB._virtualAGV.CurrentPath.DetailedPath.Any()) { var lastInfo = PUB._virtualAGV.CurrentPath.DetailedPath.Last(); // 위치와 방향이 모두 일치해야 완료된 것으로 본다. @@ -264,23 +270,23 @@ namespace Project 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}"); + } } else { - // 경로 정보가 없다면 단순히 목적지 ID와 비교한다 (Fallback) - if (PUB._virtualAGV.CurrentNode.Id == PUB._virtualAGV.TargetNode.Id) + // ... + if (PUB._virtualAGV.CurrentNode.Id == PUB._virtualAGV.TargetNode.Id) { - var node = PUB._mapCanvas.Nodes.Where(t => t.Id == PUB._virtualAGV.CurrentNodeId).FirstOrDefault(); - var rfid = node?.ID2 ?? "(X)"; - PUB.log.AddI($"목표 도착 및 정지 확인됨(MarkStop 완료, No Path Info) Node:{rfid}"); + // ... + PUB.log.AddI($"목표 도착 및 정지 확인됨(MarkStop 완료, No Path Info) Node:..."); return true; } } } - else - { - //아직 멈추지 않았다면 기다린다. - } } return false; @@ -295,34 +301,65 @@ namespace Project var dir = arDev.Narumi.eMoveDir.Forward; if (nextAction.Motor == AGVNavigationCore.Models.MotorCommand.Backward) dir = arDev.Narumi.eMoveDir.Backward; - var spd = arDev.Narumi.eMoveSpd.Low; if (nextAction.Speed == AGVNavigationCore.Models.SpeedLevel.M) spd = arDev.Narumi.eMoveSpd.Mid; else if (nextAction.Speed == AGVNavigationCore.Models.SpeedLevel.H) spd = arDev.Narumi.eMoveSpd.High; + // 방향 전환 시 정지 로직 추가 + // 이동 중인데 방향이 다르면 먼저 정지시킨다. + if (PUB.AGV.system1.agv_run) + { + if (PUB.AGV.data.Direction != dir.ToString()[0]) + { + // 2초 쿨타임 (정지 명령도 빈번한 전송 방지) + var tsCmd = DateTime.Now - LastCommandTime; + if (tsCmd.TotalSeconds >= 2.0) + { + PUB.log.Add($"방향 전환을 위해 정지 명령을 전송합니다 Current:{PUB.AGV.data.Direction} Target:{dir}"); + PUB.AGV.AGVMoveStop("Direction Change"); + LastCommandTime = DateTime.Now; + } + return false; + } + } + // 명령 설정 // 현재 상태와 다를 때만 전송 (불필요한 통신 부하 방지) if (PUB.AGV.data.Sts != bunki.ToString()[0] || PUB.AGV.data.Direction != dir.ToString()[0] || PUB.AGV.data.Speed != spd.ToString()[0]) { - var ret = PUB.AGV.AGVMoveSet(new arDev.Narumi.BunkiData + // 2초 쿨타임 적용 + var tsCmd = DateTime.Now - LastCommandTime; + if (tsCmd.TotalSeconds >= 2.0) { - Bunki = bunki, - Direction = dir, - PBSSensor = 1, - Speed = spd, - }); - if (ret == arDev.eNarumiCommandResult.Success) - PUB.log.Add($"Predict Run Setting = bunki:{bunki},dir:{dir},pbs:1,spd:{spd}"); - else - PUB.log.AddE($"Predict Run Setting = bunki:{bunki},dir:{dir},pbs:1,spd:{spd}"); + var ret = PUB.AGV.AGVMoveSet(new arDev.Narumi.BunkiData + { + Bunki = bunki, + Direction = dir, + PBSSensor = 1, + Speed = spd, + }); + + if (ret == arDev.eNarumiCommandResult.Success) + PUB.log.Add($"Predict Run Setting = bunki:{bunki},dir:{dir},pbs:1,spd:{spd}"); + else + PUB.log.AddE($"Predict Run Setting = bunki:{bunki},dir:{dir},pbs:1,spd:{spd}"); + + LastCommandTime = DateTime.Now; + } } // AGV가 정지 상태라면 구동 시작 if (PUB.AGV.system1.agv_run == false) { - var runOpt = (dir == arDev.Narumi.eMoveDir.Forward) ? arDev.Narumi.eRunOpt.Forward : arDev.Narumi.eRunOpt.Backward; - PUB.AGV.AGVMoveRun(runOpt); + // 2초 쿨타임 적용 (AGVMoveSet과 동일한 타이머 사용) + var tsCmd = DateTime.Now - LastCommandTime; + if (tsCmd.TotalSeconds >= 2.0) + { + var runOpt = (dir == arDev.Narumi.eMoveDir.Forward) ? arDev.Narumi.eRunOpt.Forward : arDev.Narumi.eRunOpt.Backward; + PUB.AGV.AGVMoveRun(runOpt); + LastCommandTime = DateTime.Now; + } } return false; diff --git a/gitpull.bat b/gitpull.bat new file mode 100644 index 0000000..0f80eed --- /dev/null +++ b/gitpull.bat @@ -0,0 +1,2 @@ +git pull +pause \ No newline at end of file