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-30 13:48:19 +09:00
parent 02e71d7446
commit 00dd50192b
29 changed files with 2397 additions and 1136 deletions
Download Patch File Download Diff File Expand all files Collapse all files

332
Cs_HMI/SubProject/AGVControl/MapControlManager.cs
View File

@@ -3,25 +3,37 @@ using System;
using System.Collections.Generic;
using System.Drawing;
using System.Linq;
using System.Runtime.CompilerServices;
using System.Security.Cryptography.X509Certificates;
using System.Threading;
using static System.Windows.Forms.VisualStyles.VisualStyleElement.TextBox;
namespace AGVControl
{
public static class MapControlManager
public class MapControlManager
{
public static AGVActionPrediction PredictResult = null;
public static AGV agv = new AGV();
public static List<RFIDPoint> RFIDPoints = new List<RFIDPoint>();
public static HashSet<RFIDConnection> rfidConnections = new HashSet<RFIDConnection>();
public AGVActionPrediction PredictResult = null;
public AGV agv = new AGV();
public List<RFIDPoint> RFIDPoints = new List<RFIDPoint>();
public HashSet<RFIDConnection> rfidConnections = new HashSet<RFIDConnection>();
private static ManualResetEvent mrepredict = new ManualResetEvent(true);
private ManualResetEvent mrepredict = new ManualResetEvent(true);
public MapControlManager()
{
}
~MapControlManager()
{
}
/// <summary>
/// 목표지정으로 모터방향이 이동하고 있는가?
/// history 데이터가 있어야 하며 기준데이터가 없는 경우 null 반환
/// </summary>
/// <returns></returns>
public static bool? IsMotDirection_To_Target()
public bool? IsMotDirection_To_Target()
{
if (agv.MovementHistory.Any() == false || agv.MovementHistory.Count < 2) return null;
if (agv.MainPath.Any() == false) return null;
@@ -38,27 +50,24 @@ namespace AGVControl
//지정된경로 반대방향으로 이동하고 있다
return preidx < curidx;
}
public static float GetDistance(Point p1, Point p2)
public float GetDistance(Point p1, Point p2)
{
float dx = p1.X - p2.X;
float dy = p1.Y - p2.Y;
return (float)Math.Sqrt(dx * dx + dy * dy); // double을 float로 명시적 캐스팅
}
public static AGVActionPrediction PredictNextAction()
public AGVActionPrediction PredictNextAction()
{
if (mrepredict.WaitOne(1) == false)
{
PredictResult = new AGVActionPrediction
{
Reason = "이전 작업이 완료되지 않았습니다",
ReasonCode = AGVActionReasonCode.busy,
MoveState = AGVMoveState.Stop,
Direction = agv.CurrentMOTDirection,
};
PredictResult = CreatePrediction("이전 작업이 완료되지 않았습니다",
AGVActionReasonCode.busy,
AGVMoveState.Stop,
agv.CurrentMOTDirection, false);
return PredictResult;
}
mrepredict.Reset();
try
{
@@ -68,58 +77,70 @@ namespace AGVControl
// 1. 위치를 모를 때 (CurrentRFID가 0 또는 미설정)
if (agv.CurrentRFID.Value == 0)
{
PredictResult = new AGVActionPrediction
{
Direction = Direction.Backward,
NextRFID = null,
Reason = "AGV 위치 미확정(처음 기동)",
ReasonCode = AGVActionReasonCode.NoPosition,
MoveState = AGVMoveState.Run
};
PredictResult = CreatePrediction("AGV 위치 미확정(처음 기동)",
AGVActionReasonCode.NoPosition,
AGVMoveState.Run,
Direction.Backward, true,
moveSpeed: AgvSpeed.Low,
moveDiv: AgvRunDirection.Straight);
return PredictResult;
}
//2. 이동방향을 모른다
if (agv.MovementHistory.Any() == false || agv.MovementHistory.Count < 2)
{
PredictResult = new AGVActionPrediction
{
Direction = Direction.Backward,
NextRFID = null,
Reason = "AGV이동방향 알수없음",
ReasonCode = AGVActionReasonCode.NoDirection,
MoveState = AGVMoveState.Run
};
PredictResult = CreatePrediction("AGV이동방향 알수없음",
AGVActionReasonCode.NoDirection,
AGVMoveState.Run,
Direction.Backward, true,
moveSpeed: AgvSpeed.Low,
moveDiv: AgvRunDirection.Straight);
return PredictResult;
}
// 3. 경로가 없거나 현재 위치가 경로에 없음
if ((agv.MainPath?.Count ?? 0) < 2)
if ((agv.MainPath?.Count ?? 0) < 2 || agv.MainPath.Last().Value != agv.TargetRFID.Value)
{
PredictResult = new AGVActionPrediction
//목적지가 없다면 진행할 수 없다
if (agv.TargetRFID == null)
{
Direction = agv.CurrentMOTDirection,
NextRFID = null,
Reason = "경로 없음 또는 현재 위치 미확정",
ReasonCode = AGVActionReasonCode.NoPath,
MoveState = AGVMoveState.Stop
};
return PredictResult;
PredictResult = CreatePrediction("경로 없음 또는 현재 위치 미확정",
AGVActionReasonCode.NoPath,
AGVMoveState.Stop,
agv.CurrentMOTDirection, true);
return PredictResult;
}
else
{
//목적지가 있는데 경로가 없다면 경로 예측을 진행해야한다.
var CurPt = agv.MovementHistory.Last();
var prlt = CalculatePath(CurPt, agv.TargetRFID);
if (prlt.Success == false)
{
PredictResult = CreatePrediction("목적지 경로 예측 실패",
AGVActionReasonCode.Unknown,
AGVMoveState.Stop,
agv.CurrentMOTDirection, true,
nextRFID: agv.TargetRFID);
return PredictResult;
}
else
{
//신규목적지에 대한 경로예측이 성공했다
agv.SubPath.Clear();
agv.MainPath = prlt.Path;
}
}
}
// 4. 경로상에서 다음 RFID 예측
int idx = agv.MainPath.FindIndex(p => p.Value == agv.CurrentRFID.Value);
if (idx < 0)
{
PredictResult = new AGVActionPrediction
{
Direction = agv.CurrentMOTDirection,
NextRFID = null,
Reason = "현재 위치가 경로에 없음",
ReasonCode = AGVActionReasonCode.NotOnPath,
MoveState = AGVMoveState.Stop
};
PredictResult = CreatePrediction("현재 위치가 경로에 없음",
AGVActionReasonCode.NotOnPath,
AGVMoveState.Stop,
agv.CurrentMOTDirection, true);
return PredictResult;
}
@@ -135,30 +156,25 @@ namespace AGVControl
//모션이동방향이 맞는가?
var IsMotDir = IsMotDirection_To_Target() ?? false;
var PrePT = agv.MovementHistory.Skip(agv.MovementHistory.Count - 1).First();
var PrePT = agv.MovementHistory.Skip(agv.MovementHistory.Count - 2).First();
var curPT = agv.MovementHistory.Last();
//리프트방향이 맞지 않다면 회전가능한 위치로 이동을 해야한다
if (IsLiftDir == false)
{
AgvSpeed? agv_spd = null;
AgvRunDirection? agv_dir = null;
//회전가능한 위치로 이동을 해야한다
//1. 가까운 회전위치를 찾는다
var nearTurnPoint = RFIDPoints.Where(t => t.IsRotatable)?.OrderBy(t => GetDistance(t.Location, agv.CurrentRFID.Location)).FirstOrDefault() ?? null;
if (nearTurnPoint == null)
{
PredictResult= new AGVActionPrediction
{
Direction = agv.CurrentMOTDirection,
NextRFID = null,
Reason = "회전 가능한 위치가 없습니다",
ReasonCode = AGVActionReasonCode.NoTurnPoint,
MoveState = AGVMoveState.Stop
};
PredictResult = CreatePrediction("회전 가능한 위치가 없습니다",
AGVActionReasonCode.NoTurnPoint,
AGVMoveState.Stop,
agv.CurrentMOTDirection, true);
return PredictResult;
}
@@ -168,22 +184,19 @@ namespace AGVControl
if (agv.CurrentRFID.Value != nearTurnPoint.Value)
{
if (agv.SubPath.Any() == false || agv.SubPath.Count < 2 ||
agv.SubPath.First().Value != PrePT.Value ||
agv.SubPath.First().Value != agv.CurrentRFID.Value ||
agv.SubPath.Last().Value != nearTurnPoint.Value)
{
var rlt = CalculatePath(PrePT, nearTurnPoint, true); //이전포인트도 추가를 해준다
var rlt = CalculatePath(agv.CurrentRFID, nearTurnPoint); //이전포인트도 추가를 해준다
if (rlt.Success) agv.SubPath = rlt.Path;
else
{
agv.SubPath.Clear();
PredictResult= new AGVActionPrediction
{
Direction = agv.CurrentMOTDirection,
NextRFID = nearTurnPoint,
Reason = "회전 위치까지의 경로를 계산할 수 없습니다",
ReasonCode = AGVActionReasonCode.PathCalcError,
MoveState = AGVMoveState.Stop
};
PredictResult = CreatePrediction("회전 위치까지의 경로를 계산할 수 없습니다",
AGVActionReasonCode.PathCalcError,
AGVMoveState.Stop,
agv.CurrentMOTDirection, true,
nextRFID: nearTurnPoint);
return PredictResult;
}
}
@@ -194,6 +207,9 @@ namespace AGVControl
var preidx = agv.SubPath.FindIndex(t => t.Value == PrePT.Value);
Direction newdirection = agv.CurrentMOTDirection;
string message = "턴위치로 이동중";
if (preidx > curidx)
{
//지정경로를 거꾸로 이동하고 있다
@@ -202,31 +218,44 @@ namespace AGVControl
else
newdirection = Direction.Forward;
message += "(방향전환)";
}
PredictResult= new AGVActionPrediction
{
Direction = newdirection,
NextRFID = nearTurnPoint,
Reason = message,
ReasonCode = AGVActionReasonCode.MoveForTurn,
MoveState = AGVMoveState.Run,
};
//도로정보를 확인하여 속도와 분기명령을 실행한다
var roadinfo = GetRoadInfo(agv.SubPath, curPT);
agv_spd = roadinfo.spd;
agv_dir = roadinfo.dir;
PredictResult = CreatePrediction(message,
AGVActionReasonCode.MoveForTurn,
AGVMoveState.Run,
newdirection, true,
moveSpeed: agv_spd,
moveDiv: agv_dir,
nextRFID: nearTurnPoint);
return PredictResult;
}
var roadinfo2 = GetRoadInfo(agv.SubPath.Any() ? agv.SubPath : agv.MainPath, curPT);
agv_spd = roadinfo2.spd;
agv_dir = roadinfo2.dir;
PredictResult = new AGVActionPrediction
{
Direction = agv.CurrentMOTDirection,
NextRFID = nearTurnPoint,
Reason = "턴 완료 대기",
ReasonCode = AGVActionReasonCode.NeedTurn,
MoveState = AGVMoveState.Stop
};
PredictResult = CreatePrediction("턴 완료 대기",
AGVActionReasonCode.NeedTurn,
AGVMoveState.Stop,
agv.CurrentMOTDirection, true,
moveSpeed: agv_spd,
moveDiv: agv_dir,
nextRFID: nearTurnPoint);
return PredictResult;
}
//보조이동선제거
if (agv.SubPath != null && agv.SubPath.Any())
agv.SubPath.Clear();
//3. 목적지위치까지 이동이 완료되지 않았다면 계속 이동을 하게한다
if (agv.CurrentRFID.Value != destRFID.Value)
{
@@ -247,39 +276,35 @@ namespace AGVControl
//경로상 바로 다음 위치를 확인한다
var nexstRFID = agv.MainPath.Skip(agv.MainPath.FindIndex(t => t.Value == curPT.Value) + 1).First();
PredictResult = new AGVActionPrediction
{
Direction = newdirection,
NextRFID = nexstRFID,
Reason = message,
ReasonCode = AGVActionReasonCode.Normal,
MoveState = AGVMoveState.Run,
};
var roadinfo = GetRoadInfo(agv.MainPath, curPT);
PredictResult = CreatePrediction(message,
AGVActionReasonCode.Normal,
AGVMoveState.Run,
newdirection, true,
moveSpeed: roadinfo.spd,
moveDiv: roadinfo.dir,
nextRFID: nexstRFID);
return PredictResult;
}
// 5. 목적지 도달 시
PredictResult = new AGVActionPrediction
{
Direction = agv.CurrentMOTDirection,
NextRFID = destRFID,
Reason = "경로의 마지막 지점(목적지 도달)",
ReasonCode = AGVActionReasonCode.Arrived,
MoveState = AGVMoveState.Stop
};
PredictResult = CreatePrediction("경로의 마지막 지점(목적지 도달)",
AGVActionReasonCode.Arrived,
AGVMoveState.Stop,
agv.CurrentMOTDirection, true,
nextRFID: destRFID);
return PredictResult;
}
catch (Exception ex)
{
PredictResult = new AGVActionPrediction
{
Direction = agv.CurrentMOTDirection,
NextRFID = null,
Reason = $"ERR:{ex.Message}",
ReasonCode = AGVActionReasonCode.Unknown,
MoveState = AGVMoveState.Stop
};
PredictResult = CreatePrediction($"ERR:{ex.Message}",
AGVActionReasonCode.Unknown,
AGVMoveState.Stop,
agv.CurrentMOTDirection, true);
return PredictResult;
}
finally
@@ -289,13 +314,43 @@ namespace AGVControl
}
(AgvSpeed? spd, AgvRunDirection? dir, RFIDConnection info) GetRoadInfo(List<RFIDPoint> paths, RFIDPoint curPT)
{
//도로정보를 확인하여 속도와 분기명령을 실행한다
AgvSpeed? agv_spd = null;
AgvRunDirection? agv_div = null;
RFIDConnection info = null;
var nextpt = paths.Skip(paths.FindIndex(t => t.Value == curPT.Value) + 1).FirstOrDefault();
if (nextpt != null)
{
var p1 = rfidConnections.Where(t => t.P1.Value == curPT.Value && t.P2.Value == nextpt.Value).FirstOrDefault();
if (p1 != null)
{
//positive
agv_spd = p1.MoveSpeedP;
agv_div = p1.MoveDirectionP;
info = p1;
}
var p2 = rfidConnections.Where(t => t.P2.Value == curPT.Value && t.P1.Value == nextpt.Value).FirstOrDefault();
if (p2 != null)
{
//negative
agv_spd = p2.MoveSpeedN;
agv_div = p2.MoveDirectionN;
info = p2;
}
}
return (agv_spd, agv_div, info);
}
/// <summary>
/// 이웃포인터를 반환합니다
/// </summary>
/// <param name="point"></param>
/// <returns></returns>
public static List<RFIDPoint> GetNeighbors(RFIDPoint pt)
public List<RFIDPoint> GetNeighbors(RFIDPoint pt)
{
var neighbors = new List<RFIDPoint>();
@@ -322,17 +377,17 @@ namespace AGVControl
return neighbors.Distinct().ToList();
}
public static RFIDPoint FindRFIDPoint(uint rfidValue)
public RFIDPoint FindRFIDPoint(uint rfidValue)
{
if (RFIDPoints == null || RFIDPoints.Any() == false) return null;
return RFIDPoints.FirstOrDefault(r => r.Value == rfidValue);
}
private static float Heuristic(Point a, Point b)
private float Heuristic(Point a, Point b)
{
return (float)Math.Sqrt(Math.Pow(a.X - b.X, 2) + Math.Pow(a.Y - b.Y, 2));
}
private static PathResult ReconstructPath(Dictionary<uint, RFIDPoint> cameFrom, RFIDPoint current)
private PathResult ReconstructPath(Dictionary<uint, RFIDPoint> cameFrom, RFIDPoint current)
{
var path = new List<RFIDPoint> { current };
while (cameFrom.ContainsKey(current.Value))
@@ -348,7 +403,7 @@ namespace AGVControl
Path = path,
};
}
public static PathResult CalculatePath(RFIDPoint start, RFIDPoint end, bool autorun)
public PathResult CalculatePath(RFIDPoint start, RFIDPoint end)
{
var openList = new List<RFIDPoint> { start };
var closedList = new List<RFIDPoint>();
@@ -410,7 +465,7 @@ namespace AGVControl
};
}
public static PathResult CalculatePath(uint tagStrt, uint tagEnd)
public PathResult CalculatePath(uint tagStrt, uint tagEnd)
{
var retval = new PathResult
{
@@ -426,7 +481,7 @@ namespace AGVControl
return retval;
}
retval = CalculatePath(startPoint, endPoint, false);
retval = CalculatePath(startPoint, endPoint);
if (retval.Success == false)
retval.Message = "경로를 찾을 수 없습니다";
@@ -439,7 +494,7 @@ namespace AGVControl
/// 이동경로정보가 없거나 목적지가 없으면 null 이 반환됨
/// </summary>
/// <returns></returns>
public static bool? IsLiftDirectionMatch()
public bool? IsLiftDirectionMatch()
{
if (agv.MovementHistory.Any() && agv.MovementHistory.Count > 1)
@@ -498,7 +553,40 @@ namespace AGVControl
}
}
// Changed 속성 설정을 위한 헬퍼 메서드
private bool IsPredictionChanged(AGVActionPrediction newPrediction)
{
if (PredictResult == null) return true; // 이전 예측이 없으면 변경됨
// Idx와 Changed를 제외하고 비교
return !PredictResult.Equals(newPrediction);
}
private AGVActionPrediction CreatePrediction(
string reason,
AGVActionReasonCode reasonCode,
AGVMoveState moveState,
Direction direction, bool IDXUpdate = true,
AgvSpeed? moveSpeed = null,
AgvRunDirection? moveDiv = null,
RFIDPoint nextRFID = null
)
{
var newPrediction = new AGVActionPrediction
{
NextRFID = nextRFID,
Reason = reason,
ReasonCode = reasonCode,
MoveState = moveState,
Direction = direction,
MoveSpeed = moveSpeed,
MoveDiv = moveDiv,
Idx = IDXUpdate ? (PredictResult?.Idx + 1 ?? 1) : (PredictResult?.Idx ?? 0)
};
newPrediction.Changed = IsPredictionChanged(newPrediction);
return newPrediction;
}
}
}