From 859a84cd2202f587a5f075d0a79ca7ab1170408c Mon Sep 17 00:00:00 2001 From: backuppc Date: Tue, 28 Oct 2025 11:46:17 +0900 Subject: [PATCH] =?UTF-8?q?=EA=B2=BD=EB=A1=9C=EC=98=88=EC=B8=A1=EB=A1=9C?= =?UTF-8?q?=EC=A7=81=20=EC=88=98=EC=A0=95=20=EC=A4=91?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../PathFinding/Planning/AGVPathfinder.cs | 4 + .../Utils/DirectionalHelper.cs | 79 ++++++++++++++----- .../Utils/DockingValidator.cs | 8 +- 3 files changed, 66 insertions(+), 25 deletions(-) diff --git a/Cs_HMI/AGVLogic/AGVNavigationCore/PathFinding/Planning/AGVPathfinder.cs b/Cs_HMI/AGVLogic/AGVNavigationCore/PathFinding/Planning/AGVPathfinder.cs index 4da985a..b8f6472 100644 --- a/Cs_HMI/AGVLogic/AGVNavigationCore/PathFinding/Planning/AGVPathfinder.cs +++ b/Cs_HMI/AGVLogic/AGVNavigationCore/PathFinding/Planning/AGVPathfinder.cs @@ -122,6 +122,7 @@ namespace AGVNavigationCore.PathFinding.Planning //1.목적지까지의 최단거리 경로를 찾는다. var pathResult = _basicPathfinder.FindPath(startNode.NodeId, targetNode.NodeId); pathResult.PrevNode = prevNode; + pathResult.PrevDirection = prevDirection; if (!pathResult.Success || pathResult.Path == null || pathResult.Path.Count == 0) return AGVPathResult.CreateFailure("각 노드간 최단 경로 계산이 실패되었습니다", 0, 0); @@ -198,6 +199,7 @@ namespace AGVNavigationCore.PathFinding.Planning //1.시작위치 - 교차로(여기까지는 현재 방향으로 그대로 이동을 한다) var path1 = _basicPathfinder.FindPath(startNode.NodeId, JunctionInPath.NodeId); path1.PrevNode = prevNode; + path1.PrevDirection = prevDirection; //다음좌표를 보고 정방향인지 역방향인지 체크한다. bool ReverseCheck = false; @@ -220,6 +222,8 @@ namespace AGVNavigationCore.PathFinding.Planning //2.교차로 - 종료위치 var path2 = _basicPathfinder.FindPath(JunctionInPath.NodeId, targetNode.NodeId); path2.PrevNode = prevNode; + path2.PrevDirection = prevDirection; + if (ReverseCheck) MakeDetailData(path2, currentDirection); else MakeDetailData(path2, ReverseDirection); diff --git a/Cs_HMI/AGVLogic/AGVNavigationCore/Utils/DirectionalHelper.cs b/Cs_HMI/AGVLogic/AGVNavigationCore/Utils/DirectionalHelper.cs index 82f2ede..9ca99f4 100644 --- a/Cs_HMI/AGVLogic/AGVNavigationCore/Utils/DirectionalHelper.cs +++ b/Cs_HMI/AGVLogic/AGVNavigationCore/Utils/DirectionalHelper.cs @@ -91,9 +91,9 @@ namespace AGVNavigationCore.Utils Console.WriteLine( $"\n[GetNextNodeByDirection] ========== 다음 노드 선택 시작 =========="); Console.WriteLine( - $" 현재노드: {currentNode.NodeId}({currentNode.Position.X:F1}, {currentNode.Position.Y:F1})"); + $" 현재노드: {currentNode.RfidId}[{currentNode.NodeId}]({currentNode.Position.X:F1}, {currentNode.Position.Y:F1})"); Console.WriteLine( - $" 이전노드: {prevNode.NodeId}({prevNode.Position.X:F1}, {prevNode.Position.Y:F1})"); + $" 이전노드: {prevNode.RfidId}[{prevNode.NodeId}]({prevNode.Position.X:F1}, {prevNode.Position.Y:F1})"); Console.WriteLine( $" 이동벡터: ({movementVector.X:F2}, {movementVector.Y:F2}) → 정규화: ({normalizedMovement.X:F3}, {normalizedMovement.Y:F3})"); Console.WriteLine( @@ -138,7 +138,7 @@ namespace AGVNavigationCore.Utils } Console.WriteLine( - $"\n [후보] {candidate.NodeId}({candidate.Position.X:F1}, {candidate.Position.Y:F1})"); + $"\n [후보] {candidate.RfidId}[{candidate.NodeId}]({candidate.Position.X:F1}, {candidate.Position.Y:F1})"); Console.WriteLine( $" 벡터: ({toNextVector.X:F2}, {toNextVector.Y:F2}), 길이: {toNextLength:F2}"); Console.WriteLine( @@ -167,7 +167,8 @@ namespace AGVNavigationCore.Utils normalizedMovement, normalizedToNext, currentNode, - candidate + candidate, + direction ); Console.WriteLine( $" 마그넷방향 적용 후: {scoreBeforeMagnet:F4} → {score:F4}"); @@ -182,7 +183,7 @@ namespace AGVNavigationCore.Utils } Console.WriteLine( - $"\n 최종선택: {bestNode?.NodeId ?? "null"} (점수: {bestScore:F4})"); + $"\n 최종선택: {bestNode?.RfidId ?? "null"}[{bestNode?.NodeId ?? "null"}] (점수: {bestScore:F4})"); Console.WriteLine( $"[GetNextNodeByDirection] ========== 다음 노드 선택 종료 ==========\n"); @@ -249,7 +250,8 @@ namespace AGVNavigationCore.Utils PointF normalizedMovement, PointF normalizedToNext, MapNode currentNode, - MapNode candidate) + MapNode candidate, + AgvDirection direction) { float adjustedScore = baseScore; @@ -262,38 +264,73 @@ namespace AGVNavigationCore.Utils Console.WriteLine( $" [마그넷 판정] Straight 보너스 +0.5: {baseScore:F4} → {adjustedScore:F4}"); } - // Left 또는 Right: 회전 방향 판단을 위해 외적 사용 + // Left 또는 Right: 모터 위치에 따른 회전 방향 판단 else if (magnetDirection == MagnetDirection.Left || magnetDirection == MagnetDirection.Right) { // 2D 외적: movement × toNext = movement.X * toNext.Y - movement.Y * toNext.X - // 양수 = 좌회전, 음수 = 우회전 float crossProduct = (normalizedMovement.X * normalizedToNext.Y) - (normalizedMovement.Y * normalizedToNext.X); - bool isLeftTurn = crossProduct > 0; - bool isRightTurn = crossProduct < 0; + bool isLeftMotorMatch = false; + bool isRightMotorMatch = false; + + // ===== 정방향(Forward) 이동 ===== + if (direction == AgvDirection.Forward) + { + // Forward 이동 시 외적 판정: + // - 외적 < 0 (음수) = 반시계 회전 = Left 모터 멈춤 + // - 외적 > 0 (양수) = 시계 회전 = Right 모터 멈춤 + // + // 예: 004 → 012 → 016 (Left 모터) + // 외적 = -0.9407 (음수) → 반시계 → Left 일치 ✅ + + isLeftMotorMatch = crossProduct < 0; // 음수 = 반시계 = Left 멈춤 + isRightMotorMatch = crossProduct > 0; // 양수 = 시계 = Right 멈춤 + } + // ===== 역방향(Backward) 이동 ===== + else // Backward + { + // Backward 이동 시 외적 판정: + // - 외적 < 0 (음수) = 시계 회전 = Left 모터 멈춤 + // - 외적 > 0 (양수) = 반시계 회전 = Right 모터 멈춤 + // + // 예: 012 → 004 → 003 (Left 모터) + // 외적 = 0.9334 (양수) → 반시계(역방향 기준 시계) → Left 일치 ✅ + + isLeftMotorMatch = crossProduct > 0; // 양수 = 시계(역) = Left 멈춤 + isRightMotorMatch = crossProduct < 0; // 음수 = 반시계(역) = Right 멈춤 + } Console.WriteLine( - $" [마그넷 판정] 외적(Cross): {crossProduct:F4}, 좌회전: {isLeftTurn}, 우회전: {isRightTurn}"); + $" [마그넷 판정] 외적(Cross): {crossProduct:F4}, Left모터일치: {isLeftMotorMatch}, Right모터일치: {isRightMotorMatch} [{direction}]"); - if ((magnetDirection == MagnetDirection.Left && isLeftTurn) || - (magnetDirection == MagnetDirection.Right && isRightTurn)) + // 외적의 절대값으로 회전 강도 판단 (0에 가까우면 약함, 1에 가까우면 강함) + float rotationStrength = Math.Abs(crossProduct); + + if ((magnetDirection == MagnetDirection.Left && isLeftMotorMatch) || + (magnetDirection == MagnetDirection.Right && isRightMotorMatch)) { - // 올바른 회전 방향: 강한 보너스 - const float CORRECT_TURN_BONUS = 1.0f; - adjustedScore += CORRECT_TURN_BONUS; + // 올바른 모터 방향: 회전 강도에 비례한 보너스 + // 강한 회전(|외적| ≈ 1): +2.0 + // 약한 회전(|외적| ≈ 0.2): +0.4 + float magnetBonus = rotationStrength * 2.0f; + adjustedScore += magnetBonus; Console.WriteLine( - $" [마그넷 판정] ✅ {magnetDirection} 방향 일치 (보너스 +1.0): {baseScore:F4} → {adjustedScore:F4}"); + $" [마그넷 판정] ✅ {magnetDirection} 모터 일치 (회전강도: {rotationStrength:F4}, 보너스 +{magnetBonus:F4}): {baseScore:F4} → {adjustedScore:F4}"); } else { - // 잘못된 회전 방향: 강한 페널티 - const float WRONG_TURN_PENALTY = 0.8f; - adjustedScore -= WRONG_TURN_PENALTY; + // 잘못된 모터 방향: 회전 강도에 비례한 페널티 + // 강한 회전(|외적| ≈ 1): -2.0 + // 약한 회전(|외적| ≈ 0.2): -0.4 + float magnetPenalty = rotationStrength * 2.0f; + adjustedScore -= magnetPenalty; + + string actualMotor = crossProduct > 0 ? "Left" : "Right"; Console.WriteLine( - $" [마그넷 판정] ❌ {magnetDirection} 방향 불일치 (실제: {(isLeftTurn ? "Left" : "Right")}, 페널티 -0.8): {baseScore:F4} → {adjustedScore:F4}"); + $" [마그넷 판정] ❌ {magnetDirection} 모터 불일치 (실제: {actualMotor}, 회전강도: {rotationStrength:F4}, 페널티 -{magnetPenalty:F4}): {baseScore:F4} → {adjustedScore:F4}"); } } diff --git a/Cs_HMI/AGVLogic/AGVNavigationCore/Utils/DockingValidator.cs b/Cs_HMI/AGVLogic/AGVNavigationCore/Utils/DockingValidator.cs index a52154a..c435922 100644 --- a/Cs_HMI/AGVLogic/AGVNavigationCore/Utils/DockingValidator.cs +++ b/Cs_HMI/AGVLogic/AGVNavigationCore/Utils/DockingValidator.cs @@ -92,14 +92,14 @@ namespace AGVNavigationCore.Utils Console.WriteLine( $" [예상] GetNextNodeByDirection 결과: {expectedNextNode?.NodeId ?? "null"}"); Console.WriteLine( - $" [실제] DetailedPath 다음 노드: {nextNode.NodeId}"); + $" [실제] DetailedPath 다음 노드: {nextNode.RfidId}[{nextNode.NodeId}]"); if (expectedNextNode != null && !expectedNextNode.NodeId.Equals(nextNode.NodeId)) { string error = - $"[DockingValidator] ⚠️ 경로 방향 불일치: " + - $"현재={curNode.RfidId}[{curNodeId}] 이전={prevNode.RfidId}[{(prevNode?.NodeId ?? string.Empty)}] " + - $"예상다음={expectedNextNode.RfidId}[{expectedNextNode.NodeId}] 실제다음={nextNode.RfidId}[{nextNodeId}]"; + $"[DockingValidator] ⚠️ 경로 방향 불일치" + + $"\n현재={curNode.RfidId}[{curNodeId}] 이전={prevNode.RfidId}[{(prevNode?.NodeId ?? string.Empty)}] " + + $"\n예상다음={expectedNextNode.RfidId}[{expectedNextNode.NodeId}] 실제다음={nextNode.RfidId}[{nextNodeId}]"; Console.WriteLine( $"[ValidateDockingDirection] ❌ 경로 방향 불일치 검출!"); Console.WriteLine(