..

Cs_HMI/SubProject/AGVControl/Models/AGV.cs

@@ -8,7 +8,19 @@ namespace AGVControl.Models
     public enum Direction
     {
         Forward = 0,
-        Backward = 1
+        Backward = 1,
+        Stop = 2
+    }
+
+    public struct movehistorydata
+    {
+        public UInt16 rfid { get; set; }
+        public Direction direction { get; set; }
+
+        public override string ToString()
+        {
+            return $"RFID:{rfid},DIR:{direction}";
+        }
     }
 
     public class AGV
@@ -24,17 +36,18 @@ namespace AGVControl.Models
         /// <summary>
         /// 현재위치가 수산되면 목적지까지의 방향값이 계산됩니다.
         /// </summary>
-        public Direction TargetDirection { get; set; }
+        public Direction TargetDirection { get; set; } = Direction.Stop;
         public bool IsMoving { get; set; }
         public List<Point> CurrentPath { get; set; } = new List<Point>();
         public List<Point> PlannedPath { get; set; }
         public List<string> PathRFIDs { get; set; }
         public Point TargetPosition { get; set; }
         public uint TargetRFID { get; set; }
-
+        public float? BodyAngle { get; set; } = null;
+        public float MotorAngle { get; set; } = 0f;
         // 이동 경로 기록을 위한 새로운 속성들
-        public List<uint> MovementHistory { get; set; } = new List<uint>();
-        public List<Point> PositionHistory { get; set; } = new List<Point>();
+        public List<movehistorydata> MovementHistory { get; } = new List<movehistorydata>();
+        public List<Point> PositionHistory { get; } = new List<Point>();
         public const int HISTORY_SIZE = 4; // 최근 4개 위치 기록
 
         public AGV()
@@ -46,6 +59,7 @@ namespace AGVControl.Models
             TargetPosition = Point.Empty;
             TargetRFID = 0;
             TargetDirection = Direction.Forward;
+            BodyAngle = null;
         }
 
         public void Move()
@@ -58,13 +72,13 @@ namespace AGVControl.Models
         }
 
         // 이동 경로에 새로운 RFID 추가
-        public void AddToMovementHistory(uint rfidValue, Point position)
+        public void AddToMovementHistory(UInt16 rfidValue, Point position, Direction direction)
         {
             // 중복 RFID가 연속으로 들어오는 경우 무시
-            if (MovementHistory.Count > 0 && MovementHistory[MovementHistory.Count - 1] == rfidValue)
+            if (MovementHistory.Count > 0 && MovementHistory.Last().rfid == rfidValue)
                 return;
 
-            MovementHistory.Add(rfidValue);
+            MovementHistory.Add(new movehistorydata { rfid = rfidValue, direction = direction }) ;
             PositionHistory.Add(position);
 
             // 기록 크기 제한
@@ -73,6 +87,14 @@ namespace AGVControl.Models
                 MovementHistory.RemoveAt(0);
                 PositionHistory.RemoveAt(0);
             }
+
+            //최초방향과 마지막 방향이 일치하지 않으면 그 이전의 데이터는 삭제한다.
+            if(MovementHistory.Count > 2 && MovementHistory.First().direction != MovementHistory.Last().direction)
+            {
+                var lastTwo = MovementHistory.Skip(MovementHistory.Count - 2).Take(2).ToArray(); // [9, 10]
+                MovementHistory.Clear();
+                MovementHistory.AddRange(lastTwo);
+            }
         }
 
         // 연결 정보 기반 실제 이동 방향 계산
@@ -82,10 +104,10 @@ namespace AGVControl.Models
                 return null;
 
             // 이전 RFID에서 현재 RFID로의 연결 확인
-            var connection = connections.FirstOrDefault(c => 
+            var connection = connections.FirstOrDefault(c =>
                 (c.StartRFID == previousRFID && c.EndRFID == currentRFID) ||
                 (c.IsBidirectional && c.StartRFID == currentRFID && c.EndRFID == previousRFID));
-            
+
             if (connection == null)
                 return null; // 연결되지 않은 경로
 
@@ -107,23 +129,30 @@ namespace AGVControl.Models
                 return true; // 검증 불가능한 경우
 
             // 최근 두 RFID 값 가져오기
-            var recentRFIDs = MovementHistory.Skip(Math.Max(0, MovementHistory.Count - 2)).Take(2).ToList();
+            var recentRFIDs = MovementHistory.Skip( MovementHistory.Count - 2).Take(2).ToList();
             if (recentRFIDs.Count < 2)
                 return true;
 
             var previousRFID = recentRFIDs[0];
             var currentRFID = recentRFIDs[1];
 
-            var actualDirection = CalculateActualDirectionByConnection(currentRFID, previousRFID, connections);
+            var actualDirection = CalculateActualDirectionByConnection(currentRFID.rfid, previousRFID.rfid, connections);
             if (!actualDirection.HasValue)
                 return true; // 연결 정보로 방향 판단 불가
 
             // 방향이 일치하지 않는 경우
             if (actualDirection.Value != expectedDirection)
             {
-                // AGV 방향을 실제 이동 방향으로 정정
+                // AGV 모터 방향을 실제 이동 방향으로 정정
                 CurrentDirection = actualDirection.Value;
                 TargetDirection = actualDirection.Value;
+
+                // 몸체 방향도 180도 회전 (결정된 경우에만)
+                if (BodyAngle.HasValue)
+                {
+                    BodyAngle = (BodyAngle.Value + 180) % 360;
+                }
+
                 return false; // 정정됨을 알림
             }
 
@@ -145,11 +174,11 @@ namespace AGVControl.Models
             var currentRFID = recentRFIDs[1];
 
             // RFID 값의 증가/감소로 방향 판단
-            if (currentRFID > prevRFID)
+            if (currentRFID.rfid > prevRFID.rfid)
             {
                 return Direction.Forward; // RFID 값이 증가하면 전진
             }
-            else if (currentRFID < prevRFID)
+            else if (currentRFID.rfid < prevRFID.rfid)
             {
                 return Direction.Backward; // RFID 값이 감소하면 후진
             }