diff --git a/Cs_HMI/Project/StateMachine/_AGV.cs b/Cs_HMI/Project/StateMachine/_AGV.cs index 4734f0e..2f1d394 100644 --- a/Cs_HMI/Project/StateMachine/_AGV.cs +++ b/Cs_HMI/Project/StateMachine/_AGV.cs @@ -41,10 +41,30 @@ namespace Project { try { - - if (PUB.mapctl != null) - PUB.mapctl.PredictNextAction(); + { + var rlt = AGVControl.MapControlManager.PredictNextAction(); + + if (rlt.ReasonCode == AGVControl.AGVActionReasonCode.busy) + { + //var premsg = $"이전 예측작업이 완료되지 않았습니다"; + //PUB.log.AddE(premsg); + //PUB.log.AddE(premsg); + //Console.WriteLine(premsg); + + //predicterr += 1; + //if (predicterr > 10) + //{ + // var premsg = $"행동예측 오류카운트 초과"; + // PUB.AGV.AGVMoveStop(premsg); + // PUB.log.AddE(premsg); + // PUB.logagv.AddE(premsg); + // Console.WriteLine(premsg); + //} + } + } + + switch (e.DataType) { @@ -63,12 +83,12 @@ namespace Project //모터방향 입력 if (PUB.AGV.data.Direction == 'B') - PUB.mapctl.agv.CurrentMOTDirection = AGVControl.Models.Direction.Backward; + AGVControl.MapControlManager.agv.CurrentMOTDirection = AGVControl.Models.Direction.Backward; else - PUB.mapctl.agv.CurrentMOTDirection = AGVControl.Models.Direction.Forward; + AGVControl.MapControlManager.agv.CurrentMOTDirection = AGVControl.Models.Direction.Forward; - PUB.mapctl.agv.IsMoving = PUB.AGV.system1.agv_run; - PUB.mapctl.agv.IsMarkCheck = PUB.AGV.system1.Mark1_check || PUB.AGV.system1.Mark2_check; + AGVControl.MapControlManager.agv.IsMoving = PUB.AGV.system1.agv_run; + AGVControl.MapControlManager.agv.IsMarkCheck = PUB.AGV.system1.Mark1_check || PUB.AGV.system1.Mark2_check; if (PUB.AGV.signal.mark_sensor == false) { diff --git a/Cs_HMI/Project/StateMachine/_BMS.cs b/Cs_HMI/Project/StateMachine/_BMS.cs index 0df32fd..8a2f681 100644 --- a/Cs_HMI/Project/StateMachine/_BMS.cs +++ b/Cs_HMI/Project/StateMachine/_BMS.cs @@ -158,9 +158,9 @@ 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; + AGVControl.MapControlManager.agv.BatteryLevel = PUB.BMS.Current_Level; + AGVControl.MapControlManager.agv.BatteryTemp1 = PUB.BMS.Current_temp1; + AGVControl.MapControlManager.agv.BatteryTemp2 = PUB.BMS.Current_temp2; if (PUB.BMS.Current_Level <= PUB.setting.ChargeStartLevel) { //배터리 레벨이 기준보다 낮다면 경고를 활성화 한다 diff --git a/Cs_HMI/Project/StateMachine/_Xbee.cs b/Cs_HMI/Project/StateMachine/_Xbee.cs index 51c8a6b..c3d7c43 100644 --- a/Cs_HMI/Project/StateMachine/_Xbee.cs +++ b/Cs_HMI/Project/StateMachine/_Xbee.cs @@ -60,7 +60,7 @@ namespace Project case ENIGProtocol.AGVCommands.Goto: //move to tag if (uint.TryParse(dataStr, out uint tagno2)) { - var currPos = PUB.mapctl.agv.CurrentRFID;///.AGVMoveToRFID(; + var currPos = AGVControl.MapControlManager.agv.CurrentRFID;///.AGVMoveToRFID(; if (PUB.mapctl.SetTargetPosition(tagno2)) PUB.log.AddI($"New Target {tagno2}"); else diff --git a/Cs_HMI/SubProject/AGVControl/MapControl.cs b/Cs_HMI/SubProject/AGVControl/MapControl.cs index f07898d..6f27ed3 100644 --- a/Cs_HMI/SubProject/AGVControl/MapControl.cs +++ b/Cs_HMI/SubProject/AGVControl/MapControl.cs @@ -5,6 +5,7 @@ using System.Drawing.Drawing2D; using System.IO; using System.Linq; using System.Text.RegularExpressions; +using System.Threading; using System.Windows.Forms; using AGVControl.Models; using AR; @@ -13,7 +14,6 @@ using COMM; namespace AGVControl { - public partial class MapControl : Control { #region 상수 정의 @@ -28,12 +28,12 @@ namespace AGVControl #region 멤버 변수 // 맵 데이터 - private List RFIDPoints; + private List mapTexts; private List customLines; private List rfidLines; - private HashSet rfidConnections; - public AGV agv; + + // 화면 조작 관련 private float zoom = 1.0f; @@ -83,12 +83,9 @@ namespace AGVControl public MapControl() { this.DoubleBuffered = true; - RFIDPoints = new List(); mapTexts = new List(); customLines = new List(); rfidLines = new List(); - rfidConnections = new HashSet(); - agv = new AGV(); UpdateToolbarRects(); } @@ -140,7 +137,7 @@ namespace AGVControl case "cline": MouseMode = (eMouseMode.addcustomline); break; case "point": MouseMode = (eMouseMode.addrfidpoint); break; case "clear": - this.agv.MainPath.Clear(); + MapControlManager.agv.MainPath.Clear(); break; case "path": @@ -161,7 +158,7 @@ namespace AGVControl return; } - var rlt = CalculatePath(vstart, vend); + var rlt = MapControlManager.CalculatePath(vstart, vend); if (rlt.Success == false) { MessageBox.Show(rlt.Message, "경로 계산", MessageBoxButtons.OK, MessageBoxIcon.Warning); @@ -236,7 +233,7 @@ namespace AGVControl } // RFID 포인트 선택 - var clickedRFID = RFIDPoints.FirstOrDefault(r => GetDistance(mapPoint, r.Location) <= SNAP_DISTANCE); + var clickedRFID = MapControlManager.RFIDPoints.FirstOrDefault(r => MapControlManager.GetDistance(mapPoint, r.Location) <= SNAP_DISTANCE); switch (mousemode) { case eMouseMode.rfidcut: @@ -313,7 +310,7 @@ namespace AGVControl } else { - var startRFID = RFIDPoints.FirstOrDefault(r => r.Location == previewStartPoint); + var startRFID = MapControlManager.RFIDPoints.FirstOrDefault(r => r.Location == previewStartPoint); if (startRFID != null) { var line = new RFIDLine @@ -345,7 +342,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 = MapControlManager.RFIDPoints.Where(t => t.Bounds.Expand(SELECTION_DISTANCE, SELECTION_DISTANCE).Contains(mapPoint)).FirstOrDefault(); if (selected_rfid != null) { UTIL.ShowPropertyDialog(selected_rfid); @@ -429,8 +426,8 @@ namespace AGVControl // 커스텀 라인 선택 및 드래그 포인트 설정 foreach (var line in customLines) { - float startDistance = GetDistance(mapPoint, line.StartPoint); - float endDistance = GetDistance(mapPoint, line.EndPoint); + float startDistance = MapControlManager.GetDistance(mapPoint, line.StartPoint); + float endDistance = MapControlManager.GetDistance(mapPoint, line.EndPoint); if (startDistance < SELECTION_DISTANCE) { @@ -450,8 +447,8 @@ namespace AGVControl // RFID 포인트 선택 - var clickedRFID = RFIDPoints.FirstOrDefault(r => - GetDistance(mapPoint, r.Location) <= SNAP_DISTANCE); + var clickedRFID = MapControlManager.RFIDPoints.FirstOrDefault(r => + MapControlManager.GetDistance(mapPoint, r.Location) <= SNAP_DISTANCE); if (clickedRFID != null) { selectedRFID = clickedRFID; @@ -686,9 +683,9 @@ namespace AGVControl private Point SnapToPoint(Point point) { // RFID 포인트와 근접한지 확인 - foreach (var rfid in RFIDPoints) + foreach (var rfid in MapControlManager.RFIDPoints) { - if (GetDistance(point, rfid.Location) <= SNAP_DISTANCE) + if (MapControlManager.GetDistance(point, rfid.Location) <= SNAP_DISTANCE) { return rfid.Location; } @@ -698,11 +695,6 @@ namespace AGVControl } - public RFIDPoint FindRFIDPoint(uint rfidValue) - { - if (RFIDPoints == null || RFIDPoints.Any() == false) return null; - return RFIDPoints.FirstOrDefault(r => r.Value == rfidValue); - } /// /// 현재위치를 설정합니다 @@ -711,25 +703,25 @@ namespace AGVControl /// public bool SetCurrentPosition(UInt16 rfidTagNo) { - var rfidPoint = FindRFIDPoint(rfidTagNo); + var rfidPoint = MapControlManager.FindRFIDPoint(rfidTagNo); if (rfidPoint != null) { // 이동 경로에 추가 (위치 업데이트보다 먼저) - agv.AddToMovementHistory(rfidTagNo, rfidPoint.Location, this.agv.CurrentMOTDirection); + MapControlManager.agv.AddToMovementHistory(rfidTagNo, rfidPoint.Location, MapControlManager.agv.CurrentMOTDirection); // AGV 위치 업데이트 - agv.CurrentRFID = rfidPoint; + MapControlManager.agv.CurrentRFID = rfidPoint; // 목적지가 설정되어 있고 경로가 있는 경우 검증 - if (agv.TargetRFID.IsEmpty == false && agv.MainPath.Count > 0) + if (MapControlManager.agv.TargetRFID.IsEmpty == false && MapControlManager.agv.MainPath.Count > 0) { // 현재 위치가 경로에 있는지 확인 - bool isOnPath = agv.MainPath.Contains(agv.CurrentRFID); + bool isOnPath = MapControlManager.agv.MainPath.Contains(MapControlManager.agv.CurrentRFID); if (!isOnPath) { // 경로를 벗어났으므로 새로운 경로 계산 - var pathResult = CalculatePath(agv.CurrentRFID, agv.TargetRFID); + var pathResult = MapControlManager.CalculatePath(MapControlManager.agv.CurrentRFID, MapControlManager.agv.TargetRFID, false); if (pathResult.Success) { SetCurrentPath(pathResult.Path); @@ -738,12 +730,12 @@ namespace AGVControl } // 목적지 RFID에 도착했고, 해당 RFID에 고정방향이 있으면 TargetDirection을 강제 설정 - if (agv.TargetRFID.Value == rfidTagNo) + if (MapControlManager.agv.TargetRFID.Value == rfidTagNo) { - var destRFID = FindRFIDPoint(rfidTagNo); + var destRFID = MapControlManager.FindRFIDPoint(rfidTagNo); if (destRFID != null && destRFID.FixedDirection.HasValue) { - agv.TargetDirection = destRFID.FixedDirection.Value; + MapControlManager.agv.TargetDirection = destRFID.FixedDirection.Value; } } @@ -753,174 +745,30 @@ namespace AGVControl return false; } + + + public bool SetTargetPosition(uint rfidValue) { - var rfidPoint = FindRFIDPoint(rfidValue); + var rfidPoint = MapControlManager.FindRFIDPoint(rfidValue); if (rfidPoint != null) { - agv.TargetRFID = rfidPoint; + MapControlManager.agv.TargetRFID = rfidPoint; this.Invalidate(); return true; } return false; } - - 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.Location, end.Location) } }; - - var maxcount = 50; - var loopcount = 0; - while (openList.Count > 0) - { - var current = openList.OrderBy(p => fScore.ContainsKey(p) ? fScore[p] : float.MaxValue).First(); - - if (current.Location == end.Location) - { - return ReconstructPath(cameFrom, current); - } - - openList.Remove(current); - closedList.Add(current); - - foreach (var neighbor in GetNeighbors(current)) - { - if (closedList.Contains(neighbor)) - continue; - - float tentativeGScore = gScore[current] + Distance(current.Location, neighbor.Location); - - if (!openList.Contains(neighbor)) - openList.Add(neighbor); - else if (tentativeGScore >= gScore[neighbor]) - continue; - - cameFrom[neighbor.Value] = current; - gScore[neighbor] = tentativeGScore; - fScore[neighbor] = gScore[neighbor] + Heuristic(neighbor.Location, end.Location); - } - } - - return new PathResult - { - 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 new PathResult - { - Path = path, - }; - } - private float Heuristic(Point a, Point b) - { - return (float)Math.Sqrt(Math.Pow(a.X - b.X, 2) + Math.Pow(a.Y - b.Y, 2)); - } - - - private float Distance(Point a, Point b) - { - return (float)Math.Sqrt(Math.Pow(a.X - b.X, 2) + Math.Pow(a.Y - b.Y, 2)); - //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.P1.Value == rfidA.Value && c.P2.Value == rfidB.Value) || - // (c.IsBidirectional && c.P1.Value == rfidB.Value && c.P2.Value == rfidA.Value)); - - //if (connection != null) - //{ - // return connection.Distance; - //} - - //return float.MaxValue; - } - - /// - /// 이웃포인터를 반환합니다 - /// - /// - /// - private List GetNeighbors(RFIDPoint pt) - { - var neighbors = new List(); - - //값이 없는 경우 오류 반환 - if (pt == null) return neighbors; - - //연결정보에서 데이터를 찾은 후 반환한다 - foreach (var connection in rfidConnections) - { - RFIDPoint nPT = null; - if (connection.P1.Value == pt.Value) - { - nPT = connection.P2; - } - else if (connection.P2.Value == pt.Value) - { - nPT = connection.P1; - } - - if (nPT != null) neighbors.Add(nPT); - } - - //중복제거후 반한 - return neighbors.Distinct().ToList(); - } - - public PathResult CalculatePath(uint tagStrt, uint tagEnd) - { - var retval = new PathResult - { - Message = string.Empty, - Path = new List(), - }; - - var sp = tagStrt; //만약시작위치가 없다면 항상 충전기 기준으로 한다 - var ep = tagEnd; - - var startPoint = FindRFIDPoint(sp); - var endPoint = FindRFIDPoint(ep); - - if (startPoint == null || endPoint == null) - { - retval.Message = "유효한 RFID 값을 입력해주세요."; - return retval; - } - - retval = CalculatePath(startPoint, endPoint); - if (retval.Success == false) - retval.Message = "경로를 찾을 수 없습니다"; - - return retval; - } - - - - public void SetRFIDPoints(List points) { - RFIDPoints = points; + MapControlManager.RFIDPoints = points; this.Invalidate(); } public void SetAGV(AGV vehicle) { - agv = vehicle; + MapControlManager.agv = vehicle; this.Invalidate(); } @@ -1051,7 +899,7 @@ namespace AGVControl public void SetCurrentPath(List path) { - agv.MainPath = path; + MapControlManager.agv.MainPath = path; this.Invalidate(); } @@ -1112,20 +960,23 @@ namespace AGVControl DrawToolbarButton(e.Graphics, item.Bounds, item.Title, item.isHovering); //예측값디스플레잉(임시) - - if (PredictResult != null) + var predictresult = MapControlManager.PredictResult; + if (predictresult != null) { - var str = $"{PredictResult.ReasonCode}|{PredictResult.MoveState}|{PredictResult.Direction}|Next:{PredictResult.NextRFID}"; + var nextRFID = predictresult.NextRFID?.Value.ToString() ?? ""; + var str = $"{predictresult.ReasonCode}|{predictresult.MoveState}|{predictresult.Direction}|Next:{nextRFID}"; var strsize = e.Graphics.MeasureString(str, this.Font); - e.Graphics.DrawString(str, this.Font, Brushes.Red, this.Right - strsize.Width - 10, this.Bottom - strsize.Height - 10); + var textcolor = Color.Red; + if (predictresult.MoveState == AGVMoveState.Stop) textcolor = Color.Gold; + using (var sb = new SolidBrush(textcolor)) + e.Graphics.DrawString(str, this.Font, sb, this.Right - strsize.Width - 10, this.Bottom - strsize.Height - 10); } - //경로정보표시(임시) var pathstr = ""; - if (agv.MainPath.Any()) + if (MapControlManager.agv.MainPath.Any()) { - pathstr = "● Path : " + string.Join("▶", agv.MainPath.Select(t => t.Value).ToArray()); + pathstr = "● Path : " + string.Join("▶", MapControlManager.agv.MainPath.Select(t => t.Value).ToArray()); //pathstr += "\n● Target Direction Match : " + (agv.IsTargetDirectionMatch ? "O" : "X"); } else pathstr = "● Path : no data"; @@ -1133,9 +984,9 @@ namespace AGVControl e.Graphics.DrawString(pathstr, f, Brushes.DeepSkyBlue, this.Left + 65, this.Top + 10); var histstr = ""; - if (agv.MovementHistory.Count > 1) + if (MapControlManager.agv.MovementHistory.Count > 1) { - histstr = "● Route : " + string.Join("▶", agv.MovementHistory.Select(t => t.Value.ToString() + $"[{t.Direction.ToString()[0]}]").ToArray()); + histstr = "● Route : " + string.Join("▶", MapControlManager.agv.MovementHistory.Select(t => t.Value.ToString() + $"[{t.Direction.ToString()[0]}]").ToArray()); } else histstr = "● Route : no data"; using (var f = new Font("Arial", 10, FontStyle.Bold)) @@ -1146,7 +997,7 @@ namespace AGVControl private void DrawRFIDPoints(Graphics g) { // RFID 포인트 그리기 - foreach (var rfid in RFIDPoints) + foreach (var rfid in MapControlManager.RFIDPoints) { var MarkerSize = 5; var half = MarkerSize / 2f; @@ -1219,16 +1070,16 @@ namespace AGVControl // AGV의 현재 위치를 중심으로 하는 원 var circleRect = new Rectangle( - agv.CurrentRFID.Location.X - halfsize, - agv.CurrentRFID.Location.Y - halfsize, + MapControlManager.agv.CurrentRFID.Location.X - halfsize, + MapControlManager.agv.CurrentRFID.Location.Y - halfsize, agvsize, agvsize); //이동경로정보를 따라서 리프트의 위치를 표시해준다. - if (agv.MovementHistory.Any() && agv.MovementHistory.Count > 1) + if (MapControlManager.agv.MovementHistory.Any() && MapControlManager.agv.MovementHistory.Count > 1) { - var prept = agv.MovementHistory.Skip(agv.MovementHistory.Count - 2).First(); - var lstpt = agv.MovementHistory.Last(); + var prept = MapControlManager.agv.MovementHistory.Skip(MapControlManager.agv.MovementHistory.Count - 2).First(); + var lstpt = MapControlManager.agv.MovementHistory.Last(); RFIDPoint TargetPT = null; @@ -1240,7 +1091,7 @@ namespace AGVControl else //앞으로이동한다면 이동방향과 동일하다 { //이전위치는 제거 하고 처음발견된 것을 대상으로 한다 - TargetPT = this.GetNeighbors(lstpt).Where(t => t.Value != prept.Value).FirstOrDefault(); + TargetPT = MapControlManager.GetNeighbors(lstpt).Where(t => t.Value != prept.Value).FirstOrDefault(); } if (TargetPT != null) @@ -1252,7 +1103,7 @@ namespace AGVControl var liftRect = new RectangleF(pt.X - circleRadius, pt.Y - circleRadius, circleRadius * 2, circleRadius * 2); g.FillEllipse(Brushes.Black, liftRect); - var liftColor = agv.IsTargetDirectionMatch ? Color.White : Color.HotPink; + var liftColor = MapControlManager.agv.IsTargetDirectionMatch ? Color.White : Color.HotPink; using (var pBorder = new Pen(liftColor, 3)) g.DrawEllipse(pBorder, liftRect); } @@ -1262,14 +1113,14 @@ namespace AGVControl // --- BodyAngle이 결정되지 않은 경우: 기본 방향으로 그림 --- - Color bgcolor = agv.BatteryLevel > 80 ? Color.Lime : (agv.BatteryLevel > 60 ? Color.Gold : Color.Tomato); + Color bgcolor = MapControlManager.agv.BatteryLevel > 80 ? Color.Lime : (MapControlManager.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, 4)) g.DrawEllipse(circlePen, circleRect); //motor direction - var str = agv.CurrentMOTDirection.ToString().Substring(0, 1); + var str = MapControlManager.agv.CurrentMOTDirection.ToString().Substring(0, 1); var strsize = g.MeasureString(str, this.Font); g.DrawString(str, this.Font, Brushes.White, circleRect, new StringFormat { @@ -1278,7 +1129,7 @@ namespace AGVControl }); //body direction - str = agv.IsTargetDirectionMatch ? "O" : "X";// .CurrentAGVDirection.ToString().Substring(0, 1).ToUpper(); + str = MapControlManager.agv.IsTargetDirectionMatch ? "O" : "X";// .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); @@ -1310,24 +1161,24 @@ namespace AGVControl // 과거 이동 경로를 화살표로 표시 private void DrawMovementHistoryArrows(Graphics g) { - if (agv.MovementHistory.Count < 2) + if (MapControlManager.agv.MovementHistory.Count < 2) return; // 최근 3개의 이동 경로 표시 (가장 오래된 것부터) - int startIndex = Math.Max(0, agv.MovementHistory.Count - 3); + int startIndex = Math.Max(0, MapControlManager.agv.MovementHistory.Count - 3); - for (int i = startIndex; i < agv.MovementHistory.Count - 1; i++) + for (int i = startIndex; i < MapControlManager.agv.MovementHistory.Count - 1; i++) { - var startRFID = agv.MovementHistory[i]; - var endRFID = agv.MovementHistory[i + 1]; + var startRFID = MapControlManager.agv.MovementHistory[i]; + var endRFID = MapControlManager.agv.MovementHistory[i + 1]; //var startPos = agv.PositionHistory[i]; //var endPos = agv.PositionHistory[i + 1]; // 시간에 따른 투명도 계산 - int age = agv.MovementHistory.Count - 1 - i; + int age = MapControlManager.agv.MovementHistory.Count - 1 - i; int alpha = Math.Max(50, 255 - (age * 50)); - var directConnection = rfidConnections.FirstOrDefault(c => + var directConnection = MapControlManager.rfidConnections.FirstOrDefault(c => (c.P1.Value == startRFID.Value && c.P2.Value == endRFID.Value) || (c.P1.Value == endRFID.Value && c.P2.Value == startRFID.Value)); @@ -1341,16 +1192,16 @@ namespace AGVControl else { // 직접 연결되지 않은 경우: 경로 탐색 후 점선 화살표 체인 - var pathResult = CalculatePath(startRFID.Value, endRFID.Value); + var pathResult = MapControlManager.CalculatePath(startRFID, endRFID, false); if (pathResult.Success && pathResult.Path != null && pathResult.Path.Count > 1) { // 경로의 첫 단계 방향으로 전체 색상 결정 Color arrowColor = Color.Gray; var firstStepEndPoint = pathResult.Path[1]; - var firstStepEndRfidPoint = RFIDPoints.FirstOrDefault(p => p.Location == firstStepEndPoint.Location); + var firstStepEndRfidPoint = MapControlManager.RFIDPoints.FirstOrDefault(p => p.Location == firstStepEndPoint.Location); if (firstStepEndRfidPoint != null) { - var firstStepConnection = rfidConnections.FirstOrDefault(c => + var firstStepConnection = MapControlManager.rfidConnections.FirstOrDefault(c => (c.P1.Value == startRFID.Value && c.P2.Value == firstStepEndRfidPoint.Value) || (c.P1.Value == firstStepEndRfidPoint.Value && c.P2.Value == startRFID.Value)); @@ -1524,11 +1375,11 @@ namespace AGVControl //연결정보에서 그림을 그린다. var didx = 0; - foreach (var connection in rfidConnections) + foreach (var connection in MapControlManager.rfidConnections) { didx += 1; - var startPoint = RFIDPoints.FirstOrDefault(p => p.Value == connection.P1.Value)?.Location ?? Point.Empty; - var endPoint = RFIDPoints.FirstOrDefault(p => p.Value == connection.P2.Value)?.Location ?? Point.Empty; + var startPoint = MapControlManager.RFIDPoints.FirstOrDefault(p => p.Value == connection.P1.Value)?.Location ?? Point.Empty; + var endPoint = MapControlManager.RFIDPoints.FirstOrDefault(p => p.Value == connection.P2.Value)?.Location ?? Point.Empty; if (startPoint.IsEmpty || endPoint.IsEmpty) continue; @@ -1554,7 +1405,7 @@ namespace AGVControl private void DrawPath(Graphics g) { - if (agv.MainPath == null || agv.MainPath.Count < 2) + if (MapControlManager.agv.MainPath == null || MapControlManager.agv.MainPath.Count < 2) return; Color pathColor = Color.FromArgb(100, Color.Lime); @@ -1563,16 +1414,16 @@ namespace AGVControl using (Pen pathPen = new Pen(pathColor, pathWidth)) { pathPen.DashStyle = System.Drawing.Drawing2D.DashStyle.Dash; - for (int i = 0; i < agv.MainPath.Count - 1; i++) + for (int i = 0; i < MapControlManager.agv.MainPath.Count - 1; i++) { - g.DrawLine(pathPen, agv.MainPath[i].Location, agv.MainPath[i + 1].Location); + g.DrawLine(pathPen, MapControlManager.agv.MainPath[i].Location, MapControlManager.agv.MainPath[i + 1].Location); } } } public List GetRFIDPoints() { - return RFIDPoints; + return MapControlManager.RFIDPoints; } public List GetRFIDLines() @@ -1593,7 +1444,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 MapControlManager.RFIDPoints) { if (rfid.Location == startPoint || rfid.Location == endPoint) continue; @@ -1631,7 +1482,8 @@ namespace AGVControl public void ClearMap() { - RFIDPoints.Clear(); + MapControlManager.RFIDPoints.Clear(); + mapTexts.Clear(); customLines.Clear(); rfidLines.Clear(); @@ -1658,7 +1510,7 @@ namespace AGVControl Value = rfidValue }; - RFIDPoints.Add(rfidPoint); + MapControlManager.RFIDPoints.Add(rfidPoint); this.Invalidate(); } @@ -1668,17 +1520,17 @@ namespace AGVControl // RFID 포인트 저장 lines.Add("[RFID_POINTS]"); - foreach (var point in RFIDPoints) + foreach (var point in MapControlManager.RFIDPoints) { 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) + foreach (var connection in MapControlManager.rfidConnections) { - var startPoint = RFIDPoints.First(p => p.Value == connection.P1.Value).Location; - var endPoint = RFIDPoints.First(p => p.Value == connection.P2.Value).Location; + var startPoint = MapControlManager.RFIDPoints.First(p => p.Value == connection.P1.Value).Location; + var endPoint = MapControlManager.RFIDPoints.First(p => p.Value == connection.P2.Value).Location; lines.Add($"{startPoint.X},{startPoint.Y},{endPoint.X},{endPoint.Y}," + $"{connection.P1},{connection.P2},{connection.DisableP1_to_P2}{connection.DisableP2_to_P1},{connection.Distance}"); } @@ -1732,7 +1584,7 @@ namespace AGVControl if (validX && validY && validN) { - if (RFIDPoints.Where(t => t.Value == valRfid).Any()) + if (MapControlManager.RFIDPoints.Where(t => t.Value == valRfid).Any()) { //이미존재한다 var newvalue = @@ -1761,7 +1613,7 @@ namespace AGVControl rfidPoint.IsTerminal = isTerminal; } - RFIDPoints.Add(rfidPoint); + MapControlManager.RFIDPoints.Add(rfidPoint); } else sb.AppendLine($"[{section}] {line}"); } @@ -1822,7 +1674,7 @@ namespace AGVControl private void ProcessRFIDConnections() { - rfidConnections.Clear(); + MapControlManager.rfidConnections.Clear(); var connectionSet = new HashSet(); foreach (var line in rfidLines) @@ -1831,7 +1683,7 @@ namespace AGVControl var end = line.EndPoint; // 1. 선 위의 모든 RFID 포인트(시작, 끝 포함)를 projectionRatio로 정렬 - var pointsOnThisLine = RFIDPoints + var pointsOnThisLine = MapControlManager.RFIDPoints .Where(p => IsPointOnLine(p.Location, start, end, 10f)) // 오차 허용치 넉넉히 .Select(p => new { @@ -1857,22 +1709,24 @@ namespace AGVControl var key = $"{Math.Min(from, to)}_{Math.Max(from, to)}"; if (connectionSet.Contains(key)) continue; - var fromItem = RFIDPoints.FirstOrDefault(p => p.Value == from); - var toItem = RFIDPoints.FirstOrDefault(p => p.Value == to); + var fromItem = MapControlManager.RFIDPoints.FirstOrDefault(p => p.Value == from); + var toItem = MapControlManager.RFIDPoints.FirstOrDefault(p => p.Value == to); var fromPt = fromItem?.Location ?? line.StartPoint; var toPt = toItem?.Location ?? line.EndPoint; - rfidConnections.Add(new RFIDConnection + MapControlManager.rfidConnections.Add(new RFIDConnection { P1 = fromItem, P2 = toItem, - Distance = GetDistance(fromPt, toPt) + Distance = MapControlManager.GetDistance(fromPt, toPt) }); connectionSet.Add(key); } } } + + // tolerance 인자를 받는 IsPointOnLine private bool IsPointOnLine(Point point, Point lineStart, Point lineEnd, float tolerance = 10.0f) { @@ -1950,7 +1804,7 @@ namespace AGVControl private void DrawTargetFlag(Graphics g) { //대상이없다면 진행하지 않습니다 - if (agv.TargetRFID.IsEmpty) return; + if (MapControlManager.agv.TargetRFID.IsEmpty) return; // 바닥에 흰색 원 그리기 using (var baseBrush = new SolidBrush(Color.Red)) @@ -1958,12 +1812,12 @@ namespace AGVControl { var baseSize = 8; g.FillEllipse(baseBrush, - agv.TargetRFID.Location.X - baseSize / 2, - agv.TargetRFID.Location.Y - baseSize / 2, + MapControlManager.agv.TargetRFID.Location.X - baseSize / 2, + MapControlManager.agv.TargetRFID.Location.Y - baseSize / 2, baseSize, baseSize); g.DrawEllipse(basePen, - agv.TargetRFID.Location.X - baseSize / 2, - agv.TargetRFID.Location.Y - baseSize / 2, + MapControlManager.agv.TargetRFID.Location.X - baseSize / 2, + MapControlManager.agv.TargetRFID.Location.Y - baseSize / 2, baseSize, baseSize); } @@ -1972,17 +1826,17 @@ namespace AGVControl { var poleLength = 27; // 40 * 2/3 ≈ 27 g.DrawLine(polePen, - agv.TargetRFID.Location.X, - agv.TargetRFID.Location.Y, - agv.TargetRFID.Location.X, - agv.TargetRFID.Location.Y - poleLength); + MapControlManager.agv.TargetRFID.Location.X, + MapControlManager.agv.TargetRFID.Location.Y, + MapControlManager.agv.TargetRFID.Location.X, + MapControlManager.agv.TargetRFID.Location.Y - poleLength); } // 깃발 그리기 Point[] flagPoints = new Point[3]; - 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); + flagPoints[0] = new Point(MapControlManager.agv.TargetRFID.Location.X, MapControlManager.agv.TargetRFID.Location.Y - 27); // 깃대 길이에 맞춤 + flagPoints[1] = new Point(MapControlManager.agv.TargetRFID.Location.X + 20, MapControlManager.agv.TargetRFID.Location.Y - 22); + flagPoints[2] = new Point(MapControlManager.agv.TargetRFID.Location.X, MapControlManager.agv.TargetRFID.Location.Y - 17); using (var flagBrush = new SolidBrush(Color.Red)) using (var flagPen = new Pen(Color.DarkRed, 1)) @@ -1992,339 +1846,26 @@ namespace AGVControl } } - /// - /// 목표지정으로 모터방향이 이동하고 있는가? - /// history 데이터가 있어야 하며 기준데이터가 없는 경우 null 반환 - /// - /// - public bool? IsMotDirection_To_Target() - { - if (agv.MovementHistory.Any() == false || agv.MovementHistory.Count < 2) return null; - if (agv.MainPath.Any() == false) return null; - var prept = agv.MovementHistory.Skip(agv.MovementHistory.Count - 2).First(); - var lstpt = agv.MovementHistory.Last(); - - //현재 이후의 경로를 가져온다 - var curidx = agv.MainPath.FindIndex(t => t.Value == lstpt.Value); - var preidx = agv.MainPath.FindIndex(t => t.Value == prept.Value); - if (curidx == -1 || preidx == -1) return null; - - //지정된경로 반대방향으로 이동하고 있다 - return preidx < curidx; - } - - /// - /// 리프트방향과 대상위치와의 방향이 일치하는가? - /// 목적지경로가 셋팅된 경우 현재 이동방향이 목적지방향과 일치하는가? - /// 이동경로정보가 없거나 목적지가 없으면 null 이 반환됨 - /// - /// - public bool? IsLiftDirectionMatch() - { - - if (agv.MovementHistory.Any() && agv.MovementHistory.Count > 1) - { - RFIDPoint TargetPT = null; - var prept = agv.MovementHistory.Skip(agv.MovementHistory.Count - 2).First(); - var lstpt = agv.MovementHistory.Last(); - - //뒤로이동하는경우라면 이전위치에 리프트가 있다. - if (lstpt.Direction == Direction.Backward) - { - TargetPT = prept; - } - else //앞으로 이동한다면 이동방향과 동일하다 - { - //이전위치는 제거 하고 처음발견된 것을 대상으로 한다 - TargetPT = this.GetNeighbors(lstpt).Where(t => t.Value != prept.Value).FirstOrDefault(); - } - - //목적지가 있다면 목적지의 방향과 일치하는지 확인해야한다 - //남은경로중에 방향이 고정된 핀이 있다면 그것과 일치하는지 확인해야 한다 - if (agv.MainPath.Any()) - { - //지정된경로 반대방향으로 이동하고 있다 - if ((IsMotDirection_To_Target() ?? false) == false) - { - return false; - } - else - { - var nextRoutes = agv.MainPath.Skip(agv.MainPath.FindIndex(t => t.Value == lstpt.Value) + 1).ToList(); - var DirectionMatch = true; - foreach (var item in nextRoutes) - { - if (item.FixedDirection != null && item.FixedDirection != lstpt.Direction) - { - DirectionMatch = false; - break; - } - } - return DirectionMatch; - } - } - else - { - //대상포인트와의 방향만 체크한다. - //고정대상이없다면 방향이 맞는것으로 한다 - return (TargetPT.FixedDirection ?? lstpt.Direction) == lstpt.Direction; - } - } - else - { - //이동된경로정보가 없다면 리프트 방향을 체크할 수 없으므로 대상과 위치가 맞지 않는걸로 기본값을 설정한다 - //이렇게 설정하면 대상으로 이동불가하고 뒤로 가도록 유도된다 - return null; - } - } - - public AGVActionPrediction PredictResult = null; - - //public static double Distance(this Point pt1, Point pt2) - //{ - // return Math.Sqrt(Math.Pow(pt1.X-pt2.X,2)+Math.Pow(pt1.Y-pt2.Y,2)); - //} - // AGV 다음행동 예측 함수 - public AGVActionPrediction PredictNextAction() - { - try - { - - - // 0. 설정경로와 리프트 방향 체크 (경로설정이 없을때에는 직선이동경로내의 방향들과 체크한다) - agv.IsTargetDirectionMatch = IsLiftDirectionMatch() ?? false; - - - - // 1. 위치를 모를 때 (CurrentRFID가 0 또는 미설정) - if (agv.CurrentRFID.Value == 0) - { - PredictResult = new AGVActionPrediction - { - Direction = Direction.Backward, - NextRFID = null, - Reason = "AGV 위치 미확정(처음 기동)", - ReasonCode = AGVActionReasonCode.NoPosition, - MoveState = AGVMoveState.Run - }; - return PredictResult; - } - - //2. 이동방향을 모른다 - if (agv.MovementHistory.Any() == false || agv.MovementHistory.Count < 2) - { - PredictResult = new AGVActionPrediction - { - Direction = Direction.Backward, - NextRFID = null, - Reason = "AGV이동방향 알수없음", - ReasonCode = AGVActionReasonCode.NoDirection, - MoveState = AGVMoveState.Run - }; - return PredictResult; - } - - - // 3. 경로가 없거나 현재 위치가 경로에 없음 - if ((agv.MainPath?.Count ?? 0) < 2) - { - PredictResult = new AGVActionPrediction - { - Direction = agv.CurrentMOTDirection, - NextRFID = null, - Reason = "경로 없음 또는 현재 위치 미확정", - ReasonCode = AGVActionReasonCode.NoPath, - MoveState = AGVMoveState.Stop - }; - return PredictResult; - } - - // 4. 경로상에서 다음 RFID 예측 - int idx = agv.MainPath.FindIndex(p => p.Value == agv.CurrentRFID.Value); - if (idx < 0) - { - PredictResult = new AGVActionPrediction - { - Direction = agv.CurrentMOTDirection, - NextRFID = null, - Reason = "현재 위치가 경로에 없음", - ReasonCode = AGVActionReasonCode.NotOnPath, - MoveState = AGVMoveState.Stop - }; - return PredictResult; - } - - - - // 4. 목적지 도달 전, 회전이 필요한경우인가? - // 목적지 RFID 정보 - var destRFID = agv.MainPath.Last(); - - //리프트 방향이 맞는가? - var IsLiftDir = IsLiftDirectionMatch() ?? false; - - //모션이동방향이 맞는가? - var IsMotDir = IsMotDirection_To_Target() ?? false; - - var PrePT = agv.MovementHistory.Skip(agv.MovementHistory.Count - 1).First(); - var curPT = agv.MovementHistory.Last(); - - - - //리프트방향이 맞지 않다면 회전가능한 위치로 이동을 해야한다 - if (IsLiftDir == false) - { - //회전가능한 위치로 이동을 해야한다 - - - - //1. 가까운 회전위치를 찾는다 - var nearTurnPoint = RFIDPoints.Where(t => t.IsRotatable)?.OrderBy(t => GetDistance(t.Location, agv.CurrentRFID.Location)).FirstOrDefault() ?? null; - if (nearTurnPoint == null) - { - return new AGVActionPrediction - { - Direction = agv.CurrentMOTDirection, - NextRFID = null, - Reason = "회전 가능한 위치가 없습니다", - ReasonCode = AGVActionReasonCode.NoTurnPoint, - MoveState = AGVMoveState.Stop - }; - } - - //2. 이동하기위한 경로계산 및 이동을 한다 (생성조건) - //2-1. 서브경로가없는경우 - //2-2. 시작과 종료번호가 다른 경우(경로가 변경이 되는 조건이다) - if (agv.SubPath.Any() == false || agv.SubPath.Count < 2 || - agv.SubPath.First().Value != PrePT.Value || - agv.SubPath.Last().Value != nearTurnPoint.Value) - { - var rlt = CalculatePath(PrePT, nearTurnPoint); //이전포인트도 추가를 해준다 - if (rlt.Success) agv.SubPath = rlt.Path; - else - { - agv.SubPath.Clear(); - return new AGVActionPrediction - { - Direction = agv.CurrentMOTDirection, - NextRFID = null, - Reason = "회전 위치까지의 경로를 계산할 수 없습니다", - ReasonCode = AGVActionReasonCode.PathCalcError, - MoveState = AGVMoveState.Stop - }; - } - } - - - //3. 턴위치까지 이동이 완료되지 않았다면 계속 이동을 하게한다 - if (agv.CurrentRFID.Value != nearTurnPoint.Value) - { - //현재 모터방향을 확인하여 대상까지 이동하도록 해야한다 - var curidx = agv.SubPath.FindIndex(t => t.Value == curPT.Value); - var preidx = agv.SubPath.FindIndex(t => t.Value == PrePT.Value); - Direction newdirection = agv.CurrentMOTDirection; - string message = "턴위치로 이동중"; - if (preidx > curidx) - { - //지정경로를 거꾸로 이동하고 있다 - if (agv.CurrentMOTDirection == Direction.Forward) - newdirection = Direction.Backward; - else - newdirection = Direction.Forward; - message += "(방향전환)"; - } - - return new AGVActionPrediction - { - Direction = newdirection, - NextRFID = null, - Reason = message, - ReasonCode = AGVActionReasonCode.MoveForTurn, - MoveState = AGVMoveState.Run, - }; - } - - - return new AGVActionPrediction - { - Direction = agv.CurrentMOTDirection, - NextRFID = null, - Reason = "턴 완료 대기", - ReasonCode = AGVActionReasonCode.NeedTurn, - MoveState = AGVMoveState.Stop - }; - } - - //리프트 방향이 맞다 - //모션방향이 맞지 않다면 이동방향을 변경해준다 - - //3. 턴위치까지 이동이 완료되지 않았다면 계속 이동을 하게한다 - if (agv.CurrentRFID.Value != destRFID.Value) - { - //현재 모터방향을 확인하여 대상까지 이동하도록 해야한다 - var curidx = agv.MainPath.FindIndex(t => t.Value == curPT.Value); - var preidx = agv.MainPath.FindIndex(t => t.Value == PrePT.Value); - Direction newdirection = agv.CurrentMOTDirection; - string message = "목적지 이동중"; - if (preidx > curidx) - { - //지정경로를 거꾸로 이동하고 있다 - if (agv.CurrentMOTDirection == Direction.Forward) - newdirection = Direction.Backward; - else - newdirection = Direction.Forward; - message += "(방향전환)"; - } - return new AGVActionPrediction - { - Direction = newdirection, - NextRFID = null, - Reason = message, - ReasonCode = AGVActionReasonCode.Normal, - MoveState = AGVMoveState.Run, - }; - } - - - // 5. 목적지 도달 시 - PredictResult = new AGVActionPrediction - { - Direction = agv.CurrentMOTDirection, - NextRFID = null, - Reason = "경로의 마지막 지점(목적지 도달)", - ReasonCode = AGVActionReasonCode.Arrived, - MoveState = AGVMoveState.Stop - }; - return PredictResult; - } - catch (Exception ex) - { - PredictResult = new AGVActionPrediction - { - Direction = agv.CurrentMOTDirection, - NextRFID = null, - Reason = $"ERR:{ex.Message}", - ReasonCode = AGVActionReasonCode.Unknown, - MoveState = AGVMoveState.Stop - }; - return PredictResult; - } - - } #endregion - #region 좌표 변환 및 계산 - public float GetDistance(Point p1, Point p2) + /// + /// 마지막으로 읽힌 연재위치에 해당하는 RFID 포인트값을 반환 합니다 + /// + public RFIDPoint GetCurrentPosition { - float dx = p1.X - p2.X; - float dy = p1.Y - p2.Y; - return (float)Math.Sqrt(dx * dx + dy * dy); // double을 float로 명시적 캐스팅 + get + { + return MapControlManager.agv.CurrentRFID; + } } + #region 좌표 변환 및 계산 + + // 화면 좌표를 실제 맵 좌표로 변환 public Point ScreenToMap(Point screenPoint) { @@ -2338,7 +1879,7 @@ namespace AGVControl private float GetProjectionRatio(Point point, Point lineStart, Point lineEnd) { - float lineLength = GetDistance(lineStart, lineEnd); + float lineLength = MapControlManager.GetDistance(lineStart, lineEnd); if (lineLength == 0) return 0; return ((point.X - lineStart.X) * (lineEnd.X - lineStart.X) + @@ -2348,8 +1889,8 @@ namespace AGVControl private float GetDistanceToLine(Point point, Point lineStart, Point lineEnd) { - float lineLength = GetDistance(lineStart, lineEnd); - if (lineLength == 0) return GetDistance(point, lineStart); + float lineLength = MapControlManager.GetDistance(lineStart, lineEnd); + if (lineLength == 0) return MapControlManager.GetDistance(point, lineStart); float t = ((point.X - lineStart.X) * (lineEnd.X - lineStart.X) + (point.Y - lineStart.Y) * (lineEnd.Y - lineStart.Y)) / (lineLength * lineLength); @@ -2359,7 +1900,7 @@ namespace AGVControl float projectionX = lineStart.X + t * (lineEnd.X - lineStart.X); float projectionY = lineStart.Y + t * (lineEnd.Y - lineStart.Y); - return GetDistance(point, new Point((int)projectionX, (int)projectionY)); + return MapControlManager.GetDistance(point, new Point((int)projectionX, (int)projectionY)); } #endregion } diff --git a/Cs_HMI/SubProject/AGVControl/MapControlManager.cs b/Cs_HMI/SubProject/AGVControl/MapControlManager.cs new file mode 100644 index 0000000..914f772 --- /dev/null +++ b/Cs_HMI/SubProject/AGVControl/MapControlManager.cs @@ -0,0 +1,504 @@ +using AGVControl.Models; +using System; +using System.Collections.Generic; +using System.Drawing; +using System.Linq; +using System.Threading; + +namespace AGVControl +{ + public static class MapControlManager + { + public static AGVActionPrediction PredictResult = null; + public static AGV agv = new AGV(); + public static List RFIDPoints = new List(); + public static HashSet rfidConnections = new HashSet(); + + private static ManualResetEvent mrepredict = new ManualResetEvent(true); + + /// + /// 목표지정으로 모터방향이 이동하고 있는가? + /// history 데이터가 있어야 하며 기준데이터가 없는 경우 null 반환 + /// + /// + public static bool? IsMotDirection_To_Target() + { + if (agv.MovementHistory.Any() == false || agv.MovementHistory.Count < 2) return null; + if (agv.MainPath.Any() == false) return null; + + + var prept = agv.MovementHistory.Skip(agv.MovementHistory.Count - 2).First(); + var lstpt = agv.MovementHistory.Last(); + + //현재 이후의 경로를 가져온다 + var curidx = agv.MainPath.FindIndex(t => t.Value == lstpt.Value); + var preidx = agv.MainPath.FindIndex(t => t.Value == prept.Value); + if (curidx == -1 || preidx == -1) return null; + + //지정된경로 반대방향으로 이동하고 있다 + return preidx < curidx; + } + public static float GetDistance(Point p1, Point p2) + { + float dx = p1.X - p2.X; + float dy = p1.Y - p2.Y; + return (float)Math.Sqrt(dx * dx + dy * dy); // double을 float로 명시적 캐스팅 + } + + public static AGVActionPrediction PredictNextAction() + { + if (mrepredict.WaitOne(1) == false) + { + PredictResult = new AGVActionPrediction + { + Reason = "이전 작업이 완료되지 않았습니다", + ReasonCode = AGVActionReasonCode.busy, + MoveState = AGVMoveState.Stop, + Direction = agv.CurrentMOTDirection, + }; + return PredictResult; + } + + mrepredict.Reset(); + try + { + // 0. 설정경로와 리프트 방향 체크 (경로설정이 없을때에는 직선이동경로내의 방향들과 체크한다) + agv.IsTargetDirectionMatch = IsLiftDirectionMatch() ?? false; + + // 1. 위치를 모를 때 (CurrentRFID가 0 또는 미설정) + if (agv.CurrentRFID.Value == 0) + { + PredictResult = new AGVActionPrediction + { + Direction = Direction.Backward, + NextRFID = null, + Reason = "AGV 위치 미확정(처음 기동)", + ReasonCode = AGVActionReasonCode.NoPosition, + MoveState = AGVMoveState.Run + }; + return PredictResult; + } + + //2. 이동방향을 모른다 + if (agv.MovementHistory.Any() == false || agv.MovementHistory.Count < 2) + { + PredictResult = new AGVActionPrediction + { + Direction = Direction.Backward, + NextRFID = null, + Reason = "AGV이동방향 알수없음", + ReasonCode = AGVActionReasonCode.NoDirection, + MoveState = AGVMoveState.Run + }; + return PredictResult; + } + + + // 3. 경로가 없거나 현재 위치가 경로에 없음 + if ((agv.MainPath?.Count ?? 0) < 2) + { + PredictResult = new AGVActionPrediction + { + Direction = agv.CurrentMOTDirection, + NextRFID = null, + Reason = "경로 없음 또는 현재 위치 미확정", + ReasonCode = AGVActionReasonCode.NoPath, + MoveState = AGVMoveState.Stop + }; + return PredictResult; + } + + // 4. 경로상에서 다음 RFID 예측 + int idx = agv.MainPath.FindIndex(p => p.Value == agv.CurrentRFID.Value); + if (idx < 0) + { + PredictResult = new AGVActionPrediction + { + Direction = agv.CurrentMOTDirection, + NextRFID = null, + Reason = "현재 위치가 경로에 없음", + ReasonCode = AGVActionReasonCode.NotOnPath, + MoveState = AGVMoveState.Stop + }; + return PredictResult; + } + + + + // 4. 목적지 도달 전, 회전이 필요한경우인가? + // 목적지 RFID 정보 + var destRFID = agv.MainPath.Last(); + + //리프트 방향이 맞는가? + var IsLiftDir = IsLiftDirectionMatch() ?? false; + + //모션이동방향이 맞는가? + var IsMotDir = IsMotDirection_To_Target() ?? false; + + var PrePT = agv.MovementHistory.Skip(agv.MovementHistory.Count - 1).First(); + var curPT = agv.MovementHistory.Last(); + + + + //리프트방향이 맞지 않다면 회전가능한 위치로 이동을 해야한다 + if (IsLiftDir == false) + { + //회전가능한 위치로 이동을 해야한다 + + + + //1. 가까운 회전위치를 찾는다 + var nearTurnPoint = RFIDPoints.Where(t => t.IsRotatable)?.OrderBy(t => GetDistance(t.Location, agv.CurrentRFID.Location)).FirstOrDefault() ?? null; + if (nearTurnPoint == null) + { + PredictResult= new AGVActionPrediction + { + Direction = agv.CurrentMOTDirection, + NextRFID = null, + Reason = "회전 가능한 위치가 없습니다", + ReasonCode = AGVActionReasonCode.NoTurnPoint, + MoveState = AGVMoveState.Stop + }; + return PredictResult; + } + + //2. 이동하기위한 경로계산 및 이동을 한다 (생성조건) + //2-1. 서브경로가없는경우 + //2-2. 시작과 종료번호가 다른 경우(경로가 변경이 되는 조건이다) + if (agv.CurrentRFID.Value != nearTurnPoint.Value) + { + if (agv.SubPath.Any() == false || agv.SubPath.Count < 2 || + agv.SubPath.First().Value != PrePT.Value || + agv.SubPath.Last().Value != nearTurnPoint.Value) + { + var rlt = CalculatePath(PrePT, nearTurnPoint, true); //이전포인트도 추가를 해준다 + if (rlt.Success) agv.SubPath = rlt.Path; + else + { + agv.SubPath.Clear(); + PredictResult= new AGVActionPrediction + { + Direction = agv.CurrentMOTDirection, + NextRFID = nearTurnPoint, + Reason = "회전 위치까지의 경로를 계산할 수 없습니다", + ReasonCode = AGVActionReasonCode.PathCalcError, + MoveState = AGVMoveState.Stop + }; + return PredictResult; + } + } + + + //현재 모터방향을 확인하여 대상까지 이동하도록 해야한다 + var curidx = agv.SubPath.FindIndex(t => t.Value == curPT.Value); + var preidx = agv.SubPath.FindIndex(t => t.Value == PrePT.Value); + Direction newdirection = agv.CurrentMOTDirection; + string message = "턴위치로 이동중"; + if (preidx > curidx) + { + //지정경로를 거꾸로 이동하고 있다 + if (agv.CurrentMOTDirection == Direction.Forward) + newdirection = Direction.Backward; + else + newdirection = Direction.Forward; + message += "(방향전환)"; + } + + PredictResult= new AGVActionPrediction + { + Direction = newdirection, + NextRFID = nearTurnPoint, + Reason = message, + ReasonCode = AGVActionReasonCode.MoveForTurn, + MoveState = AGVMoveState.Run, + }; + return PredictResult; + } + + + PredictResult = new AGVActionPrediction + { + Direction = agv.CurrentMOTDirection, + NextRFID = nearTurnPoint, + Reason = "턴 완료 대기", + ReasonCode = AGVActionReasonCode.NeedTurn, + MoveState = AGVMoveState.Stop + }; + return PredictResult; + } + + //3. 목적지위치까지 이동이 완료되지 않았다면 계속 이동을 하게한다 + if (agv.CurrentRFID.Value != destRFID.Value) + { + //현재 모터방향을 확인하여 대상까지 이동하도록 해야한다 + var curidx = agv.MainPath.FindIndex(t => t.Value == curPT.Value); + var preidx = agv.MainPath.FindIndex(t => t.Value == PrePT.Value); + Direction newdirection = agv.CurrentMOTDirection; + string message = "목적지 이동중"; + if (preidx > curidx) + { + //지정경로를 거꾸로 이동하고 있다 + if (agv.CurrentMOTDirection == Direction.Forward) + newdirection = Direction.Backward; + else + newdirection = Direction.Forward; + message += "(방향전환)"; + } + + //경로상 바로 다음 위치를 확인한다 + var nexstRFID = agv.MainPath.Skip(agv.MainPath.FindIndex(t => t.Value == curPT.Value) + 1).First(); + PredictResult = new AGVActionPrediction + { + Direction = newdirection, + NextRFID = nexstRFID, + Reason = message, + ReasonCode = AGVActionReasonCode.Normal, + MoveState = AGVMoveState.Run, + }; + return PredictResult; + } + + + // 5. 목적지 도달 시 + PredictResult = new AGVActionPrediction + { + Direction = agv.CurrentMOTDirection, + NextRFID = destRFID, + Reason = "경로의 마지막 지점(목적지 도달)", + ReasonCode = AGVActionReasonCode.Arrived, + MoveState = AGVMoveState.Stop + }; + return PredictResult; + } + catch (Exception ex) + { + PredictResult = new AGVActionPrediction + { + Direction = agv.CurrentMOTDirection, + NextRFID = null, + Reason = $"ERR:{ex.Message}", + ReasonCode = AGVActionReasonCode.Unknown, + MoveState = AGVMoveState.Stop + }; + return PredictResult; + } + finally + { + mrepredict.Set(); + } + + } + + + /// + /// 이웃포인터를 반환합니다 + /// + /// + /// + public static List GetNeighbors(RFIDPoint pt) + { + var neighbors = new List(); + + //값이 없는 경우 오류 반환 + if (pt == null) return neighbors; + + //연결정보에서 데이터를 찾은 후 반환한다 + foreach (var connection in rfidConnections) + { + RFIDPoint nPT = null; + if (connection.P1.Value == pt.Value) + { + nPT = connection.P2; + } + else if (connection.P2.Value == pt.Value) + { + nPT = connection.P1; + } + + if (nPT != null) neighbors.Add(nPT); + } + + //중복제거후 반한 + return neighbors.Distinct().ToList(); + } + + public static RFIDPoint FindRFIDPoint(uint rfidValue) + { + if (RFIDPoints == null || RFIDPoints.Any() == false) return null; + return RFIDPoints.FirstOrDefault(r => r.Value == rfidValue); + } + private static float Heuristic(Point a, Point b) + { + return (float)Math.Sqrt(Math.Pow(a.X - b.X, 2) + Math.Pow(a.Y - b.Y, 2)); + } + + private static PathResult ReconstructPath(Dictionary cameFrom, RFIDPoint current) + { + var path = new List { current }; + while (cameFrom.ContainsKey(current.Value)) + { + current = cameFrom[current.Value]; + if (path.Contains(current) == false) + path.Insert(0, current); + else + break; //왜인지 반복되는경우가있어 종료한다 + } + return new PathResult + { + Path = path, + }; + } + public static PathResult CalculatePath(RFIDPoint start, RFIDPoint end, bool autorun) + { + 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) } }; + + + + if (start.Value == end.Value) + { + return new PathResult + { + Message = "시작위치와 대상위치가 동일 합니다", + Path = null, + }; + } + + //if (autorun) openList.Clear(); + + + while (openList.Count > 0) + { + var current = openList.OrderBy(p => fScore.ContainsKey(p) ? fScore[p] : float.MaxValue).First(); + + if (current.Value == end.Value) + { + return ReconstructPath(cameFrom, current); + } + + openList.Remove(current); + closedList.Add(current); + + var nb = GetNeighbors(current); + foreach (var neighbor in nb) + { + if (closedList.Contains(neighbor)) + continue; + + float tentativeGScore = gScore[current] + GetDistance(current.Location, neighbor.Location); + + if (!openList.Contains(neighbor)) + openList.Add(neighbor); + else if (tentativeGScore >= gScore[neighbor]) + continue; + + cameFrom[neighbor.Value] = (RFIDPoint)current; + gScore[neighbor] = tentativeGScore; + fScore[neighbor] = gScore[neighbor] + Heuristic(neighbor.Location, end.Location); + + + } + + } + + return new PathResult + { + Path = openList, + }; + + } + public static PathResult CalculatePath(uint tagStrt, uint tagEnd) + { + var retval = new PathResult + { + Message = string.Empty, + Path = new List(), + }; + + var startPoint = FindRFIDPoint(tagStrt); + var endPoint = FindRFIDPoint(tagEnd); + if (startPoint == null || endPoint == null) + { + retval.Message = "유효한 RFID 값을 입력해주세요."; + return retval; + } + + retval = CalculatePath(startPoint, endPoint, false); + if (retval.Success == false) + retval.Message = "경로를 찾을 수 없습니다"; + + return retval; + } + + /// + /// 리프트방향과 대상위치와의 방향이 일치하는가? + /// 목적지경로가 셋팅된 경우 현재 이동방향이 목적지방향과 일치하는가? + /// 이동경로정보가 없거나 목적지가 없으면 null 이 반환됨 + /// + /// + public static bool? IsLiftDirectionMatch() + { + + if (agv.MovementHistory.Any() && agv.MovementHistory.Count > 1) + { + RFIDPoint TargetPT = null; + var prept = agv.MovementHistory.Skip(agv.MovementHistory.Count - 2).First(); + var lstpt = agv.MovementHistory.Last(); + + //뒤로이동하는경우라면 이전위치에 리프트가 있다. + if (lstpt.Direction == Direction.Backward) + { + TargetPT = prept; + } + else //앞으로 이동한다면 이동방향과 동일하다 + { + //이전위치는 제거 하고 처음발견된 것을 대상으로 한다 + TargetPT = GetNeighbors(lstpt).Where(t => t.Value != prept.Value).FirstOrDefault(); + } + + //목적지가 있다면 목적지의 방향과 일치하는지 확인해야한다 + //남은경로중에 방향이 고정된 핀이 있다면 그것과 일치하는지 확인해야 한다 + if (agv.MainPath.Any()) + { + //지정된경로 반대방향으로 이동하고 있다 + if ((IsMotDirection_To_Target() ?? false) == false) + { + return false; + } + else + { + var nextRoutes = agv.MainPath.Skip(agv.MainPath.FindIndex(t => t.Value == lstpt.Value) + 1).ToList(); + var DirectionMatch = true; + foreach (var item in nextRoutes) + { + if (item.FixedDirection != null && item.FixedDirection != lstpt.Direction) + { + DirectionMatch = false; + break; + } + } + return DirectionMatch; + } + } + else + { + //대상포인트와의 방향만 체크한다. + //고정대상이없다면 방향이 맞는것으로 한다 + return (TargetPT.FixedDirection ?? lstpt.Direction) == lstpt.Direction; + } + } + else + { + //이동된경로정보가 없다면 리프트 방향을 체크할 수 없으므로 대상과 위치가 맞지 않는걸로 기본값을 설정한다 + //이렇게 설정하면 대상으로 이동불가하고 뒤로 가도록 유도된다 + return null; + } + } + + + + } +} diff --git a/Cs_HMI/SubProject/AGVControl/Models/AGVActionPrediction.cs b/Cs_HMI/SubProject/AGVControl/Models/AGVActionPrediction.cs index ca00e9e..1baf77b 100644 --- a/Cs_HMI/SubProject/AGVControl/Models/AGVActionPrediction.cs +++ b/Cs_HMI/SubProject/AGVControl/Models/AGVActionPrediction.cs @@ -5,7 +5,7 @@ namespace AGVControl public class AGVActionPrediction { public Direction Direction { get; set; } - public uint? NextRFID { get; set; } + public RFIDPoint NextRFID { get; set; } public string Reason { get; set; } public AGVActionReasonCode ReasonCode { get; set; } public AGVMoveState MoveState { get; set; } // RUN 또는 STOP diff --git a/Cs_HMI/SubProject/AGVControl/Models/enumStruct.cs b/Cs_HMI/SubProject/AGVControl/Models/enumStruct.cs index 97c14d8..d718263 100644 --- a/Cs_HMI/SubProject/AGVControl/Models/enumStruct.cs +++ b/Cs_HMI/SubProject/AGVControl/Models/enumStruct.cs @@ -26,5 +26,6 @@ namespace AGVControl PathCalcError, NoDirection, MoveForTurn, + busy, } } \ No newline at end of file diff --git a/Cs_HMI/SubProject/AGVControl/agvControl.csproj b/Cs_HMI/SubProject/AGVControl/agvControl.csproj index c7300d7..5cd191e 100644 --- a/Cs_HMI/SubProject/AGVControl/agvControl.csproj +++ b/Cs_HMI/SubProject/AGVControl/agvControl.csproj @@ -45,6 +45,7 @@ + Component