..

AGVLogic/AGVNavigationCore/PathFinding/Planning/AGVPathfinder.cs

@@ -506,7 +506,150 @@ namespace AGVNavigationCore.PathFinding.Planning
             //일반노드도 일부 경로를 계산한다.
             // 일반노드 3 -> 
 
+            retval.Add(new MapZonePathData
+            {
+                NodeSta = StationType.Normal,
+                NodeEnd = StationType.Loader,
+                Monitor = MapZoneMonitor.LeftTop,
+                Path = new List<string> { "3B", "71B" }
+            });
+            retval.Add(new MapZonePathData
+            {
+                NodeSta = StationType.Normal,
+                NodeEnd = StationType.Cleaner,
+                Monitor = MapZoneMonitor.LeftTop,
+                Path = new List<string> { "3T", "3B", "70B" }
+            });
+            retval.Add(new MapZonePathData
+            {
+                NodeSta = StationType.Normal,
+                NodeEnd = StationType.Plating,
+                Monitor = MapZoneMonitor.LeftTop,
+                Path = new List<string> { "3B", "11B", "72B" }
+            });
+            retval.Add(new MapZonePathData
+            {
+                NodeSta = StationType.Normal,
+                NodeEnd = StationType.Charger,
+                Monitor = MapZoneMonitor.LeftTop,
+                Path = new List<string> { "3T", "3F", "11F", "7B", "10B", "6B", "73B" }
+            });
+            retval.Add(new MapZonePathData
+            {
+                NodeSta = StationType.Normal,
+                NodeEnd = StationType.Buffer,
+                Monitor = MapZoneMonitor.LeftTop,
+                Path = new List<string> { "3B", "11B", "7B", "8B", "9B", "20B", "34B", "33B", "32B", "31B", "35B", "36B" }
+            });
+
+            retval.Add(new MapZonePathData
+            {
+                NodeSta = StationType.Normal,
+                NodeEnd = StationType.Loader,
+                Monitor = MapZoneMonitor.RightBtm,
+                Path = new List<string> { "3T", "3B", "71B" }
+            });
+            retval.Add(new MapZonePathData
+            {
+                NodeSta = StationType.Normal,
+                NodeEnd = StationType.Cleaner,
+                Monitor = MapZoneMonitor.RightBtm,
+                Path = new List<string> { "3B", "70B" }
+            });
+            retval.Add(new MapZonePathData
+            {
+                NodeSta = StationType.Normal,
+                NodeEnd = StationType.Plating,
+                Monitor = MapZoneMonitor.RightBtm,
+                Path = new List<string> { "3T", "3B", "11B", "72B" }
+            });
+            retval.Add(new MapZonePathData
+            {
+                NodeSta = StationType.Normal,
+                NodeEnd = StationType.Charger,
+                Monitor = MapZoneMonitor.RightBtm,
+                Path = new List<string> { "3F", "11F", "7B", "10B", "6B", "73B" }
+            });
+            retval.Add(new MapZonePathData
+            {
+                NodeSta = StationType.Normal,
+                NodeEnd = StationType.Buffer,
+                Monitor = MapZoneMonitor.RightBtm,
+                Path = new List<string> { "3T", "3B", "11B", "7B", "8B", "9B", "20B", "34B", "33B", "32B", "31B", "35B", "36B" }
+            });
+
             // 일반노드 7 ->
