From 213467fe3f95aac4f3f9223372a679ec27003188 Mon Sep 17 00:00:00 2001 From: backuppc Date: Fri, 13 Feb 2026 10:41:10 +0900 Subject: [PATCH] .. --- .../AGVNavigationCore.csproj | 3 +- .../Planning => Models}/NodeMotorInfo.cs | 4 +- .../AGVNavigationCore/Models/VirtualAGV.cs | 6 +- .../PathFinding/Core/AStarPathfinder.cs | 93 +--- .../PathFinding/Core/Utility.cs | 106 ++++ .../PathFinding/Planning/AGVPathfinder.cs | 471 ++++++++++++++++++ .../Forms/SimulatorForm.Designer.cs | 23 +- AGVLogic/AGVSimulator/Forms/SimulatorForm.cs | 42 ++ ...lock.통신 프로토콜_AGV_V350_LF_25.01.10_r2.xlsx# | 1 + 9 files changed, 645 insertions(+), 104 deletions(-) rename AGVLogic/AGVNavigationCore/{PathFinding/Planning => Models}/NodeMotorInfo.cs (97%) create mode 100644 AGVLogic/AGVNavigationCore/PathFinding/Core/Utility.cs create mode 100644 Document/통신프로토콜/.~lock.통신 프로토콜_AGV_V350_LF_25.01.10_r2.xlsx# diff --git a/AGVLogic/AGVNavigationCore/AGVNavigationCore.csproj b/AGVLogic/AGVNavigationCore/AGVNavigationCore.csproj index c8a7e54..12de61b 100644 --- a/AGVLogic/AGVNavigationCore/AGVNavigationCore.csproj +++ b/AGVLogic/AGVNavigationCore/AGVNavigationCore.csproj @@ -84,6 +84,7 @@ + @@ -93,7 +94,7 @@ - + UserControl diff --git a/AGVLogic/AGVNavigationCore/PathFinding/Planning/NodeMotorInfo.cs b/AGVLogic/AGVNavigationCore/Models/NodeMotorInfo.cs similarity index 97% rename from AGVLogic/AGVNavigationCore/PathFinding/Planning/NodeMotorInfo.cs rename to AGVLogic/AGVNavigationCore/Models/NodeMotorInfo.cs index 9758cf5..6162bf6 100644 --- a/AGVLogic/AGVNavigationCore/PathFinding/Planning/NodeMotorInfo.cs +++ b/AGVLogic/AGVNavigationCore/Models/NodeMotorInfo.cs @@ -1,6 +1,4 @@ -using AGVNavigationCore.Models; - -namespace AGVNavigationCore.PathFinding.Planning +namespace AGVNavigationCore.Models { /// /// AGV 마그넷 센서 방향 제어 diff --git a/AGVLogic/AGVNavigationCore/Models/VirtualAGV.cs b/AGVLogic/AGVNavigationCore/Models/VirtualAGV.cs index c641820..27fdc2a 100644 --- a/AGVLogic/AGVNavigationCore/Models/VirtualAGV.cs +++ b/AGVLogic/AGVNavigationCore/Models/VirtualAGV.cs @@ -738,13 +738,13 @@ namespace AGVNavigationCore.Models MagnetPosition magnetPos; switch (nodeInfo.MagnetDirection) { - case PathFinding.Planning.MagnetDirection.Left: + case MagnetDirection.Left: magnetPos = MagnetPosition.L; break; - case PathFinding.Planning.MagnetDirection.Right: + case MagnetDirection.Right: magnetPos = MagnetPosition.R; break; - case PathFinding.Planning.MagnetDirection.Straight: + case MagnetDirection.Straight: default: magnetPos = MagnetPosition.S; break; diff --git a/AGVLogic/AGVNavigationCore/PathFinding/Core/AStarPathfinder.cs b/AGVLogic/AGVNavigationCore/PathFinding/Core/AStarPathfinder.cs index 4d67267..10b7712 100644 --- a/AGVLogic/AGVNavigationCore/PathFinding/Core/AStarPathfinder.cs +++ b/AGVLogic/AGVNavigationCore/PathFinding/Core/AStarPathfinder.cs @@ -333,98 +333,7 @@ namespace AGVNavigationCore.PathFinding.Core // } //} - /// - /// 두 경로 결과를 합치기 - /// 이전 경로의 마지막 노드와 현재 경로의 시작 노드가 같으면 시작 노드를 제거하고 합침 - /// - /// 이전 경로 결과 - /// 현재 경로 결과 - /// 합쳐진 경로 결과 - public AGVPathResult CombineResults( AGVPathResult previousResult, AGVPathResult currentResult) - { - // 입력 검증 - if (previousResult == null) - return currentResult; - - if (currentResult == null) - return previousResult; - - if (!previousResult.Success) - return AGVPathResult.CreateFailure( - $"이전 경로 결과 실패: {previousResult.Message}", - previousResult.CalculationTimeMs); - - if (!currentResult.Success) - return AGVPathResult.CreateFailure( - $"현재 경로 결과 실패: {currentResult.Message}", - currentResult.CalculationTimeMs); - - // 경로가 비어있는 경우 처리 - if (previousResult.Path == null || previousResult.Path.Count == 0) - return currentResult; - - if (currentResult.Path == null || currentResult.Path.Count == 0) - return previousResult; - - // 합친 경로 생성 - var combinedPath = new List(previousResult.Path); - var combinedCommands = new List(previousResult.Commands); - var combinedDetailedPath = new List(previousResult.DetailedPath ?? new List()); - - // 이전 경로의 마지막 노드와 현재 경로의 시작 노드 비교 - string lastNodeOfPrevious = previousResult.Path[previousResult.Path.Count - 1].Id; - string firstNodeOfCurrent = currentResult.Path[0].Id; - - if (lastNodeOfPrevious == firstNodeOfCurrent) - { - // 첫 번째 노드 제거 (중복 제거) - combinedPath.RemoveAt(combinedPath.Count - 1); - combinedPath.AddRange(currentResult.Path); - - // DetailedPath도 첫 번째 노드 제거 - if (currentResult.DetailedPath != null && currentResult.DetailedPath.Count > 0) - { - combinedDetailedPath.RemoveAt(combinedDetailedPath.Count - 1); - combinedDetailedPath.AddRange(currentResult.DetailedPath); - } - } - else - { - // 그대로 붙임 - combinedPath.AddRange(currentResult.Path); - - // DetailedPath도 그대로 붙임 - if (currentResult.DetailedPath != null && currentResult.DetailedPath.Count > 0) - { - combinedDetailedPath.AddRange(currentResult.DetailedPath); - } - } - - // 명령어 합치기 - combinedCommands.AddRange(currentResult.Commands); - - // 총 거리 합산 - float combinedDistance = previousResult.TotalDistance + currentResult.TotalDistance; - - // 계산 시간 합산 - long combinedCalculationTime = previousResult.CalculationTimeMs + currentResult.CalculationTimeMs; - - // 합쳐진 결과 생성 - var result = AGVPathResult.CreateSuccess( - combinedPath, - combinedCommands, - combinedDistance, - combinedCalculationTime - ); - - // DetailedPath 설정 - result.DetailedPath = combinedDetailedPath; - result.PrevNode = previousResult.PrevNode; - result.PrevDirection = previousResult.PrevDirection; - - return result; - } - + ///// ///// 여러 목적지 중 가장 가까운 노드로의 경로 찾기 diff --git a/AGVLogic/AGVNavigationCore/PathFinding/Core/Utility.cs b/AGVLogic/AGVNavigationCore/PathFinding/Core/Utility.cs new file mode 100644 index 0000000..e56e004 --- /dev/null +++ b/AGVLogic/AGVNavigationCore/PathFinding/Core/Utility.cs @@ -0,0 +1,106 @@ +using AGVNavigationCore.Models; +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; +using System.Threading.Tasks; + +namespace AGVNavigationCore.PathFinding.Core +{ + public static class Utility + { + /// + /// 두 경로 결과를 합치기 + /// 이전 경로의 마지막 노드와 현재 경로의 시작 노드가 같으면 시작 노드를 제거하고 합침 + /// + /// 이전 경로 결과 + /// 현재 경로 결과 + /// 합쳐진 경로 결과 + public static AGVPathResult CombineResults(AGVPathResult previousResult, AGVPathResult currentResult) + { + // 입력 검증 + if (previousResult == null) + return currentResult; + + if (currentResult == null) + return previousResult; + + if (!previousResult.Success) + return AGVPathResult.CreateFailure( + $"이전 경로 결과 실패: {previousResult.Message}", + previousResult.CalculationTimeMs); + + if (!currentResult.Success) + return AGVPathResult.CreateFailure( + $"현재 경로 결과 실패: {currentResult.Message}", + currentResult.CalculationTimeMs); + + // 경로가 비어있는 경우 처리 + if (previousResult.Path == null || previousResult.Path.Count == 0) + return currentResult; + + if (currentResult.Path == null || currentResult.Path.Count == 0) + return previousResult; + + // 합친 경로 생성 + var combinedPath = new List(previousResult.Path); + var combinedCommands = new List(previousResult.Commands); + var combinedDetailedPath = new List(previousResult.DetailedPath ?? new List()); + + // 이전 경로의 마지막 노드와 현재 경로의 시작 노드 비교 + string lastNodeOfPrevious = previousResult.Path[previousResult.Path.Count - 1].Id; + string firstNodeOfCurrent = currentResult.Path[0].Id; + + if (lastNodeOfPrevious == firstNodeOfCurrent) + { + // 첫 번째 노드 제거 (중복 제거) + combinedPath.RemoveAt(combinedPath.Count - 1); + combinedPath.AddRange(currentResult.Path); + + // DetailedPath도 첫 번째 노드 제거 + if (currentResult.DetailedPath != null && currentResult.DetailedPath.Count > 0) + { + combinedDetailedPath.RemoveAt(combinedDetailedPath.Count - 1); + combinedDetailedPath.AddRange(currentResult.DetailedPath); + } + } + else + { + // 그대로 붙임 + combinedPath.AddRange(currentResult.Path); + + // DetailedPath도 그대로 붙임 + if (currentResult.DetailedPath != null && currentResult.DetailedPath.Count > 0) + { + combinedDetailedPath.AddRange(currentResult.DetailedPath); + } + } + + // 명령어 합치기 + combinedCommands.AddRange(currentResult.Commands); + + // 총 거리 합산 + float combinedDistance = previousResult.TotalDistance + currentResult.TotalDistance; + + // 계산 시간 합산 + long combinedCalculationTime = previousResult.CalculationTimeMs + currentResult.CalculationTimeMs; + + // 합쳐진 결과 생성 + var result = AGVPathResult.CreateSuccess( + combinedPath, + combinedCommands, + combinedDistance, + combinedCalculationTime + ); + + // DetailedPath 설정 + result.DetailedPath = combinedDetailedPath; + result.PrevNode = previousResult.PrevNode; + result.PrevDirection = previousResult.PrevDirection; + + return result; + } + + + } +} diff --git a/AGVLogic/AGVNavigationCore/PathFinding/Planning/AGVPathfinder.cs b/AGVLogic/AGVNavigationCore/PathFinding/Planning/AGVPathfinder.cs index 641a469..7124f28 100644 --- a/AGVLogic/AGVNavigationCore/PathFinding/Planning/AGVPathfinder.cs +++ b/AGVLogic/AGVNavigationCore/PathFinding/Planning/AGVPathfinder.cs @@ -194,6 +194,477 @@ namespace AGVNavigationCore.PathFinding.Planning + /// + /// 새로운 경로 계산 로직 (방향성 A* + 제약조건) + /// 1. 180도 회전은 RFID 3번에서만 가능 + /// 2. 120도 미만 예각 회전 불가 (단, RFID 3번에서 스위치백은 가능) + /// 3. 목적지 도킹 방향 준수 + /// + public AGVPathResult CalculatePath_new(MapNode startNode, MapNode targetNode, MapNode prevNode, AgvDirection prevDir) + { + if (startNode == null || targetNode == null) + return AGVPathResult.CreateFailure("시작/종료노드가 지정되지 않음"); + + // 초기 상태 설정 + var openSet = new List(); + var closedSet = new HashSet(); // Key: "CurrentID_PrevID" + + // 시작 상태 생성 + var startState = new SearchState + { + CurrentNode = startNode, + PreviousNode = prevNode, // 진입 방향 계산용 + CurrentDirection = prevDir, // 현재 모터 방향 + GCost = 0, + HCost = CalculateHeuristic(startNode, targetNode), + Parent = null + }; + + openSet.Add(startState); + + while (openSet.Count > 0) + { + // F Cost가 가장 낮은 상태 선택 + var currentState = GetLowestFCostState(openSet); + openSet.Remove(currentState); + + // 방문 기록 (상태 기반: 현재노드 + 진입노드) + string stateKey = GetStateKey(currentState); + if (closedSet.Contains(stateKey)) continue; + closedSet.Add(stateKey); + + // 목적지 도달 검사 + if (currentState.CurrentNode.Id == targetNode.Id) + { + // 도킹 방향 제약 조건 확인 + if (IsDockingDirectionValid(currentState, targetNode)) + { + return ReconstructPath_New(currentState); + } + // 도킹 방향이 안 맞으면? -> 이 경로로는 불가. 다른 경로 탐색 계속. + // (단, 제자리 회전이 가능한 경우라면 여기서 추가 처리를 할 수도 있음) + // 현재 로직상 도착 후 제자리 회전은 없으므로 Pass. + } + + // 이웃 노드 탐색 + foreach (var nextNodeId in currentState.CurrentNode.ConnectedNodes) + { + var nextNode = _mapNodes.FirstOrDefault(n => n.Id == nextNodeId); + if (nextNode == null || !nextNode.IsActive) continue; + + // 이동 가능 여부 및 비용 계산 (회전 제약 포함) + var moveTry = CheckMove(currentState, nextNode); + + if (moveTry.IsPossible) + { + var newState = new SearchState + { + CurrentNode = nextNode, + PreviousNode = currentState.CurrentNode, + CurrentDirection = moveTry.NextDirection, + GCost = currentState.GCost + moveTry.Cost, + HCost = CalculateHeuristic(nextNode, targetNode), + Parent = currentState, + TurnType = moveTry.TurnType // 디버깅용 + }; + + // 이미 방문한 더 나은 경로가 있는지 확인 + // (여기서는 ClosedSet만 체크하고 OpenSet 내 중복 처리는 생략 - 간단 구현) + // A* 최적화를 위해 OpenSet 내 동일 상태(Key)가 있고 G Cost가 더 낮다면 Skip해야 함. + + string newStateKey = GetStateKey(newState); + if (closedSet.Contains(newStateKey)) continue; + + var existingOpen = openSet.FirstOrDefault(s => GetStateKey(s) == newStateKey); + if (existingOpen != null) + { + if (newState.GCost < existingOpen.GCost) + { + openSet.Remove(existingOpen); + openSet.Add(newState); + } + } + else + { + openSet.Add(newState); + } + } + } + } + + return AGVPathResult.CreateFailure("조건을 만족하는 경로를 찾을 수 없습니다."); + } + + #region Helper Classes & Methods for CalculatePath_new + + private class SearchState + { + 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 + } + + private struct MoveResult + { + public bool IsPossible; + public float Cost; + public AgvDirection NextDirection; + public string TurnType; + } + + private string GetStateKey(SearchState state) + { + string prevId = state.PreviousNode?.Id ?? "null"; + // 모터 방향도 상태에 포함해야 함 (같은 노드, 같은 진입이라도 모터방향 다르면 다른 상태 - 스위치백 때문) + return $"{state.CurrentNode.Id}_{prevId}_{state.CurrentDirection}"; + } + + private SearchState GetLowestFCostState(List openSet) + { + SearchState lowest = openSet[0]; + for (int i = 1; i < openSet.Count; i++) + { + if (openSet[i].FCost < lowest.FCost) + lowest = openSet[i]; + } + return lowest; + } + + private float CalculateHeuristic(MapNode from, MapNode to) + { + // 유클리드 거리 + float dx = from.Position.X - to.Position.X; + float dy = from.Position.Y - to.Position.Y; + return (float)Math.Sqrt(dx * dx + dy * dy); + } + + private MoveResult CheckMove(SearchState current, MapNode next) + { + var res = new MoveResult { IsPossible = false, Cost = float.MaxValue }; + + // 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) + { + // Vector 1 (Prev -> Curr) + double v1x = current.CurrentNode.Position.X - current.PreviousNode.Position.X; + double v1y = current.CurrentNode.Position.Y - current.PreviousNode.Position.Y; + + // Vector 2 (Curr -> Next) + double v2x = next.Position.X - current.CurrentNode.Position.X; + double v2y = next.Position.Y - current.CurrentNode.Position.Y; + + // 각도 계산 (0 ~ 180) + // 내적 이용: a·b = |a||b|cosθ + double dot = v1x * v2x + v1y * v2y; + double mag1 = Math.Sqrt(v1x * v1x + v1y * v1y); + double mag2 = Math.Sqrt(v2x * v2x + v2y * v2y); + + if (mag1 > 0 && mag2 > 0) + { + double cosTheta = dot / (mag1 * mag2); + // 부동소수점 오차 보정 + 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턴(완전 뒤로) + // 주의: 벡터 방향 기준임. + // 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턴) + } + } + else + { + angle = 0; // 시작점에서는 직진으로 간주 (또는 자유) + } + + // 제약 조건 체크 + // 조건 2: 120도 미만 예각 회전 불가? + // 사용자 멘트: "11->3->4 ... 120도 이상 완만해야 이동 가능... 3->4는 각도가 훨씬 작아서 커브 못틈" + // 여기서 "각도"가 내각(Inner Angle)을 말하는지, 진행 경로의 꺾임각을 말하는지 중요. + // 11->3->4 그림을 보면 "V"자 형태에 가까움. (Sharp Turn) + // 즉, "진행 방향의 꺾임각"이 크다(예: 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도 미만) + + // Case A: Normal Move (완만한 턴) + if (!isSharpTurn) + { + res.IsPossible = true; + res.Cost = dist; + res.NextDirection = current.CurrentDirection; // 방향 유지 + res.TurnType = "Normal"; + return res; + } + + // Case B: Sharp Turn or U-Turn + if (isSharpTurn) + { + if (isRfid3) + { + // 3번 노드에서는 무슨 짓이든(?) 가능하다. (Switch Back) + // 멈춰서 방향을 바꾸므로, 모터 방향도 바뀜 (FWD -> BWD or BWD -> FWD) + // 비용 패널티 추가 (멈추고 바꾸는 시간 등) + res.IsPossible = true; + res.Cost = dist + 5000; // 큰 비용 (Switch Penalty) + res.NextDirection = (current.CurrentDirection == AgvDirection.Forward) + ? AgvDirection.Backward + : AgvDirection.Forward; + res.TurnType = "SwitchBack"; + return res; + } + else + { + // 3번 노드가 아닌데 Sharp Turn 하려고 함 -> 불가 + res.IsPossible = false; + return res; + } + } + + return res; + } + + private bool IsDockingDirectionValid(SearchState state, MapNode target) + { + if (target.DockDirection == DockingDirection.DontCare) return true; + + // 도착 시 모터 방향 확인 + // SearchState에 저장된 CurrentDirection이 도착 시 모터 방향임. + + if (target.DockDirection == DockingDirection.Forward) + return state.CurrentDirection == AgvDirection.Forward; + + if (target.DockDirection == DockingDirection.Backward) + return state.CurrentDirection == AgvDirection.Backward; + + return true; + } + + private AGVPathResult ReconstructPath_New(SearchState endState) + { + var result = new AGVPathResult { Success = true, TotalDistance = endState.GCost }; + var pathList = new List(); + var detailedList = new List(); + + var current = endState; + var pathStack = new Stack(); + + while (current != null) + { + pathStack.Push(current); + current = current.Parent; + } + + // Path 재구성 + int seq = 1; + while (pathStack.Count > 0) + { + var step = pathStack.Pop(); + pathList.Add(step.CurrentNode); + + // Detailed Info (마그넷 방향 계산 등은 후처리 필요할 수도 있으나 여기선 단순화) + // SearchState에는 "어떤 방향으로 왔는지"가 저장되어 있음. + + // 마그넷 방향 계산 (다음 노드가 있을 때) + MagnetDirection magDir = MagnetDirection.Straight; + if (pathStack.Count > 0) + { + var nextStep = pathStack.Peek(); + // Current -> NextStep 이동 시 마그넷 방향 + 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; + + // 만약 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); + } + + result.Path = pathList; + result.DetailedPath = detailedList; + + return result; + } + + #endregion + + public enum MapZone + { + None, + Buffer, // 91 ~ 07 + Charger, // 73 ~ 10 + Plating, // 72 ~ 05 + Loader, // 71 ~ 04 + Cleaner, // 70 ~ 01 + Junction // Hub (11, 12, etc) + } + + 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; + + // 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; + } + + 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) + { + return CalculatePath_new(startNode, targetNode, prevNode, prevDir); + } + + // 2. Hub Logic (Buffer -> Hub -> Target, etc.) + // Logic: Start -> ExitNode -> Hub -> EntryNode -> Target + + MapNode exitNode = GetZoneExitNode(startZone); + MapNode entryNode = GetZoneEntryNode(targetZone); + + // 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) + { + // Fallback to normal search if zone logic fails + return CalculatePath_new(startNode, targetNode, prevNode, prevDir); + } + + // 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}"); + + // 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; + + var path2 = CalculatePath_new(exitNode, entryNode, prevNode1, lastDir1); + if (!path2.Success) return AGVPathResult.CreateFailure($"Hub Crossing Failure: {exitNode.ID2}->{entryNode.ID2}"); + + // 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; + + var path3 = CalculatePath_new(entryNode, targetNode, prevNode2, lastDir2); + if (!path3.Success) return AGVPathResult.CreateFailure($"Zone Entry Failure: {entryNode.ID2}->{targetNode.ID2}"); + + // Merge Paths + var merged = Utility.CombineResults(path1, path2); + merged = Utility.CombineResults(merged, path3); + + return merged; + } + + private MapNode GetZoneExitNode(MapZone zone) + { + int exitRfid = -1; + switch (zone) + { + case MapZone.Buffer: exitRfid = 7; break; + case MapZone.Charger: exitRfid = 10; break; // Or 6? Assuming 10 based on flow + case MapZone.Plating: exitRfid = 5; break; + case MapZone.Loader: exitRfid = 4; break; + case MapZone.Cleaner: exitRfid = 1; break; + case MapZone.Junction: return null; + } + return _mapNodes.FirstOrDefault(n => n.RfidId == exitRfid); + } + + private MapNode GetZoneEntryNode(MapZone zone) + { + int entryRfid = -1; + switch (zone) + { + case MapZone.Buffer: entryRfid = 7; break; // Bi-directional entry/exit? + // Usually Buffer entry might be different (e.g. 91?). + // But user didn't specify directional flow constraints for zones other than turn logic. + // Let's assume Entry = Exit for single-lane spurs, or define specific entry points. + // If Buffer is 91~07, maybe Entry is 7? + case MapZone.Charger: entryRfid = 10; break; + case MapZone.Plating: entryRfid = 5; break; + case MapZone.Loader: entryRfid = 4; break; + case MapZone.Cleaner: entryRfid = 1; break; + } + return _mapNodes.FirstOrDefault(n => n.RfidId == entryRfid); + } + /// /// 길목(Gateway) 기반 고급 경로 계산 (기존 SimulatorForm.CalcPath 이관) /// diff --git a/AGVLogic/AGVSimulator/Forms/SimulatorForm.Designer.cs b/AGVLogic/AGVSimulator/Forms/SimulatorForm.Designer.cs index f3e8cc4..15027a8 100644 --- a/AGVLogic/AGVSimulator/Forms/SimulatorForm.Designer.cs +++ b/AGVLogic/AGVSimulator/Forms/SimulatorForm.Designer.cs @@ -86,6 +86,7 @@ namespace AGVSimulator.Forms this._statusLabel = new System.Windows.Forms.ToolStripStatusLabel(); this._coordLabel = new System.Windows.Forms.ToolStripStatusLabel(); this.prb1 = new System.Windows.Forms.ToolStripProgressBar(); + this.sbFile = new System.Windows.Forms.ToolStripStatusLabel(); this._controlPanel = new System.Windows.Forms.Panel(); this.groupBox1 = new System.Windows.Forms.GroupBox(); this.propertyNode = new System.Windows.Forms.PropertyGrid(); @@ -121,7 +122,7 @@ namespace AGVSimulator.Forms this._liftDirectionLabel = new System.Windows.Forms.Label(); this._motorDirectionLabel = new System.Windows.Forms.Label(); this.timer1 = new System.Windows.Forms.Timer(this.components); - this.sbFile = new System.Windows.Forms.ToolStripStatusLabel(); + this.button1 = new System.Windows.Forms.Button(); this._menuStrip.SuspendLayout(); this._toolStrip.SuspendLayout(); this._statusStrip.SuspendLayout(); @@ -466,6 +467,12 @@ namespace AGVSimulator.Forms this.prb1.Name = "prb1"; this.prb1.Size = new System.Drawing.Size(200, 16); // + // sbFile + // + this.sbFile.Name = "sbFile"; + this.sbFile.Size = new System.Drawing.Size(17, 17); + this.sbFile.Text = "--"; + // // _controlPanel // this._controlPanel.BackColor = System.Drawing.SystemColors.Control; @@ -540,6 +547,7 @@ namespace AGVSimulator.Forms // // _pathGroup // + this._pathGroup.Controls.Add(this.button1); this._pathGroup.Controls.Add(this.btPath2); this._pathGroup.Controls.Add(this._clearPathButton); this._pathGroup.Controls.Add(this._targetCalcButton); @@ -825,11 +833,15 @@ namespace AGVSimulator.Forms this.timer1.Interval = 500; this.timer1.Tick += new System.EventHandler(this.timer1_Tick); // - // sbFile + // button1 // - this.sbFile.Name = "sbFile"; - this.sbFile.Size = new System.Drawing.Size(17, 17); - this.sbFile.Text = "--"; + this.button1.Location = new System.Drawing.Point(21, 201); + this.button1.Name = "button1"; + this.button1.Size = new System.Drawing.Size(106, 25); + this.button1.TabIndex = 11; + this.button1.Text = "경로 계산2"; + this.button1.UseVisualStyleBackColor = true; + this.button1.Click += new System.EventHandler(this.button1_Click); // // SimulatorForm // @@ -946,5 +958,6 @@ namespace AGVSimulator.Forms private System.Windows.Forms.Button btPath2; private System.Windows.Forms.ToolStripMenuItem btSelectMapEditor; private System.Windows.Forms.ToolStripStatusLabel sbFile; + private System.Windows.Forms.Button button1; } } \ No newline at end of file diff --git a/AGVLogic/AGVSimulator/Forms/SimulatorForm.cs b/AGVLogic/AGVSimulator/Forms/SimulatorForm.cs index 20aa68f..f78ad3f 100644 --- a/AGVLogic/AGVSimulator/Forms/SimulatorForm.cs +++ b/AGVLogic/AGVSimulator/Forms/SimulatorForm.cs @@ -2588,7 +2588,21 @@ namespace AGVSimulator.Forms this._simulatorCanvas.HighlightNodeId = (result.Gateway?.Id ?? string.Empty); return result; } + /// + /// 길목(Gateway) 기반 경로 계산 + /// 버퍼-버퍼 상태에서는 별도의 추가 로직을 적용합니다 + /// + public AGVPathResult CalcPath_New(MapNode startNode, MapNode targetNode, List nodes, + MapNode prevNode, AgvDirection prevDir) + { + // Core Logic으로 이관됨 + var pathFinder = new AGVPathfinder(nodes); + var result = pathFinder.CalculateScriptedPath(startNode, targetNode, prevNode, prevDir); + //게이트웨이노드를 하이라이트강조 한단 + this._simulatorCanvas.HighlightNodeId = (result.Gateway?.Id ?? string.Empty); + return result; + } private void ApplyResultToSimulator(AGVPathResult result, VirtualAGV agv) { _simulatorCanvas.CurrentPath = result; @@ -2622,5 +2636,33 @@ namespace AGVSimulator.Forms } } } + + private void button1_Click(object sender, EventArgs e) + { + // 1. 기본 정보 획득 + if (_startNodeCombo.SelectedItem == null || _startNodeCombo.Text == "선택하세요") SetStartNodeFromAGVPosition(); + if (_startNodeCombo.SelectedItem == null || _targetNodeCombo.SelectedItem == null) + { + MessageBox.Show("시작/목표 노드를 확인하세요"); + return; + } + + //var selectedAGV = _agvListCombo.SelectedItem as VirtualAGV; + //if (selectedAGV == null) return AGVPathResult.CreateFailure("Virtual AGV 없음"); + var selectedAGV = _agvListCombo.SelectedItem as VirtualAGV; + + // 경로계산2 (Gateway Logic) + var startNode = (_startNodeCombo.SelectedItem as ComboBoxItem)?.Value; + var targetNode = (_targetNodeCombo.SelectedItem as ComboBoxItem)?.Value; + var rlt = CalcPath_New(startNode, targetNode, this._simulatorCanvas.Nodes, selectedAGV.PrevNode, selectedAGV.PrevDirection); + if (rlt.Success == false) MessageBox.Show(rlt.Message, "알림", MessageBoxButtons.OK, MessageBoxIcon.Information); + else + { + // 8. 적용 + + ApplyResultToSimulator(rlt, selectedAGV); + UpdateAdvancedPathDebugInfo(rlt); + } + } } } \ No newline at end of file diff --git a/Document/통신프로토콜/.~lock.통신 프로토콜_AGV_V350_LF_25.01.10_r2.xlsx# b/Document/통신프로토콜/.~lock.통신 프로토콜_AGV_V350_LF_25.01.10_r2.xlsx# new file mode 100644 index 0000000..525b64b --- /dev/null +++ b/Document/통신프로토콜/.~lock.통신 프로토콜_AGV_V350_LF_25.01.10_r2.xlsx# @@ -0,0 +1 @@ +,BACKUPPC/1,backuppc,13.02.2026 10:03,file:///C:/Users/1/AppData/Roaming/LibreOffice/4; \ No newline at end of file