Amkor/ENIG
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

..

Browse Source
This commit is contained in:
chi
2025-06-20 16:37:22 +09:00
parent f7615396d5
commit 3fcbbfe354
9 changed files with 450 additions and 210 deletions
Download Patch File Download Diff File Expand all files Collapse all files

471
Cs_HMI/SubProject/AGVControl/MapControl.cs
View File

@@ -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<Point> GetNeighbors(Point point)
{
var neighbors = new List<Point>();
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

59
Cs_HMI/SubProject/AGVControl/Models/AGV.cs
View File

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