+            retval.Add(new MapZonePathData
+            {
+                NodeSta = StationType.Normal,
+                NodeEnd = StationType.Loader,
+                Monitor = MapZoneMonitor.LeftTop,
+                Path = new List<string> { "7F", "11F", "3B", "71B" }
+            });
+            retval.Add(new MapZonePathData
+            {
+                NodeSta = StationType.Normal,
+                NodeEnd = StationType.Cleaner,
+                Monitor = MapZoneMonitor.LeftTop,
+                Path = new List<string> { "7F", "11F", "3T", "3B", "70B" }
+            });
+            retval.Add(new MapZonePathData
+            {
+                NodeSta = StationType.Normal,
+                NodeEnd = StationType.Plating,
+                Monitor = MapZoneMonitor.LeftTop,
+                Path = new List<string> { "7F", "11B", "72B" }
+            });
+            retval.Add(new MapZonePathData
+            {
+                NodeSta = StationType.Normal,
+                NodeEnd = StationType.Charger,
+                Monitor = MapZoneMonitor.LeftTop,
+                Path = new List<string> { "7F", "11F", "3T", "3F", "11F", "7B", "10B", "6B", "73B" }
+            });
+            retval.Add(new MapZonePathData
+            {
+                NodeSta = StationType.Normal,
+                NodeEnd = StationType.Buffer,
+                Monitor = MapZoneMonitor.LeftTop,
+                Path = new List<string> { "7B", "8B", "9B", "20B", "34B", "33B", "32B", "31B", "35B", "36B" }
+            });
+
+            retval.Add(new MapZonePathData
+            {
+                NodeSta = StationType.Normal,
+                NodeEnd = StationType.Loader,
+                Monitor = MapZoneMonitor.RightBtm,
+                Path = new List<string> { "7B", "11B", "3T", "3B", "71B" }
+            });
+            retval.Add(new MapZonePathData
+            {
+                NodeSta = StationType.Normal,
+                NodeEnd = StationType.Cleaner,
+                Monitor = MapZoneMonitor.RightBtm,
+                Path = new List<string> { "7B", "11B", "3B", "70B" }
+            });
+            retval.Add(new MapZonePathData
+            {
+                NodeSta = StationType.Normal,
+                NodeEnd = StationType.Plating,
+                Monitor = MapZoneMonitor.RightBtm,
+                Path = new List<string> { "7B", "11B", "3T", "3B", "11B", "72B" }
+            });
+            retval.Add(new MapZonePathData
+            {
+                NodeSta = StationType.Normal,
+                NodeEnd = StationType.Charger,
+                Monitor = MapZoneMonitor.RightBtm,
+                Path = new List<string> { "7B", "10B", "6B", "73B" }
+            });
+            retval.Add(new MapZonePathData
+            {
+                NodeSta = StationType.Normal,
+                NodeEnd = StationType.Buffer,
+                Monitor = MapZoneMonitor.RightBtm,
+                Path = new List<string> { "7B", "11B", "3T", "3B", "11B", "7B", "8B", "9B", "20B", "34B", "33B", "32B", "31B", "35B", "36B" }
+            });
 
             return retval;
         }
@@ -564,23 +707,71 @@ namespace AGVNavigationCore.PathFinding.Planning
             // 반전되어 구해지는 현상이 있으므로, 이 경우에만 monitorMode를 반대로 뒤집어서 찾는다.
             if (startZone != targetZone && startNode.StationType == StationType.Normal)
             {
-                monitorMode = monitorMode == MapZoneMonitor.LeftTop ? MapZoneMonitor.RightBtm : MapZoneMonitor.LeftTop;
+                //monitorMode = monitorMode == MapZoneMonitor.LeftTop ? MapZoneMonitor.RightBtm : MapZoneMonitor.LeftTop;
             }
 
             // 모니터방향이 일치하고 대상노드가 동일한 경로를 찾는다
             var zonepath = GetMapZonePathData();
 
             IEnumerable<MapZonePathData> candidates;
-            // 목적지가 현재 위치와 동일한 경우 처리
 
             //시작이 일반노드라면 , 경로가 등록된 주변 노드로 이동해서 이후경로를 계산하자
             //일반노드용 기준은 7,3이있다. 
-            //11번은 7로 이동해서 경로를 계산한다.
+            //11번은 3로 이동해서 경로를 계산한다.
             //10,6번은 7로 이동해서 경로를 계산한다. 
-            //대상노드까지 CalculateScriptedPath를 재귀로 호출해서 계산을 완료한 후, 대상에서 목적지계산해서 전체 경로를 만들어낸다.
+            //대상노드까지 CalculateScriptedPath를 재귀로 호출해서 계산을 완료한 후, 대상에서 목적지 계산해서 전체 경로를 만들어낸다.
+            if (startNode.StationType == StationType.Normal && startNode.RfidId != 3 && startNode.RfidId != 7)
+            {
+                var newtargetRFID = startNode.RfidId == 11 ? 3 : 7;
+                var refNode = _mapNodes.FirstOrDefault(n => n.RfidId == newtargetRFID);
+                if (refNode != null && refNode.Id != startNode.Id)
+                {
+                    // 기준 노드(7 or 3)까지의 기본 경로를 먼저 구함
+                    var pathToRef = this.FindBasicPath(startNode, refNode, prevNode, prevDir);
+                    if (pathToRef != null && pathToRef.Success)
+                    {
+                        var lastDet = pathToRef.DetailedPath.Last();
+                        var secondLastNode = pathToRef.Path.Count > 1 ? pathToRef.Path[pathToRef.Path.Count - 2] : prevNode;
+
+                        // 기준 노드(7 or 3)에서 목적지까지의 경로를 재귀적으로 계산 (이 때 하드코딩된 zonepath가 활용됨)
+                        var pathFromRef = CalculateScriptedPath(refNode, targetNode, secondLastNode, lastDet.MotorDirection);
+                        if (pathFromRef != null && pathFromRef.Success)
+                        {
+                            // 중복 노드 제거를 위해 첫 번째 경로의 마지막 노드 정보를 제거하고 합침
+                            pathToRef.Path.RemoveAt(pathToRef.Path.Count - 1);
+                            pathToRef.DetailedPath.RemoveAt(pathToRef.DetailedPath.Count - 1);
+                            return CombinePaths(pathToRef, pathFromRef);
+                        }
+                    }
+                }
+            }
+
+            //이곳에서 시작,종료노드가 완전히 일치하는 경로를 찾고 있다면 그것을 바로 반환한다
+            //그런경우는 복잡하게 추가 계산할 필요가 없으니까
+            var exactMatchList = zonepath.Where(d =>
+                d.NodeSta == startNode.StationType &&
+                d.NodeEnd == targetNode.StationType &&
+                d.Monitor == monitorMode);
+
+            var exactMatch = exactMatchList.FirstOrDefault(d=>
+                d.Path.First().StartsWith(startNode.RfidId.ToString()) &&
+                d.Path.Last().StartsWith(targetNode.RfidId.ToString()));
 
 
 
