Amkor/ENIG
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Fix oscillation in AGV movement, add command cooldown, and enhance debug logs

Browse Source
This commit is contained in:
backuppc
2026-01-29 14:51:57 +09:00
parent 1da1f2de28
commit e99edbe04d
3 changed files with 68 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files

89
HMI/Project/StateMachine/Step/_Util.cs
View File

@@ -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;