경로역방향 발생시 프로그램 통신 죽는 현상 수정

Cs_HMI/Project/StateMachine/_AGV.cs

@@ -41,10 +41,30 @@ namespace Project
         {
             try
             {
-
-
                 if (PUB.mapctl != null)
-                    PUB.mapctl.PredictNextAction();
+                {
+                    var rlt = AGVControl.MapControlManager.PredictNextAction();
+
+                    if (rlt.ReasonCode == AGVControl.AGVActionReasonCode.busy)
+                    {
+                        //var premsg = $"이전 예측작업이 완료되지 않았습니다";
+                        //PUB.log.AddE(premsg);
+                        //PUB.log.AddE(premsg);
+                        //Console.WriteLine(premsg);
+
+                        //predicterr += 1;
+                        //if (predicterr > 10)
+                        //{
+                        //    var premsg = $"행동예측 오류카운트 초과";
+                        //    PUB.AGV.AGVMoveStop(premsg);
+                        //    PUB.log.AddE(premsg);
+                        //    PUB.logagv.AddE(premsg);
+                        //    Console.WriteLine(premsg);
+                        //}
+                    }
+                }
+        
+
 
                 switch (e.DataType)
                 {
@@ -63,12 +83,12 @@ namespace Project
 
                             //모터방향 입력
                             if (PUB.AGV.data.Direction == 'B')
-                                PUB.mapctl.agv.CurrentMOTDirection = AGVControl.Models.Direction.Backward;
+                                AGVControl.MapControlManager.agv.CurrentMOTDirection = AGVControl.Models.Direction.Backward;
                             else
-                                PUB.mapctl.agv.CurrentMOTDirection = AGVControl.Models.Direction.Forward;
+                                AGVControl.MapControlManager.agv.CurrentMOTDirection = AGVControl.Models.Direction.Forward;
 
-                            PUB.mapctl.agv.IsMoving = PUB.AGV.system1.agv_run;
+                            AGVControl.MapControlManager.agv.IsMoving = PUB.AGV.system1.agv_run;
-                            PUB.mapctl.agv.IsMarkCheck = PUB.AGV.system1.Mark1_check || PUB.AGV.system1.Mark2_check;
+                            AGVControl.MapControlManager.agv.IsMarkCheck = PUB.AGV.system1.Mark1_check || PUB.AGV.system1.Mark2_check;
 
                             if (PUB.AGV.signal.mark_sensor == false)
                             {

Cs_HMI/Project/StateMachine/_BMS.cs

@@ -158,9 +158,9 @@ namespace Project
 
         private void Bms_BMSDataReceive(object sender, EventArgs e)
         {
-            PUB.mapctl.agv.BatteryLevel = PUB.BMS.Current_Level;
+            AGVControl.MapControlManager.agv.BatteryLevel = PUB.BMS.Current_Level;
-            PUB.mapctl.agv.BatteryTemp1 = PUB.BMS.Current_temp1;
+            AGVControl.MapControlManager.agv.BatteryTemp1 = PUB.BMS.Current_temp1;
-            PUB.mapctl.agv.BatteryTemp2 = PUB.BMS.Current_temp2;
+            AGVControl.MapControlManager.agv.BatteryTemp2 = PUB.BMS.Current_temp2;
             if (PUB.BMS.Current_Level <= PUB.setting.ChargeStartLevel)
             {
                 //배터리 레벨이 기준보다 낮다면 경고를 활성화 한다

Cs_HMI/Project/StateMachine/_Xbee.cs

@@ -60,7 +60,7 @@ namespace Project
                     case ENIGProtocol.AGVCommands.Goto:  //move to tag
                         if (uint.TryParse(dataStr, out uint tagno2))
                         {
-                            var currPos = PUB.mapctl.agv.CurrentRFID;///.AGVMoveToRFID(;
+                            var currPos = AGVControl.MapControlManager.agv.CurrentRFID;///.AGVMoveToRFID(;
                             if (PUB.mapctl.SetTargetPosition(tagno2))
                                 PUB.log.AddI($"New Target {tagno2}");
                             else

Cs_HMI/SubProject/AGVControl/MapControlManager.cs

@@ -0,0 +1,504 @@
+using AGVControl.Models;
+using System;
+using System.Collections.Generic;
+using System.Drawing;
+using System.Linq;
+using System.Threading;
+
+namespace AGVControl
+{
+    public static 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>();
+
+        private static ManualResetEvent mrepredict = new ManualResetEvent(true);
+
+        /// <summary>
+        /// 목표지정으로 모터방향이 이동하고 있는가?
+        /// history 데이터가 있어야 하며 기준데이터가 없는 경우 null 반환
+        /// </summary>
+        /// <returns></returns>
+        public static bool? IsMotDirection_To_Target()
+        {
+            if (agv.MovementHistory.Any() == false || agv.MovementHistory.Count < 2) return null;
+            if (agv.MainPath.Any() == false) return null;
+
+
+            var prept = agv.MovementHistory.Skip(agv.MovementHistory.Count - 2).First();
+            var lstpt = agv.MovementHistory.Last();
+
+            //현재 이후의 경로를 가져온다
+            var curidx = agv.MainPath.FindIndex(t => t.Value == lstpt.Value);
+            var preidx = agv.MainPath.FindIndex(t => t.Value == prept.Value);
+            if (curidx == -1 || preidx == -1) return null;
+
+            //지정된경로 반대방향으로 이동하고 있다
+            return preidx < curidx;
+        }
+        public static 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()
+        {
+            if (mrepredict.WaitOne(1) == false)
+            {
+                PredictResult = new AGVActionPrediction
+                {
+                    Reason = "이전 작업이 완료되지 않았습니다",
+                    ReasonCode = AGVActionReasonCode.busy,
+                    MoveState = AGVMoveState.Stop,
+                    Direction = agv.CurrentMOTDirection,
+                };
+                return PredictResult;
+            }
+          
+            mrepredict.Reset();
+            try
+            {
+                // 0. 설정경로와 리프트 방향 체크 (경로설정이 없을때에는 직선이동경로내의 방향들과 체크한다)
+                agv.IsTargetDirectionMatch = IsLiftDirectionMatch() ?? false;
+
+                // 1. 위치를 모를 때 (CurrentRFID가 0 또는 미설정)
+                if (agv.CurrentRFID.Value == 0)
+                {
+                    PredictResult = new AGVActionPrediction
+                    {
+                        Direction = Direction.Backward,
+                        NextRFID = null,
+                        Reason = "AGV 위치 미확정(처음 기동)",
+                        ReasonCode = AGVActionReasonCode.NoPosition,
+                        MoveState = AGVMoveState.Run
+                    };
+                    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
+                    };
+                    return PredictResult;
+                }
+
+
+                // 3. 경로가 없거나 현재 위치가 경로에 없음
+                if ((agv.MainPath?.Count ?? 0) < 2)
+                {
+                    PredictResult = new AGVActionPrediction
+                    {
+                        Direction = agv.CurrentMOTDirection,
+                        NextRFID = null,
+                        Reason = "경로 없음 또는 현재 위치 미확정",
+                        ReasonCode = AGVActionReasonCode.NoPath,
+                        MoveState = AGVMoveState.Stop
+                    };
+                    return PredictResult;
+                }
+
+                // 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
+                    };
+                    return PredictResult;
+                }
+
+
+
+                // 4. 목적지 도달 전, 회전이 필요한경우인가?
+                // 목적지 RFID 정보
+                var destRFID = agv.MainPath.Last();
+
+                //리프트 방향이 맞는가?
+                var IsLiftDir = IsLiftDirectionMatch() ?? false;
+
+                //모션이동방향이 맞는가?
+                var IsMotDir = IsMotDirection_To_Target() ?? false;
+
+                var PrePT = agv.MovementHistory.Skip(agv.MovementHistory.Count - 1).First();
+                var curPT = agv.MovementHistory.Last();
+
+
+
+                //리프트방향이 맞지 않다면 회전가능한 위치로 이동을 해야한다
+                if (IsLiftDir == false)
+                {
+                    //회전가능한 위치로 이동을 해야한다
+
+
+
+                    //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
+                        };
+                        return PredictResult;
+                    }
+
+                    //2. 이동하기위한 경로계산 및 이동을 한다 (생성조건)
+                    //2-1. 서브경로가없는경우
+                    //2-2. 시작과 종료번호가 다른 경우(경로가 변경이 되는 조건이다)
+                    if (agv.CurrentRFID.Value != nearTurnPoint.Value)
+                    {
+                        if (agv.SubPath.Any() == false || agv.SubPath.Count < 2 ||
+                        agv.SubPath.First().Value != PrePT.Value ||
+                        agv.SubPath.Last().Value != nearTurnPoint.Value)
+                        {
+                            var rlt = CalculatePath(PrePT, nearTurnPoint, true);  //이전포인트도 추가를 해준다
+                            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
+                                };
+                                return PredictResult;
+                            }
+                        }
+
+
+                        //현재 모터방향을 확인하여 대상까지 이동하도록 해야한다
+                        var curidx = agv.SubPath.FindIndex(t => t.Value == curPT.Value);
+                        var preidx = agv.SubPath.FindIndex(t => t.Value == PrePT.Value);
+                        Direction newdirection = agv.CurrentMOTDirection;
+                        string message = "턴위치로 이동중";
+                        if (preidx > curidx)
+                        {
+                            //지정경로를 거꾸로 이동하고 있다
+                            if (agv.CurrentMOTDirection == Direction.Forward)
+                                newdirection = Direction.Backward;
+                            else
+                                newdirection = Direction.Forward;
+                            message += "(방향전환)";
+                        }
+
+                        PredictResult= new AGVActionPrediction
+                        {
+                            Direction = newdirection,
+                            NextRFID = nearTurnPoint,
+                            Reason = message,
+                            ReasonCode = AGVActionReasonCode.MoveForTurn,
+                            MoveState = AGVMoveState.Run,
+                        };
+                        return PredictResult;
+                    }
+
+
+                    PredictResult =  new AGVActionPrediction
+                    {
+                        Direction = agv.CurrentMOTDirection,
+                        NextRFID = nearTurnPoint,
+                        Reason = "턴 완료 대기",
+                        ReasonCode = AGVActionReasonCode.NeedTurn,
+                        MoveState = AGVMoveState.Stop
+                    };
+                    return PredictResult;
+                }
+
+                //3. 목적지위치까지 이동이 완료되지 않았다면 계속 이동을 하게한다
+                if (agv.CurrentRFID.Value != destRFID.Value)
+                {
+                    //현재 모터방향을 확인하여 대상까지 이동하도록 해야한다
+                    var curidx = agv.MainPath.FindIndex(t => t.Value == curPT.Value);
+                    var preidx = agv.MainPath.FindIndex(t => t.Value == PrePT.Value);
+                    Direction newdirection = agv.CurrentMOTDirection;
+                    string message = "목적지 이동중";
+                    if (preidx > curidx)
+                    {
+                        //지정경로를 거꾸로 이동하고 있다
+                        if (agv.CurrentMOTDirection == Direction.Forward)
+                            newdirection = Direction.Backward;
+                        else
+                            newdirection = Direction.Forward;
+                        message += "(방향전환)";
+                    }
+
+                    //경로상 바로 다음 위치를 확인한다
+                    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,
+                    };
+                    return PredictResult;
+                }
+
+
+                // 5. 목적지 도달 시
+                PredictResult = new AGVActionPrediction
+                {
+                    Direction = agv.CurrentMOTDirection,
+                    NextRFID = destRFID,
+                    Reason = "경로의 마지막 지점(목적지 도달)",
+                    ReasonCode = AGVActionReasonCode.Arrived,
+                    MoveState = AGVMoveState.Stop
+                };
+                return PredictResult;
+            }
+            catch (Exception ex)
+            {
+                PredictResult = new AGVActionPrediction
+                {
+                    Direction = agv.CurrentMOTDirection,
+                    NextRFID = null,
+                    Reason = $"ERR:{ex.Message}",
+                    ReasonCode = AGVActionReasonCode.Unknown,
+                    MoveState = AGVMoveState.Stop
+                };
+                return PredictResult;
+            }
+            finally
+            {
+                mrepredict.Set();
+            }
+
+        }
+
+
+        /// <summary>
+        /// 이웃포인터를 반환합니다
+        /// </summary>
+        /// <param name="point"></param>
+        /// <returns></returns>
+        public static List<RFIDPoint> GetNeighbors(RFIDPoint pt)
+        {
+            var neighbors = new List<RFIDPoint>();
+
+            //값이 없는 경우 오류 반환
+            if (pt == null) return neighbors;
+
+            //연결정보에서 데이터를 찾은 후 반환한다
+            foreach (var connection in rfidConnections)
+            {
+                RFIDPoint nPT = null;
+                if (connection.P1.Value == pt.Value)
+                {
+                    nPT = connection.P2;
+                }
+                else if (connection.P2.Value == pt.Value)
+                {
+                    nPT = connection.P1;
+                }
+
+                if (nPT != null) neighbors.Add(nPT);
+            }
+
+            //중복제거후 반한
+            return neighbors.Distinct().ToList();
+        }
+
+        public static 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)
+        {
+            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)
+        {
+            var path = new List<RFIDPoint> { current };
+            while (cameFrom.ContainsKey(current.Value))
+            {
+                current = cameFrom[current.Value];
+                if (path.Contains(current) == false)
+                    path.Insert(0, current);
+                else
+                    break;  //왜인지 반복되는경우가있어 종료한다
+            }
+            return new PathResult
+            {
+                Path = path,
+            };
+        }
+        public static PathResult CalculatePath(RFIDPoint start, RFIDPoint end, bool autorun)
+        {
+            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) } };
+
+
+
+            if (start.Value == end.Value)
+            {
+                return new PathResult
+                {
+                    Message = "시작위치와 대상위치가 동일 합니다",
+                    Path = null,
+                };
+            }
+
+            //if (autorun) openList.Clear();
+
+
+            while (openList.Count > 0)
+            {
+                var current = openList.OrderBy(p => fScore.ContainsKey(p) ? fScore[p] : float.MaxValue).First();
+
+                if (current.Value == end.Value)
+                {
+                    return ReconstructPath(cameFrom, current);
+                }
+
+                openList.Remove(current);
+                closedList.Add(current);
+
+                var nb = GetNeighbors(current);
+                foreach (var neighbor in nb)
+                {
+                    if (closedList.Contains(neighbor))
+                        continue;
+
+                    float tentativeGScore = gScore[current] + GetDistance(current.Location, neighbor.Location);
+
+                    if (!openList.Contains(neighbor))
+                        openList.Add(neighbor);
+                    else if (tentativeGScore >= gScore[neighbor])
+                        continue;
+
+                    cameFrom[neighbor.Value] = (RFIDPoint)current;
+                    gScore[neighbor] = tentativeGScore;
+                    fScore[neighbor] = gScore[neighbor] + Heuristic(neighbor.Location, end.Location);
+
+
+                }
+
+            }
+
+            return new PathResult
+            {
+                Path = openList,
+            };
+
+        }
+        public static PathResult CalculatePath(uint tagStrt, uint tagEnd)
+        {
+            var retval = new PathResult
+            {
+                Message = string.Empty,
+                Path = new List<RFIDPoint>(),
+            };
+
+            var startPoint = FindRFIDPoint(tagStrt);
+            var endPoint = FindRFIDPoint(tagEnd);
+            if (startPoint == null || endPoint == null)
+            {
+                retval.Message = "유효한 RFID 값을 입력해주세요.";
+                return retval;
+            }
+
+            retval = CalculatePath(startPoint, endPoint, false);
+            if (retval.Success == false)
+                retval.Message = "경로를 찾을 수 없습니다";
+
+            return retval;
+        }
+
+        /// <summary>
+        /// 리프트방향과 대상위치와의 방향이 일치하는가?
+        /// 목적지경로가 셋팅된 경우 현재 이동방향이 목적지방향과 일치하는가?
+        /// 이동경로정보가 없거나 목적지가 없으면 null 이 반환됨
+        /// </summary>
+        /// <returns></returns>
+        public static bool? IsLiftDirectionMatch()
+        {
+
+            if (agv.MovementHistory.Any() && agv.MovementHistory.Count > 1)
+            {
+                RFIDPoint TargetPT = null;
+                var prept = agv.MovementHistory.Skip(agv.MovementHistory.Count - 2).First();
+                var lstpt = agv.MovementHistory.Last();
+
+                //뒤로이동하는경우라면 이전위치에 리프트가 있다.
+                if (lstpt.Direction == Direction.Backward)
+                {
+                    TargetPT = prept;
+                }
+                else //앞으로 이동한다면 이동방향과 동일하다
+                {
+                    //이전위치는 제거 하고 처음발견된 것을 대상으로 한다
+                    TargetPT = GetNeighbors(lstpt).Where(t => t.Value != prept.Value).FirstOrDefault();
+                }
+
+                //목적지가 있다면 목적지의 방향과 일치하는지 확인해야한다
+                //남은경로중에 방향이 고정된 핀이 있다면 그것과 일치하는지 확인해야 한다
+                if (agv.MainPath.Any())
+                {
+                    //지정된경로 반대방향으로 이동하고 있다
+                    if ((IsMotDirection_To_Target() ?? false) == false)
+                    {
+                        return false;
+                    }
+                    else
+                    {
+                        var nextRoutes = agv.MainPath.Skip(agv.MainPath.FindIndex(t => t.Value == lstpt.Value) + 1).ToList();
+                        var DirectionMatch = true;
+                        foreach (var item in nextRoutes)
+                        {
+                            if (item.FixedDirection != null && item.FixedDirection != lstpt.Direction)
+                            {
+                                DirectionMatch = false;
+                                break;
+                            }
+                        }
+                        return DirectionMatch;
+                    }
+                }
+                else
+                {
+                    //대상포인트와의 방향만 체크한다.
+                    //고정대상이없다면 방향이 맞는것으로 한다
+                    return (TargetPT.FixedDirection ?? lstpt.Direction) == lstpt.Direction;
+                }
+            }
+            else
+            {
+                //이동된경로정보가 없다면 리프트 방향을 체크할 수 없으므로 대상과 위치가 맞지 않는걸로 기본값을 설정한다
+                //이렇게 설정하면 대상으로 이동불가하고 뒤로 가도록 유도된다
+                return null;
+            }
+        }
+
+
+
+    }
+}

Cs_HMI/SubProject/AGVControl/Models/AGVActionPrediction.cs

@@ -5,7 +5,7 @@ namespace AGVControl
     public class AGVActionPrediction
     {
         public Direction Direction { get; set; }
-        public uint? NextRFID { get; set; }
+        public RFIDPoint NextRFID { get; set; }
         public string Reason { get; set; }
         public AGVActionReasonCode ReasonCode { get; set; }
         public AGVMoveState MoveState { get; set; } // RUN 또는 STOP

Cs_HMI/SubProject/AGVControl/Models/enumStruct.cs

@@ -26,5 +26,6 @@ namespace AGVControl
         PathCalcError,
         NoDirection,
         MoveForTurn,
+        busy,
     }
 }

Cs_HMI/SubProject/AGVControl/agvControl.csproj

@@ -45,6 +45,7 @@
     <Reference Include="System.Xml" />
   </ItemGroup>
   <ItemGroup>
+    <Compile Include="MapControlManager.cs" />
     <Compile Include="Models\AGVActionPrediction.cs" />
     <Compile Include="BatteryLevelGauge.cs">
       <SubType>Component</SubType>