Fix oscillation in AGV movement, add command cooldown, and enhance debug logs
This commit is contained in:
backuppc
@@ -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();
|
||||
|
||||
@@ -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,13 +252,12 @@ 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())
|
||||
{
|
||||
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)
|
||||
{
|
||||
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,15 +301,36 @@ 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])
|
||||
{
|
||||
// 2초 쿨타임 적용
|
||||
var tsCmd = DateTime.Now - LastCommandTime;
|
||||
if (tsCmd.TotalSeconds >= 2.0)
|
||||
{
|
||||
var ret = PUB.AGV.AGVMoveSet(new arDev.Narumi.BunkiData
|
||||
{
|
||||
@@ -312,17 +339,27 @@ namespace Project
|
||||
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)
|
||||
{
|
||||
// 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;
|
||||
|
||||
2
gitpull.bat
Normal file
2
gitpull.bat
Normal file
@@ -0,0 +1,2 @@
|
||||
git pull
|
||||
pause
|
||||
Reference in New Issue
Block a user