+            if (exactMatch != null)
+            {
+                int startIndex = exactMatch.Path.FindIndex(p => p == startTag);
+                if (startIndex == -1) startIndex = exactMatch.Path.FindIndex(p => p.StartsWith(startNode.RfidId.ToString()));
+                int endIndex = exactMatch.Path.FindLastIndex(p => p.StartsWith(targetNode.RfidId.ToString()));
+
+                if (startIndex != -1 && endIndex != -1 && startIndex <= endIndex)
+                {
+                    var slicedPath = exactMatch.Path.Skip(startIndex).Take(endIndex - startIndex + 1).ToList();
+                    return ConvertHardcodedPathToResult(slicedPath, startNode, prevNode, prevDir);
+                }
+            }
+
 
             // 시작Zone과 목표Zone이 다른 경우에만 하드코딩된 zonepath 검색
             if (startZone != targetZone)
@@ -835,10 +1026,130 @@ namespace AGVNavigationCore.PathFinding.Planning
             return AGVPathResult.CreateFailure("경로를 계산할 수 없습니다");
         }
 
+
         private MapZoneMonitor GetMonitorMode(MapNode startNode, MapNode prevNode, AgvDirection prevDir)
         {
             if (prevNode == null) return MapZoneMonitor.RightBtm;
 
+            //모니터방향도 상황에 따라 다른경우가 있다. 이것도 하드코딩하다.
+            //prev -> start 와 모터방향(prevdir) 에 따라서 경우의 수를 입력한다.
+            //일반노드가아닌 노드와, 일반노드중 7,11을 포함해서 모든 경우를 정의해야한다.
+
+            //3 - junction(turn)
+            if (startNode.RfidId == 3)
+            {
+                if (prevDir == AgvDirection.Forward)
+                {
+                    if (prevNode.RfidId == 70)
+                        return MapZoneMonitor.RightBtm;
+                    else if (prevNode.RfidId == 11 || prevNode.RfidId == 71)
+                        return MapZoneMonitor.LeftTop;
+                }
+                else if (prevDir == AgvDirection.Backward)
+                {
+                    if (prevNode.RfidId == 70)
+                        return MapZoneMonitor.LeftTop;
+                    else if (prevNode.RfidId == 11 || prevNode.RfidId == 71)
+                        return MapZoneMonitor.RightBtm;
+                }
+            }
+
+            //7 - junction
+            if (startNode.RfidId == 7)
+            {
+                if (prevDir == AgvDirection.Forward)
+                {
+                    if (prevNode.RfidId == 10 || prevNode.RfidId == 11)
+                        return MapZoneMonitor.RightBtm;
+                    else if (prevNode.RfidId == 8)
+                        return MapZoneMonitor.LeftTop;
+                }
+                else if (prevDir == AgvDirection.Backward)
+                {
+                    if (prevNode.RfidId == 10 || prevNode.RfidId == 11)
+                        return MapZoneMonitor.LeftTop;
+                    else if (prevNode.RfidId == 8)
+                        return MapZoneMonitor.RightBtm;
+                }
+            }
+
+            //70 - cleaner
+            if (startNode.RfidId == 70)
+            {
+                if (prevDir == AgvDirection.Forward)
+                {
+                    if (prevNode.RfidId == 3)
+                        return MapZoneMonitor.LeftTop;
+                }
+                else if (prevDir == AgvDirection.Backward)
+                {
+                    if (prevNode.RfidId == 3)
+                        return MapZoneMonitor.RightBtm;
+                }
+            }
+
+            //71 - loader
+            if (startNode.RfidId == 71)
+            {
+                if (prevDir == AgvDirection.Forward)
+                {
+                    if (prevNode.RfidId == 3)
+                        return MapZoneMonitor.RightBtm;
+                }
+                else if (prevDir == AgvDirection.Backward)
+                {
+                    if (prevNode.RfidId == 3)
+                        return MapZoneMonitor.LeftTop;
+                }
+            }
+
+
+            //73 - charger
+            if (startNode.RfidId == 73)
+            {
+                if (prevDir == AgvDirection.Forward)
+                {
+                    if (prevNode.RfidId == 6)
+                        return MapZoneMonitor.LeftTop;
+                }
+                else if (prevDir == AgvDirection.Backward)
+                {
+                    if (prevNode.RfidId == 6)
+                        return MapZoneMonitor.RightBtm;
+                }
+            }
+
+            //72 -- plating
+            if (startNode.RfidId == 72)
+            {
+                if (prevDir == AgvDirection.Forward)
+                {
+                    if (prevNode.RfidId == 11)
+                        return MapZoneMonitor.LeftTop;
+                }
+                else if (prevDir == AgvDirection.Backward)
+                {
+                    if (prevNode.RfidId == 11)
+                        return MapZoneMonitor.RightBtm;
+                }
+            }
+
+            //31~34,35,36 --buffer
+            if (startNode.RfidId >= 31 && startNode.RfidId <= 36)
+            {
+                if (prevDir == AgvDirection.Forward)
+                {
+                    if (prevNode.RfidId == 20)
+                        return MapZoneMonitor.LeftTop;
+                }
+                else if (prevDir == AgvDirection.Backward)
+                {
+                    if (prevNode.RfidId == 20)
+                        return MapZoneMonitor.RightBtm;
+                }
+            }
+
+
             int dx = startNode.Position.X - prevNode.Position.X;
             int dy = startNode.Position.Y - prevNode.Position.Y;
 
@@ -853,8 +1164,15 @@ namespace AGVNavigationCore.PathFinding.Planning
             }
 
             return isMonitorLeft ? MapZoneMonitor.LeftTop : MapZoneMonitor.RightBtm;
