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

..

Browse Source
This commit is contained in:
backuppc
2026-01-15 13:44:56 +09:00
parent 0396c61517
commit 82592e117d
6 changed files with 429 additions and 116 deletions
Download Patch File Download Diff File Expand all files Collapse all files

77
Cs_HMI/AGVLogic/AGVNavigationCore/PathFinding/Planning/AGVPathfinder.cs
View File

@@ -111,13 +111,13 @@ namespace AGVNavigationCore.PathFinding.Planning
return null;
}
public AGVPathResult FindPathAStar(MapNode startNode, MapNode targetNode)
{
// 기본값으로 경로 탐색 (이전 위치 = 현재 위치, 방향 = 전진)
return _basicPathfinder.FindPathAStar(startNode, targetNode);
}
/// <summary>
/// 이 작업후에 MakeMagnetDirection 를 추가로 실행 하세요
@@ -140,6 +140,7 @@ namespace AGVNavigationCore.PathFinding.Planning
if (!pathResult.Success)
return AGVPathResult.CreateFailure(pathResult.ErrorMessage ?? "경로 없음", 0, 0);
// 3. 상세 데이터 생성 (갈림길 마그넷 방향 계산 포함)
// 3. 상세 데이터 생성 (갈림길 마그넷 방향 계산 포함)
if (pathResult.Path != null && pathResult.Path.Count > 0)
{
@@ -149,53 +150,51 @@ namespace AGVNavigationCore.PathFinding.Planning
var node = pathResult.Path[i];
var nextNode = (i + 1 < pathResult.Path.Count) ? pathResult.Path[i + 1] : null;
// 마그넷 방향 계산 (갈림길인 경우)
// 마그넷 방향 계산 (갈림길인 경우)
MagnetDirection magnetDirection = MagnetDirection.Straight;
if ((node.ConnectedNodes?.Count ?? 0) >= 2 && nextNode != null)
{
// 갈림길인 경우: 진입 방향과 진출 방향의 각도 계산
//var prevNode = pathResult.Path[i - 1];
//var nextNode = pathResult.Path[i + 1];
// 진입 각도 계산 (이전 노드 → 현재 노드)
//double entryAngle = Math.Atan2(
// node.Position.Y - prevNode.Position.Y,
// node.Position.X - prevNode.Position.X
//) * 180.0 / Math.PI;
// 진출 각도 계산 (현재 노드 → 다음 노드)
double exitAngle = Math.Atan2(
nextNode.Position.Y - node.Position.Y,
nextNode.Position.X - node.Position.X
) * 180.0 / Math.PI;
// 각도 차이 계산 (-180~180 범위로 정규화)
double angleDiff = exitAngle;// - entryAngle;
while (angleDiff > 180) angleDiff -= 360;
while (angleDiff < -180) angleDiff += 360;
// 10도 이상 차이나면 좌/우회전 처리
if (Math.Abs(angleDiff) >= 10)
//갈림길에 있다면 미리 방향을 저장해준다.
if ((node.ConnectedNodes?.Count ?? 0) > 2 && nextNode != null)
{
switch (node.RfidId)
{
if (angleDiff > 0)
{
magnetDirection = MagnetDirection.Right;
}
else
{
magnetDirection = MagnetDirection.Left;
}
case 6:
if (nextNode.RfidId == 7)
magnetDirection = MagnetDirection.Left;
else if (nextNode.RfidId == 13)
magnetDirection = MagnetDirection.Right;
else
return AGVPathResult.CreateFailure($"{node.ID2}->{nextNode.ID2} 의 (목표)갈림길 방향이 입력되지 않았습니다", 0, 0);
break;
case 7:
if (nextNode.RfidId == 6)
magnetDirection = MagnetDirection.Right;
else
return AGVPathResult.CreateFailure( $"{node.ID2}->{nextNode.ID2} 의 (목표)갈림길 방향이 입력되지 않았습니다", 0, 0);
break;
case 13:
if (nextNode.RfidId == 6)
magnetDirection = MagnetDirection.Left;
else
return AGVPathResult.CreateFailure( $"{node.ID2}->{nextNode.ID2} 의 (목표)갈림길 방향이 입력되지 않았습니다", 0, 0);
break;
default:
return AGVPathResult.CreateFailure( $"{node.ID2}->{nextNode.ID2} 의 (시작)갈림길 방향이 입력되지 않았습니다", 0, 0);
}
}
var nodeInfo = new NodeMotorInfo(i + 1, node.Id, node.RfidId, prevDirection, nextNode.Id, magnetDirection);
var nodeInfo = new NodeMotorInfo(i + 1, node.Id, node.RfidId, prevDirection, nextNode, magnetDirection);
// 속도 설정
var mapNode = _mapNodes.FirstOrDefault(n => n.Id == node.Id);
if (mapNode != null) nodeInfo.Speed = mapNode.SpeedLimit;
if (mapNode != null)
{
nodeInfo.Speed = mapNode.SpeedLimit;
detailedPath.Add(nodeInfo);
}
detailedPath.Add(nodeInfo);
}
pathResult.DetailedPath = detailedPath;
}
@@ -203,7 +202,7 @@ namespace AGVNavigationCore.PathFinding.Planning
return pathResult;
}
}

6
Cs_HMI/AGVLogic/AGVNavigationCore/PathFinding/Planning/NodeMotorInfo.cs
View File

@@ -60,7 +60,7 @@ namespace AGVNavigationCore.PathFinding.Planning
/// <summary>
/// 다음 노드 ID (경로예측용)
/// </summary>
public string NextNodeId { get; set; }
public MapNode NextNode { get; set; }
/// <summary>
/// 회전 가능 노드 여부
@@ -87,14 +87,14 @@ namespace AGVNavigationCore.PathFinding.Planning
/// </summary>
public string SpecialActionDescription { get; set; }
public NodeMotorInfo(int seqno,string nodeId,ushort rfid, AgvDirection motorDirection, string nextNodeId = null, MagnetDirection magnetDirection = MagnetDirection.Straight)
public NodeMotorInfo(int seqno,string nodeId,ushort rfid, AgvDirection motorDirection, MapNode nextNodeId = null, MagnetDirection magnetDirection = MagnetDirection.Straight)
{
seq = seqno;
NodeId = nodeId;
RfidId = rfid;
MotorDirection = motorDirection;
MagnetDirection = magnetDirection;
NextNodeId = nextNodeId;
NextNode = nextNodeId;
CanRotate = false;
IsDirectionChangePoint = false;
RequiresSpecialAction = false;