경로예측로직 수정 중

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);
 

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 isLeftMotorMatch = false;
-                bool isRightTurn = crossProduct < 0;
+                bool isRightMotorMatch = false;
 
-                Console.WriteLine(
+                // ===== 정방향(Forward) 이동 =====
-                    $"    [마그넷 판정] 외적(Cross): {crossProduct:F4}, 좌회전: {isLeftTurn}, 우회전: {isRightTurn}");
+                if (direction == AgvDirection.Forward)
-
-                if ((magnetDirection == MagnetDirection.Left && isLeftTurn) ||
-                    (magnetDirection == MagnetDirection.Right && isRightTurn))
                 {
-                    // 올바른 회전 방향: 강한 보너스
+                    // Forward 이동 시 외적 판정:
-                    const float CORRECT_TURN_BONUS = 1.0f;
+                    // - 외적 < 0 (음수) = 반시계 회전 = Left 모터 멈춤
-                    adjustedScore += CORRECT_TURN_BONUS;
+                    // - 외적 > 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(
-                        $"    [마그넷 판정] ✅ {magnetDirection} 방향 일치 (보너스 +1.0): {baseScore:F4} → {adjustedScore:F4}");
+                    $"    [마그넷 판정] 외적(Cross): {crossProduct:F4}, Left모터일치: {isLeftMotorMatch}, Right모터일치: {isRightMotorMatch} [{direction}]");
+
+                // 외적의 절대값으로 회전 강도 판단 (0에 가까우면 약함, 1에 가까우면 강함)
+                float rotationStrength = Math.Abs(crossProduct);
+
+                if ((magnetDirection == MagnetDirection.Left && isLeftMotorMatch) ||
+                    (magnetDirection == MagnetDirection.Right && isRightMotorMatch))
+                {
+                    // 올바른 모터 방향: 회전 강도에 비례한 보너스
+                    // 강한 회전(|외적| ≈ 1): +2.0
+                    // 약한 회전(|외적| ≈ 0.2): +0.4
+                    float magnetBonus = rotationStrength * 2.0f;
+                    adjustedScore += magnetBonus;
+
+                    Console.WriteLine(
+                        $"    [마그넷 판정] ✅ {magnetDirection} 모터 일치 (회전강도: {rotationStrength:F4}, 보너스 +{magnetBonus:F4}): {baseScore:F4} → {adjustedScore:F4}");
                 }
                 else
                 {
-                    // 잘못된 회전 방향: 강한 페널티
+                    // 잘못된 모터 방향: 회전 강도에 비례한 페널티
-                    const float WRONG_TURN_PENALTY = 0.8f;
+                    // 강한 회전(|외적| ≈ 1): -2.0
-                    adjustedScore -= WRONG_TURN_PENALTY;
+                    // 약한 회전(|외적| ≈ 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}");
                 }
             }
 

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] ⚠️ 경로 방향 불일치: " +
+                                $"[DockingValidator] ⚠️ 경로 방향 불일치" +
-                                $"현재={curNode.RfidId}[{curNodeId}] 이전={prevNode.RfidId}[{(prevNode?.NodeId ?? string.Empty)}] " +
+                                $"\n현재={curNode.RfidId}[{curNodeId}] 이전={prevNode.RfidId}[{(prevNode?.NodeId ?? string.Empty)}] " +
-                                $"예상다음={expectedNextNode.RfidId}[{expectedNextNode.NodeId}] 실제다음={nextNode.RfidId}[{nextNodeId}]";
+                                $"\n예상다음={expectedNextNode.RfidId}[{expectedNextNode.NodeId}] 실제다음={nextNode.RfidId}[{nextNodeId}]";
                             Console.WriteLine(
                                 $"[ValidateDockingDirection] ❌ 경로 방향 불일치 검출!");
                             Console.WriteLine(