+
+
+
+
+
         }
 
+
+
         private AGVPathResult ConvertHardcodedPathToResult(List<string> pathStrings, MapNode startNode, MapNode prevNode, AgvDirection prevDir)
         {
             var result = new AGVPathResult { Success = true };

HMI/Project/StateMachine/Step/_Util.cs

@@ -194,28 +194,6 @@ namespace Project
             }
             else VAR.I32[eVarInt32.PathValidationError] = 0;
 
-            //현재위치 기준으로 재 계산하여. 더 최적화된 루트가 있다면 처리를 해준다.
-            /*
-            //TODO: 이제 경로가 하드코딩되어있으니 자동 갱신은 사용하지 않는다.
-            if (PUB._virtualAGV.CurrentPath.DetailedPath.Count > 5)
-            {
-                var PathResult2 = CalcPath(PUB._virtualAGV.CurrentNode, PUB._virtualAGV.TargetNode);
-                if (PathResult2 != null && PathResult2.Success)
-                {
-                    //절반이상 경로가 짧을때에는 재계산을 하게한다
-                    var halfcnt = (int)(PUB._virtualAGV.CurrentPath.DetailedPath.Count / 2.0);
-                    if (PathResult2.DetailedPath.Count > 2 && PathResult2.DetailedPath.Count < halfcnt)
-                    {
-                        var msg = $"단축경로가 확인되었습니다. 경로를 삭제 합니다";
-                        PUB.log.AddE(msg);
-                        Console.WriteLine(msg);
-                        PUB._virtualAGV.SetPath(null);
-                        return false;
-                    }
-
-                }
-            }
-            */
 
             //predict 를 이용하여 다음 이동을 모두 확인한다.
             var nextAction = PUB._virtualAGV.Predict();
@@ -331,6 +309,9 @@ namespace Project
             else
             {
                 //PREDICT에서 이동을 명령했따
+
+
+                
                 var bunki = arDev.Narumi.eBunki.Strate;
                 if (nextAction.Magnet == AGVNavigationCore.Models.MagnetPosition.L) bunki = arDev.Narumi.eBunki.Left;
                 else if (nextAction.Magnet == AGVNavigationCore.Models.MagnetPosition.R) bunki = arDev.Narumi.eBunki.Right;