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

7
Cs_HMI/Project/StateMachine/_AGV.cs
View File

@@ -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기능으로 전환한다
}

2
Cs_HMI/Project/StateMachine/_Xbee.cs
View File

@@ -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)
{

1
Cs_HMI/Project/ViewForm/fAuto.Designer.cs generated
View File

@@ -41,7 +41,6 @@ namespace Project.ViewForm
// timer1
//
timer1.Interval = 200;
timer1.Tick += timer1_Tick;
//
// ctlAuto1
//

66
Cs_HMI/Project/ViewForm/fAuto.cs
View File

@@ -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;
}
}
}

50
Cs_HMI/Project/ViewForm/fAuto.resx
View File

@@ -1,17 +1,17 @@
<?xml version="1.0" encoding="utf-8"?>
<root>
<!--
<!--
Microsoft ResX Schema
Version 2.0
The primary goals of this format is to allow a simple XML format
that is mostly human readable. The generation and parsing of the
various data types are done through the TypeConverter classes
The primary goals of this format is to allow a simple XML format
that is mostly human readable. The generation and parsing of the
various data types are done through the TypeConverter classes
associated with the data types.
Example:
... ado.net/XML headers & schema ...
<resheader name="resmimetype">text/microsoft-resx</resheader>
<resheader name="version">2.0</resheader>
@@ -26,36 +26,36 @@
<value>[base64 mime encoded string representing a byte array form of the .NET Framework object]</value>
<comment>This is a comment</comment>
</data>
There are any number of "resheader" rows that contain simple
There are any number of "resheader" rows that contain simple
name/value pairs.
Each data row contains a name, and value. The row also contains a
type or mimetype. Type corresponds to a .NET class that support
text/value conversion through the TypeConverter architecture.
Classes that don't support this are serialized and stored with the
Each data row contains a name, and value. The row also contains a
type or mimetype. Type corresponds to a .NET class that support
text/value conversion through the TypeConverter architecture.
Classes that don't support this are serialized and stored with the
mimetype set.
The mimetype is used for serialized objects, and tells the
ResXResourceReader how to depersist the object. This is currently not
The mimetype is used for serialized objects, and tells the
ResXResourceReader how to depersist the object. This is currently not
extensible. For a given mimetype the value must be set accordingly:
Note - application/x-microsoft.net.object.binary.base64 is the format
that the ResXResourceWriter will generate, however the reader can
Note - application/x-microsoft.net.object.binary.base64 is the format
that the ResXResourceWriter will generate, however the reader can
read any of the formats listed below.
mimetype: application/x-microsoft.net.object.binary.base64
value : The object must be serialized with
value : The object must be serialized with
: System.Runtime.Serialization.Formatters.Binary.BinaryFormatter
: and then encoded with base64 encoding.
mimetype: application/x-microsoft.net.object.soap.base64
value : The object must be serialized with
value : The object must be serialized with
: System.Runtime.Serialization.Formatters.Soap.SoapFormatter
: and then encoded with base64 encoding.
mimetype: application/x-microsoft.net.object.bytearray.base64
value : The object must be serialized into a byte array
value : The object must be serialized into a byte array
: using a System.ComponentModel.TypeConverter
: and then encoded with base64 encoding.
-->

2
Cs_HMI/SubProject/AGV/Narumi.cs
View File

@@ -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;

2
Cs_HMI/SubProject/AGV/Structure/AgvData.cs
View File

@@ -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;

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 값이 감소하면 후진
}