diff --git a/Cs_HMI/Project/StateMachine/_AGV.cs b/Cs_HMI/Project/StateMachine/_AGV.cs index c7dc72b..1b72243 100644 --- a/Cs_HMI/Project/StateMachine/_AGV.cs +++ b/Cs_HMI/Project/StateMachine/_AGV.cs @@ -39,6 +39,9 @@ namespace Project bool _charging = false; private void AGV_DataReceive(object sender, arDev.Narumi.DataEventArgs e) { + if (PUB.mapctl != null) + PUB.mapctl.PredictNextAction(); + switch (e.DataType) { case arDev.Narumi.DataType.STS: @@ -54,7 +57,7 @@ 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') + if (PUB.AGV.data.Direction == 'B') PUB.mapctl.agv.CurrentDirection = AGVControl.Models.Direction.Backward; else PUB.mapctl.agv.CurrentDirection = AGVControl.Models.Direction.Forward; @@ -170,7 +173,7 @@ namespace Project else { //위치는 찾았다 해당 위치가 내 목적지라면 mark stop기능으로 전환한다 - + } diff --git a/Cs_HMI/Project/StateMachine/_Xbee.cs b/Cs_HMI/Project/StateMachine/_Xbee.cs index 56d0570..020f08a 100644 --- a/Cs_HMI/Project/StateMachine/_Xbee.cs +++ b/Cs_HMI/Project/StateMachine/_Xbee.cs @@ -39,7 +39,7 @@ namespace Project { if (PUB.setting.XBE_ID == tID) { - if (uint.TryParse(targstr, out uint tagno)) + if (ushort.TryParse(targstr, out ushort tagno)) { if (PUB.mapctl.SetCurrentPosition(tagno) == true) { diff --git a/Cs_HMI/Project/ViewForm/fAuto.Designer.cs b/Cs_HMI/Project/ViewForm/fAuto.Designer.cs index 1a918a8..efe2f3a 100644 --- a/Cs_HMI/Project/ViewForm/fAuto.Designer.cs +++ b/Cs_HMI/Project/ViewForm/fAuto.Designer.cs @@ -41,7 +41,6 @@ namespace Project.ViewForm // timer1 // timer1.Interval = 200; - timer1.Tick += timer1_Tick; // // ctlAuto1 // diff --git a/Cs_HMI/Project/ViewForm/fAuto.cs b/Cs_HMI/Project/ViewForm/fAuto.cs index a5d3634..5a86d7f 100644 --- a/Cs_HMI/Project/ViewForm/fAuto.cs +++ b/Cs_HMI/Project/ViewForm/fAuto.cs @@ -40,7 +40,7 @@ namespace Project.ViewForm // ctlAuto1.dev_plc = PUB.PLC; ctlAuto1.dev_bms = PUB.BMS; ctlAuto1.dev_xbe = PUB.XBE; - this.timer1.Start(); + PUB.AGV.DataReceive += AGV_DataReceive; @@ -67,6 +67,8 @@ namespace Project.ViewForm if (rlt == false) AR.UTIL.MsgE(errmsg); } } + + this.timer1.Start(); } private void AGV_DataReceive(object sender, arDev.Narumi.DataEventArgs e) { @@ -106,40 +108,44 @@ namespace Project.ViewForm } bool tmrun = false; - private void timer1_Tick(object sender, EventArgs e) - { - if (this.Visible == false) return; - if (tmrun == true) return; - tmrun = true; - - this.ctlAuto1.OnUpdateMode = true; - - if (this.ctlAuto1.Scean == CtlAuto.eScean.Progress) - { - ctlAuto1.ProgressVal = PUB.Result.SMSG_ProgressValue; - ctlAuto1.ProgressMax = PUB.Result.SMSG_ProgressMax; - ctlAuto1.StatusMessage = VAR.STR?.Get(eVarString.StatusMessage) ?? string.Empty; - } - this.ctlAuto1.StopMessage = string.Empty; - - if (PUB.sm.Step == StateMachine.eSMStep.RUN) - { - this.ctlAuto1.runStep = PUB.sm.RunStep; - } - else - { - this.ctlAuto1.runStep = ERunStep.READY; - } - this.ctlAuto1.OnUpdateMode = false; - this.ctlAuto1.Invalidate(); - tmrun = false; - } - + private void fAuto_VisibleChanged(object sender, EventArgs e) { this.timer1.Enabled = this.Visible; if (timer1.Enabled) timer1.Start(); else timer1.Stop(); } + + private void timer1_Tick_1(object sender, EventArgs e) + { + //if (this.Visible == false) return; + //if (tmrun == true) return; + //tmrun = true; + + //this.ctlAuto1.OnUpdateMode = true; + + //if (this.ctlAuto1.Scean == CtlAuto.eScean.Progress) + //{ + // ctlAuto1.ProgressVal = PUB.Result.SMSG_ProgressValue; + // ctlAuto1.ProgressMax = PUB.Result.SMSG_ProgressMax; + // ctlAuto1.StatusMessage = VAR.STR?.Get(eVarString.StatusMessage) ?? string.Empty; + //} + //this.ctlAuto1.StopMessage = string.Empty; + + //if (PUB.sm.Step == StateMachine.eSMStep.RUN) + //{ + // this.ctlAuto1.runStep = PUB.sm.RunStep; + //} + //else + //{ + // this.ctlAuto1.runStep = ERunStep.READY; + //} + //this.ctlAuto1.OnUpdateMode = false; + //this.ctlAuto1.Invalidate(); + + //PUB.mapctl.PredictNextAction(); + + //tmrun = false; + } } } diff --git a/Cs_HMI/Project/ViewForm/fAuto.resx b/Cs_HMI/Project/ViewForm/fAuto.resx index 5655bc4..aac33d5 100644 --- a/Cs_HMI/Project/ViewForm/fAuto.resx +++ b/Cs_HMI/Project/ViewForm/fAuto.resx @@ -1,17 +1,17 @@  - diff --git a/Cs_HMI/SubProject/AGV/Narumi.cs b/Cs_HMI/SubProject/AGV/Narumi.cs index 382fc92..86ee95c 100644 --- a/Cs_HMI/SubProject/AGV/Narumi.cs +++ b/Cs_HMI/SubProject/AGV/Narumi.cs @@ -388,7 +388,7 @@ namespace arDev { //221123 chi 숫자로변경 var tagnostr = rcvdNow.Substring(3); - if (uint.TryParse(tagnostr, out uint tagnoint)) + if (ushort.TryParse(tagnostr, out ushort tagnoint)) { var Changed = !old_TagString.Equals(tagnostr); data.TagString = tagnostr; diff --git a/Cs_HMI/SubProject/AGV/Structure/AgvData.cs b/Cs_HMI/SubProject/AGV/Structure/AgvData.cs index e0e3f9e..cc6df48 100644 --- a/Cs_HMI/SubProject/AGV/Structure/AgvData.cs +++ b/Cs_HMI/SubProject/AGV/Structure/AgvData.cs @@ -15,7 +15,7 @@ namespace arDev public string TagString { get; set; } = string.Empty; - public uint TagNo { get; set; } = 0; + public ushort TagNo { get; set; } = 0; public string CallString { get; set; } = string.Empty; public int CallNo { get; set; } = -1; public string CCAString { get; set; } = string.Empty; diff --git a/Cs_HMI/SubProject/AGVControl/MapControl.cs b/Cs_HMI/SubProject/AGVControl/MapControl.cs index 80b1150..57acb42 100644 --- a/Cs_HMI/SubProject/AGVControl/MapControl.cs +++ b/Cs_HMI/SubProject/AGVControl/MapControl.cs @@ -1,19 +1,14 @@ using System; using System.Collections.Generic; -using System.ComponentModel.Design; using System.Drawing; -using System.Drawing.Design; using System.Drawing.Drawing2D; using System.IO; using System.Linq; -using System.Net.NetworkInformation; -using System.Security.Cryptography; -using System.Security.Permissions; +using System.Security.Cryptography.X509Certificates; using System.Text.RegularExpressions; using System.Windows.Forms; using AGVControl.Models; using AR; -using static System.Net.Mime.MediaTypeNames; namespace AGVControl { @@ -242,7 +237,7 @@ namespace AGVControl var tag = AR.UTIL.InputBox("input rfid tag value"); if (tag.Item1 && tag.Item2 != "" && uint.TryParse(tag.Item2, out uint val) == true) { - var targetRFID = SetCurrentPosition(val); + var targetRFID = SetCurrentPosition((ushort)val); } break; case "save": @@ -285,8 +280,7 @@ namespace AGVControl if (od.ShowDialog() == DialogResult.OK) { - - this.LoadFromFile(filename, out string errmsg); + this.LoadFromFile(od.FileName, out string errmsg); if (errmsg.isEmpty() == false) UTIL.MsgE(errmsg); this.Invalidate(); } @@ -409,7 +403,7 @@ namespace AGVControl var selected_rfid = rfidPoints.Where(t => t.Bounds.Expand(SELECTION_DISTANCE, SELECTION_DISTANCE).Contains(mapPoint)).FirstOrDefault(); if (selected_rfid != null) { - UTIL.ShowPropertyDialog(selected_rfid); + UTIL.ShowPropertyDialog(selected_rfid); this.Invalidate(); return; } @@ -418,7 +412,7 @@ namespace AGVControl var selected_txt = mapTexts.Where(t => t.Bounds.Expand(SELECTION_DISTANCE, SELECTION_DISTANCE).Contains(mapPoint)).FirstOrDefault(); if (selected_txt != null) { - UTIL.ShowPropertyDialog(selected_txt); + UTIL.ShowPropertyDialog(selected_txt); this.Invalidate(); return; } @@ -729,21 +723,77 @@ namespace AGVControl return rfidPoints.FirstOrDefault(r => r.RFIDValue == rfidValue); } - public bool SetCurrentPosition(uint rfidValue) + public bool SetCurrentPosition(UInt16 rfidValue) { var rfidPoint = FindRFIDPoint(rfidValue); if (rfidPoint != null) { - // 이전 위치 저장 (방향 검증용) - Point previousPosition = agv.CurrentPosition; - uint previousRFID = agv.CurrentRFID; + // 이동 경로에 추가 (위치 업데이트보다 먼저) + agv.AddToMovementHistory(rfidValue, rfidPoint.Location, this.agv.CurrentDirection); // AGV 위치 업데이트 agv.CurrentPosition = rfidPoint.Location; agv.CurrentRFID = rfidValue; - // 이동 경로에 추가 - agv.AddToMovementHistory(rfidValue, rfidPoint.Location); + // --- 동체 방향(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);// + } + + } + + // 목적지가 설정되어 있고 경로가 있는 경우 검증 if (agv.TargetPosition != Point.Empty && agv.CurrentPath.Count > 0) @@ -761,15 +811,7 @@ namespace AGVControl } } - // AGV의 방향 결정 - if (agv.CurrentPath.Count > 0) - { - // 다음 목적지 찾기 - var nextPoint = agv.CurrentPath[0]; - // 회전 가능 여부를 고려하여 방향 결정 - agv.TargetDirection = DetermineDirection(agv.CurrentPosition, nextPoint, agv.TargetPosition); - } } // 목적지 RFID에 도착했고, 해당 RFID에 고정방향이 있으면 TargetDirection을 강제 설정 @@ -782,8 +824,8 @@ namespace AGVControl } } - // 방향 검증 및 정정 (이전 위치가 있고 경로가 있는 경우) - if (previousRFID != 0 && agv.CurrentPath.Count > 0) + // 방향 검증 및 정정 (세 번째 이동부터, BodyAngle이 결정된 후) + if (agv.BodyAngle.HasValue && agv.MovementHistory.Count > 2) { // RFID 연결 정보 기반 예상 방향 계산 Direction? expectedDirection = null; @@ -794,14 +836,14 @@ namespace AGVControl 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 => + 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) { // 연결된 경로이므로 방향 결정 @@ -906,37 +948,50 @@ namespace AGVControl private float Distance(Point a, Point b) { - // RFID 라인을 통한 연결 확인 - var rfidLines = GetRFIDLines(); - var directConnection = rfidLines.FirstOrDefault(line => - (line.StartPoint == a && line.EndPoint == b) || - (line.StartPoint == b && line.EndPoint == a)); + var rfidA = rfidPoints.FirstOrDefault(p => p.Location == a); + var rfidB = rfidPoints.FirstOrDefault(p => p.Location == b); - if (directConnection != null) + 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)); + + if (connection != null) { - return directConnection.Distance; + return connection.Distance; } - // 직접 연결되지 않은 경우 매우 큰 값 반환 return float.MaxValue; } private List GetNeighbors(Point point) { var neighbors = new List(); - var rfidLines = GetRFIDLines(); + var currentRfidPoint = rfidPoints.FirstOrDefault(p => p.Location == point); + if (currentRfidPoint == null) return neighbors; - // RFID 라인에서 이웃 노드 찾기 - foreach (var line in rfidLines) + uint currentRfid = currentRfidPoint.RFIDValue; + + foreach (var connection in rfidConnections) { - if (line.StartPoint == point) + uint neighborRfidVal = 0; + if (connection.StartRFID == currentRfid) { - neighbors.Add(line.EndPoint); + neighborRfidVal = connection.EndRFID; } - else if (line.EndPoint == point) + else if (connection.EndRFID == currentRfid && connection.IsBidirectional) { - // 양방향 연결인 경우에만 시작점을 이웃으로 추가 - neighbors.Add(line.StartPoint); + neighborRfidVal = connection.StartRFID; + } + + if (neighborRfidVal != 0) + { + var neighborRfidPoint = rfidPoints.FirstOrDefault(p => p.RFIDValue == neighborRfidVal); + if (neighborRfidPoint != null) + { + neighbors.Add(neighborRfidPoint.Location); + } } } @@ -1227,6 +1282,7 @@ namespace AGVControl DrawPath(e.Graphics); DrawAGV(e.Graphics); + DrawAGVMotor(e.Graphics); DrawTargetFlag(e.Graphics); // 목적지 깃발 그리기 추가 // 선택된 개체 강조 표시 @@ -1267,6 +1323,16 @@ namespace AGVControl // 툴바 버튼 그리기 foreach (var item in this.toolbarRects) DrawToolbarButton(e.Graphics, item.Bounds, item.Title, item.isHovering); + + //예측값디스플레잉(임시) + + if (PredictResult != null) + { + var str = $"{PredictResult.ReasonCode}|{PredictResult.MoveState}|{PredictResult.Direction}|Next:{PredictResult.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); + } + } private void DrawRFIDPoints(Graphics g) @@ -1277,7 +1343,7 @@ namespace AGVControl var MarkerSize = 5; var half = MarkerSize / 2f; rfid.Bounds = new RectangleF(rfid.Location.X - half, rfid.Location.Y - half, MarkerSize, MarkerSize); - + // 종단 RFID는 특별한 색상으로 표시 Color pointColor; if (rfid.IsTerminal) @@ -1292,37 +1358,37 @@ namespace AGVControl { pointColor = Color.Green; // 일반은 초록색 } - + using (var brush = new SolidBrush(pointColor)) { g.FillEllipse(brush, rfid.Bounds); } - + // 고정방향이 있으면 테두리 색상 표시 if (rfid.FixedDirection.HasValue) { Color borderColor = rfid.FixedDirection.Value == Direction.Forward ? Color.DeepSkyBlue : Color.Gold; using (var pen = new Pen(borderColor, 2)) { - g.DrawEllipse(pen, rfid.Bounds.Expand(5,5)); + g.DrawEllipse(pen, rfid.Bounds.Expand(5, 5)); } } - + // 종단 RFID는 특별한 테두리 표시 if (rfid.IsTerminal) { using (var pen = new Pen(Color.Red, 3)) { pen.DashStyle = System.Drawing.Drawing2D.DashStyle.Dash; - g.DrawEllipse(pen, rfid.Bounds.Expand(8,8)); + g.DrawEllipse(pen, rfid.Bounds.Expand(8, 8)); } } var str = rfid.RFIDValue.ToString(); - g.DrawString(str, this.Font, Brushes.DarkGray, rfid.Bounds.X, rfid.Bounds.Y+5); + g.DrawString(str, this.Font, Brushes.DarkGray, rfid.Bounds.X, rfid.Bounds.Y + 5); } - + } private void DrawAGV(Graphics g) @@ -1336,84 +1402,212 @@ namespace AGVControl agv.CurrentPosition.Y - halfsize, agvsize, agvsize); - // AGV 몸체 회전 각도 계산 - float bodyRotation = 0f; - if (agv.CurrentPath != null && agv.CurrentPath.Count > 1) + if (agv.BodyAngle.HasValue) { - // 현재 위치에서 다음 목적지 방향 계산 - int currentIdx = agv.CurrentPath.FindIndex(p => p == agv.CurrentPosition); - if (currentIdx >= 0 && currentIdx < agv.CurrentPath.Count - 1) - { - Point nextPoint = agv.CurrentPath[currentIdx + 1]; - float deltaX = nextPoint.X - agv.CurrentPosition.X; - float deltaY = nextPoint.Y - agv.CurrentPosition.Y; - bodyRotation = (float)Math.Atan2(deltaY, deltaX) * 180f / (float)Math.PI; - } + // --- BodyAngle이 결정된 경우: AGV를 회전시켜 그림 --- + var originalTransform = g.Transform; + g.TranslateTransform(agv.CurrentPosition.X, agv.CurrentPosition.Y); + g.RotateTransform(agv.BodyAngle.Value + 90); // 리프트가 위쪽(0, -1)을 기본으로 하므로 90도 보정 + + // 원 그리기 (회전된 좌표계 기준) + 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); + + // 리프트 그리기 (회전된 좌표계 기준) + 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); } - // 그래픽스 상태 저장 - var originalTransform = g.Transform; - - // AGV 위치로 이동하고 회전 + // 과거 이동 경로 화살표 그리기 + DrawMovementHistoryArrows(g); + } + + private void DrawAGVMotor(Graphics g) + { + 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(bodyRotation); + g.RotateTransform(motorAngle + 90); // 삼각형이 위쪽(Y축 음수)을 향하도록 90도 보정 - // 원 그리기 (회전된 상태) - 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); - - // 리프트 그리기 (회전된 상태, 항상 전진 방향쪽) - 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; - - // 삼각형 화살표 그리기 (현재 이동 방향, 회전하지 않음) + // 삼각형 포인트 계산 (회전 중심점 0,0 기준) Point[] trianglePoints = new Point[3]; var arrowSize = halfsize - 5; + trianglePoints[0] = new Point(0, -arrowSize); // 꼭짓점 + trianglePoints[1] = new Point(-arrowSize, arrowSize); // 왼쪽 아래 + trianglePoints[2] = new Point(arrowSize, arrowSize); // 오른쪽 아래 - // AGV의 현재 이동 방향에 따라 삼각형 포인트 계산 - switch (agv.CurrentDirection) - { - case Direction.Forward: - trianglePoints[0] = new Point(agv.CurrentPosition.X, agv.CurrentPosition.Y - arrowSize); - trianglePoints[1] = new Point(agv.CurrentPosition.X - arrowSize, agv.CurrentPosition.Y + arrowSize); - trianglePoints[2] = new Point(agv.CurrentPosition.X + arrowSize, agv.CurrentPosition.Y + arrowSize); - break; - case Direction.Backward: - trianglePoints[0] = new Point(agv.CurrentPosition.X, agv.CurrentPosition.Y + arrowSize); - trianglePoints[1] = new Point(agv.CurrentPosition.X - arrowSize, agv.CurrentPosition.Y - arrowSize); - trianglePoints[2] = new Point(agv.CurrentPosition.X + arrowSize, agv.CurrentPosition.Y - arrowSize); - break; - } - + // 삼각형 그리기 using (var arrowBrush = new SolidBrush(Color.FromArgb(200, Color.White))) g.FillPolygon(arrowBrush, trianglePoints); using (var arrowPen = new Pen(Color.Black, 2)) g.DrawPolygon(arrowPen, trianglePoints); - //g.DrawImage(Properties.Resources.ico_navi_40, circleRect); + g.Restore(gState); + } + + // 과거 이동 경로를 화살표로 표시 + private void DrawMovementHistoryArrows(Graphics g) + { + if (agv.MovementHistory.Count < 2 || agv.PositionHistory.Count < 2) + return; + + // 최근 3개의 이동 경로 표시 (가장 오래된 것부터) + int startIndex = Math.Max(0, agv.MovementHistory.Count - 3); + + for (int i = startIndex; i < agv.MovementHistory.Count - 1; i++) + { + var startRFID = agv.MovementHistory[i]; + var endRFID = agv.MovementHistory[i + 1]; + var startPos = agv.PositionHistory[i]; + var endPos = agv.PositionHistory[i + 1]; + + // 시간에 따른 투명도 계산 + int age = agv.MovementHistory.Count - 1 - i; + int alpha = Math.Max(50, 255 - (age * 50)); + + var directConnection = rfidConnections.FirstOrDefault(c => + (c.StartRFID == startRFID.rfid && c.EndRFID == endRFID.rfid) || + (c.IsBidirectional && c.StartRFID == endRFID.rfid && c.EndRFID == startRFID.rfid)); + + if (directConnection != null) + { + // 직접 연결된 경우: 실선 화살표 + Color arrowColor = (directConnection.StartRFID == startRFID.rfid) ? Color.Green : Color.Red; + arrowColor = Color.FromArgb(alpha, arrowColor); + DrawArrow(g, startPos, endPos, arrowColor, 3); + } + else + { + // 직접 연결되지 않은 경우: 경로 탐색 후 점선 화살표 체인 + var pathResult = CalculatePath(startRFID.rfid, endRFID.rfid); + 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); + 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)); + + if (firstStepConnection != null) + { + arrowColor = (firstStepConnection.StartRFID == startRFID.rfid) ? Color.Green : Color.Red; + } + } + + arrowColor = Color.FromArgb(alpha, arrowColor); + + // 경로의 각 세그먼트를 점선 화살표로 그리기 + for (int j = 0; j < pathResult.Path.Count - 1; j++) + { + Point segmentStart = pathResult.Path[j]; + Point segmentEnd = pathResult.Path[j + 1]; + DrawDashedArrow(g, segmentStart, segmentEnd, arrowColor, 3); + } + } + } + } + } + + // 점선 화살표 그리기 헬퍼 메서드 + private void DrawDashedArrow(Graphics g, Point start, Point end, Color color, int width) + { + using (var pen = new Pen(color, width)) + { + pen.DashStyle = System.Drawing.Drawing2D.DashStyle.Dash; + + // 선 그리기 + g.DrawLine(pen, start, end); + + // 화살표 머리 그리기 (실선으로) + pen.DashStyle = System.Drawing.Drawing2D.DashStyle.Solid; + var arrowSize = 8; + var angle = Math.Atan2(end.Y - start.Y, end.X - start.X); + var arrowAngle = Math.PI / 6; // 30도 + + // 화살표 끝점에서 약간 뒤로 이동 + var arrowStart = new PointF( + end.X - (float)(arrowSize * Math.Cos(angle)), + end.Y - (float)(arrowSize * Math.Sin(angle)) + ); + + // 화살표 날개 그리기 + var arrow1 = new PointF( + arrowStart.X - (float)(arrowSize * Math.Cos(angle - arrowAngle)), + arrowStart.Y - (float)(arrowSize * Math.Sin(angle - arrowAngle)) + ); + var arrow2 = new PointF( + arrowStart.X - (float)(arrowSize * Math.Cos(angle + arrowAngle)), + arrowStart.Y - (float)(arrowSize * Math.Sin(angle + arrowAngle)) + ); + + g.DrawLine(pen, arrowStart, arrow1); + g.DrawLine(pen, arrowStart, arrow2); + } + } + + // 실선 화살표 그리기 헬퍼 메서드 + private void DrawArrow(Graphics g, Point start, Point end, Color color, int width) + { + using (var pen = new Pen(color, width)) + { + // 선 그리기 + g.DrawLine(pen, start, end); + + // 화살표 머리 그리기 + var arrowSize = 8; + var angle = Math.Atan2(end.Y - start.Y, end.X - start.X); + var arrowAngle = Math.PI / 6; // 30도 + + // 화살표 끝점에서 약간 뒤로 이동 + var arrowStart = new PointF( + end.X - (float)(arrowSize * Math.Cos(angle)), + end.Y - (float)(arrowSize * Math.Sin(angle)) + ); + + // 화살표 날개 그리기 + var arrow1 = new PointF( + arrowStart.X - (float)(arrowSize * Math.Cos(angle - arrowAngle)), + arrowStart.Y - (float)(arrowSize * Math.Sin(angle - arrowAngle)) + ); + var arrow2 = new PointF( + arrowStart.X - (float)(arrowSize * Math.Cos(angle + arrowAngle)), + arrowStart.Y - (float)(arrowSize * Math.Sin(angle + arrowAngle)) + ); + + g.DrawLine(pen, arrowStart, arrow1); + g.DrawLine(pen, arrowStart, arrow2); + } } private void DrawCustomLines(Graphics g) @@ -1733,13 +1927,13 @@ namespace AGVControl var newvalue = sb.AppendLine($"rfid중복{valRfid}"); } - + var rfidPoint = new RFIDPoint { Location = new Point(valX, valY), RFIDValue = valRfid }; - + // 추가 속성 로드 (기본값 처리) if (rfidParts.Length >= 4) { @@ -1755,7 +1949,7 @@ namespace AGVControl bool.TryParse(rfidParts[5], out isTerminal); rfidPoint.IsTerminal = isTerminal; } - + rfidPoints.Add(rfidPoint); } else sb.AppendLine($"[{section}] {line}"); @@ -2016,13 +2210,14 @@ namespace AGVControl return agv.CurrentDirection; } + public AGVActionPrediction PredictResult = null; // AGV 행동 예측 함수 public AGVActionPrediction PredictNextAction() { // 1. 위치를 모를 때 (CurrentRFID가 0 또는 미설정) if (agv.CurrentRFID == 0) { - return new AGVActionPrediction + PredictResult = new AGVActionPrediction { Direction = Direction.Backward, NextRFID = null, @@ -2030,12 +2225,13 @@ namespace AGVControl ReasonCode = AGVActionReasonCode.NoPosition, MoveState = AGVMoveState.Run }; + return PredictResult; } // 2. 경로가 없거나 현재 위치가 경로에 없음 if (agv.CurrentPath == null || agv.CurrentPath.Count < 2 || agv.CurrentPosition == Point.Empty) { - return new AGVActionPrediction + PredictResult = new AGVActionPrediction { Direction = agv.CurrentDirection, NextRFID = null, @@ -2043,13 +2239,14 @@ namespace AGVControl ReasonCode = AGVActionReasonCode.NoPath, MoveState = AGVMoveState.Stop }; + return PredictResult; } // 3. 경로상에서 다음 RFID 예측 int idx = agv.CurrentPath.FindIndex(p => p == agv.CurrentPosition); if (idx < 0) { - return new AGVActionPrediction + PredictResult = new AGVActionPrediction { Direction = agv.CurrentDirection, NextRFID = null, @@ -2057,6 +2254,7 @@ namespace AGVControl ReasonCode = AGVActionReasonCode.NotOnPath, MoveState = AGVMoveState.Stop }; + return PredictResult; } // 4. 목적지 도달 전, 방향 미리 판단 및 회전 위치 예측 @@ -2072,7 +2270,7 @@ namespace AGVControl var beforeDest = agv.CurrentPath[agv.CurrentPath.Count - 2]; float arriveDeltaX = destPoint.X - beforeDest.X; float arriveDeltaY = destPoint.Y - beforeDest.Y; - Direction arriveDir = (Math.Abs(arriveDeltaX) > Math.Abs(arriveDeltaY)) ? + Direction arriveDir = (Math.Abs(arriveDeltaX) > Math.Abs(arriveDeltaY)) ? (arriveDeltaX > 0 ? Direction.Forward : Direction.Backward) : (arriveDeltaY > 0 ? Direction.Forward : Direction.Backward); if (arriveDir != destRFID.FixedDirection.Value) @@ -2091,7 +2289,7 @@ namespace AGVControl if (idx == lastRotatableIdx) { var rfid = rfidPoints.FirstOrDefault(r => r.Location == agv.CurrentPath[lastRotatableIdx]); - return new AGVActionPrediction + PredictResult = new AGVActionPrediction { Direction = agv.CurrentDirection, NextRFID = rfid?.RFIDValue, @@ -2099,6 +2297,7 @@ namespace AGVControl ReasonCode = AGVActionReasonCode.NeedTurn, MoveState = AGVMoveState.Stop }; + return PredictResult; } else if (idx < lastRotatableIdx) { @@ -2106,10 +2305,10 @@ namespace AGVControl 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; - Direction moveDir = (Math.Abs(moveDeltaX) > Math.Abs(moveDeltaY)) ? + Direction moveDir = (Math.Abs(moveDeltaX) > Math.Abs(moveDeltaY)) ? (moveDeltaX > 0 ? Direction.Forward : Direction.Backward) : (moveDeltaY > 0 ? Direction.Forward : Direction.Backward); - return new AGVActionPrediction + PredictResult = new AGVActionPrediction { Direction = moveDir, NextRFID = rfid?.RFIDValue, @@ -2117,10 +2316,11 @@ namespace AGVControl ReasonCode = AGVActionReasonCode.Normal, MoveState = AGVMoveState.Run }; + return PredictResult; } } // 회전 가능한 위치가 없음 (STOP) - return new AGVActionPrediction + PredictResult = new AGVActionPrediction { Direction = agv.CurrentDirection, NextRFID = null, @@ -2128,6 +2328,7 @@ namespace AGVControl ReasonCode = AGVActionReasonCode.NoTurnPoint, MoveState = AGVMoveState.Stop }; + return PredictResult; } } } @@ -2135,7 +2336,7 @@ namespace AGVControl // 5. 목적지 도달 시(방향이 맞는 경우) (STOP) if (idx == agv.CurrentPath.Count - 1) { - return new AGVActionPrediction + PredictResult = new AGVActionPrediction { Direction = agv.CurrentDirection, NextRFID = null, @@ -2143,20 +2344,21 @@ namespace AGVControl ReasonCode = AGVActionReasonCode.Arrived, MoveState = AGVMoveState.Stop }; + return PredictResult; } // 6. 일반 경로 주행 (RUN) Point nextPoint = agv.CurrentPath[idx + 1]; var nextRFID = rfidPoints.FirstOrDefault(r => r.Location == nextPoint)?.RFIDValue; - + // X, Y 좌표 모두 고려한 방향 판단 float deltaX = nextPoint.X - agv.CurrentPosition.X; float deltaY = nextPoint.Y - agv.CurrentPosition.Y; - Direction nextDir = (Math.Abs(deltaX) > Math.Abs(deltaY)) ? + Direction nextDir = (Math.Abs(deltaX) > Math.Abs(deltaY)) ? (deltaX > 0 ? Direction.Forward : Direction.Backward) : (deltaY > 0 ? Direction.Forward : Direction.Backward); - return new AGVActionPrediction + PredictResult = new AGVActionPrediction { Direction = nextDir, NextRFID = nextRFID, @@ -2164,6 +2366,7 @@ namespace AGVControl ReasonCode = AGVActionReasonCode.Normal, MoveState = AGVMoveState.Run }; + return PredictResult; } #endregion diff --git a/Cs_HMI/SubProject/AGVControl/Models/AGV.cs b/Cs_HMI/SubProject/AGVControl/Models/AGV.cs index 02fae10..63ee563 100644 --- a/Cs_HMI/SubProject/AGVControl/Models/AGV.cs +++ b/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 /// /// 현재위치가 수산되면 목적지까지의 방향값이 계산됩니다. /// - public Direction TargetDirection { get; set; } + public Direction TargetDirection { get; set; } = Direction.Stop; public bool IsMoving { get; set; } 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; set; } = new List(); - public List PositionHistory { get; set; } = new List(); + public List MovementHistory { get; } = new List(); + public List PositionHistory { get; } = new List(); 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 값이 감소하면 후진 }