diff --git a/AGVLogic/AGVNavigationCore/Models/NodeMotorInfo.cs b/AGVLogic/AGVNavigationCore/Models/NodeMotorInfo.cs
index 6162bf6..2e462d6 100644
--- a/AGVLogic/AGVNavigationCore/Models/NodeMotorInfo.cs
+++ b/AGVLogic/AGVNavigationCore/Models/NodeMotorInfo.cs
@@ -80,13 +80,16 @@ namespace AGVNavigationCore.Models
///
public bool IsPass { get; set; }
+ public bool IsTurn { get; set; }
///
/// 특수 동작 설명
///
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;
diff --git a/AGVLogic/AGVNavigationCore/PathFinding/Planning/AGVPathfinder.cs b/AGVLogic/AGVNavigationCore/PathFinding/Planning/AGVPathfinder.cs
index 7124f28..7278f84 100644
--- a/AGVLogic/AGVNavigationCore/PathFinding/Planning/AGVPathfinder.cs
+++ b/AGVLogic/AGVNavigationCore/PathFinding/Planning/AGVPathfinder.cs
@@ -208,7 +208,7 @@ namespace AGVNavigationCore.PathFinding.Planning
// 초기 상태 설정
var openSet = new List();
var closedSet = new HashSet(); // Key: "CurrentID_PrevID"
-
+
// 시작 상태 생성
var startState = new SearchState
{
@@ -271,7 +271,7 @@ namespace AGVNavigationCore.PathFinding.Planning
// 이미 방문한 더 나은 경로가 있는지 확인
// (여기서는 ClosedSet만 체크하고 OpenSet 내 중복 처리는 생략 - 간단 구현)
// A* 최적화를 위해 OpenSet 내 동일 상태(Key)가 있고 G Cost가 더 낮다면 Skip해야 함.
-
+
string newStateKey = GetStateKey(newState);
if (closedSet.Contains(newStateKey)) continue;
@@ -302,11 +302,11 @@ namespace AGVNavigationCore.PathFinding.Planning
public MapNode CurrentNode { get; set; }
public MapNode PreviousNode { get; set; }
public AgvDirection CurrentDirection { get; set; } // 현재 모터 방향 (Forward/Backward)
-
+
public float GCost { get; set; }
public float HCost { get; set; }
public float FCost => GCost + HCost;
-
+
public SearchState Parent { get; set; }
public string TurnType { get; set; } // Debug info
}
@@ -351,11 +351,11 @@ namespace AGVNavigationCore.PathFinding.Planning
// 1. 기본 거리 비용
float dist = (float)Math.Sqrt(Math.Pow(current.CurrentNode.Position.X - next.Position.X, 2) + Math.Pow(current.CurrentNode.Position.Y - next.Position.Y, 2));
-
+
// 2. 회전 각도 계산
// Vector Prev -> Curr
// Vector Curr -> Next
-
+
double angle = 180.0; // Straight
if (current.PreviousNode != null)
{
@@ -379,7 +379,7 @@ namespace AGVNavigationCore.PathFinding.Planning
// 부동소수점 오차 보정
if (cosTheta > 1.0) cosTheta = 1.0;
if (cosTheta < -1.0) cosTheta = -1.0;
-
+
double rad = Math.Acos(cosTheta);
// 외적을 이용해 좌/우 판별이 가능하지만, 여기서는 "꺾인 정도"만 중요하므로 내적각(0~180)만 사용
// 180도 = 직진, 90도 = 직각, 0도 = U턴(완전 뒤로)
@@ -387,14 +387,14 @@ namespace AGVNavigationCore.PathFinding.Planning
// Prev->Curr 가 (1,0) 이고 Curr->Next가 (1,0) 이면 각도 0도? 아니면 180?
// 위 내적 계산에서 같은 방향이면 cos=1 -> acos=0. 즉 0도가 직진임.
// 180도가 U턴(역방향).
-
+
// 변환: 사용자가 "120도 이상 완만해야" 라고 했음.
// 그림상 11->3->4는 예각(Sharp turn).
// 직선 주행시 각도 변화량(Deviation)으로 생각하면:
// 직진 = 0도 변화.
// 90도 턴 = 90도 변화.
// U턴 = 180도 변화.
-
+
angle = rad * (180.0 / Math.PI); // 0(직진) ~ 180(U턴)
}
}
@@ -411,15 +411,15 @@ namespace AGVNavigationCore.PathFinding.Planning
// 즉, "진행 방향의 꺾임각"이 크다(예: 150도 꺾임) = "내각"이 작다(30도).
// "120도 이상 완만해야" -> 내각이 120도 이상(점잖은 턴)이어야 한다.
// 즉, 꺾임각(Deviation)은 60도 이하여야 한다.
-
+
// 내 알고리즘의 `angle`은 "꺾임각(Deviation)"임. (0=직진, 180=U턴)
// 따라서 "내각 120도 이상" == "꺾임각 60도 이하".
// 그러므로 angle <= 60 이어야 Normal Turn 가능.
-
+
// 3번 노드 (RFID 3) 특수성:
// "3번 노드에서만 180도 턴 가능"
// "3->4 처럼 각이 좁은 경우 3번으로 가서 모터방향 바꿔서 4번으로 감"
-
+
bool isRfid3 = current.CurrentNode.RfidId == 3;
bool isSharpTurn = angle > 60.0; // 60도보다 많이 꺾이면 Sharp Turn (내각 120도 미만)
@@ -443,8 +443,8 @@ namespace AGVNavigationCore.PathFinding.Planning
// 비용 패널티 추가 (멈추고 바꾸는 시간 등)
res.IsPossible = true;
res.Cost = dist + 5000; // 큰 비용 (Switch Penalty)
- res.NextDirection = (current.CurrentDirection == AgvDirection.Forward)
- ? AgvDirection.Backward
+ res.NextDirection = (current.CurrentDirection == AgvDirection.Forward)
+ ? AgvDirection.Backward
: AgvDirection.Forward;
res.TurnType = "SwitchBack";
return res;
@@ -466,7 +466,7 @@ namespace AGVNavigationCore.PathFinding.Planning
// 도착 시 모터 방향 확인
// SearchState에 저장된 CurrentDirection이 도착 시 모터 방향임.
-
+
if (target.DockDirection == DockingDirection.Forward)
return state.CurrentDirection == AgvDirection.Forward;
@@ -484,7 +484,7 @@ namespace AGVNavigationCore.PathFinding.Planning
var current = endState;
var pathStack = new Stack();
-
+
while (current != null)
{
pathStack.Push(current);
@@ -497,10 +497,10 @@ namespace AGVNavigationCore.PathFinding.Planning
{
var step = pathStack.Pop();
pathList.Add(step.CurrentNode);
-
+
// Detailed Info (마그넷 방향 계산 등은 후처리 필요할 수도 있으나 여기선 단순화)
// SearchState에는 "어떤 방향으로 왔는지"가 저장되어 있음.
-
+
// 마그넷 방향 계산 (다음 노드가 있을 때)
MagnetDirection magDir = MagnetDirection.Straight;
if (pathStack.Count > 0)
@@ -510,33 +510,37 @@ namespace AGVNavigationCore.PathFinding.Planning
if (step.CurrentNode.MagnetDirections.ContainsKey(nextStep.CurrentNode.Id))
{
var magPos = step.CurrentNode.MagnetDirections[nextStep.CurrentNode.Id];
- if (magPos == MagnetPosition.L) magDir = MagnetDirection.Left;
- else if (magPos == MagnetPosition.R) magDir = MagnetDirection.Right;
+ if (magPos == MagnetPosition.L) magDir = MagnetDirection.Left;
+ else if (magPos == MagnetPosition.R) magDir = MagnetDirection.Right;
- // 만약 SwitchBack 상황이라면?
- // 모터 방향이 바뀌었다면 마그넷 방향도 그에 맞춰야 하나?
- // 기존 로직 참고: MagnetDirection은 "진행 방향 기준" 좌/우 인가? 아님 절대적?
- // 보통 "갈림길에서 어느 쪽" 인지 나타냄.
+ // 만약 SwitchBack 상황이라면?
+ // 모터 방향이 바뀌었다면 마그넷 방향도 그에 맞춰야 하나?
+ // 기존 로직 참고: MagnetDirection은 "진행 방향 기준" 좌/우 인가? 아님 절대적?
+ // 보통 "갈림길에서 어느 쪽" 인지 나타냄.
}
}
-
+
detailedList.Add(new NodeMotorInfo(seq++, step.CurrentNode.Id, step.CurrentNode.RfidId, step.CurrentDirection, null, magDir)); // NextNode는 일단 null
}
-
+
// NextNode 정보 채우기
for (int i = 0; i < detailedList.Count - 1; i++)
{
- detailedList[i].NextNode = _mapNodes.FirstOrDefault(n => n.Id == detailedList[i+1].NodeId);
+ detailedList[i].NextNode = _mapNodes.FirstOrDefault(n => n.Id == detailedList[i + 1].NodeId);
}
result.Path = pathList;
result.DetailedPath = detailedList;
-
+
return result;
}
-
- #endregion
+ #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 Path { get; set; }
}
+ public List GetMapZonePathData()
+ {
+ var retval = new List();
+
+ // Buffer -> ...
+ retval.Add(new MapZonePathData { NodeSta = StationType.Buffer, NodeEnd = StationType.Charger, Monitor = MapZoneMonitor.RightBtm, Path = new List { "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 { "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 { "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 { "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 { "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 { "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 { "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 { "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 { "71B", "4B", "3B", "2B", "1B", "70B" } });
+ retval.Add(new MapZonePathData { NodeSta = StationType.Loader, NodeEnd = StationType.Cleaner, Monitor = MapZoneMonitor.LeftTop, Path = new List { "71F", "4F", "3T", "3B", "2B", "1B", "70B" } });
+ retval.Add(new MapZonePathData { NodeSta = StationType.Loader, NodeEnd = StationType.Charger, Monitor = MapZoneMonitor.RightBtm, Path = new List { "71B", "4B", "3T", "3B", "11B", "5B", "6B", "73B" } });
+ retval.Add(new MapZonePathData { NodeSta = StationType.Loader, NodeEnd = StationType.Charger, Monitor = MapZoneMonitor.LeftTop, Path = new List { "71F", "4F", "3B", "11B", "5B", "6B", "73B" } });
+ retval.Add(new MapZonePathData { NodeSta = StationType.Loader, NodeEnd = StationType.Plating, Monitor = MapZoneMonitor.RightBtm, Path = new List { "71B", "4B", "3T", "3B", "11B", "5B", "72B" } });
+ retval.Add(new MapZonePathData { NodeSta = StationType.Loader, NodeEnd = StationType.Plating, Monitor = MapZoneMonitor.LeftTop, Path = new List { "71F", "4F", "3B", "11B", "5B", "72B" } });
+ retval.Add(new MapZonePathData { NodeSta = StationType.Loader, NodeEnd = StationType.Buffer, Monitor = MapZoneMonitor.RightBtm, Path = new List { "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 { "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 { "70F", "1F", "2F", "3T", "3B", "4B", "71B" } });
+ retval.Add(new MapZonePathData { NodeSta = StationType.Cleaner, NodeEnd = StationType.Loader, Monitor = MapZoneMonitor.LeftTop, Path = new List { "70B", "1B", "2B", "3B", "4B", "71B" } });
+ retval.Add(new MapZonePathData { NodeSta = StationType.Cleaner, NodeEnd = StationType.Charger, Monitor = MapZoneMonitor.RightBtm, Path = new List { "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 { "70B", "1B", "2B", "3B", "11B", "5B", "6B", "73B" } });
+ retval.Add(new MapZonePathData { NodeSta = StationType.Cleaner, NodeEnd = StationType.Plating, Monitor = MapZoneMonitor.RightBtm, Path = new List { "70F", "1F", "2F", "3T", "3B", "11B", "5B", "72B" } });
+ retval.Add(new MapZonePathData { NodeSta = StationType.Cleaner, NodeEnd = StationType.Plating, Monitor = MapZoneMonitor.LeftTop, Path = new List { "70B", "1B", "2B", "3B", "11B", "5B", "72B" } });
+ retval.Add(new MapZonePathData { NodeSta = StationType.Cleaner, NodeEnd = StationType.Buffer, Monitor = MapZoneMonitor.RightBtm, Path = new List { "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 { "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 { "72F", "5F", "11F", "3B", "4B", "71B" } });
+ retval.Add(new MapZonePathData { NodeSta = StationType.Plating, NodeEnd = StationType.Loader, Monitor = MapZoneMonitor.LeftTop, Path = new List { "72B", "5B", "11B", "3T", "3B", "4B", "71B" } });
+ retval.Add(new MapZonePathData { NodeSta = StationType.Plating, NodeEnd = StationType.Charger, Monitor = MapZoneMonitor.RightBtm, Path = new List { "72F", "5F", "11B", "5B", "6B", "73B" } });
+ retval.Add(new MapZonePathData { NodeSta = StationType.Plating, NodeEnd = StationType.Charger, Monitor = MapZoneMonitor.LeftTop, Path = new List { "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 { "72F", "5F", "11F", "3T", "3B", "2B", "1B", "70B" } });
+ retval.Add(new MapZonePathData { NodeSta = StationType.Plating, NodeEnd = StationType.Cleaner, Monitor = MapZoneMonitor.LeftTop, Path = new List { "72B", "5B", "11B", "3B", "2B", "1B", "70B" } });
+ retval.Add(new MapZonePathData { NodeSta = StationType.Plating, NodeEnd = StationType.Buffer, Monitor = MapZoneMonitor.RightBtm, Path = new List { "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 { "72B", "5B", "11B", "3T", "3B", "11B", "12B", "7B", "8B", "9B", "34B", "33B", "32B", "31B", "35B", "36B" } });
+
+ return retval;
+ }
+ ///
+ /// 해당 노드가 속하는 존을 반환한다.
+ ///
+ ///
+ ///
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;
-
- // Charger: 73~10
- if (rfid == 73 || rfid == 6 || rfid == 10) return MapZone.Charger;
+ Dictionary ZoneList = GetMapZoneNodeList();
+ var zone = ZoneList.Where(t => t.Value.Contains(rfid)).FirstOrDefault();
+ if (zone.Value == null) return MapZone.None;
- // 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 GetMapZoneNodeList()
+ {
+ Dictionary ZoneList = new Dictionary();
+ 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';
+
+ // 시작 태그 검색용 (예: "91F")
+ string startTag = $"{startNode.RfidId}{motDir}";
- MapNode exitNode = GetZoneExitNode(startZone);
- MapNode entryNode = GetZoneEntryNode(targetZone);
+ // 모니터방향이 일치하고 대상노드가 동일한 경로를 찾는다
+ var zonepath = GetMapZonePathData();
+
+ // 모든 후보 경로 검색
+ 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 Start/Target are in Junction or Unknown, handle gracefully
- if (startZone == MapZone.Junction) exitNode = startNode;
- if (targetZone == MapZone.Junction) entryNode = targetNode;
-
- if (exitNode == null || entryNode == null)
+ 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;
+ }
+ }
+ }
+
+ if (bestPath != null)
+ {
+ // 추출된 경로 조각
+ var slicedPath = bestPath.Path.Skip(bestStartIndex).Take(bestEndIndex - bestStartIndex + 1).ToList();
+ var a = ConvertHardcodedPathToResult(slicedPath, startNode, prevNode, prevDir);
+ return a;
+ }
}
- // 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}");
+ // 하드코딩된 경로가 없으면 기존 A* 로직으로 대체 또는 실패 반환
+ return null;
+ }
- // 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;
+ private MapZoneMonitor GetMonitorMode(MapNode startNode, MapNode prevNode, AgvDirection prevDir)
+ {
+ if (prevNode == null) return MapZoneMonitor.RightBtm;
- var path2 = CalculatePath_new(exitNode, entryNode, prevNode1, lastDir1);
- if (!path2.Success) return AGVPathResult.CreateFailure($"Hub Crossing Failure: {exitNode.ID2}->{entryNode.ID2}");
+ int dx = startNode.Position.X - prevNode.Position.X;
+ int dy = startNode.Position.Y - prevNode.Position.Y;
- // 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;
+ bool isMonitorLeft = false;
+ if (Math.Abs(dx) > Math.Abs(dy)) // Horizontal
+ {
+ isMonitorLeft = (prevDir == AgvDirection.Backward);
+ }
+ else // Vertical
+ {
+ isMonitorLeft = (prevDir == AgvDirection.Forward);
+ }
- var path3 = CalculatePath_new(entryNode, targetNode, prevNode2, lastDir2);
- if (!path3.Success) return AGVPathResult.CreateFailure($"Zone Entry Failure: {entryNode.ID2}->{targetNode.ID2}");
+ return isMonitorLeft ? MapZoneMonitor.LeftTop : MapZoneMonitor.RightBtm;
+ }
- // Merge Paths
- var merged = Utility.CombineResults(path1, path2);
- merged = Utility.CombineResults(merged, path3);
+ private AGVPathResult ConvertHardcodedPathToResult(List pathStrings, MapNode startNode, MapNode prevNode, AgvDirection prevDir)
+ {
+ var result = new AGVPathResult { Success = true };
+ var pathList = new List();
+ var detailedList = new List();
- return merged;
+ 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 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)
@@ -649,7 +823,7 @@ namespace AGVNavigationCore.PathFinding.Planning
private MapNode GetZoneEntryNode(MapZone zone)
{
- int entryRfid = -1;
+ int entryRfid = -1;
switch (zone)
{
case MapZone.Buffer: entryRfid = 7; break; // Bi-directional entry/exit?
@@ -736,7 +910,7 @@ namespace AGVNavigationCore.PathFinding.Planning
if (!fixpath)
{
-
+
if (targetNode.StationType == StationType.Limit || targetNode.StationType == StationType.Normal)
{
@@ -790,7 +964,7 @@ namespace AGVNavigationCore.PathFinding.Planning
if (pathToGateway.Path.Count > 1)
{
//다음이동방향이 이전노드와 동일하다면? 되돌아가야한다는것이다
- predictNext = pathToGateway.Path[1];
+ predictNext = pathToGateway.Path[1];
if (predictNext.Id == prevNode.Id)
{
var reverseDir = prevDir == AgvDirection.Backward ? AgvDirection.Forward : AgvDirection.Backward;
diff --git a/AGVLogic/AGVSimulator/Forms/SimulatorForm.cs b/AGVLogic/AGVSimulator/Forms/SimulatorForm.cs
index f78ad3f..c385149 100644
--- a/AGVLogic/AGVSimulator/Forms/SimulatorForm.cs
+++ b/AGVLogic/AGVSimulator/Forms/SimulatorForm.cs
@@ -1291,7 +1291,7 @@ namespace AGVSimulator.Forms
var flags = new List();
- 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}");
}
@@ -1596,7 +1598,7 @@ namespace AGVSimulator.Forms
MotorDirection = directionName,
CurrentPosition = GetNodeDisplayName(currentNode),
TargetPosition = GetNodeDisplayName(targetNode),
- DockingPosition = (targetNode.StationType == StationType.Charger ) ? "충전기" : "장비"
+ DockingPosition = (targetNode.StationType == StationType.Charger) ? "충전기" : "장비"
};
if (calcResult.Success)