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.EMERGENCY] = PUB.AGV.error.Emergency;
//모터방향 입력
if (PUB.AGV.data.Direction == 'B')
PUB.mapctl.agv.CurrentDirection = AGVControl.Models.Direction.Backward;
PUB.mapctl.agv.CurrentMOTDirection = AGVControl.Models.Direction.Backward;
else
PUB.mapctl.agv.CurrentDirection = AGVControl.Models.Direction.Forward;
PUB.mapctl.agv.CurrentMOTDirection = AGVControl.Models.Direction.Forward;
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)
{
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)
{
//배터리 레벨이 기준보다 낮다면 경고를 활성화 한다

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

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

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

@@ -2,6 +2,7 @@ using System;
using System.Drawing;
using System.Collections.Generic;
using System.Linq;
using System.Security.Permissions;
namespace AGVControl.Models
{
@@ -12,64 +13,98 @@ namespace AGVControl.Models
Stop = 2
}
public struct movehistorydata
public class CRFIDData
{
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 override string ToString()
{
return $"RFID:{rfid},DIR:{direction}";
return $"RFID:{rfid},DIR:{direction},P:{Position.X},{Position.Y}";
}
}
public class AGV
{
public Point CurrentPosition { get; set; }
public uint CurrentRFID { get; set; }
public float BatteryLevel { get; set; } = 0f;
/// <summary>
/// AGV에서 방향값이 수산됩니다.
/// RFID 번호
/// </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>
public Direction TargetDirection { get; set; } = Direction.Stop;
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<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<Point> PositionHistory { get; } = new List<Point>();
public const int HISTORY_SIZE = 4; // 최근 4개 위치 기록
public AGV()
{
CurrentPath = new List<Point>();
CurrentPath = new List<RFIDPoint>();
PlannedPath = new List<Point>();
PathRFIDs = new List<string>();
CurrentDirection = Direction.Forward;
TargetPosition = Point.Empty;
TargetRFID = 0;
CurrentRFID = new RFIDPoint();
TargetRFID = new RFIDPoint();
TargetDirection = Direction.Forward;
BodyAngle = null;
// BodyAngle = null;
}
public void Move()
{
if (CurrentPath.Count > 0)
{
CurrentPosition = CurrentPath[0];
CurrentPath.RemoveAt(0);
}
}
// 이동 경로에 새로운 RFID 추가
public void AddToMovementHistory(UInt16 rfidValue, Point position, Direction direction)
@@ -78,18 +113,16 @@ namespace AGVControl.Models
if (MovementHistory.Count > 0 && MovementHistory.Last().rfid == rfidValue)
return;
MovementHistory.Add(new movehistorydata { rfid = rfidValue, direction = direction }) ;
PositionHistory.Add(position);
MovementHistory.Add(new movehistorydata { rfid = rfidValue, direction = direction, Position = position });
// 기록 크기 제한
if (MovementHistory.Count > HISTORY_SIZE)
{
MovementHistory.RemoveAt(0);
PositionHistory.RemoveAt(0);
}
//최초방향과 마지막 방향이 일치하지 않으면 그 이전의 데이터는 삭제한다.
if(MovementHistory.Count > 2 && MovementHistory.First().direction != MovementHistory.Last().direction)
if (MovementHistory.Count > 2 && MovementHistory.First().direction != MovementHistory.Last().direction)
{
var lastTwo = MovementHistory.Skip(MovementHistory.Count - 2).Take(2).ToArray(); // [9, 10]
MovementHistory.Clear();
@@ -129,7 +162,7 @@ namespace AGVControl.Models
return true; // 검증 불가능한 경우
// 최근 두 RFID 값 가져오기
var recentRFIDs = MovementHistory.Skip( MovementHistory.Count - 2).Take(2).ToList();
var recentRFIDs = MovementHistory.Skip(MovementHistory.Count - 2).Take(2).ToList();
if (recentRFIDs.Count < 2)
return true;
@@ -144,15 +177,9 @@ namespace AGVControl.Models
if (actualDirection.Value != expectedDirection)
{
// AGV 모터 방향을 실제 이동 방향으로 정정
CurrentDirection = actualDirection.Value;
CurrentAGVDirection = actualDirection.Value;
TargetDirection = actualDirection.Value;
// 몸체 방향도 180도 회전 (결정된 경우에만)
if (BodyAngle.HasValue)
{
BodyAngle = (BodyAngle.Value + 180) % 360;
}
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

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

@@ -6,7 +6,7 @@ namespace AGVControl.Models
public class RFIDPoint
{
public Point Location { get; set; }
public uint RFIDValue { get; set; }
public uint Value { get; set; }
public string NextRFID { get; set; } // 다음 RFID 포인트의 값
public bool IsBidirectional { get; set; } // 양방향 연결 여부
public bool IsRotatable { get; set; } // 회전 가능 여부
@@ -14,12 +14,29 @@ namespace AGVControl.Models
public bool IsTerminal { 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()
{
IsRotatable = false; // 기본값은 회전 불가능
IsBidirectional = true; // 기본값은 양방향
FixedDirection = null;
IsTerminal = false; // 기본값은 종단 아님
Clear();
}
}
}