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

..

Browse Source
This commit is contained in:
backuppc
2026-01-06 17:35:34 +09:00
parent 649d87cae3
commit 90340f4a7d
39 changed files with 2127 additions and 685 deletions
Download Patch File Download Diff File Expand all files Collapse all files

103
Cs_HMI/Project/StateMachine/Step/_Util.cs
View File

@@ -34,7 +34,7 @@ namespace Project
return false;
}
return true;
}
@@ -44,7 +44,7 @@ namespace Project
/// <returns></returns>
public bool CheckLiderStop()
{
if (PUB.AGV.system1.stop_by_front_detect == true)
{
var tsSpeak = DateTime.Now - LastSpeakTime;
@@ -67,8 +67,9 @@ namespace Project
/// <returns></returns>
Boolean UpdateMotionPositionForMark(string sender)
{
//현재위치를 모르는 상태라면 이동하여 현재 위치를 찾는다
if (_SM_RUN_POSCHK(false, new TimeSpan()) == false) return false;
////현재위치를 모르는 상태라면 처리하지 않는다
if (PUB._virtualAGV.CurrentNode == null || PUB._virtualAGV.PrevNode == null)
return false;
//현재위치노드 오류
var currentNode = PUB.FindByNodeID(PUB._virtualAGV.CurrentNode.Id);
@@ -119,6 +120,7 @@ namespace Project
}
else
{
//계산은 되었으나 현재위치가 전체 경로 없다면 시작노드를 현재로 변경해야한다.
PUB._mapCanvas.CurrentPath = PathResult.result;
PUB._virtualAGV.SetPath(PathResult.result);
}
@@ -133,19 +135,50 @@ namespace Project
{
PUB.AGV.AGVMoveStop("Path Integrity Fail");
}
PUB.log.AddE($"경로 무결성 오류");
PUB.sm.SetNewRunStep(ERunStep.READY);
PUB.log.AddE($"경로 무결성 오류로 인해 경로를 삭제 합니다");
Console.WriteLine($"경로 무결성 오류로 인해 경로를 삭제 합니다");
PUB._virtualAGV.SetPath(null);
VAR.I32[eVarInt32.PathValidationError] += 1;
if (VAR.I32[eVarInt32.PathValidationError] > 50)
{
PUB.log.AddE($"연속 경로 무결성 오류로 인해 중지 합니다");
PUB.sm.SetNewRunStep(ERunStep.ERROR);
}
return false;
}
else VAR.I32[eVarInt32.PathValidationError] = 0;
//현재위치 기준으로 재 계산하여. 더 최적화된 루트가 있다면 처리를 해준다.
if (PUB._virtualAGV.CurrentPath.DetailedPath.Count > 5)
{
var PathResult2 = CalcPath(PUB._virtualAGV.CurrentNode, PUB._virtualAGV.TargetNode);
if (PathResult2.result != null && PathResult2.result.Success)
{
//절반이상 경로가 짧을때에는 재계산을 하게한다
var halfcnt = (int)(PUB._virtualAGV.CurrentPath.DetailedPath.Count / 2.0);
if (PathResult2.result.DetailedPath.Count < halfcnt)
{
var msg = $"단축경로가 확인되었습니다. 경로를 삭제 합니다";
PUB.log.AddE(msg);
Console.WriteLine(msg);
PUB._virtualAGV.SetPath(null);
}
return false;
}
}
//predict 를 이용하여 다음 이동을 모두 확인한다.
var nextAction = PUB._virtualAGV.Predict();
if(nextAction.Reason == AGVNavigationCore.Models.eAGVCommandReason.PathOut)
if (nextAction.Reason == AGVNavigationCore.Models.eAGVCommandReason.PathOut)
{
//경로이탈
PUB._virtualAGV.CurrentPath.DetailedPath.Clear();
var logmessage = $"경로이탈감지 시작노드를 현재위치로 설정합니다 START:{PUB._virtualAGV.StartNode},CURRENT:{PUB._virtualAGV.CurrentNode}";
PUB.log.AddE(logmessage);
Console.WriteLine(logmessage);
PUB._virtualAGV.ClearPath();//.DetailedPath.Clear();
PUB._virtualAGV.StartNode = PUB._virtualAGV.CurrentNode;
return false;
}
@@ -158,7 +191,7 @@ namespace Project
$"현재 상태: {PUB._virtualAGV.CurrentState}\n" +
$"현재 방향: {PUB._virtualAGV.CurrentDirection}\n" +
$"위치 확정: {PUB._virtualAGV.IsPositionConfirmed} (RFID {PUB._virtualAGV.DetectedRfidCount}개)\n" +
$"현재 노드: {PUB._virtualAGV.CurrentNode.Id ?? "없음"}";
$"현재 노드: {PUB._virtualAGV.CurrentNodeID2}";
//모터에서 정지를 요청했다
if (nextAction.Motor == AGVNavigationCore.Models.MotorCommand.Stop)
@@ -181,13 +214,33 @@ namespace Project
// 목적지 도착 여부 확인
// 현재 노드가 타겟 노드와 같고, 위치가 확정된 상태라면 도착으로 간주
// 단, AGV가 실제로 멈췄는지 확인 (agv_run == false)
if (PUB._virtualAGV.IsPositionConfirmed &&
PUB._virtualAGV.CurrentNode.Id == PUB._virtualAGV.TargetNode.Id)
if (PUB._virtualAGV.IsPositionConfirmed)
{
if (PUB.AGV.system1.agv_run == false)
{
PUB.log.AddI($"목표 도착 및 정지 확인됨(MarkStop 완료). Node:{PUB._virtualAGV.CurrentNode.Id}");
return true;
//목적지도착완료시
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 완료) Node:{rfid}");
return true;
}
//목적지가 버퍼라면 그 앞에 멈춘다
if (PUB._virtualAGV.TargetNode.StationType == AGVNavigationCore.Models.StationType.Buffer &&
PUB._virtualAGV.CurrentPath != null && PUB._virtualAGV.CurrentPath.DetailedPath.Any())
{
if (PUB._virtualAGV.CurrentNode.Id == PUB._virtualAGV.CurrentPath.DetailedPath.Last().NodeId)
{
PUB.log.AddI($"목표(버퍼) 도착 및 정지 확인됨(MarkStop 완료). Node:{PUB._virtualAGV.CurrentNodeID2}");
return true;
}
}
}
else
{
//아직 멈추지 않았다면 기다린다.
}
}
@@ -213,14 +266,17 @@ namespace Project
PUB.AGV.data.Direction != dir.ToString()[0] ||
PUB.AGV.data.Speed != spd.ToString()[0])
{
PUB.AGV.AGVMoveSet(new arDev.Narumi.BunkiData
var ret = PUB.AGV.AGVMoveSet(new arDev.Narumi.BunkiData
{
Bunki = bunki,
Direction = dir,
PBSSensor = 1,
Speed = spd,
});
PUB.log.Add($"Predict Run Setting = bunki:{bunki},dir:{dir},pbs:1,spd:{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}");
}
// AGV가 정지 상태라면 구동 시작
@@ -250,14 +306,18 @@ namespace Project
PUB.AGV.error.Emergency == false &&
PUB.AGV.system1.agv_run == false)
{
PUB.AGV.AGVMoveSet(new arDev.Narumi.BunkiData
var ret = PUB.AGV.AGVMoveSet(new arDev.Narumi.BunkiData
{
Bunki = arDev.Narumi.eBunki.Strate,
Direction = arDev.Narumi.eMoveDir.Forward,
PBSSensor = 1,
Speed = arDev.Narumi.eMoveSpd.Low,
});
PUB.AGV.AGVMoveRun( arDev.Narumi.eRunOpt.Forward);//
if (ret == arDev.eNarumiCommandResult.Success)
{
PUB.AGV.AGVMoveRun(arDev.Narumi.eRunOpt.Forward);//
}
tm_gocharge_command = DateTime.Now;
}
}
@@ -278,14 +338,15 @@ namespace Project
PUB.AGV.error.Emergency == false &&
PUB.AGV.system1.agv_run == false)
{
PUB.AGV.AGVMoveSet(new arDev.Narumi.BunkiData
var ret = PUB.AGV.AGVMoveSet(new arDev.Narumi.BunkiData
{
Bunki = arDev.Narumi.eBunki.Strate,
Direction = arDev.Narumi.eMoveDir.Backward,
PBSSensor = 1,
Speed = arDev.Narumi.eMoveSpd.Low,
});
PUB.AGV.AGVMoveRun(arDev.Narumi.eRunOpt.Backward);//
if (ret == arDev.eNarumiCommandResult.Success)
PUB.AGV.AGVMoveRun(arDev.Narumi.eRunOpt.Backward);//
LastCommandTime = DateTime.Now;
}
}
@@ -311,7 +372,7 @@ namespace Project
// 만약 수행되지 않았다면 여기서 수행.
if (pathResult.DockingValidation == null)
{
pathResult.DockingValidation = AGVNavigationCore.Utils.DockingValidator.ValidateDockingDirection(pathResult, PUB._mapCanvas.Nodes);
pathResult.DockingValidation = AGVNavigationCore.Utils.DockingValidator.ValidateDockingDirection(pathResult, PUB._mapCanvas.Nodes);
}
// 검증 결과 확인