From ba18564719b9727fc9999e0ccaf407bc85ead278 Mon Sep 17 00:00:00 2001 From: chi Date: Mon, 23 Jun 2025 17:38:24 +0900 Subject: [PATCH] .. --- Cs_HMI/Project/StateMachine/_AGV.cs | 5 +- Cs_HMI/Project/StateMachine/_BMS.cs | 2 + Cs_HMI/SubProject/AGVControl/MapControl.cs | 492 ++++++------------ Cs_HMI/SubProject/AGVControl/Models/AGV.cs | 177 +++---- .../SubProject/AGVControl/Models/RFIDPoint.cs | 21 +- 5 files changed, 253 insertions(+), 444 deletions(-) diff --git a/Cs_HMI/Project/StateMachine/_AGV.cs b/Cs_HMI/Project/StateMachine/_AGV.cs index 1b72243..e8770db 100644 --- a/Cs_HMI/Project/StateMachine/_AGV.cs +++ b/Cs_HMI/Project/StateMachine/_AGV.cs @@ -57,10 +57,11 @@ namespace Project VAR.BOOL[eVarBool.AGV_ERROR] = PUB.AGV.error.Value > 0; VAR.BOOL[eVarBool.EMERGENCY] = PUB.AGV.error.Emergency; + //모터방향 입력 if (PUB.AGV.data.Direction == 'B') - PUB.mapctl.agv.CurrentDirection = AGVControl.Models.Direction.Backward; + PUB.mapctl.agv.CurrentMOTDirection = AGVControl.Models.Direction.Backward; else - PUB.mapctl.agv.CurrentDirection = AGVControl.Models.Direction.Forward; + PUB.mapctl.agv.CurrentMOTDirection = AGVControl.Models.Direction.Forward; if (PUB.AGV.signal.mark_sensor == false) { diff --git a/Cs_HMI/Project/StateMachine/_BMS.cs b/Cs_HMI/Project/StateMachine/_BMS.cs index ed17150..0df32fd 100644 --- a/Cs_HMI/Project/StateMachine/_BMS.cs +++ b/Cs_HMI/Project/StateMachine/_BMS.cs @@ -159,6 +159,8 @@ namespace Project private void Bms_BMSDataReceive(object sender, EventArgs e) { PUB.mapctl.agv.BatteryLevel = PUB.BMS.Current_Level; + PUB.mapctl.agv.BatteryTemp1 = PUB.BMS.Current_temp1; + PUB.mapctl.agv.BatteryTemp2 = PUB.BMS.Current_temp2; if (PUB.BMS.Current_Level <= PUB.setting.ChargeStartLevel) { //배터리 레벨이 기준보다 낮다면 경고를 활성화 한다 diff --git a/Cs_HMI/SubProject/AGVControl/MapControl.cs b/Cs_HMI/SubProject/AGVControl/MapControl.cs index 57acb42..b3d5807 100644 --- a/Cs_HMI/SubProject/AGVControl/MapControl.cs +++ b/Cs_HMI/SubProject/AGVControl/MapControl.cs @@ -36,12 +36,14 @@ namespace AGVControl return StartRFID.GetHashCode() ^ EndRFID.GetHashCode(); } } - public struct PathResult + public class PathResult { - public bool Success { get; set; } + public bool Success { get; set; } = false; public string Message { get; set; } - public List Path { get; set; } + public List Path { get; set; } + + } public enum AGVMoveState @@ -85,7 +87,7 @@ namespace AGVControl #region 멤버 변수 // 맵 데이터 - private List rfidPoints; + private List RFIDPoints; private List mapTexts; private List customLines; private List rfidLines; @@ -140,7 +142,7 @@ namespace AGVControl public MapControl() { this.DoubleBuffered = true; - rfidPoints = new List(); + RFIDPoints = new List(); mapTexts = new List(); customLines = new List(); rfidLines = new List(); @@ -291,7 +293,7 @@ namespace AGVControl } // RFID 포인트 선택 - var clickedRFID = rfidPoints.FirstOrDefault(r => GetDistance(mapPoint, r.Location) <= SNAP_DISTANCE); + var clickedRFID = RFIDPoints.FirstOrDefault(r => GetDistance(mapPoint, r.Location) <= SNAP_DISTANCE); switch (mousemode) { case eMouseMode.rfidcut: @@ -368,7 +370,7 @@ namespace AGVControl } else { - var startRFID = rfidPoints.FirstOrDefault(r => r.Location == previewStartPoint); + var startRFID = RFIDPoints.FirstOrDefault(r => r.Location == previewStartPoint); if (startRFID != null) { var line = new RFIDLine @@ -400,7 +402,7 @@ namespace AGVControl var mapPoint = ScreenToMap(e.Location); //RFID 포인트 찾기 - var selected_rfid = rfidPoints.Where(t => t.Bounds.Expand(SELECTION_DISTANCE, SELECTION_DISTANCE).Contains(mapPoint)).FirstOrDefault(); + var selected_rfid = RFIDPoints.Where(t => t.Bounds.Expand(SELECTION_DISTANCE, SELECTION_DISTANCE).Contains(mapPoint)).FirstOrDefault(); if (selected_rfid != null) { UTIL.ShowPropertyDialog(selected_rfid); @@ -505,7 +507,7 @@ namespace AGVControl // RFID 포인트 선택 - var clickedRFID = rfidPoints.FirstOrDefault(r => + var clickedRFID = RFIDPoints.FirstOrDefault(r => GetDistance(mapPoint, r.Location) <= SNAP_DISTANCE); if (clickedRFID != null) { @@ -705,7 +707,7 @@ namespace AGVControl private Point SnapToPoint(Point point) { // RFID 포인트와 근접한지 확인 - foreach (var rfid in rfidPoints) + foreach (var rfid in RFIDPoints) { if (GetDistance(point, rfid.Location) <= SNAP_DISTANCE) { @@ -719,87 +721,31 @@ namespace AGVControl public RFIDPoint FindRFIDPoint(uint rfidValue) { - if (rfidPoints == null || rfidPoints.Any() == false) return null; - return rfidPoints.FirstOrDefault(r => r.RFIDValue == rfidValue); + if (RFIDPoints == null || RFIDPoints.Any() == false) return null; + return RFIDPoints.FirstOrDefault(r => r.Value == rfidValue); } - public bool SetCurrentPosition(UInt16 rfidValue) + /// + /// 현재위치를 설정합니다 + /// + /// RFID TagNo + /// + public bool SetCurrentPosition(UInt16 rfidTagNo) { - var rfidPoint = FindRFIDPoint(rfidValue); + var rfidPoint = FindRFIDPoint(rfidTagNo); if (rfidPoint != null) { // 이동 경로에 추가 (위치 업데이트보다 먼저) - agv.AddToMovementHistory(rfidValue, rfidPoint.Location, this.agv.CurrentDirection); + agv.AddToMovementHistory(rfidTagNo, rfidPoint.Location, this.agv.CurrentMOTDirection); // AGV 위치 업데이트 - agv.CurrentPosition = rfidPoint.Location; - agv.CurrentRFID = rfidValue; - - // --- 동체 방향(BodyAngle) 결정 로직 --- - if (!agv.BodyAngle.HasValue && agv.MovementHistory.Count >= 2) - { - // 두 번째 RFID가 인식된 시점 - var history = agv.PositionHistory.Skip(Math.Max(0, agv.PositionHistory.Count - 2)).Take(2).ToList(); - Point firstPos = history[0]; - Point secondPos = history[1]; - - // 두 점 사이의 각도 계산 - float deltaX = secondPos.X - firstPos.X; - float deltaY = secondPos.Y - firstPos.Y; - float baseAngle = (float)Math.Atan2(deltaY, deltaX) * 180f / (float)Math.PI; - - // 모터 방향(CurrentDirection)에 따라 최종 BodyAngle 결정 - if (agv.CurrentDirection == Direction.Backward) - { - // 후진 중이었다면, 몸체는 반대 방향을 보고 있었음 - agv.BodyAngle = (baseAngle + 180) % 360; - } - else - { - // 전진 또는 미정의 상태였다면, 몸체는 이동 방향을 보고 있었음 - agv.BodyAngle = baseAngle; - } - } - - // AGV의 모터 방향 결정 - if (agv.MovementHistory.Count > 1) - { - // 다음 목적지 찾기 - var lastP1 = agv.MovementHistory.Last(); - var lastP2 = agv.MovementHistory.Skip(agv.MovementHistory.Count - 2).Take(1).First(); - - // 모터 각도 계산 및 업데이트 - var prePoint = rfidPoints.Where(t => t.RFIDValue == lastP2.rfid).FirstOrDefault(); - if (prePoint != null) - { - float deltaX; - float deltaY; - if (agv.CurrentDirection == Direction.Forward) - { - deltaX = agv.CurrentPosition.X - prePoint.Bounds.X; - deltaY = agv.CurrentPosition.Y - prePoint.Bounds.Y; - } - else - { - deltaX = prePoint.Bounds.X - agv.CurrentPosition.X; - deltaY = prePoint.Bounds.Y - agv.CurrentPosition.Y; - } - - agv.MotorAngle = (float)Math.Atan2(deltaY, deltaX) * 180f / (float)Math.PI; - - // 회전 가능 여부를 고려하여 방향 결정 - //agv.TargetDirection = DetermineDirection(agv.CurrentPosition, nextPoint, agv.TargetPosition);// - } - - } - - + agv.CurrentRFID = rfidPoint; // 목적지가 설정되어 있고 경로가 있는 경우 검증 - if (agv.TargetPosition != Point.Empty && agv.CurrentPath.Count > 0) + if (agv.TargetRFID.IsEmpty == false && agv.CurrentPath.Count > 0) { // 현재 위치가 경로에 있는지 확인 - bool isOnPath = agv.CurrentPath.Contains(agv.CurrentPosition); + bool isOnPath = agv.CurrentPath.Contains(agv.CurrentRFID); if (!isOnPath) { @@ -810,66 +756,18 @@ namespace AGVControl SetCurrentPath(pathResult.Path); } } - - } // 목적지 RFID에 도착했고, 해당 RFID에 고정방향이 있으면 TargetDirection을 강제 설정 - if (agv.TargetRFID == rfidValue) + if (agv.TargetRFID.Value == rfidTagNo) { - var destRFID = FindRFIDPoint(rfidValue); + var destRFID = FindRFIDPoint(rfidTagNo); if (destRFID != null && destRFID.FixedDirection.HasValue) { agv.TargetDirection = destRFID.FixedDirection.Value; } } - // 방향 검증 및 정정 (세 번째 이동부터, BodyAngle이 결정된 후) - if (agv.BodyAngle.HasValue && agv.MovementHistory.Count > 2) - { - // RFID 연결 정보 기반 예상 방향 계산 - Direction? expectedDirection = null; - int currentIdx = agv.CurrentPath.FindIndex(p => p == agv.CurrentPosition); - if (currentIdx >= 0 && currentIdx < agv.CurrentPath.Count - 1) - { - // 현재 위치와 다음 위치의 RFID 찾기 - var currentRFIDPoint = FindRFIDPoint(agv.CurrentRFID); - var nextPoint = agv.CurrentPath[currentIdx + 1]; - var nextRFIDPoint = rfidPoints.FirstOrDefault(p => p.Location == nextPoint); - - if (currentRFIDPoint != null && nextRFIDPoint != null) - { - // rfidConnections에서 연결 정보 확인 - var connection = rfidConnections.FirstOrDefault(c => - (c.StartRFID == currentRFIDPoint.RFIDValue && c.EndRFID == nextRFIDPoint.RFIDValue) || - (c.IsBidirectional && c.StartRFID == nextRFIDPoint.RFIDValue && c.EndRFID == currentRFIDPoint.RFIDValue)); - - if (connection != null) - { - // 연결된 경로이므로 방향 결정 - if (connection.StartRFID == currentRFIDPoint.RFIDValue && connection.EndRFID == nextRFIDPoint.RFIDValue) - { - expectedDirection = Direction.Forward; // Start -> End 방향 - } - else - { - expectedDirection = Direction.Backward; // End -> Start 방향 - } - } - } - } - - if (expectedDirection.HasValue) - { - bool directionValid = agv.ValidateAndCorrectDirectionByConnection(expectedDirection.Value, rfidConnections.ToList()); - if (!directionValid) - { - // 방향이 정정되었음을 로그로 기록 - Console.WriteLine($"AGV 방향 정정 (연결 정보 기반): 예상 {expectedDirection.Value} -> 실제 {agv.CurrentDirection}"); - } - } - } - this.Invalidate(); return true; } @@ -881,8 +779,7 @@ namespace AGVControl var rfidPoint = FindRFIDPoint(rfidValue); if (rfidPoint != null) { - agv.TargetPosition = rfidPoint.Location; - agv.TargetRFID = rfidValue; + agv.TargetRFID = rfidPoint; this.Invalidate(); return true; } @@ -890,19 +787,19 @@ namespace AGVControl } - private List CalculatePath(Point start, Point end) + private PathResult CalculatePath(RFIDPoint start, RFIDPoint end) { - var openList = new List { start }; - var closedList = new List(); - var cameFrom = new Dictionary(); - var gScore = new Dictionary { { start, 0 } }; - var fScore = new Dictionary { { start, Heuristic(start, end) } }; + var openList = new List { start }; + var closedList = new List(); + var cameFrom = new Dictionary(); + var gScore = new Dictionary { { start, 0 } }; + var fScore = new Dictionary { { start, Heuristic(start.Location, end.Location) } }; while (openList.Count > 0) { var current = openList.OrderBy(p => fScore.ContainsKey(p) ? fScore[p] : float.MaxValue).First(); - if (current == end) + if (current.Location == end.Location) { return ReconstructPath(cameFrom, current); } @@ -915,30 +812,38 @@ namespace AGVControl if (closedList.Contains(neighbor)) continue; - float tentativeGScore = gScore[current] + Distance(current, neighbor); + float tentativeGScore = gScore[current] + Distance(current.Location, neighbor.Location); if (!openList.Contains(neighbor)) openList.Add(neighbor); else if (tentativeGScore >= gScore[neighbor]) continue; - cameFrom[neighbor] = current; + cameFrom[neighbor.Value] = current; gScore[neighbor] = tentativeGScore; - fScore[neighbor] = gScore[neighbor] + Heuristic(neighbor, end); + fScore[neighbor] = gScore[neighbor] + Heuristic(neighbor.Location, end.Location); } } - return null; - } - private List ReconstructPath(Dictionary cameFrom, Point current) - { - var path = new List { current }; - while (cameFrom.ContainsKey(current)) + return new PathResult { - current = cameFrom[current]; + Success = true, + Path = openList, + }; + } + private PathResult ReconstructPath(Dictionary cameFrom, RFIDPoint current) + { + var path = new List { current }; + while (cameFrom.ContainsKey(current.Value)) + { + current = cameFrom[current.Value]; path.Insert(0, current); } - return path; + return new PathResult + { + Success = true, + Path = path, + }; } private float Heuristic(Point a, Point b) { @@ -948,14 +853,14 @@ namespace AGVControl private float Distance(Point a, Point b) { - var rfidA = rfidPoints.FirstOrDefault(p => p.Location == a); - var rfidB = rfidPoints.FirstOrDefault(p => p.Location == b); + var rfidA = RFIDPoints.FirstOrDefault(p => p.Location == a); + var rfidB = RFIDPoints.FirstOrDefault(p => p.Location == b); if (rfidA == null || rfidB == null) return float.MaxValue; var connection = rfidConnections.FirstOrDefault(c => - (c.StartRFID == rfidA.RFIDValue && c.EndRFID == rfidB.RFIDValue) || - (c.IsBidirectional && c.StartRFID == rfidB.RFIDValue && c.EndRFID == rfidA.RFIDValue)); + (c.StartRFID == rfidA.Value && c.EndRFID == rfidB.Value) || + (c.IsBidirectional && c.StartRFID == rfidB.Value && c.EndRFID == rfidA.Value)); if (connection != null) { @@ -965,13 +870,13 @@ namespace AGVControl return float.MaxValue; } - private List GetNeighbors(Point point) + private List GetNeighbors(RFIDPoint point) { - var neighbors = new List(); - var currentRfidPoint = rfidPoints.FirstOrDefault(p => p.Location == point); + var neighbors = new List(); + var currentRfidPoint = RFIDPoints.FirstOrDefault(p => p.Location == point.Location); if (currentRfidPoint == null) return neighbors; - uint currentRfid = currentRfidPoint.RFIDValue; + uint currentRfid = currentRfidPoint.Value; foreach (var connection in rfidConnections) { @@ -987,36 +892,16 @@ namespace AGVControl if (neighborRfidVal != 0) { - var neighborRfidPoint = rfidPoints.FirstOrDefault(p => p.RFIDValue == neighborRfidVal); + var neighborRfidPoint = RFIDPoints.FirstOrDefault(p => p.Value == neighborRfidVal); if (neighborRfidPoint != null) { - neighbors.Add(neighborRfidPoint.Location); + neighbors.Add(neighborRfidPoint); } } } return neighbors.Distinct().ToList(); } - public void SetPath(List rfids) - { - if (rfids == null || rfids.Count == 0) - return; - - var path = new List(); - foreach (var rfid in rfids) - { - var point = GetRFIDPoints().FirstOrDefault(p => p.RFIDValue == rfid); - if (point != null) - { - path.Add(point.Location); - } - } - - if (path.Count > 0) - { - SetCurrentPath(path); - } - } public PathResult CalculatePath(uint tagStrt, uint tagEnd) { @@ -1024,7 +909,7 @@ namespace AGVControl { Message = string.Empty, Success = false, - Path = new List(), + Path = new List(), }; var sp = tagStrt; //만약시작위치가 없다면 항상 충전기 기준으로 한다 @@ -1039,7 +924,7 @@ namespace AGVControl return retval; } - retval.Path = CalculatePath(startPoint.Location, endPoint.Location); + retval = CalculatePath(startPoint, endPoint); if (retval.Path != null && retval.Path.Any()) { //SetCurrentPath(retval.Path); @@ -1071,7 +956,7 @@ namespace AGVControl // 선 근처의 RFID 포인트들을 찾아서 거리에 따라 정렬 var nearbyPoints = new List<(RFIDPoint Point, float Distance, float ProjectionRatio)>(); - foreach (var rfid in rfidPoints) + foreach (var rfid in RFIDPoints) { if (rfid.Location == line.StartPoint || rfid.Location == line.EndPoint) continue; @@ -1127,7 +1012,7 @@ namespace AGVControl public void SetRFIDPoints(List points) { - rfidPoints = points; + RFIDPoints = points; this.Invalidate(); } @@ -1262,7 +1147,7 @@ namespace AGVControl } } - public void SetCurrentPath(List path) + public void SetCurrentPath(List path) { agv.CurrentPath = path; this.Invalidate(); @@ -1338,7 +1223,7 @@ namespace AGVControl private void DrawRFIDPoints(Graphics g) { // RFID 포인트 그리기 - foreach (var rfid in rfidPoints) + foreach (var rfid in RFIDPoints) { var MarkerSize = 5; var half = MarkerSize / 2f; @@ -1384,7 +1269,7 @@ namespace AGVControl } } - var str = rfid.RFIDValue.ToString(); + var str = rfid.Value.ToString(); g.DrawString(str, this.Font, Brushes.DarkGray, rfid.Bounds.X, rfid.Bounds.Y + 5); } @@ -1398,47 +1283,32 @@ namespace AGVControl // AGV의 현재 위치를 중심으로 하는 원 var circleRect = new Rectangle( - agv.CurrentPosition.X - halfsize, - agv.CurrentPosition.Y - halfsize, + agv.CurrentRFID.Location.X - halfsize, + agv.CurrentRFID.Location.Y - halfsize, agvsize, agvsize); - if (agv.BodyAngle.HasValue) + + // --- BodyAngle이 결정되지 않은 경우: 기본 방향으로 그림 --- + Color bgcolor = agv.BatteryLevel > 80 ? Color.Lime : (agv.BatteryLevel > 60 ? Color.Gold : Color.Tomato); + using (var circleBrush = new SolidBrush(Color.FromArgb(150, bgcolor))) + g.FillEllipse(circleBrush, circleRect); + using (var circlePen = new Pen(Color.Black, 2)) + g.DrawEllipse(circlePen, circleRect); + + //motor direction + var str = agv.CurrentMOTDirection.ToString().Substring(0, 1); + var strsize = g.MeasureString(str, this.Font); + g.DrawString(str, this.Font, Brushes.White, circleRect, new StringFormat { - // --- BodyAngle이 결정된 경우: AGV를 회전시켜 그림 --- - var originalTransform = g.Transform; - g.TranslateTransform(agv.CurrentPosition.X, agv.CurrentPosition.Y); - g.RotateTransform(agv.BodyAngle.Value + 90); // 리프트가 위쪽(0, -1)을 기본으로 하므로 90도 보정 + Alignment = StringAlignment.Center, + LineAlignment = StringAlignment.Center + }); - // 원 그리기 (회전된 좌표계 기준) - Color bgcolor = agv.BatteryLevel > 80 ? Color.Lime : (agv.BatteryLevel > 60 ? Color.Gold : Color.Tomato); - using (var circleBrush = new SolidBrush(Color.FromArgb(150, bgcolor))) - g.FillEllipse(circleBrush, -halfsize, -halfsize, agvsize, agvsize); - using (var circlePen = new Pen(Color.Black, 2)) - g.DrawEllipse(circlePen, -halfsize, -halfsize, agvsize, agvsize); + //body direction + str = agv.CurrentAGVDirection.ToString().Substring(0, 1).ToUpper(); + strsize = g.MeasureString(str, this.Font); + g.DrawString(str, this.Font, Brushes.Gold, circleRect.X + (circleRect.Width / 2f) - (strsize.Width / 2f), circleRect.Bottom + 3); - // 리프트 그리기 (회전된 좌표계 기준) - var liftWidth = circleRect.Width; - var liftHeight = (int)(circleRect.Height * 0.4); - var liftOffset = halfsize + 1; - var liftRect = new Rectangle(-liftWidth / 2, -halfsize - liftOffset, liftWidth, liftHeight); - using (var liftBrush = new SolidBrush(Color.FromArgb(200, Color.DarkGray))) - g.FillRectangle(liftBrush, liftRect); - using (var liftPen = new Pen(Color.Black, 1)) - g.DrawRectangle(liftPen, liftRect); - using (var connectionPen = new Pen(Color.Black, 2)) - g.DrawLine(connectionPen, 0, -halfsize, 0, -halfsize - liftOffset); - - g.Transform = originalTransform; // 그래픽스 상태 복원 - } - else - { - // --- BodyAngle이 결정되지 않은 경우: 기본 방향으로 그림 --- - Color bgcolor = agv.BatteryLevel > 80 ? Color.Lime : (agv.BatteryLevel > 60 ? Color.Gold : Color.Tomato); - using (var circleBrush = new SolidBrush(Color.FromArgb(150, bgcolor))) - g.FillEllipse(circleBrush, circleRect); - using (var circlePen = new Pen(Color.Black, 2)) - g.DrawEllipse(circlePen, circleRect); - } // 과거 이동 경로 화살표 그리기 DrawMovementHistoryArrows(g); @@ -1449,13 +1319,6 @@ namespace AGVControl var agvsize = 30; var halfsize = (int)(agvsize / 2); - // AGV의 모터 각도를 가져옴 - float motorAngle = agv.MotorAngle; - - var gState = g.Save(); - g.TranslateTransform(agv.CurrentPosition.X, agv.CurrentPosition.Y); - g.RotateTransform(motorAngle + 90); // 삼각형이 위쪽(Y축 음수)을 향하도록 90도 보정 - // 삼각형 포인트 계산 (회전 중심점 0,0 기준) Point[] trianglePoints = new Point[3]; var arrowSize = halfsize - 5; @@ -1468,14 +1331,12 @@ namespace AGVControl g.FillPolygon(arrowBrush, trianglePoints); using (var arrowPen = new Pen(Color.Black, 2)) g.DrawPolygon(arrowPen, trianglePoints); - - g.Restore(gState); } // 과거 이동 경로를 화살표로 표시 private void DrawMovementHistoryArrows(Graphics g) { - if (agv.MovementHistory.Count < 2 || agv.PositionHistory.Count < 2) + if (agv.MovementHistory.Count < 2) return; // 최근 3개의 이동 경로 표시 (가장 오래된 것부터) @@ -1485,8 +1346,8 @@ namespace AGVControl { var startRFID = agv.MovementHistory[i]; var endRFID = agv.MovementHistory[i + 1]; - var startPos = agv.PositionHistory[i]; - var endPos = agv.PositionHistory[i + 1]; + //var startPos = agv.PositionHistory[i]; + //var endPos = agv.PositionHistory[i + 1]; // 시간에 따른 투명도 계산 int age = agv.MovementHistory.Count - 1 - i; @@ -1499,9 +1360,9 @@ namespace AGVControl if (directConnection != null) { // 직접 연결된 경우: 실선 화살표 - Color arrowColor = (directConnection.StartRFID == startRFID.rfid) ? Color.Green : Color.Red; + Color arrowColor = (directConnection.StartRFID == startRFID.rfid) ? Color.Lime : Color.Red; arrowColor = Color.FromArgb(alpha, arrowColor); - DrawArrow(g, startPos, endPos, arrowColor, 3); + DrawArrow(g, startRFID.Position, endRFID.Position, arrowColor, 3); } else { @@ -1512,16 +1373,16 @@ namespace AGVControl // 경로의 첫 단계 방향으로 전체 색상 결정 Color arrowColor = Color.Gray; var firstStepEndPoint = pathResult.Path[1]; - var firstStepEndRfidPoint = rfidPoints.FirstOrDefault(p => p.Location == firstStepEndPoint); + var firstStepEndRfidPoint = RFIDPoints.FirstOrDefault(p => p.Location == firstStepEndPoint.Location); if (firstStepEndRfidPoint != null) { var firstStepConnection = rfidConnections.FirstOrDefault(c => - (c.StartRFID == startRFID.rfid && c.EndRFID == firstStepEndRfidPoint.RFIDValue) || - (c.IsBidirectional && c.StartRFID == firstStepEndRfidPoint.RFIDValue && c.EndRFID == startRFID.rfid)); + (c.StartRFID == startRFID.rfid && c.EndRFID == firstStepEndRfidPoint.Value) || + (c.IsBidirectional && c.StartRFID == firstStepEndRfidPoint.Value && c.EndRFID == startRFID.rfid)); if (firstStepConnection != null) { - arrowColor = (firstStepConnection.StartRFID == startRFID.rfid) ? Color.Green : Color.Red; + arrowColor = (firstStepConnection.StartRFID == startRFID.rfid) ? Color.Lime : Color.Red; } } @@ -1530,8 +1391,8 @@ namespace AGVControl // 경로의 각 세그먼트를 점선 화살표로 그리기 for (int j = 0; j < pathResult.Path.Count - 1; j++) { - Point segmentStart = pathResult.Path[j]; - Point segmentEnd = pathResult.Path[j + 1]; + Point segmentStart = pathResult.Path[j].Location; + Point segmentEnd = pathResult.Path[j + 1].Location; DrawDashedArrow(g, segmentStart, segmentEnd, arrowColor, 3); } } @@ -1690,8 +1551,8 @@ namespace AGVControl foreach (var connection in rfidConnections) { - var startPoint = rfidPoints.FirstOrDefault(p => p.RFIDValue == connection.StartRFID)?.Location ?? Point.Empty; - var endPoint = rfidPoints.FirstOrDefault(p => p.RFIDValue == connection.EndRFID)?.Location ?? Point.Empty; + var startPoint = RFIDPoints.FirstOrDefault(p => p.Value == connection.StartRFID)?.Location ?? Point.Empty; + var endPoint = RFIDPoints.FirstOrDefault(p => p.Value == connection.EndRFID)?.Location ?? Point.Empty; if (startPoint.IsEmpty || endPoint.IsEmpty) continue; @@ -1754,14 +1615,14 @@ namespace AGVControl pathPen.DashStyle = System.Drawing.Drawing2D.DashStyle.Dash; for (int i = 0; i < agv.CurrentPath.Count - 1; i++) { - g.DrawLine(pathPen, agv.CurrentPath[i], agv.CurrentPath[i + 1]); + g.DrawLine(pathPen, agv.CurrentPath[i].Location, agv.CurrentPath[i + 1].Location); } } } public List GetRFIDPoints() { - return rfidPoints; + return RFIDPoints; } public List GetRFIDLines() @@ -1782,7 +1643,7 @@ namespace AGVControl var lineVector = new Point(endPoint.X - startPoint.X, endPoint.Y - startPoint.Y); var lineLength = (float)Math.Sqrt(lineVector.X * lineVector.X + lineVector.Y * lineVector.Y); - foreach (var rfid in rfidPoints) + foreach (var rfid in RFIDPoints) { if (rfid.Location == startPoint || rfid.Location == endPoint) continue; @@ -1820,7 +1681,7 @@ namespace AGVControl public void ClearMap() { - rfidPoints.Clear(); + RFIDPoints.Clear(); mapTexts.Clear(); customLines.Clear(); rfidLines.Clear(); @@ -1844,10 +1705,10 @@ namespace AGVControl var rfidPoint = new RFIDPoint { Location = mapLocation, - RFIDValue = rfidValue + Value = rfidValue }; - rfidPoints.Add(rfidPoint); + RFIDPoints.Add(rfidPoint); this.Invalidate(); } @@ -1857,17 +1718,17 @@ namespace AGVControl // RFID 포인트 저장 lines.Add("[RFID_POINTS]"); - foreach (var point in rfidPoints) + foreach (var point in RFIDPoints) { - lines.Add($"{point.Location.X},{point.Location.Y},{point.RFIDValue},{point.IsRotatable},{point.FixedDirection},{point.IsTerminal}"); + lines.Add($"{point.Location.X},{point.Location.Y},{point.Value},{point.IsRotatable},{point.FixedDirection},{point.IsTerminal}"); } // RFID 라인 저장 lines.Add("[RFID_LINES]"); foreach (var connection in rfidConnections) { - var startPoint = rfidPoints.First(p => p.RFIDValue == connection.StartRFID).Location; - var endPoint = rfidPoints.First(p => p.RFIDValue == connection.EndRFID).Location; + var startPoint = RFIDPoints.First(p => p.Value == connection.StartRFID).Location; + var endPoint = RFIDPoints.First(p => p.Value == connection.EndRFID).Location; lines.Add($"{startPoint.X},{startPoint.Y},{endPoint.X},{endPoint.Y}," + $"{connection.StartRFID},{connection.EndRFID},{connection.IsBidirectional},{connection.Distance}"); } @@ -1921,7 +1782,7 @@ namespace AGVControl if (validX && validY && validN) { - if (rfidPoints.Where(t => t.RFIDValue == valRfid).Any()) + if (RFIDPoints.Where(t => t.Value == valRfid).Any()) { //이미존재한다 var newvalue = @@ -1931,7 +1792,7 @@ namespace AGVControl var rfidPoint = new RFIDPoint { Location = new Point(valX, valY), - RFIDValue = valRfid + Value = valRfid }; // 추가 속성 로드 (기본값 처리) @@ -1950,7 +1811,7 @@ namespace AGVControl rfidPoint.IsTerminal = isTerminal; } - rfidPoints.Add(rfidPoint); + RFIDPoints.Add(rfidPoint); } else sb.AppendLine($"[{section}] {line}"); } @@ -2020,11 +1881,11 @@ namespace AGVControl var end = line.EndPoint; // 1. 선 위의 모든 RFID 포인트(시작, 끝 포함)를 projectionRatio로 정렬 - var pointsOnThisLine = rfidPoints + var pointsOnThisLine = RFIDPoints .Where(p => IsPointOnLine(p.Location, start, end, 10f)) // 오차 허용치 넉넉히 .Select(p => new { - RFID = p.RFIDValue, + RFID = p.Value, Ratio = GetProjectionRatio(p.Location, start, end) }) .ToList(); @@ -2046,8 +1907,8 @@ namespace AGVControl var key = $"{Math.Min(from, to)}_{Math.Max(from, to)}"; if (connectionSet.Contains(key)) continue; - var fromPt = rfidPoints.FirstOrDefault(p => p.RFIDValue == from)?.Location ?? line.StartPoint; - var toPt = rfidPoints.FirstOrDefault(p => p.RFIDValue == to)?.Location ?? line.EndPoint; + var fromPt = RFIDPoints.FirstOrDefault(p => p.Value == from)?.Location ?? line.StartPoint; + var toPt = RFIDPoints.FirstOrDefault(p => p.Value == to)?.Location ?? line.EndPoint; rfidConnections.Add(new RFIDConnection { @@ -2130,9 +1991,14 @@ namespace AGVControl } } + /// + /// 목적지에 깃발 표시 + /// + /// private void DrawTargetFlag(Graphics g) { - if (agv.TargetPosition == Point.Empty) return; + //대상이없다면 진행하지 않습니다 + if (agv.TargetRFID.IsEmpty) return; // 바닥에 흰색 원 그리기 using (var baseBrush = new SolidBrush(Color.Red)) @@ -2140,12 +2006,12 @@ namespace AGVControl { var baseSize = 8; g.FillEllipse(baseBrush, - agv.TargetPosition.X - baseSize / 2, - agv.TargetPosition.Y - baseSize / 2, + agv.TargetRFID.Location.X - baseSize / 2, + agv.TargetRFID.Location.Y - baseSize / 2, baseSize, baseSize); g.DrawEllipse(basePen, - agv.TargetPosition.X - baseSize / 2, - agv.TargetPosition.Y - baseSize / 2, + agv.TargetRFID.Location.X - baseSize / 2, + agv.TargetRFID.Location.Y - baseSize / 2, baseSize, baseSize); } @@ -2154,17 +2020,17 @@ namespace AGVControl { var poleLength = 27; // 40 * 2/3 ≈ 27 g.DrawLine(polePen, - agv.TargetPosition.X, - agv.TargetPosition.Y, - agv.TargetPosition.X, - agv.TargetPosition.Y - poleLength); + agv.TargetRFID.Location.X, + agv.TargetRFID.Location.Y, + agv.TargetRFID.Location.X, + agv.TargetRFID.Location.Y - poleLength); } // 깃발 그리기 Point[] flagPoints = new Point[3]; - flagPoints[0] = new Point(agv.TargetPosition.X, agv.TargetPosition.Y - 27); // 깃대 길이에 맞춤 - flagPoints[1] = new Point(agv.TargetPosition.X + 20, agv.TargetPosition.Y - 22); - flagPoints[2] = new Point(agv.TargetPosition.X, agv.TargetPosition.Y - 17); + flagPoints[0] = new Point(agv.TargetRFID.Location.X, agv.TargetRFID.Location.Y - 27); // 깃대 길이에 맞춤 + flagPoints[1] = new Point(agv.TargetRFID.Location.X + 20, agv.TargetRFID.Location.Y - 22); + flagPoints[2] = new Point(agv.TargetRFID.Location.X, agv.TargetRFID.Location.Y - 17); using (var flagBrush = new SolidBrush(Color.Red)) using (var flagPen = new Pen(Color.DarkRed, 1)) @@ -2174,48 +2040,14 @@ namespace AGVControl } } - // 회전 가능 여부 토글 함수 - public void ToggleRotatable(uint rfidValue) - { - var rfidPoint = FindRFIDPoint(rfidValue); - if (rfidPoint != null) - { - rfidPoint.IsRotatable = !rfidPoint.IsRotatable; - this.Invalidate(); - } - } - // 회전 가능 여부 확인 함수 - public bool IsPointRotatable(uint rfidValue) - { - var rfidPoint = FindRFIDPoint(rfidValue); - return rfidPoint?.IsRotatable ?? false; - } - - // 경로 계산 시 회전 가능 여부를 고려하여 방향 결정 - private Direction DetermineDirection(Point current, Point next, Point target) - { - // 현재 위치가 회전 가능한 구간인 경우 - var currentRFID = rfidPoints.FirstOrDefault(p => p.Location == current); - if (currentRFID?.IsRotatable ?? false) - { - // 목적지 방향으로 직접 방향 결정 - if (target.X > current.X) return Direction.Forward; - if (target.X < current.X) return Direction.Backward; - if (target.Y > current.Y) return Direction.Forward; - if (target.Y < current.Y) return Direction.Backward; - } - - // 회전 불가능한 구간인 경우 현재 진행 방향 유지 - return agv.CurrentDirection; - } public AGVActionPrediction PredictResult = null; // AGV 행동 예측 함수 public AGVActionPrediction PredictNextAction() { // 1. 위치를 모를 때 (CurrentRFID가 0 또는 미설정) - if (agv.CurrentRFID == 0) + if (agv.CurrentRFID.Value == 0) { PredictResult = new AGVActionPrediction { @@ -2229,11 +2061,11 @@ namespace AGVControl } // 2. 경로가 없거나 현재 위치가 경로에 없음 - if (agv.CurrentPath == null || agv.CurrentPath.Count < 2 || agv.CurrentPosition == Point.Empty) + if ((agv.CurrentPath?.Count ?? 0) < 2 ) { PredictResult = new AGVActionPrediction { - Direction = agv.CurrentDirection, + Direction = agv.CurrentMOTDirection, NextRFID = null, Reason = "경로 없음 또는 현재 위치 미확정", ReasonCode = AGVActionReasonCode.NoPath, @@ -2243,12 +2075,12 @@ namespace AGVControl } // 3. 경로상에서 다음 RFID 예측 - int idx = agv.CurrentPath.FindIndex(p => p == agv.CurrentPosition); + int idx = agv.CurrentPath.FindIndex(p => p.Value == agv.CurrentRFID.Value); if (idx < 0) { PredictResult = new AGVActionPrediction { - Direction = agv.CurrentDirection, + Direction = agv.CurrentMOTDirection, NextRFID = null, Reason = "현재 위치가 경로에 없음", ReasonCode = AGVActionReasonCode.NotOnPath, @@ -2260,16 +2092,16 @@ namespace AGVControl // 4. 목적지 도달 전, 방향 미리 판단 및 회전 위치 예측 // 목적지 RFID 정보 var destPoint = agv.CurrentPath.Last(); - var destRFID = rfidPoints.FirstOrDefault(r => r.Location == destPoint); - if (destRFID != null && destRFID.FixedDirection.HasValue) + var destRFID = RFIDPoints.FirstOrDefault(r => r.Value == destPoint.Value); // + if (destRFID != null && destRFID.FixedDirection.HasValue) //대상에 진입방향이 고정되어 있는지? { // 목적지에 도달할 때의 방향 예측 if (agv.CurrentPath.Count >= 2) { // 목적지 바로 전 위치에서 목적지로 이동할 때의 방향 var beforeDest = agv.CurrentPath[agv.CurrentPath.Count - 2]; - float arriveDeltaX = destPoint.X - beforeDest.X; - float arriveDeltaY = destPoint.Y - beforeDest.Y; + float arriveDeltaX = destPoint.Location.X - beforeDest.Location.X; + float arriveDeltaY = destPoint.Location.Y - beforeDest.Location.Y; Direction arriveDir = (Math.Abs(arriveDeltaX) > Math.Abs(arriveDeltaY)) ? (arriveDeltaX > 0 ? Direction.Forward : Direction.Backward) : (arriveDeltaY > 0 ? Direction.Forward : Direction.Backward); @@ -2279,7 +2111,7 @@ namespace AGVControl int lastRotatableIdx = -1; for (int i = 0; i < agv.CurrentPath.Count - 1; i++) { - var rfid = rfidPoints.FirstOrDefault(r => r.Location == agv.CurrentPath[i]); + var rfid = RFIDPoints.FirstOrDefault(r => r.Location == agv.CurrentPath[i].Location); if (rfid != null && rfid.IsRotatable) lastRotatableIdx = i; } @@ -2288,11 +2120,11 @@ namespace AGVControl // 회전 가능한 위치에 도달하면 NeedTurn 반환 (STOP) if (idx == lastRotatableIdx) { - var rfid = rfidPoints.FirstOrDefault(r => r.Location == agv.CurrentPath[lastRotatableIdx]); + var rfid = RFIDPoints.FirstOrDefault(r => r.Location == agv.CurrentPath[lastRotatableIdx].Location); PredictResult = new AGVActionPrediction { - Direction = agv.CurrentDirection, - NextRFID = rfid?.RFIDValue, + Direction = agv.CurrentMOTDirection, + NextRFID = rfid?.Value, Reason = "목적지 진입방향 맞추기 위해 회전 필요", ReasonCode = AGVActionReasonCode.NeedTurn, MoveState = AGVMoveState.Stop @@ -2302,16 +2134,16 @@ namespace AGVControl else if (idx < lastRotatableIdx) { // 회전 가능한 위치까지 이동 안내 (RUN) - var rfid = rfidPoints.FirstOrDefault(r => r.Location == agv.CurrentPath[lastRotatableIdx]); - float moveDeltaX = agv.CurrentPath[lastRotatableIdx].X - agv.CurrentPosition.X; - float moveDeltaY = agv.CurrentPath[lastRotatableIdx].Y - agv.CurrentPosition.Y; + var rfid = RFIDPoints.FirstOrDefault(r => r.Location == agv.CurrentPath[lastRotatableIdx].Location); + float moveDeltaX = agv.CurrentPath[lastRotatableIdx].Location.X - agv.CurrentRFID.Location.X; + float moveDeltaY = agv.CurrentPath[lastRotatableIdx].Location.Y - agv.CurrentRFID.Location.Y; Direction moveDir = (Math.Abs(moveDeltaX) > Math.Abs(moveDeltaY)) ? (moveDeltaX > 0 ? Direction.Forward : Direction.Backward) : (moveDeltaY > 0 ? Direction.Forward : Direction.Backward); PredictResult = new AGVActionPrediction { Direction = moveDir, - NextRFID = rfid?.RFIDValue, + NextRFID = rfid?.Value, Reason = "회전 가능한 위치로 이동 중", ReasonCode = AGVActionReasonCode.Normal, MoveState = AGVMoveState.Run @@ -2322,7 +2154,7 @@ namespace AGVControl // 회전 가능한 위치가 없음 (STOP) PredictResult = new AGVActionPrediction { - Direction = agv.CurrentDirection, + Direction = agv.CurrentMOTDirection, NextRFID = null, Reason = "경로상에 회전 가능한 위치가 없음", ReasonCode = AGVActionReasonCode.NoTurnPoint, @@ -2338,7 +2170,7 @@ namespace AGVControl { PredictResult = new AGVActionPrediction { - Direction = agv.CurrentDirection, + Direction = agv.CurrentMOTDirection, NextRFID = null, Reason = "경로의 마지막 지점(목적지 도달)", ReasonCode = AGVActionReasonCode.Arrived, @@ -2348,12 +2180,12 @@ namespace AGVControl } // 6. 일반 경로 주행 (RUN) - Point nextPoint = agv.CurrentPath[idx + 1]; - var nextRFID = rfidPoints.FirstOrDefault(r => r.Location == nextPoint)?.RFIDValue; + var nextPoint = agv.CurrentPath[idx + 1]; + var nextRFID = RFIDPoints.FirstOrDefault(r => r.Value == nextPoint.Value)?.Value; // X, Y 좌표 모두 고려한 방향 판단 - float deltaX = nextPoint.X - agv.CurrentPosition.X; - float deltaY = nextPoint.Y - agv.CurrentPosition.Y; + float deltaX = nextPoint.Location.X - agv.CurrentRFID.Location.X; + float deltaY = nextPoint.Location.Y - agv.CurrentRFID.Location.Y; Direction nextDir = (Math.Abs(deltaX) > Math.Abs(deltaY)) ? (deltaX > 0 ? Direction.Forward : Direction.Backward) : (deltaY > 0 ? Direction.Forward : Direction.Backward); diff --git a/Cs_HMI/SubProject/AGVControl/Models/AGV.cs b/Cs_HMI/SubProject/AGVControl/Models/AGV.cs index 63ee563..8880fe3 100644 --- a/Cs_HMI/SubProject/AGVControl/Models/AGV.cs +++ b/Cs_HMI/SubProject/AGVControl/Models/AGV.cs @@ -2,6 +2,7 @@ using System; using System.Drawing; using System.Collections.Generic; using System.Linq; +using System.Security.Permissions; namespace AGVControl.Models { @@ -12,65 +13,99 @@ namespace AGVControl.Models Stop = 2 } - public struct movehistorydata + public class CRFIDData { public UInt16 rfid { get; set; } + public Point Position { get; set; } + public override string ToString() + { + return $"RFID:{rfid},P:{Position.X},{Position.Y}"; + } + } + + public class movehistorydata : CRFIDData + { public Direction direction { get; set; } public override string ToString() { - return $"RFID:{rfid},DIR:{direction}"; + return $"RFID:{rfid},DIR:{direction},P:{Position.X},{Position.Y}"; } } public class AGV { - public Point CurrentPosition { get; set; } - public uint CurrentRFID { get; set; } - public float BatteryLevel { get; set; } = 0f; + /// - /// AGV에서 방향값이 수산됩니다. + /// RFID 번호 /// - public Direction CurrentDirection { get; set; } + public RFIDPoint CurrentRFID { get; set; } + + /// + /// 목적지가 셋팅된경우 해당 값 + /// + + public RFIDPoint TargetRFID { get; set; } + + /// + /// 배터리잔량(%) + /// + public float BatteryLevel { get; set; } = 0f; + + /// + /// 배터리온도(board) + /// + public double BatteryTemp1 { get; set; } = 0; + + /// + /// 배터리온도(cell) + /// + public double BatteryTemp2 { get; set; } = 0; + + /// + /// AGV + /// + public Direction CurrentAGVDirection { get; set; } + + /// + /// AGV모터 방향 + /// 외부에서 값이 상시 업데이트 됩니다. + /// + public Direction CurrentMOTDirection { get; set; } /// /// 현재위치가 수산되면 목적지까지의 방향값이 계산됩니다. /// public Direction TargetDirection { get; set; } = Direction.Stop; public bool IsMoving { get; set; } - public List CurrentPath { get; set; } = new List(); + + /// + /// 경로검색으로 입력된 경로 + /// + public List CurrentPath { get; set; } = new List(); + public List PlannedPath { get; set; } public List 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 MovementHistory { get; } = new List(); - public List PositionHistory { get; } = new List(); + public const int HISTORY_SIZE = 4; // 최근 4개 위치 기록 public AGV() { - CurrentPath = new List(); + CurrentPath = new List(); PlannedPath = new List(); PathRFIDs = new List(); - CurrentDirection = Direction.Forward; - TargetPosition = Point.Empty; - TargetRFID = 0; + + CurrentRFID = new RFIDPoint(); + TargetRFID = new RFIDPoint(); + TargetDirection = Direction.Forward; - BodyAngle = null; - } - - public void Move() - { - if (CurrentPath.Count > 0) - { - CurrentPosition = CurrentPath[0]; - CurrentPath.RemoveAt(0); - } + // BodyAngle = null; } + // 이동 경로에 새로운 RFID 추가 public void AddToMovementHistory(UInt16 rfidValue, Point position, Direction direction) { @@ -78,18 +113,16 @@ namespace AGVControl.Models if (MovementHistory.Count > 0 && MovementHistory.Last().rfid == rfidValue) return; - MovementHistory.Add(new movehistorydata { rfid = rfidValue, direction = direction }) ; - PositionHistory.Add(position); + MovementHistory.Add(new movehistorydata { rfid = rfidValue, direction = direction, Position = position }); // 기록 크기 제한 if (MovementHistory.Count > HISTORY_SIZE) { MovementHistory.RemoveAt(0); - PositionHistory.RemoveAt(0); } //최초방향과 마지막 방향이 일치하지 않으면 그 이전의 데이터는 삭제한다. - if(MovementHistory.Count > 2 && MovementHistory.First().direction != MovementHistory.Last().direction) + if (MovementHistory.Count > 2 && MovementHistory.First().direction != MovementHistory.Last().direction) { var lastTwo = MovementHistory.Skip(MovementHistory.Count - 2).Take(2).ToArray(); // [9, 10] MovementHistory.Clear(); @@ -129,7 +162,7 @@ namespace AGVControl.Models return true; // 검증 불가능한 경우 // 최근 두 RFID 값 가져오기 - var recentRFIDs = MovementHistory.Skip( MovementHistory.Count - 2).Take(2).ToList(); + var recentRFIDs = MovementHistory.Skip(MovementHistory.Count - 2).Take(2).ToList(); if (recentRFIDs.Count < 2) return true; @@ -144,15 +177,9 @@ namespace AGVControl.Models if (actualDirection.Value != expectedDirection) { // AGV 모터 방향을 실제 이동 방향으로 정정 - CurrentDirection = actualDirection.Value; + CurrentAGVDirection = actualDirection.Value; TargetDirection = actualDirection.Value; - // 몸체 방향도 180도 회전 (결정된 경우에만) - if (BodyAngle.HasValue) - { - BodyAngle = (BodyAngle.Value + 180) % 360; - } - return false; // 정정됨을 알림 } @@ -188,77 +215,7 @@ namespace AGVControl.Models } } - // 경로상 RFID 순서 기반 예상 방향 계산 - public Direction? CalculateExpectedDirectionByRFID() - { - if (CurrentPath == null || CurrentPath.Count < 2) - return null; - - // 현재 위치의 RFID 찾기 - var currentRFID = CurrentRFID; - if (currentRFID == 0) - return null; - - // 경로상 다음 RFID 찾기 - int currentIdx = CurrentPath.FindIndex(p => p == CurrentPosition); - if (currentIdx < 0 || currentIdx >= CurrentPath.Count - 1) - return null; - - // 다음 위치의 RFID 찾기 (MapControl에서 RFID 정보 필요) - // 이 부분은 MapControl에서 처리하도록 수정 필요 - return null; - } - - // 실제 이동 방향 계산 (기존 메서드 - 호환성 유지) - public Direction? CalculateActualDirection() - { - if (MovementHistory.Count < 2) - return null; - - // 최근 두 위치로부터 실제 이동 방향 계산 - var recentPositions = PositionHistory.Skip(Math.Max(0, PositionHistory.Count - 2)).Take(2).ToList(); - if (recentPositions.Count < 2) - return null; - - var prevPos = recentPositions[0]; - var currentPos = recentPositions[1]; - - // X축 이동이 더 큰 경우 - if (Math.Abs(currentPos.X - prevPos.X) > Math.Abs(currentPos.Y - prevPos.Y)) - { - return currentPos.X > prevPos.X ? Direction.Forward : Direction.Backward; - } - // Y축 이동이 더 큰 경우 - else - { - return currentPos.Y > prevPos.Y ? Direction.Forward : Direction.Backward; - } - } - - // 경로상 예상 방향 계산 - public Direction? CalculateExpectedDirection() - { - if (CurrentPath == null || CurrentPath.Count < 2) - return null; - - int currentIdx = CurrentPath.FindIndex(p => p == CurrentPosition); - if (currentIdx < 0 || currentIdx >= CurrentPath.Count - 1) - return null; - - var currentPos = CurrentPath[currentIdx]; - var nextPos = CurrentPath[currentIdx + 1]; - - // X축 이동이 더 큰 경우 - if (Math.Abs(nextPos.X - currentPos.X) > Math.Abs(nextPos.Y - currentPos.Y)) - { - return nextPos.X > currentPos.X ? Direction.Forward : Direction.Backward; - } - // Y축 이동이 더 큰 경우 - else - { - return nextPos.Y > currentPos.Y ? Direction.Forward : Direction.Backward; - } - } + } public class PathNode diff --git a/Cs_HMI/SubProject/AGVControl/Models/RFIDPoint.cs b/Cs_HMI/SubProject/AGVControl/Models/RFIDPoint.cs index 87e05ef..a511473 100644 --- a/Cs_HMI/SubProject/AGVControl/Models/RFIDPoint.cs +++ b/Cs_HMI/SubProject/AGVControl/Models/RFIDPoint.cs @@ -6,7 +6,7 @@ namespace AGVControl.Models public class RFIDPoint { public Point Location { get; set; } - public uint RFIDValue { get; set; } + public uint Value { get; set; } public string NextRFID { get; set; } // 다음 RFID 포인트의 값 public bool IsBidirectional { get; set; } // 양방향 연결 여부 public bool IsRotatable { get; set; } // 회전 가능 여부 @@ -14,12 +14,29 @@ namespace AGVControl.Models public bool IsTerminal { get; set; } // 종단 여부 public RectangleF Bounds { get; set; } + public void Clear() + { + this.Location = Point.Empty; + this.Value = 0; + } + + public bool IsEmpty + { + get + { + //RFID값이나 위치 값이 없으면 비어있는 것으로 한다. + if (this.Location.IsEmpty) return true; + if ((this.Value < 1)) return true; + return false; + } + } public RFIDPoint() - { + { IsRotatable = false; // 기본값은 회전 불가능 IsBidirectional = true; // 기본값은 양방향 FixedDirection = null; IsTerminal = false; // 기본값은 종단 아님 + Clear(); } } } \ No newline at end of file