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-13 14:55:28 +09:00
parent 213467fe3f
commit dbc53e3146
3 changed files with 271 additions and 92 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
AGVLogic/AGVNavigationCore/Models/NodeMotorInfo.cs
View File

@@ -80,13 +80,16 @@ namespace AGVNavigationCore.Models
/// </summary>
public bool IsPass { get; set; }
public bool IsTurn { get; set; }
/// <summary>
/// 특수 동작 설명
/// </summary>
public string SpecialActionDescription { get; set; }
public NodeMotorInfo(int seqno,string nodeId,ushort rfid, AgvDirection motorDirection, MapNode nextNodeId = null, MagnetDirection magnetDirection = MagnetDirection.Straight)
public NodeMotorInfo(int seqno,string nodeId,ushort rfid,
AgvDirection motorDirection, MapNode nextNodeId = null, MagnetDirection magnetDirection = MagnetDirection.Straight,bool turn=false)
{
IsTurn = turn;
seq = seqno;
NodeId = nodeId;
RfidId = rfid;

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

@@ -536,7 +536,11 @@ namespace AGVNavigationCore.PathFinding.Planning
}
#endregion
public enum MapZoneMonitor
{
LeftTop,
RightBtm,
}
public enum MapZone
{
None,
@@ -545,91 +549,261 @@ namespace AGVNavigationCore.PathFinding.Planning
Plating, // 72 ~ 05
Loader, // 71 ~ 04
Cleaner, // 70 ~ 01
Junction // Hub (11, 12, etc)
Junction, // Hub (11, 12, etc)
Turn,
}
public class MapZonePathData
{
public StationType NodeSta { get; set; }
public StationType NodeEnd { get; set; }
public MapZoneMonitor Monitor { get; set; }
public List<string> Path { get; set; }
}
public List<MapZonePathData> GetMapZonePathData()
{
var retval = new List<MapZonePathData>();
// Buffer -> ...
retval.Add(new MapZonePathData { NodeSta = StationType.Buffer, NodeEnd = StationType.Charger, Monitor = MapZoneMonitor.RightBtm, Path = new List<string> { "36F", "35F", "31F", "32F", "33F", "34F", "7F", "12F", "11B", "5B", "6B", "73B" } });
retval.Add(new MapZonePathData { NodeSta = StationType.Buffer, NodeEnd = StationType.Charger, Monitor = MapZoneMonitor.LeftTop, Path = new List<string> { "36B", "35B", "31B", "32B", "33B", "34B", "7B", "10B", "6B", "73B" } });
retval.Add(new MapZonePathData { NodeSta = StationType.Buffer, NodeEnd = StationType.Plating, Monitor = MapZoneMonitor.RightBtm, Path = new List<string> { "36F", "35F", "31F", "32F", "33F", "34F", "7F", "12F", "11B", "5B", "72B" } });
retval.Add(new MapZonePathData { NodeSta = StationType.Buffer, NodeEnd = StationType.Plating, Monitor = MapZoneMonitor.LeftTop, Path = new List<string> { "36B", "35B", "31B", "32B", "33B", "34B", "7B", "12B", "11B", "3T", "3B", "11B", "5B", "72B" } });
retval.Add(new MapZonePathData { NodeSta = StationType.Buffer, NodeEnd = StationType.Loader, Monitor = MapZoneMonitor.RightBtm, Path = new List<string> { "36F", "35F", "31F", "32F", "33F", "34F", "7F", "12F", "11F", "3B", "4B", "71B" } });
retval.Add(new MapZonePathData { NodeSta = StationType.Buffer, NodeEnd = StationType.Loader, Monitor = MapZoneMonitor.LeftTop, Path = new List<string> { "36B", "35B", "31B", "32B", "33B", "34B", "7B", "12B", "11B", "3T", "3B", "4B", "71B" } });
retval.Add(new MapZonePathData { NodeSta = StationType.Buffer, NodeEnd = StationType.Cleaner, Monitor = MapZoneMonitor.RightBtm, Path = new List<string> { "36F", "35F", "31F", "32F", "33F", "34F", "7F", "12F", "11F", "3T", "3B", "2B", "1B", "70B" } });
retval.Add(new MapZonePathData { NodeSta = StationType.Buffer, NodeEnd = StationType.Cleaner, Monitor = MapZoneMonitor.LeftTop, Path = new List<string> { "36B", "35B", "31B", "32B", "33B", "34B", "7B", "12B", "11B", "3B", "2B", "1B", "70B" } });
// Loader -> ...
retval.Add(new MapZonePathData { NodeSta = StationType.Loader, NodeEnd = StationType.Cleaner, Monitor = MapZoneMonitor.RightBtm, Path = new List<string> { "71B", "4B", "3B", "2B", "1B", "70B" } });
retval.Add(new MapZonePathData { NodeSta = StationType.Loader, NodeEnd = StationType.Cleaner, Monitor = MapZoneMonitor.LeftTop, Path = new List<string> { "71F", "4F", "3T", "3B", "2B", "1B", "70B" } });
retval.Add(new MapZonePathData { NodeSta = StationType.Loader, NodeEnd = StationType.Charger, Monitor = MapZoneMonitor.RightBtm, Path = new List<string> { "71B", "4B", "3T", "3B", "11B", "5B", "6B", "73B" } });
retval.Add(new MapZonePathData { NodeSta = StationType.Loader, NodeEnd = StationType.Charger, Monitor = MapZoneMonitor.LeftTop, Path = new List<string> { "71F", "4F", "3B", "11B", "5B", "6B", "73B" } });
retval.Add(new MapZonePathData { NodeSta = StationType.Loader, NodeEnd = StationType.Plating, Monitor = MapZoneMonitor.RightBtm, Path = new List<string> { "71B", "4B", "3T", "3B", "11B", "5B", "72B" } });
retval.Add(new MapZonePathData { NodeSta = StationType.Loader, NodeEnd = StationType.Plating, Monitor = MapZoneMonitor.LeftTop, Path = new List<string> { "71F", "4F", "3B", "11B", "5B", "72B" } });
retval.Add(new MapZonePathData { NodeSta = StationType.Loader, NodeEnd = StationType.Buffer, Monitor = MapZoneMonitor.RightBtm, Path = new List<string> { "71B", "4B", "3T", "3B", "11B", "12B", "7B", "8B", "9B", "34B", "33B", "32B", "31B", "35B", "36B" } });
retval.Add(new MapZonePathData { NodeSta = StationType.Loader, NodeEnd = StationType.Buffer, Monitor = MapZoneMonitor.LeftTop, Path = new List<string> { "71F", "4F", "3B", "11B", "12B", "7B", "8B", "9B", "34B", "33B", "32B", "31B", "35B", "36B" } });
// Cleaner -> ...
retval.Add(new MapZonePathData { NodeSta = StationType.Cleaner, NodeEnd = StationType.Loader, Monitor = MapZoneMonitor.RightBtm, Path = new List<string> { "70F", "1F", "2F", "3T", "3B", "4B", "71B" } });
retval.Add(new MapZonePathData { NodeSta = StationType.Cleaner, NodeEnd = StationType.Loader, Monitor = MapZoneMonitor.LeftTop, Path = new List<string> { "70B", "1B", "2B", "3B", "4B", "71B" } });
retval.Add(new MapZonePathData { NodeSta = StationType.Cleaner, NodeEnd = StationType.Charger, Monitor = MapZoneMonitor.RightBtm, Path = new List<string> { "70F", "1F", "2F", "3T", "3B", "11B", "5B", "6B", "73B" } });
retval.Add(new MapZonePathData { NodeSta = StationType.Cleaner, NodeEnd = StationType.Charger, Monitor = MapZoneMonitor.LeftTop, Path = new List<string> { "70B", "1B", "2B", "3B", "11B", "5B", "6B", "73B" } });
retval.Add(new MapZonePathData { NodeSta = StationType.Cleaner, NodeEnd = StationType.Plating, Monitor = MapZoneMonitor.RightBtm, Path = new List<string> { "70F", "1F", "2F", "3T", "3B", "11B", "5B", "72B" } });
retval.Add(new MapZonePathData { NodeSta = StationType.Cleaner, NodeEnd = StationType.Plating, Monitor = MapZoneMonitor.LeftTop, Path = new List<string> { "70B", "1B", "2B", "3B", "11B", "5B", "72B" } });
retval.Add(new MapZonePathData { NodeSta = StationType.Cleaner, NodeEnd = StationType.Buffer, Monitor = MapZoneMonitor.RightBtm, Path = new List<string> { "70F", "1F", "2F", "3T", "3B", "11B", "12B", "7B", "8B", "9B", "34B", "33B", "32B", "31B", "35B", "36B" } });
retval.Add(new MapZonePathData { NodeSta = StationType.Cleaner, NodeEnd = StationType.Buffer, Monitor = MapZoneMonitor.LeftTop, Path = new List<string> { "70B", "1B", "2B", "3B", "11B", "12B", "7B", "8B", "9B", "34B", "33B", "32B", "31B", "35B", "36B" } });
// Plating -> ...
retval.Add(new MapZonePathData { NodeSta = StationType.Plating, NodeEnd = StationType.Loader, Monitor = MapZoneMonitor.RightBtm, Path = new List<string> { "72F", "5F", "11F", "3B", "4B", "71B" } });
retval.Add(new MapZonePathData { NodeSta = StationType.Plating, NodeEnd = StationType.Loader, Monitor = MapZoneMonitor.LeftTop, Path = new List<string> { "72B", "5B", "11B", "3T", "3B", "4B", "71B" } });
retval.Add(new MapZonePathData { NodeSta = StationType.Plating, NodeEnd = StationType.Charger, Monitor = MapZoneMonitor.RightBtm, Path = new List<string> { "72F", "5F", "11B", "5B", "6B", "73B" } });
retval.Add(new MapZonePathData { NodeSta = StationType.Plating, NodeEnd = StationType.Charger, Monitor = MapZoneMonitor.LeftTop, Path = new List<string> { "72B", "5B", "11B", "3T", "3B", "11B", "5B", "6B", "73B" } });
retval.Add(new MapZonePathData { NodeSta = StationType.Plating, NodeEnd = StationType.Cleaner, Monitor = MapZoneMonitor.RightBtm, Path = new List<string> { "72F", "5F", "11F", "3T", "3B", "2B", "1B", "70B" } });
retval.Add(new MapZonePathData { NodeSta = StationType.Plating, NodeEnd = StationType.Cleaner, Monitor = MapZoneMonitor.LeftTop, Path = new List<string> { "72B", "5B", "11B", "3B", "2B", "1B", "70B" } });
retval.Add(new MapZonePathData { NodeSta = StationType.Plating, NodeEnd = StationType.Buffer, Monitor = MapZoneMonitor.RightBtm, Path = new List<string> { "72F", "5F", "11B", "12B", "7B", "8B", "9B", "34B", "33B", "32B", "31B", "35B", "36B" } });
retval.Add(new MapZonePathData { NodeSta = StationType.Plating, NodeEnd = StationType.Buffer, Monitor = MapZoneMonitor.LeftTop, Path = new List<string> { "72B", "5B", "11B", "3T", "3B", "11B", "12B", "7B", "8B", "9B", "34B", "33B", "32B", "31B", "35B", "36B" } });
return retval;
}
/// <summary>
/// 해당 노드가 속하는 존을 반환한다.
/// </summary>
/// <param name="node"></param>
/// <returns></returns>
public MapZone GetMapZone(MapNode node)
{
if (node == null) return MapZone.None;
int rfid = node.RfidId;
// Buffer: 91~07 (Linear)
// Assuming 91 is start, 07 is end.
// Range check might be tricky if IDs are not sequential.
// Using precise list based on map description if possible, acts as a catch-all for now.
if (rfid == 91 || (rfid >= 31 && rfid <= 36) || (rfid >= 7 && rfid <= 9)) return MapZone.Buffer;
Dictionary<MapZone, int[]> ZoneList = GetMapZoneNodeList();
var zone = ZoneList.Where(t => t.Value.Contains(rfid)).FirstOrDefault();
if (zone.Value == null) return MapZone.None;
// Charger: 73~10
if (rfid == 73 || rfid == 6 || rfid == 10) return MapZone.Charger;
// Plating: 72~5
if (rfid == 72 || rfid == 5) return MapZone.Plating;
// Loader: 71~4
if (rfid == 71 || rfid == 4) return MapZone.Loader;
// Cleaner: 70~1
if (rfid == 70 || rfid == 1 || rfid == 2 || rfid == 3) return MapZone.Cleaner;
// Junction (Hub)
if (rfid == 11 || rfid == 12) return MapZone.Junction;
return MapZone.None;
return zone.Key;
}
public Dictionary<MapZone, int[]> GetMapZoneNodeList()
{
Dictionary<MapZone, int[]> ZoneList = new Dictionary<MapZone, int[]>();
ZoneList.Add(MapZone.Turn, new int[] { 3 });
ZoneList.Add(MapZone.Buffer, new int[] { 91, 36, 35, 31, 32, 33, 34, 9, 8, 7 });
ZoneList.Add(MapZone.Charger, new int[] { 73, 6, 10 });
ZoneList.Add(MapZone.Junction, new int[] { 12, 11 });
ZoneList.Add(MapZone.Plating, new int[] { 72, 5 });
ZoneList.Add(MapZone.Loader, new int[] { 71, 4 });
ZoneList.Add(MapZone.Cleaner, new int[] { 70, 1, 2 });
return ZoneList;
}
public AGVPathResult CalculateScriptedPath(MapNode startNode, MapNode targetNode, MapNode prevNode, AgvDirection prevDir)
{
var startZone = GetMapZone(startNode);
var targetZone = GetMapZone(targetNode);
// 1. Same Zone or Trivial Case -> Use CalculatePath_new
if (startZone == targetZone && startZone != MapZone.None && startZone != MapZone.Junction)
// 존이 확인되지 않는다면 오류
if (startZone == MapZone.None || targetZone == MapZone.None)
{
return CalculatePath_new(startNode, targetNode, prevNode, prevDir);
// return AGVPathResult.CreateFailure($"Zone not found: {startNode.ID2}->{targetNode.ID2}");
}
// 2. Hub Logic (Buffer -> Hub -> Target, etc.)
// Logic: Start -> ExitNode -> Hub -> EntryNode -> Target
var monitorMode = GetMonitorMode(startNode, prevNode, prevDir);
var motDir = prevDir == AgvDirection.Forward ? 'F' : 'B';
MapNode exitNode = GetZoneExitNode(startZone);
MapNode entryNode = GetZoneEntryNode(targetZone);
// 시작 태그 검색용 (예: "91F")
string startTag = $"{startNode.RfidId}{motDir}";
// If Start/Target are in Junction or Unknown, handle gracefully
if (startZone == MapZone.Junction) exitNode = startNode;
if (targetZone == MapZone.Junction) entryNode = targetNode;
// 모니터방향이 일치하고 대상노드가 동일한 경로를 찾는다
var zonepath = GetMapZonePathData();
if (exitNode == null || entryNode == null)
// 모든 후보 경로 검색
var candidates = zonepath.Where(d =>
d.Monitor == monitorMode &&
d.NodeEnd == targetNode.StationType &&
d.Path.Any(p => p.StartsWith(startNode.RfidId.ToString())) && // 시작 포인트 포함
d.Path.Any(p => p.StartsWith(targetNode.RfidId.ToString())) // 끝 포인트 포함
).ToList();
if (candidates.Any())
{
// Fallback to normal search if zone logic fails
return CalculatePath_new(startNode, targetNode, prevNode, prevDir);
MapZonePathData bestPath = null;
int bestStartIndex = -1;
int bestEndIndex = -1;
int minPathLength = int.MaxValue;
foreach (var candidate in candidates)
{
// 시작 태그와 가장 일치하는 인덱스 찾기 (방향까지 고려 "91F")
int startIndex = candidate.Path.FindIndex(p => p == startTag);
if (startIndex == -1) // 방향이 안 맞으면 그냥 RFID로만 찾기
startIndex = candidate.Path.FindIndex(p => p.StartsWith(startNode.RfidId.ToString()));
// 끝 태그 인덱스 (뒤에서부터 찾기)
int endIndex = candidate.Path.FindLastIndex(p => p.StartsWith(targetNode.RfidId.ToString()));
if (startIndex != -1 && endIndex != -1 && startIndex < endIndex)
{
int length = endIndex - startIndex;
if (length < minPathLength)
{
minPathLength = length;
bestPath = candidate;
bestStartIndex = startIndex;
bestEndIndex = endIndex;
}
}
}
// Path 1: Start -> Exit
var path1 = CalculatePath_new(startNode, exitNode, prevNode, prevDir);
if (!path1.Success) return AGVPathResult.CreateFailure($"Zone Exit Failure: {startNode.ID2}->{exitNode.ID2}");
if (bestPath != null)
{
// 추출된 경로 조각
var slicedPath = bestPath.Path.Skip(bestStartIndex).Take(bestEndIndex - bestStartIndex + 1).ToList();
var a = ConvertHardcodedPathToResult(slicedPath, startNode, prevNode, prevDir);
return a;
}
}
// Path 2: Exit -> Entry (Hub Crossing)
// Use CalculatePath_new for Hub crossing relative to Arrival Direction
var lastNode1 = path1.Path.Last();
var lastDir1 = path1.DetailedPath.Last().MotorDirection;
var prevNode1 = path1.Path.Count > 1 ? path1.Path[path1.Path.Count - 2] : prevNode;
// 하드코딩된 경로가 없으면 기존 A* 로직으로 대체 또는 실패 반환
return null;
}
var path2 = CalculatePath_new(exitNode, entryNode, prevNode1, lastDir1);
if (!path2.Success) return AGVPathResult.CreateFailure($"Hub Crossing Failure: {exitNode.ID2}->{entryNode.ID2}");
private MapZoneMonitor GetMonitorMode(MapNode startNode, MapNode prevNode, AgvDirection prevDir)
{
if (prevNode == null) return MapZoneMonitor.RightBtm;
// Path 3: Entry -> Target
var lastNode2 = path2.Path.Last();
var lastDir2 = path2.DetailedPath.Last().MotorDirection;
var prevNode2 = path2.Path.Count > 1 ? path2.Path[path2.Path.Count - 2] : lastNode1;
int dx = startNode.Position.X - prevNode.Position.X;
int dy = startNode.Position.Y - prevNode.Position.Y;
var path3 = CalculatePath_new(entryNode, targetNode, prevNode2, lastDir2);
if (!path3.Success) return AGVPathResult.CreateFailure($"Zone Entry Failure: {entryNode.ID2}->{targetNode.ID2}");
bool isMonitorLeft = false;
if (Math.Abs(dx) > Math.Abs(dy)) // Horizontal
{
isMonitorLeft = (prevDir == AgvDirection.Backward);
}
else // Vertical
{
isMonitorLeft = (prevDir == AgvDirection.Forward);
}
// Merge Paths
var merged = Utility.CombineResults(path1, path2);
merged = Utility.CombineResults(merged, path3);
return isMonitorLeft ? MapZoneMonitor.LeftTop : MapZoneMonitor.RightBtm;
}
return merged;
private AGVPathResult ConvertHardcodedPathToResult(List<string> pathStrings, MapNode startNode, MapNode prevNode, AgvDirection prevDir)
{
var result = new AGVPathResult { Success = true };
var pathList = new List<MapNode>();
var detailedList = new List<NodeMotorInfo>();
int seq = 1;
for (int i = 0; i < pathStrings.Count; i++)
{
string s = pathStrings[i];
if (string.IsNullOrEmpty(s)) continue;
string rfIdStr = "";
char flag = ' ';
foreach (char c in s)
{
if (char.IsDigit(c)) rfIdStr += c;
else flag = c;
}
var node = _mapNodes.FirstOrDefault(n => n.RfidId.ToString() == rfIdStr);
if (node == null) continue;
// Determine Motor Direction from Flag or Maintain Previous
AgvDirection motorDir = detailedList.Count > 0 ? detailedList.Last().MotorDirection : prevDir;
bool isTurn = false;
if (flag == 'F') motorDir = AgvDirection.Forward;
else if (flag == 'B') motorDir = AgvDirection.Backward;
else if (flag == 'T') isTurn = true;
pathList.Add(node);
// Magnet direction lookup
MagnetDirection magDir = MagnetDirection.Straight;
if (i + 1 < pathStrings.Count)
{
var nextTag = pathStrings[i + 1];
string nextRfidStr = "";
foreach (char c in nextTag) if (char.IsDigit(c)) nextRfidStr += c;
var nextNode = _mapNodes.FirstOrDefault(n => n.RfidId.ToString() == nextRfidStr);
if (nextNode != null && node.MagnetDirections.ContainsKey(nextNode.Id))
{
var magPos = node.MagnetDirections[nextNode.Id];
if (magPos == MagnetPosition.R) magDir = MagnetDirection.Right;
else if (magPos == MagnetPosition.L) magDir = MagnetDirection.Left;
}
}
var info = new NodeMotorInfo(seq++, node.Id, node.RfidId, motorDir, null, magDir, isTurn);
detailedList.Add(info);
}
// Connect NextNode pointers
for (int i = 0; i < detailedList.Count - 1; i++)
{
detailedList[i].NextNode = pathList[i + 1];
}
result.Path = pathList;
result.DetailedPath = detailedList;
result.TotalDistance = CalculatePathDistance(pathList);
return result;
}
private float CalculatePathDistance(List<MapNode> path)
{
float dist = 0;
for (int i = 0; i < path.Count - 1; i++)
{
dist += (float)Math.Sqrt(Math.Pow(path[i].Position.X - path[i + 1].Position.X, 2) + Math.Pow(path[i].Position.Y - path[i + 1].Position.Y, 2));
}
return dist;
}
private MapNode GetZoneExitNode(MapZone zone)

4
AGVLogic/AGVSimulator/Forms/SimulatorForm.cs
View File

@@ -1291,7 +1291,7 @@ namespace AGVSimulator.Forms
var flags = new List<string>();
if (info.CanRotate) flags.Add("회전가능");
if (info.IsTurn) flags.Add("회전");
if (info.IsDirectionChangePoint) flags.Add("방향전환");
if (info.RequiresSpecialAction) flags.Add($"특수동작:{info.SpecialActionDescription}");
if (info.MagnetDirection != MagnetDirection.Straight) flags.Add($"마그넷:{info.MagnetDirection}");
@@ -1322,6 +1322,8 @@ namespace AGVSimulator.Forms
else if (motorInfo.IsDirectionChangePoint && motorInfo.CanRotate)
motorSymbol += "[↻]";
if (motorInfo.IsTurn) motorSymbol = "[TURN]";
pathWithDetails.Add($"{rfidId}{motorSymbol}");
}