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-14 17:38:07 +09:00
parent d5516f9708
commit b4da1cca00
1 changed files with 11 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -147,31 +147,31 @@ namespace AGVNavigationCore.PathFinding.Planning
for (int i = 0; i < pathResult.Path.Count; i++) for (int i = 0; i < pathResult.Path.Count; i++)
{ {
var node = pathResult.Path[i]; var node = pathResult.Path[i];
string nextNodeId = (i + 1 < pathResult.Path.Count) ? pathResult.Path[i + 1].Id : null; var nextNode = (i + 1 < pathResult.Path.Count) ? pathResult.Path[i + 1] : null;
// 마그넷 방향 계산 (갈림길인 경우) // 마그넷 방향 계산 (갈림길인 경우)
MagnetDirection magnetDirection = MagnetDirection.Straight; MagnetDirection magnetDirection = MagnetDirection.Straight;
if (node.ConnectedNodes != null && node.ConnectedNodes.Count >= 3 && i > 0 && nextNodeId != null) if ((node.ConnectedNodes?.Count ?? 0) >= 2 && nextNode != null)
{ {
// 갈림길인 경우: 진입 방향과 진출 방향의 각도 계산 // 갈림길인 경우: 진입 방향과 진출 방향의 각도 계산
var prevNode = pathResult.Path[i - 1]; //var prevNode = pathResult.Path[i - 1];
var nextNode = pathResult.Path[i + 1]; //var nextNode = pathResult.Path[i + 1];
// 진입 각도 계산 (이전 노드 → 현재 노드) // 진입 각도 계산 (이전 노드 → 현재 노드)
double entryAngle = Math.Atan2( //double entryAngle = Math.Atan2(
node.Position.Y - prevNode.Position.Y, // node.Position.Y - prevNode.Position.Y,
node.Position.X - prevNode.Position.X // node.Position.X - prevNode.Position.X
) * 180.0 / Math.PI; //) * 180.0 / Math.PI;
// 진출 각도 계산 (현재 노드 → 다음 노드) // 진출 각도 계산 (현재 노드 → 다음 노드)
double exitAngle = Math.Atan2( double exitAngle = Math.Atan2(
nextNode.Position.Y - node.Position.Y, nextNode.Position.Y - node.Position.Y,
nextNode.Position.X - node.Position.X nextNode.Position.X - node.Position.X
) * 180.0 / Math.PI; ) * 180.0 / Math.PI;
// 각도 차이 계산 (-180~180 범위로 정규화) // 각도 차이 계산 (-180~180 범위로 정규화)
double angleDiff = exitAngle - entryAngle; double angleDiff = exitAngle;// - entryAngle;
while (angleDiff > 180) angleDiff -= 360; while (angleDiff > 180) angleDiff -= 360;
while (angleDiff < -180) angleDiff += 360; while (angleDiff < -180) angleDiff += 360;
@@ -189,7 +189,7 @@ namespace AGVNavigationCore.PathFinding.Planning
} }
} }
var nodeInfo = new NodeMotorInfo(i + 1, node.Id, node.RfidId, prevDirection, nextNodeId, magnetDirection); var nodeInfo = new NodeMotorInfo(i + 1, node.Id, node.RfidId, prevDirection, nextNode.Id, magnetDirection);
// 속도 설정 // 속도 설정
var mapNode = _mapNodes.FirstOrDefault(n => n.Id == node.Id); var mapNode = _mapNodes.FirstOrDefault(n => n.Id == node.Id);