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-23 17:38:24 +09:00
parent 3fcbbfe354
commit ba18564719
5 changed files with 253 additions and 444 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -57,10 +57,11 @@ namespace Project
VAR.BOOL[eVarBool.AGV_ERROR] = PUB.AGV.error.Value > 0; VAR.BOOL[eVarBool.AGV_ERROR] = PUB.AGV.error.Value > 0;
VAR.BOOL[eVarBool.EMERGENCY] = PUB.AGV.error.Emergency; 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; PUB.mapctl.agv.CurrentMOTDirection = AGVControl.Models.Direction.Backward;
else else
PUB.mapctl.agv.CurrentDirection = AGVControl.Models.Direction.Forward; PUB.mapctl.agv.CurrentMOTDirection = AGVControl.Models.Direction.Forward;
if (PUB.AGV.signal.mark_sensor == false) if (PUB.AGV.signal.mark_sensor == false)
{ {

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

@@ -159,6 +159,8 @@ namespace Project
private void Bms_BMSDataReceive(object sender, EventArgs e) private void Bms_BMSDataReceive(object sender, EventArgs e)
{ {
PUB.mapctl.agv.BatteryLevel = PUB.BMS.Current_Level; PUB.mapctl.agv.BatteryLevel = PUB.BMS.Current_Level;
PUB.mapctl.agv.BatteryTemp1 = PUB.BMS.Current_temp1;
PUB.mapctl.agv.BatteryTemp2 = PUB.BMS.Current_temp2;
if (PUB.BMS.Current_Level <= PUB.setting.ChargeStartLevel) if (PUB.BMS.Current_Level <= PUB.setting.ChargeStartLevel)
{ {
//배터리 레벨이 기준보다 낮다면 경고를 활성화 한다 //배터리 레벨이 기준보다 낮다면 경고를 활성화 한다

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

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

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

@@ -2,6 +2,7 @@ using System;
using System.Drawing; using System.Drawing;
using System.Collections.Generic; using System.Collections.Generic;
using System.Linq; using System.Linq;
using System.Security.Permissions;
namespace AGVControl.Models namespace AGVControl.Models
{ {
@@ -12,64 +13,98 @@ namespace AGVControl.Models
Stop = 2 Stop = 2
} }
public struct movehistorydata public class CRFIDData
{ {
public UInt16 rfid { get; set; } public UInt16 rfid { get; set; }
public Point Position { get; set; }
public override string ToString()
{
return $"RFID:{rfid},P:{Position.X},{Position.Y}";
}
}
public class movehistorydata : CRFIDData
{
public Direction direction { get; set; } public Direction direction { get; set; }
public override string ToString() public override string ToString()
{ {
return $"RFID:{rfid},DIR:{direction}"; return $"RFID:{rfid},DIR:{direction},P:{Position.X},{Position.Y}";
} }
} }
public class AGV public class AGV
{ {
public Point CurrentPosition { get; set; }
public uint CurrentRFID { get; set; }
public float BatteryLevel { get; set; } = 0f;
/// <summary> /// <summary>
/// AGV에서 방향값이 수산됩니다. /// RFID 번호
/// </summary> /// </summary>
public Direction CurrentDirection { get; set; } public RFIDPoint CurrentRFID { get; set; }
/// <summary>
/// 목적지가 셋팅된경우 해당 값
/// </summary>
public RFIDPoint TargetRFID { get; set; }
/// <summary>
/// 배터리잔량(%)
/// </summary>
public float BatteryLevel { get; set; } = 0f;
/// <summary>
/// 배터리온도(board)
/// </summary>
public double BatteryTemp1 { get; set; } = 0;
/// <summary>
/// 배터리온도(cell)
/// </summary>
public double BatteryTemp2 { get; set; } = 0;
/// <summary>
/// AGV
/// </summary>
public Direction CurrentAGVDirection { get; set; }
/// <summary>
/// AGV모터 방향
/// 외부에서 값이 상시 업데이트 됩니다.
/// </summary>
public Direction CurrentMOTDirection { get; set; }
/// <summary> /// <summary>
/// 현재위치가 수산되면 목적지까지의 방향값이 계산됩니다. /// 현재위치가 수산되면 목적지까지의 방향값이 계산됩니다.
/// </summary> /// </summary>
public Direction TargetDirection { get; set; } = Direction.Stop; public Direction TargetDirection { get; set; } = Direction.Stop;
public bool IsMoving { get; set; } public bool IsMoving { get; set; }
public List<Point> CurrentPath { get; set; } = new List<Point>();
/// <summary>
/// 경로검색으로 입력된 경로
/// </summary>
public List<RFIDPoint> CurrentPath { get; set; } = new List<RFIDPoint>();
public List<Point> PlannedPath { get; set; } public List<Point> PlannedPath { get; set; }
public List<string> PathRFIDs { 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<movehistorydata> MovementHistory { get; } = new List<movehistorydata>(); public List<movehistorydata> MovementHistory { get; } = new List<movehistorydata>();
public List<Point> PositionHistory { get; } = new List<Point>();
public const int HISTORY_SIZE = 4; // 최근 4개 위치 기록 public const int HISTORY_SIZE = 4; // 최근 4개 위치 기록
public AGV() public AGV()
{ {
CurrentPath = new List<Point>(); CurrentPath = new List<RFIDPoint>();
PlannedPath = new List<Point>(); PlannedPath = new List<Point>();
PathRFIDs = new List<string>(); PathRFIDs = new List<string>();
CurrentDirection = Direction.Forward;
TargetPosition = Point.Empty; CurrentRFID = new RFIDPoint();
TargetRFID = 0; TargetRFID = new RFIDPoint();
TargetDirection = Direction.Forward; TargetDirection = Direction.Forward;
BodyAngle = null; // BodyAngle = null;
} }
public void Move()
{
if (CurrentPath.Count > 0)
{
CurrentPosition = CurrentPath[0];
CurrentPath.RemoveAt(0);
}
}
// 이동 경로에 새로운 RFID 추가 // 이동 경로에 새로운 RFID 추가
public void AddToMovementHistory(UInt16 rfidValue, Point position, Direction direction) public void AddToMovementHistory(UInt16 rfidValue, Point position, Direction direction)
@@ -78,14 +113,12 @@ namespace AGVControl.Models
if (MovementHistory.Count > 0 && MovementHistory.Last().rfid == rfidValue) if (MovementHistory.Count > 0 && MovementHistory.Last().rfid == rfidValue)
return; return;
MovementHistory.Add(new movehistorydata { rfid = rfidValue, direction = direction }) ; MovementHistory.Add(new movehistorydata { rfid = rfidValue, direction = direction, Position = position });
PositionHistory.Add(position);
// 기록 크기 제한 // 기록 크기 제한
if (MovementHistory.Count > HISTORY_SIZE) if (MovementHistory.Count > HISTORY_SIZE)
{ {
MovementHistory.RemoveAt(0); MovementHistory.RemoveAt(0);
PositionHistory.RemoveAt(0);
} }
//최초방향과 마지막 방향이 일치하지 않으면 그 이전의 데이터는 삭제한다. //최초방향과 마지막 방향이 일치하지 않으면 그 이전의 데이터는 삭제한다.
@@ -144,15 +177,9 @@ namespace AGVControl.Models
if (actualDirection.Value != expectedDirection) if (actualDirection.Value != expectedDirection)
{ {
// AGV 모터 방향을 실제 이동 방향으로 정정 // AGV 모터 방향을 실제 이동 방향으로 정정
CurrentDirection = actualDirection.Value; CurrentAGVDirection = actualDirection.Value;
TargetDirection = actualDirection.Value; TargetDirection = actualDirection.Value;
// 몸체 방향도 180도 회전 (결정된 경우에만)
if (BodyAngle.HasValue)
{
BodyAngle = (BodyAngle.Value + 180) % 360;
}
return false; // 정정됨을 알림 return false; // 정정됨을 알림
} }
@@ -188,77 +215,7 @@ namespace AGVControl.Models
} }
} }
// 경로상 RFID 순서 기반 예상 방향 계산
public Direction? CalculateExpectedDirectionByRFID()
{
if (CurrentPath == null || CurrentPath.Count < 2)
return null;
// 현재 위치의 RFID 찾기
var currentRFID = CurrentRFID;
if (currentRFID == 0)
return null;
// 경로상 다음 RFID 찾기
int currentIdx = CurrentPath.FindIndex(p => p == CurrentPosition);
if (currentIdx < 0 || currentIdx >= CurrentPath.Count - 1)
return null;
// 다음 위치의 RFID 찾기 (MapControl에서 RFID 정보 필요)
// 이 부분은 MapControl에서 처리하도록 수정 필요
return null;
}
// 실제 이동 방향 계산 (기존 메서드 - 호환성 유지)
public Direction? CalculateActualDirection()
{
if (MovementHistory.Count < 2)
return null;
// 최근 두 위치로부터 실제 이동 방향 계산
var recentPositions = PositionHistory.Skip(Math.Max(0, PositionHistory.Count - 2)).Take(2).ToList();
if (recentPositions.Count < 2)
return null;
var prevPos = recentPositions[0];
var currentPos = recentPositions[1];
// X축 이동이 더 큰 경우
if (Math.Abs(currentPos.X - prevPos.X) > Math.Abs(currentPos.Y - prevPos.Y))
{
return currentPos.X > prevPos.X ? Direction.Forward : Direction.Backward;
}
// Y축 이동이 더 큰 경우
else
{
return currentPos.Y > prevPos.Y ? Direction.Forward : Direction.Backward;
}
}
// 경로상 예상 방향 계산
public Direction? CalculateExpectedDirection()
{
if (CurrentPath == null || CurrentPath.Count < 2)
return null;
int currentIdx = CurrentPath.FindIndex(p => p == CurrentPosition);
if (currentIdx < 0 || currentIdx >= CurrentPath.Count - 1)
return null;
var currentPos = CurrentPath[currentIdx];
var nextPos = CurrentPath[currentIdx + 1];
// X축 이동이 더 큰 경우
if (Math.Abs(nextPos.X - currentPos.X) > Math.Abs(nextPos.Y - currentPos.Y))
{
return nextPos.X > currentPos.X ? Direction.Forward : Direction.Backward;
}
// Y축 이동이 더 큰 경우
else
{
return nextPos.Y > currentPos.Y ? Direction.Forward : Direction.Backward;
}
}
} }
public class PathNode public class PathNode

19
Cs_HMI/SubProject/AGVControl/Models/RFIDPoint.cs
View File

@@ -6,7 +6,7 @@ namespace AGVControl.Models
public class RFIDPoint public class RFIDPoint
{ {
public Point Location { get; set; } public Point Location { get; set; }
public uint RFIDValue { get; set; } public uint Value { get; set; }
public string NextRFID { get; set; } // 다음 RFID 포인트의 값 public string NextRFID { get; set; } // 다음 RFID 포인트의 값
public bool IsBidirectional { get; set; } // 양방향 연결 여부 public bool IsBidirectional { get; set; } // 양방향 연결 여부
public bool IsRotatable { get; set; } // 회전 가능 여부 public bool IsRotatable { get; set; } // 회전 가능 여부
@@ -14,12 +14,29 @@ namespace AGVControl.Models
public bool IsTerminal { get; set; } // 종단 여부 public bool IsTerminal { get; set; } // 종단 여부
public RectangleF Bounds { get; set; } public RectangleF Bounds { get; set; }
public void Clear()
{
this.Location = Point.Empty;
this.Value = 0;
}
public bool IsEmpty
{
get
{
//RFID값이나 위치 값이 없으면 비어있는 것으로 한다.
if (this.Location.IsEmpty) return true;
if ((this.Value < 1)) return true;
return false;
}
}
public RFIDPoint() public RFIDPoint()
{ {
IsRotatable = false; // 기본값은 회전 불가능 IsRotatable = false; // 기본값은 회전 불가능
IsBidirectional = true; // 기본값은 양방향 IsBidirectional = true; // 기본값은 양방향
FixedDirection = null; FixedDirection = null;
IsTerminal = false; // 기본값은 종단 아님 IsTerminal = false; // 기본값은 종단 아님
Clear();
} }
} }
} }