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-02-27 16:15:30 +09:00
parent 03a53d49bf
commit 43e7458866
2 changed files with 332 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files

340
AGVLogic/AGVNavigationCore/PathFinding/Planning/AGVPathfinder.cs
View File

@@ -506,7 +506,150 @@ namespace AGVNavigationCore.PathFinding.Planning
//일반노드도 일부 경로를 계산한다.
// 일반노드 3 ->
retval.Add(new MapZonePathData
{
NodeSta = StationType.Normal,
NodeEnd = StationType.Loader,
Monitor = MapZoneMonitor.LeftTop,
Path = new List<string> { "3B", "71B" }
});
retval.Add(new MapZonePathData
{
NodeSta = StationType.Normal,
NodeEnd = StationType.Cleaner,
Monitor = MapZoneMonitor.LeftTop,
Path = new List<string> { "3T", "3B", "70B" }
});
retval.Add(new MapZonePathData
{
NodeSta = StationType.Normal,
NodeEnd = StationType.Plating,
Monitor = MapZoneMonitor.LeftTop,
Path = new List<string> { "3B", "11B", "72B" }
});
retval.Add(new MapZonePathData
{
NodeSta = StationType.Normal,
NodeEnd = StationType.Charger,
Monitor = MapZoneMonitor.LeftTop,
Path = new List<string> { "3T", "3F", "11F", "7B", "10B", "6B", "73B" }
});
retval.Add(new MapZonePathData
{
NodeSta = StationType.Normal,
NodeEnd = StationType.Buffer,
Monitor = MapZoneMonitor.LeftTop,
Path = new List<string> { "3B", "11B", "7B", "8B", "9B", "20B", "34B", "33B", "32B", "31B", "35B", "36B" }
});
retval.Add(new MapZonePathData
{
NodeSta = StationType.Normal,
NodeEnd = StationType.Loader,
Monitor = MapZoneMonitor.RightBtm,
Path = new List<string> { "3T", "3B", "71B" }
});
retval.Add(new MapZonePathData
{
NodeSta = StationType.Normal,
NodeEnd = StationType.Cleaner,
Monitor = MapZoneMonitor.RightBtm,
Path = new List<string> { "3B", "70B" }
});
retval.Add(new MapZonePathData
{
NodeSta = StationType.Normal,
NodeEnd = StationType.Plating,
Monitor = MapZoneMonitor.RightBtm,
Path = new List<string> { "3T", "3B", "11B", "72B" }
});
retval.Add(new MapZonePathData
{
NodeSta = StationType.Normal,
NodeEnd = StationType.Charger,
Monitor = MapZoneMonitor.RightBtm,
Path = new List<string> { "3F", "11F", "7B", "10B", "6B", "73B" }
});
retval.Add(new MapZonePathData
{
NodeSta = StationType.Normal,
NodeEnd = StationType.Buffer,
Monitor = MapZoneMonitor.RightBtm,
Path = new List<string> { "3T", "3B", "11B", "7B", "8B", "9B", "20B", "34B", "33B", "32B", "31B", "35B", "36B" }
});
// 일반노드 7 ->
retval.Add(new MapZonePathData
{
NodeSta = StationType.Normal,
NodeEnd = StationType.Loader,
Monitor = MapZoneMonitor.LeftTop,
Path = new List<string> { "7F", "11F", "3B", "71B" }
});
retval.Add(new MapZonePathData
{
NodeSta = StationType.Normal,
NodeEnd = StationType.Cleaner,
Monitor = MapZoneMonitor.LeftTop,
Path = new List<string> { "7F", "11F", "3T", "3B", "70B" }
});
retval.Add(new MapZonePathData
{
NodeSta = StationType.Normal,
NodeEnd = StationType.Plating,
Monitor = MapZoneMonitor.LeftTop,
Path = new List<string> { "7F", "11B", "72B" }
});
retval.Add(new MapZonePathData
{
NodeSta = StationType.Normal,
NodeEnd = StationType.Charger,
Monitor = MapZoneMonitor.LeftTop,
Path = new List<string> { "7F", "11F", "3T", "3F", "11F", "7B", "10B", "6B", "73B" }
});
retval.Add(new MapZonePathData
{
NodeSta = StationType.Normal,
NodeEnd = StationType.Buffer,
Monitor = MapZoneMonitor.LeftTop,
Path = new List<string> { "7B", "8B", "9B", "20B", "34B", "33B", "32B", "31B", "35B", "36B" }
});
retval.Add(new MapZonePathData
{
NodeSta = StationType.Normal,
NodeEnd = StationType.Loader,
Monitor = MapZoneMonitor.RightBtm,
Path = new List<string> { "7B", "11B", "3T", "3B", "71B" }
});
retval.Add(new MapZonePathData
{
NodeSta = StationType.Normal,
NodeEnd = StationType.Cleaner,
Monitor = MapZoneMonitor.RightBtm,
Path = new List<string> { "7B", "11B", "3B", "70B" }
});
retval.Add(new MapZonePathData
{
NodeSta = StationType.Normal,
NodeEnd = StationType.Plating,
Monitor = MapZoneMonitor.RightBtm,
Path = new List<string> { "7B", "11B", "3T", "3B", "11B", "72B" }
});
retval.Add(new MapZonePathData
{
NodeSta = StationType.Normal,
NodeEnd = StationType.Charger,
Monitor = MapZoneMonitor.RightBtm,
Path = new List<string> { "7B", "10B", "6B", "73B" }
});
retval.Add(new MapZonePathData
{
NodeSta = StationType.Normal,
NodeEnd = StationType.Buffer,
Monitor = MapZoneMonitor.RightBtm,
Path = new List<string> { "7B", "11B", "3T", "3B", "11B", "7B", "8B", "9B", "20B", "34B", "33B", "32B", "31B", "35B", "36B" }
});
return retval;
}
@@ -564,23 +707,71 @@ namespace AGVNavigationCore.PathFinding.Planning
// 반전되어 구해지는 현상이 있으므로, 이 경우에만 monitorMode를 반대로 뒤집어서 찾는다.
if (startZone != targetZone && startNode.StationType == StationType.Normal)
{
monitorMode = monitorMode == MapZoneMonitor.LeftTop ? MapZoneMonitor.RightBtm : MapZoneMonitor.LeftTop;
//monitorMode = monitorMode == MapZoneMonitor.LeftTop ? MapZoneMonitor.RightBtm : MapZoneMonitor.LeftTop;
}
// 모니터방향이 일치하고 대상노드가 동일한 경로를 찾는다
var zonepath = GetMapZonePathData();
IEnumerable<MapZonePathData> candidates;
// 목적지가 현재 위치와 동일한 경우 처리
//시작이 일반노드라면 , 경로가 등록된 주변 노드로 이동해서 이후경로를 계산하자
//일반노드용 기준은 7,3이있다.
//11번은 7로 이동해서 경로를 계산한다.
//11번은 3로 이동해서 경로를 계산한다.
//10,6번은 7로 이동해서 경로를 계산한다.
//대상노드까지 CalculateScriptedPath를 재귀로 호출해서 계산을 완료한 후, 대상에서 목적지계산해서 전체 경로를 만들어낸다.
//대상노드까지 CalculateScriptedPath를 재귀로 호출해서 계산을 완료한 후, 대상에서 목적지 계산해서 전체 경로를 만들어낸다.
if (startNode.StationType == StationType.Normal && startNode.RfidId != 3 && startNode.RfidId != 7)
{
var newtargetRFID = startNode.RfidId == 11 ? 3 : 7;
var refNode = _mapNodes.FirstOrDefault(n => n.RfidId == newtargetRFID);
if (refNode != null && refNode.Id != startNode.Id)
{
// 기준 노드(7 or 3)까지의 기본 경로를 먼저 구함
var pathToRef = this.FindBasicPath(startNode, refNode, prevNode, prevDir);
if (pathToRef != null && pathToRef.Success)
{
var lastDet = pathToRef.DetailedPath.Last();
var secondLastNode = pathToRef.Path.Count > 1 ? pathToRef.Path[pathToRef.Path.Count - 2] : prevNode;
// 기준 노드(7 or 3)에서 목적지까지의 경로를 재귀적으로 계산 (이 때 하드코딩된 zonepath가 활용됨)
var pathFromRef = CalculateScriptedPath(refNode, targetNode, secondLastNode, lastDet.MotorDirection);
if (pathFromRef != null && pathFromRef.Success)
{
// 중복 노드 제거를 위해 첫 번째 경로의 마지막 노드 정보를 제거하고 합침
pathToRef.Path.RemoveAt(pathToRef.Path.Count - 1);
pathToRef.DetailedPath.RemoveAt(pathToRef.DetailedPath.Count - 1);
return CombinePaths(pathToRef, pathFromRef);
}
}
}
}
//이곳에서 시작,종료노드가 완전히 일치하는 경로를 찾고 있다면 그것을 바로 반환한다
//그런경우는 복잡하게 추가 계산할 필요가 없으니까
var exactMatchList = zonepath.Where(d =>
d.NodeSta == startNode.StationType &&
d.NodeEnd == targetNode.StationType &&
d.Monitor == monitorMode);
var exactMatch = exactMatchList.FirstOrDefault(d=>
d.Path.First().StartsWith(startNode.RfidId.ToString()) &&
d.Path.Last().StartsWith(targetNode.RfidId.ToString()));
if (exactMatch != null)
{
int startIndex = exactMatch.Path.FindIndex(p => p == startTag);
if (startIndex == -1) startIndex = exactMatch.Path.FindIndex(p => p.StartsWith(startNode.RfidId.ToString()));
int endIndex = exactMatch.Path.FindLastIndex(p => p.StartsWith(targetNode.RfidId.ToString()));
if (startIndex != -1 && endIndex != -1 && startIndex <= endIndex)
{
var slicedPath = exactMatch.Path.Skip(startIndex).Take(endIndex - startIndex + 1).ToList();
return ConvertHardcodedPathToResult(slicedPath, startNode, prevNode, prevDir);
}
}
// 시작Zone과 목표Zone이 다른 경우에만 하드코딩된 zonepath 검색
if (startZone != targetZone)
@@ -704,7 +895,7 @@ namespace AGVNavigationCore.PathFinding.Planning
if (path2Candidate == null) return AGVPathResult.CreateFailure("턴 포인트에서 목표로 향하는 하드코딩 경로를 찾을 수 없습니다.");
int path2StartIdx = path2Candidate.Path.FindIndex(p => p == "3T");
int path2StartIdx = path2Candidate.Path.FindIndex(p => p == "3T");
int path2EndIdx = path2Candidate.Path.FindLastIndex(p => p == $"{targetNode.RfidId}B");
var slicedPath2 = path2Candidate.Path.Skip(path2StartIdx).Take(path2EndIdx - path2StartIdx + 1).ToList();
@@ -747,7 +938,7 @@ namespace AGVNavigationCore.PathFinding.Planning
if (endBufferNode == null) return AGVPathResult.CreateFailure("버퍼 끝 노드(7)를 찾을 수 없습니다.");
var overPathFull = this.FindBasicPath(startNode, endBufferNode, prevNode, AgvDirection.Forward);
if (overPathFull == null || !overPathFull.Success)
if (overPathFull == null || !overPathFull.Success)
return AGVPathResult.CreateFailure("Overshoot 전체 경로(7번 방향) 탐색 실패");
int targetIdx = overPathFull.Path.FindIndex(n => n.Id == targetNode.Id);
@@ -755,11 +946,11 @@ namespace AGVNavigationCore.PathFinding.Planning
return AGVPathResult.CreateFailure("Overshoot를 위한 여유 공간(다음 노드)이 없습니다.");
// 목표 노드 다음 노드(오버슈트 지점)까지만 잘라내어 새 경로 구성
var overPath = new AGVPathResult
{
Success = true,
Path = overPathFull.Path.Take(targetIdx + 2).ToList(),
DetailedPath = overPathFull.DetailedPath.Take(targetIdx + 2).ToList()
var overPath = new AGVPathResult
{
Success = true,
Path = overPathFull.Path.Take(targetIdx + 2).ToList(),
DetailedPath = overPathFull.DetailedPath.Take(targetIdx + 2).ToList()
};
var autoOverNode = overPath.Path.Last(); // 오버슈트 된 곳
@@ -835,10 +1026,130 @@ namespace AGVNavigationCore.PathFinding.Planning
return AGVPathResult.CreateFailure("경로를 계산할 수 없습니다");
}
private MapZoneMonitor GetMonitorMode(MapNode startNode, MapNode prevNode, AgvDirection prevDir)
{
if (prevNode == null) return MapZoneMonitor.RightBtm;
//모니터방향도 상황에 따라 다른경우가 있다. 이것도 하드코딩하다.
//prev -> start 와 모터방향(prevdir) 에 따라서 경우의 수를 입력한다.
//일반노드가아닌 노드와, 일반노드중 7,11을 포함해서 모든 경우를 정의해야한다.
//3 - junction(turn)
if (startNode.RfidId == 3)
{
if (prevDir == AgvDirection.Forward)
{
if (prevNode.RfidId == 70)
return MapZoneMonitor.RightBtm;
else if (prevNode.RfidId == 11 || prevNode.RfidId == 71)
return MapZoneMonitor.LeftTop;
}
else if (prevDir == AgvDirection.Backward)
{
if (prevNode.RfidId == 70)
return MapZoneMonitor.LeftTop;
else if (prevNode.RfidId == 11 || prevNode.RfidId == 71)
return MapZoneMonitor.RightBtm;
}
}
//7 - junction
if (startNode.RfidId == 7)
{
if (prevDir == AgvDirection.Forward)
{
if (prevNode.RfidId == 10 || prevNode.RfidId == 11)
return MapZoneMonitor.RightBtm;
else if (prevNode.RfidId == 8)
return MapZoneMonitor.LeftTop;
}
else if (prevDir == AgvDirection.Backward)
{
if (prevNode.RfidId == 10 || prevNode.RfidId == 11)
return MapZoneMonitor.LeftTop;
else if (prevNode.RfidId == 8)
return MapZoneMonitor.RightBtm;
}
}
//70 - cleaner
if (startNode.RfidId == 70)
{
if (prevDir == AgvDirection.Forward)
{
if (prevNode.RfidId == 3)
return MapZoneMonitor.LeftTop;
}
else if (prevDir == AgvDirection.Backward)
{
if (prevNode.RfidId == 3)
return MapZoneMonitor.RightBtm;
}
}
//71 - loader
if (startNode.RfidId == 71)
{
if (prevDir == AgvDirection.Forward)
{
if (prevNode.RfidId == 3)
return MapZoneMonitor.RightBtm;
}
else if (prevDir == AgvDirection.Backward)
{
if (prevNode.RfidId == 3)
return MapZoneMonitor.LeftTop;
}
}
//73 - charger
if (startNode.RfidId == 73)
{
if (prevDir == AgvDirection.Forward)
{
if (prevNode.RfidId == 6)
return MapZoneMonitor.LeftTop;
}
else if (prevDir == AgvDirection.Backward)
{
if (prevNode.RfidId == 6)
return MapZoneMonitor.RightBtm;
}
}
//72 -- plating
if (startNode.RfidId == 72)
{
if (prevDir == AgvDirection.Forward)
{
if (prevNode.RfidId == 11)
return MapZoneMonitor.LeftTop;
}
else if (prevDir == AgvDirection.Backward)
{
if (prevNode.RfidId == 11)
return MapZoneMonitor.RightBtm;
}
}
//31~34,35,36 --buffer
if (startNode.RfidId >= 31 && startNode.RfidId <= 36)
{
if (prevDir == AgvDirection.Forward)
{
if (prevNode.RfidId == 20)
return MapZoneMonitor.LeftTop;
}
else if (prevDir == AgvDirection.Backward)
{
if (prevNode.RfidId == 20)
return MapZoneMonitor.RightBtm;
}
}
int dx = startNode.Position.X - prevNode.Position.X;
int dy = startNode.Position.Y - prevNode.Position.Y;
@@ -853,8 +1164,15 @@ namespace AGVNavigationCore.PathFinding.Planning
}
return isMonitorLeft ? MapZoneMonitor.LeftTop : MapZoneMonitor.RightBtm;
}
private AGVPathResult ConvertHardcodedPathToResult(List<string> pathStrings, MapNode startNode, MapNode prevNode, AgvDirection prevDir)
{
var result = new AGVPathResult { Success = true };

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

@@ -194,28 +194,6 @@ namespace Project
}
else VAR.I32[eVarInt32.PathValidationError] = 0;
//현재위치 기준으로 재 계산하여. 더 최적화된 루트가 있다면 처리를 해준다.
/*
//TODO: 이제 경로가 하드코딩되어있으니 자동 갱신은 사용하지 않는다.
if (PUB._virtualAGV.CurrentPath.DetailedPath.Count > 5)
{
var PathResult2 = CalcPath(PUB._virtualAGV.CurrentNode, PUB._virtualAGV.TargetNode);
if (PathResult2 != null && PathResult2.Success)
{
//절반이상 경로가 짧을때에는 재계산을 하게한다
var halfcnt = (int)(PUB._virtualAGV.CurrentPath.DetailedPath.Count / 2.0);
if (PathResult2.DetailedPath.Count > 2 && PathResult2.DetailedPath.Count < halfcnt)
{
var msg = $"단축경로가 확인되었습니다. 경로를 삭제 합니다";
PUB.log.AddE(msg);
Console.WriteLine(msg);
PUB._virtualAGV.SetPath(null);
return false;
}
}
}
*/
//predict 를 이용하여 다음 이동을 모두 확인한다.
var nextAction = PUB._virtualAGV.Predict();
@@ -331,6 +309,9 @@ namespace Project
else
{
//PREDICT에서 이동을 명령했따
var bunki = arDev.Narumi.eBunki.Strate;
if (nextAction.Magnet == AGVNavigationCore.Models.MagnetPosition.L) bunki = arDev.Narumi.eBunki.Left;
else if (nextAction.Magnet == AGVNavigationCore.Models.MagnetPosition.R) bunki = arDev.Narumi.eBunki.Right;