..

Cs_HMI/Project/AGV4.csproj

@@ -279,6 +279,9 @@
     <Compile Include="StateMachine\Step\_SM_RUN_CHGOFF.cs">
       <SubType>Form</SubType>
     </Compile>
+    <Compile Include="StateMachine\Step\_SM_RUN_GOTO.cs">
+      <SubType>Form</SubType>
+    </Compile>
     <Compile Include="StateMachine\Step\_SM_RUN_GOCHARGECHECK.cs">
       <SubType>Form</SubType>
     </Compile>

Cs_HMI/Project/Class/Lang.cs

@@ -9,21 +9,21 @@ namespace Project
 {
     public static class Lang
     {
-        public static string 상차 = "상차";
-        public static string 하차 = "하차";
-        public static string 연결실패 = "연결 실패";
+
+        public static string 목적지가없어대기상태로전환합니다 = "목적지가 설정되지 않아 대기상태로 전환 합니다";
+        public static string 위치로이동합니다 = "위치로 이동 합니다";
+        public static string 전방에물체가감지되었습니다 = "전방에 물체가 감지 되었습니다";
+        public static string 비상정지신호가감지되었습니다 = "비상정지 신호가 감지 되었습니다";
+        public static string 비상정지로인해작업을중단합니다 = "비상정지로 인해 작업을 중단 합니다";
+        public static string 선로를이탈했습니다 = "선로를 이탈 했습니다";
+        public static string 충전이필요합니다자동모드로전환하세요 = "충전이 필요 합니다. 자동모드로 전환 하세요";
+        public static string 충전이필요합니다 = "충전이 필요 합니다";
+        public static string 배터리통신에문제가있습니다 = "배터리 통신에 문제가 있습니다";
+        public static string 자동운전상태가아닙니다 = "자동운전 상태가 아닙니다";
         public static string 비상정지 = "비상 정지";
-        public static string 홈검색완료 = "홈 검색 완료";
+        public static string 연결실패 = "연결 실패";
         public static string 현재위치를검색합니다 = "현재 위치를 검색 합니다";
         public static string 충전완료로해제합니다 = "충전 완료로 해제 합니다";
-        public static string 오버로드감지 = "오버로드 감지";
-        public static string 상차작업을시작합니다 { get { return string.Format("{0} 작업을 시작 합니다",상차); } }
-        public static string 하차작업을시작합니다 { get { return string.Format("{0} 작업을 시작 합니다", 하차); } }
-        public static string 안전커버를내려주세요 = "안전 커버를 내려 주세요";
-        public static string 안전커버를올려주세요 = "안전 커버를 올려 주세요";
-        public static string 안전커버를올리면하차가완료됩니다 = "안전 커버를 올리면 하차가 완료 됩니다";
-        public static string 안전커버를올리면상차가완료됩니다 = "안전 커버를 올리면 상차가 완료 됩니다";
-        public static string 안전커버를내립니다 = "안전 커버를 내립니다";
         public static string 장비상태초기화 = "장비 상태 초기화";
         public static string 충전을시작합니다 = "충전을 시작 합니다";
         public static string 충전을해제합니다 = "충전을 해제 합니다";
@@ -40,36 +40,42 @@ namespace Project
         public static string 충전시작명령을전송합니다 = "충전 시작명령을 전송 합니다";
         public static string 충전기위치오류로작업을취소합니다 = "충전기 위치 오류로 인해 작업을 취소 합니다";
         public static string 프로그램이초기화되었습니다 = "프로그램이 초기화 되었습니다";
-        public static string 홈으로이동합니다 = "홈 으로 이동 합니다";
-        public static string 전방에물체가감지되었습니다 = "전방에 물체가 감지 되었습니다";
-        public static string 비상정지신호가감지되었습니다 = "비상정지 신호가 감지 되었습니다";
-        public static string 비상정지로인해작업을중단합니다 = "비상정지로 인해 작업을 중단 합니다";
-        public static string 선로를이탈했습니다 = "선로를 이탈 했습니다";
-        public static string 충전이필요합니다자동모드로전환하세요 = "충전이 필요 합니다. 자동모드로 전환 하세요";
-        public static string 충전이필요합니다 = "충전이 필요 합니다";
-        public static string 배터리통신에문제가있습니다 = "배터리 통신에 문제가 있습니다";
-        public static string 자동운전상태가아닙니다 = "자동운전 상태가 아닙니다";
-        public static string 위치로이동합니다 = "위치로 이동 합니다";
         public static string 대기상태로전환합니다 = "대기 상태로 전환 합니다";
-        public static string 홈이동완료대기상태로전환합니다 = "홈 이동이 완료 되었습니다. 대기 상태로 전환 합니다";
-        public static string 커버를내립니다 = "커버를 내립니다";
-        public static string 커버를올립니다 = "커버를 올립니다";
-        public static string 홈검색을시작합니다 = "홈 검색을 시작 합니다";
-        public static string 홈위치로이동합니다 = "홈 위치로 이동 합니다";
-        public static string 하차작업이완료되었습니다 = "하차 작업이 완료 되었습니다";
-        public static string 상차작업이완료되었습니다 = "상차 작업이 완료 되었습니다";
         public static string 작업종료 = "작업 종료";
         public static string 자동전환 = "자동 전환";
         public static string 충전기위치오류 = "충전기 위치 오류";
         public static string 다음마크위치에서정지합니다 = "다음 마크위치에서 정지 합니다";
         public static string 충전이감지되어수동충전으로전환합니다 = "충전이 감지되어 수동충전으로 전환 합니다";
+        public static string PLC연결실패 { get { return string.Format("PLC {0}", 연결실패); } }
+        public static string PLC통신실패 = "PLC 통신실패";
+        public static string AGV연결실패 { get { return string.Format("AGV {0}", 연결실패); } }
+        public static string 예측값이계산되지않아이동을중단합니다 = "이동 예측이 동작하지 않습니다. 개발부서에 문의 하세요";
+        public static string 목적지이동이완료되었습니다 = "목적지 이동 이 완료 되었습니다";
+
+
+
+        public static string 상차 = "상차";
+        public static string 하차 = "하차";
+        public static string 홈검색완료 = "홈 검색 완료";
+        public static string 오버로드감지 = "오버로드 감지";
+        public static string 상차작업을시작합니다 { get { return string.Format("{0} 작업을 시작 합니다",상차); } }
+        public static string 하차작업을시작합니다 { get { return string.Format("{0} 작업을 시작 합니다", 하차); } }
+        public static string 안전커버를내려주세요 = "안전 커버를 내려 주세요";
+        public static string 안전커버를올려주세요 = "안전 커버를 올려 주세요";
+        public static string 안전커버를올리면상차가완료됩니다 = "안전 커버를 올리면 상차가 완료 됩니다";
+        public static string 안전커버를내립니다 = "안전 커버를 내립니다";
+        public static string 홈으로이동합니다 = "홈 으로 이동 합니다";
+        public static string 홈이동완료대기상태로전환합니다 = "홈 이동이 완료 되었습니다. 대기 상태로 전환 합니다";
+        public static string 커버를내립니다 = "커버를 내립니다";
+        public static string 커버를올립니다 = "커버를 올립니다";
+     
+        public static string 홈위치로이동합니다 = "홈 위치로 이동 합니다";
+        public static string 하차작업이완료되었습니다 = "하차 작업이 완료 되었습니다";
+        public static string 상차작업이완료되었습니다 = "상차 작업이 완료 되었습니다";
         public static string 커버업대기상태가아닙니다 = "커버 업 대기 상태가 아닙니다";
         public static string QC이동버튼은상하차에서만사용가능합니다 = "QC이동 버튼은 상,하차 에서만 사용 가능합니다";
         public static string QA이동버튼은상하차에서만사용가능합니다 = "QA이동 버튼은 상,하차 에서만 사용 가능합니다";
         public static string 하차상태에서만사용가능합니다 = "하차 상태에서만 사용가능 합니다";
-
-        public static string PLC연결실패 { get { return string.Format("PLC {0}", 연결실패); } }
-        public static string PLC통신실패 = "PLC 통신실패";
-        public static string AGV연결실패 { get { return string.Format("AGV {0}",연결실패); } }
+        
     }
 }

Cs_HMI/Project/Device/Xbee.cs

@@ -24,15 +24,15 @@ namespace Project.Device
 
         public class MessageArgs : EventArgs
         {
-            public bool IsError { get; set; }   
+            public bool IsError { get; set; }
             public string Message { get; set; }
-            public MessageArgs(bool iserr,string m)
+            public MessageArgs(bool iserr, string m)
             {
                 this.IsError = iserr;
                 this.Message = m;
             }
         }
-        
+
         public event EventHandler<MessageArgs> MessageReceived;
         public event EventHandler<EEProtocol.DataEventArgs> ProtocReceived;
 
@@ -95,7 +95,7 @@ namespace Project.Device
 
             ProtocReceived?.Invoke(this, e);
 
-            
+
 
 
         }
@@ -119,7 +119,7 @@ namespace Project.Device
         public void SendMoveComplete(string tag)
         {
             var id = PUB.setting.XBE_ID;
-            byte cmd = 2;
+            byte cmd = (byte)ENIGProtocol.AGVCommandEH.Arrived;
             var data = System.Text.Encoding.Default.GetBytes(tag);
             var packet = proto.CreatePacket(id, cmd, data);
             Send(packet);
@@ -131,12 +131,28 @@ namespace Project.Device
         public void SendRFIDTag(string tag)
         {
             var id = PUB.setting.XBE_ID;
-            byte cmd = 3;
+            byte cmd = (byte)ENIGProtocol.AGVCommandEH.ReadRFID;
             var data = System.Text.Encoding.Default.GetBytes(tag);
             var packet = proto.CreatePacket(id, cmd, data);
             Send(packet);
         }
 
+        /// <summary>
+        /// 오류코드를 호스트에 전송합니다
+        /// </summary>
+        /// <param name="errcode"></param>
+        public void SendError(ENIGProtocol.AGVErrorCode errcode, string errormessage)
+        {
+            var id = PUB.setting.XBE_ID;
+            byte cmd = (byte)ENIGProtocol.AGVCommandEH.Error;
+            if (errormessage.Length > 30) errormessage = errormessage.Substring(0, 29);
+
+            var data = new byte[] { (byte)errcode };
+            var datamsg = System.Text.Encoding.Default.GetBytes(errormessage);
+
+            var packet = proto.CreatePacket(id, cmd, data);
+            Send(packet);
+        }
 
 
 
@@ -208,7 +224,8 @@ namespace Project.Device
                 Array.Copy(tagBytes, 0, data, 7, 6);
 
                 // 데이터 전송
-                var packet = proto.CreatePacket(PUB.setting.XBE_ID, 9, data);
+                var cmd = (byte)ENIGProtocol.AGVCommandEH.Status;
+                var packet = proto.CreatePacket(PUB.setting.XBE_ID, cmd, data);
                 Send(packet);
                 LastStatusSendTime = DateTime.Now;
             }

Cs_HMI/Project/StateMachine/Step/_SM_RUN.cs

@@ -29,18 +29,29 @@ namespace Project
             //가동불가 조건 확인
             if (CheckStopCondition() == false)
             {
-                PUB.sm.SetNewStep(eSMStep.PAUSE);
+                PUB.sm.SetNewStep(eSMStep.IDLE);
+                return;
+            }
+
+            //HW 연결오류
+            if (PUB.AGV.IsOpen == false)
+            {
+                PUB.Result.SetResultMessage(eResult.Hardware, eECode.AGVCONN, eNextStep.ERROR);
+                PUB.sm.SetNewStep(eSMStep.IDLE);
                 return;
             }
 
             //이머전시상태라면 stop 처리한다.
-            if (PUB.AGV.error.Emergency && PUB.AGV.system1.agv_stop == true &&
+            if (PUB.AGV.error.Emergency &&
+                PUB.AGV.system1.agv_stop == true &&
                 PUB.AGV.system1.stop_by_front_detect == false)
             {
                 PUB.Speak(Lang.비상정지로인해작업을중단합니다);
                 PUB.sm.SetNewStep(eSMStep.IDLE);
+                return;
             }
 
+
             //스텝이 변경되었다면?
             if (PUB.sm.RunStep != PUB.sm.RunStepNew)
             {
@@ -49,6 +60,7 @@ namespace Project
             }
             else runStepisFirst = false;
 
+            //처음시작이라면 시작시간을 설정한다
             if (isFirst)
             {
                 if (PUB.sm.RunStep == ERunStep.READY)
@@ -57,12 +69,52 @@ namespace Project
                     VAR.TIME.Update(eVarTime.RunStart);
             }
 
+            //자동모드에서 대기상태 (추가동작없음)
+            if (PUB.sm.RunStep == ERunStep.READY)
+            {
+                _SM_RUN_READY(runStepisFirst, PUB.sm.GetRunSteptime);
+                return;
+            }
+
+            //#############################################
+            //##  이 후에는 모든 동작루틴이 온다
+            //#############################################
+
+            //라이더 센서에서 멈춘경우 처리
+            if (PUB.AGV.system1.stop_by_front_detect == true)
+            {
+                var tsSpeak = DateTime.Now - LastSpeakTime;
+                if (tsSpeak.TotalSeconds >= PUB.setting.doorSoundTerm)
+                {
+                    PUB.Speak(Lang.전방에물체가감지되었습니다);
+                    LastSpeakTime = DateTime.Now;
+                }
+                return;
+            }
+
+            //나머지 상황체크
             switch (PUB.sm.RunStep)
             {
-                case ERunStep.READY:
-                    _SM_RUN_READY(runStepisFirst, PUB.sm.GetRunSteptime);
+                case ERunStep.GOTO: //목적지까지 이동하는 경우
+                    _SM_RUN_GOTO(runStepisFirst, PUB.sm.GetRunSteptime);
                     break;
 
+                case ERunStep.MARKSTROPB:   //후진방향으로 마크스탑
+                case ERunStep.MARKSTOPF:    //전진방향으로 마크스탑
+
+                    //이동중이지 않다면 먼저 이동을 진행한다
+                    var agvDir = PUB.sm.RunStep == ERunStep.MARKSTOPF ? arDev.Narumi.eRunOpt.Forward : arDev.Narumi.eRunOpt.Backward;
+                    PUB.AGV.AGVMoveRun(agvDir);
+
+                    //이동중이라면 마크스탑을 입력한다
+                    PUB.AGV.AGVMoveStop("run-markstropb", arDev.Narumi.eStopOpt.MarkStop);
+
+                    //마크스탑신호를 확인한다.(최대 5초)
+
+                    //신호가 확인되지 않으면 오류로 정지한다
+                    break;
+
+
                 case ERunStep.GOCHARGE: //충전위치로 이동
                     if (runStepisFirst)
                     {
@@ -196,6 +248,9 @@ namespace Project
                     }
                     break;
             }
+
+
+
         }
 
         bool CheckStopCondition()

Cs_HMI/Project/StateMachine/Step/_SM_RUN_CHGOFF.cs

@@ -14,7 +14,7 @@ namespace Project
 
         public Boolean _SM_RUN_CHGOFF(bool isFirst, TimeSpan stepTime)
         {
-            //
+            //충전중인지 확인한다.
             if (VAR.BOOL[eVarBool.FLAG_CHARGEONA] == true || PUB.AGV.system1.Battery_charging == true)
             {
                 if (isFirst)
@@ -36,7 +36,7 @@ namespace Project
                 }
                 else
                 {
-                    //OFF전송이 처음이라면 시간 섲렁
+                    //OFF전송이 처음이라면 시간 설정
                     if (VAR.TIME.IsSet(eVarTime.SendChargeOff)==false)
                         VAR.TIME[eVarTime.SendChargeOff] = DateTime.Now.AddSeconds(-10);
 
@@ -47,13 +47,12 @@ namespace Project
                         PUB.AGV.AGVCharge(PUB.setting.ChargerID, false);
                         VAR.TIME.Update(eVarTime.SendChargeOff);
                     }
-                   
                 }
                 return false;
             }
             else
             {
-                //PUB.logsys.Add($"충전OFF확인완료");
+                //중전이 해제된 상태이다
                 return true;
             }
         }

Cs_HMI/Project/StateMachine/Step/_SM_RUN_GOTO.cs

@@ -0,0 +1,297 @@
+using System;
+using System.Collections.Generic;
+using System.Drawing;
+using System.Linq;
+using System.Text;
+using Project.StateMachine;
+using COMM;
+using AR;
+
+namespace Project
+{
+    public partial class fMain
+    {
+        byte GotoTurnStep = 0;
+        DateTime GotoTurnSetTime = DateTime.Now;
+        public Boolean _SM_RUN_GOTO(bool isFirst, TimeSpan stepTime)
+        {
+            ///명령어 재전송 간격(기본 2초)
+            var CommandInterval = 2;
+
+            //충전 상태가 OFF되어야 동작하게한다
+            if (_SM_RUN_CHGOFF(isFirst, stepTime) == false)
+                return false;
+
+            //최초시작이라면 시간변수 초기화
+            if (isFirst)
+            {
+                PUB.log.Add($"[>>] _SM_RUN_GOTO");
+                VAR.TIME.Update(eVarTime.CheckGotoTargetSet);
+            }
+
+            //목적지가 설정되었는지 체크한다.
+            if (PUB.mapctl.Manager.agv.TargetRFID.IsEmpty)
+            {
+                //최대 5초간 설정여부를 확인하고
+                if (VAR.TIME.RUN(eVarTime.CheckGotoTargetSet).TotalSeconds > 5)
+                {
+                    //실패시에는 READY로 전환한다.
+                    PUB.sm.SetNewRunStep(ERunStep.READY);
+                    PUB.Speak(Lang.목적지가없어대기상태로전환합니다);
+                }
+                return false;
+            }
+
+            var idx = 1;
+            var BeforePredictIdx = -1;
+            var predict = PUB.mapctl.Manager.PredictResult;
+            if (PUB.sm.RunStepSeq == idx++)
+            {
+                PUB.Speak(Lang.위치로이동합니다);
+                PUB.log.Add($"목적지 위치 이동시작({PUB.mapctl.Manager.agv.TargetRFID.Value})");
+                VAR.TIME.Update(eVarTime.CheckGotoTargetSet);
+                VAR.TIME.Set(eVarTime.SendGotoCommand, DateTime.Now.AddDays(-1));
+                PUB.sm.UpdateRunStepSeq();
+                return false;
+            }
+            else if (PUB.sm.RunStepSeq == idx++)
+            {
+                //멈춰야하는경우
+                if (predict.MoveState == AGVControl.AGVMoveState.Stop)
+                {
+                    if (PUB.AGV.system1.agv_run)
+                    {
+                        if (VAR.TIME.RUN(eVarTime.SendGotoCommand).TotalSeconds > 2)
+                        {
+                            PUB.Speak("AGV Stop");
+                            PUB.AGV.AGVMoveStop("Predict", arDev.Narumi.eStopOpt.Stop);
+                            VAR.TIME.Update(eVarTime.SendGotoCommand);
+                        }
+                        return false;
+                    }
+                    else
+                    {
+                        //완료되었거나 턴을진행해야한다
+                        if (predict.ReasonCode == AGVControl.AGVActionReasonCode.Arrived ||
+                            predict.ReasonCode == AGVControl.AGVActionReasonCode.NeedTurnMove ||
+                            predict.ReasonCode == AGVControl.AGVActionReasonCode.NeedTurnPoint)
+                        {
+                            GotoTurnStep = 0;
+                            GotoTurnSetTime = DateTime.Now.AddDays(-1);
+                            PUB.sm.UpdateRunStepSeq();
+                        }
+                        return false;
+                    }
+                }
+                else //이동해야하는 경우
+                {
+                    //속도와 방향이 불일치하는 경우 다시 설정한다 (속도: H,L,M,[S]
+                    AGVControl.AgvDir AGV_Direction = (AGVControl.AgvDir)PUB.AGV.data.Direction;
+                    AGVControl.AgvSpeed AGV_Speed = (AGVControl.AgvSpeed)PUB.AGV.data.Speed;
+                    AGVControl.AgvSts AGV_Sts = (AGVControl.AgvSts)PUB.AGV.data.Sts;
+
+                    //상태값이 바뀌었다면 전송을 해야한다
+                    if (predict.Direction != AGV_Direction || predict.MoveSpeed != AGV_Speed || predict.MoveDiv != AGV_Sts)
+                    {
+                        if (VAR.TIME.RUN(eVarTime.SendGotoCommand).TotalSeconds > CommandInterval)
+                        {
+                            arDev.Narumi.eBunki v_bunki = arDev.Narumi.eBunki.Strate;
+                            if (predict.MoveDiv == AGVControl.AgvSts.Straight) v_bunki = arDev.Narumi.eBunki.Strate;
+                            else if (predict.MoveDiv == AGVControl.AgvSts.Left) v_bunki = arDev.Narumi.eBunki.Left;
+                            else if (predict.MoveDiv == AGVControl.AgvSts.Right) v_bunki = arDev.Narumi.eBunki.Right;
+
+                            arDev.Narumi.eMoveDir v_dir = arDev.Narumi.eMoveDir.Backward;
+                            if (predict.Direction == AGVControl.AgvDir.Forward) v_dir = arDev.Narumi.eMoveDir.Forward;
+                            else if (predict.Direction == AGVControl.AgvDir.Backward) v_dir = arDev.Narumi.eMoveDir.Backward;
+
+                            arDev.Narumi.eMoveSpd v_spd = arDev.Narumi.eMoveSpd.Low;
+                            if (predict.MoveSpeed == AGVControl.AgvSpeed.Middle) v_spd = arDev.Narumi.eMoveSpd.Middle;
+                            else if (predict.MoveSpeed == AGVControl.AgvSpeed.High) v_spd = arDev.Narumi.eMoveSpd.High;
+                            else if (predict.MoveSpeed == AGVControl.AgvSpeed.Low) v_spd = arDev.Narumi.eMoveSpd.Low;
+
+                            //이동셋팅을 해준다
+                            PUB.AGV.AGVMoveSet(new arDev.Narumi.BunkiData
+                            {
+                                Bunki = v_bunki,
+                                Direction = v_dir,
+                                PBSSensor = 1,
+                                Speed = v_spd,
+                            });
+
+                            if (predict.MoveSpeed == AGVControl.AgvSpeed.MarkStop)
+                            {
+                                PUB.AGV.AGVMoveStop("Predict", arDev.Narumi.eStopOpt.Stop);
+                            }
+                            VAR.TIME.Update(eVarTime.SendGotoCommand);
+                        }
+                        return false;
+                    }
+
+                    //정지상태라면 이동 명령을 전달한다
+                    if (PUB.AGV.system1.agv_run == false)
+                    {
+                        if (VAR.TIME.RUN(eVarTime.SendGotoCommand).TotalSeconds > CommandInterval)
+                        {
+                            PUB.Speak("AGV Start");
+
+                            arDev.Narumi.eRunOpt v_dir = arDev.Narumi.eRunOpt.Backward;
+                            if (predict.Direction == AGVControl.AgvDir.Forward) v_dir = arDev.Narumi.eRunOpt.Forward;
+                            else if (predict.Direction == AGVControl.AgvDir.Backward) v_dir = arDev.Narumi.eRunOpt.Backward;
+
+                            PUB.AGV.AGVMoveRun(v_dir);
+                            VAR.TIME.Update(eVarTime.SendGotoCommand);
+                        }
+                        return false;
+                    }
+                }
+
+                //예측이 업데이트되지 않으면 오류 처리해야한다
+                if (BeforePredictIdx == -1) BeforePredictIdx = (int)predict.Idx;
+                else if (BeforePredictIdx != predict.Idx)   //이전사용한 IDX와  다르다면 예측이 실행된 경우이다
+                    BeforePredictIdx = (int)predict.Idx;
+                else
+                {
+                    //5초이상 예측값이 업데이트되지 않으면 오류 처리한다.
+                    var tsPredict = DateTime.Now - predict.CreateTime;
+                    if (tsPredict.TotalSeconds > 5)
+                    {
+                        PUB.XBE.SendError(ENIGProtocol.AGVErrorCode.PredictFix, Lang.예측값이계산되지않아이동을중단합니다);
+                        PUB.Speak(Lang.예측값이계산되지않아이동을중단합니다);
+                        PUB.sm.SetNewRunStep(ERunStep.READY);
+                    }
+                }
+
+                return false;
+            }
+            else if (PUB.sm.RunStepSeq == idx++)
+            {
+                if (predict.ReasonCode == AGVControl.AGVActionReasonCode.Arrived)
+                {
+                    PUB.Speak(Lang.목적지이동이완료되었습니다);
+                    PUB.sm.SetNewRunStep(ERunStep.READY);
+                    PUB.sm.UpdateRunStepSeq();
+                }
+                else if (predict.ReasonCode == AGVControl.AGVActionReasonCode.NeedTurnMove ||
+                    predict.ReasonCode == AGVControl.AGVActionReasonCode.NeedTurnPoint)
+                {
+                    //턴을 해야하는 경우이다
+                    //좌턴을 기본으로 진행하며, 좌턴이동 후 마크스탑을 입력한다
+                    if (GotoTurnStep == 0)
+                    {
+                        //턴을 한적이 없으므로 턴을 먼저 진행한다
+                        arDev.Narumi.eMoveDir moveDir = arDev.Narumi.eMoveDir.Backward;
+                        if (predict.Direction == AGVControl.AgvDir.Forward) moveDir = arDev.Narumi.eMoveDir.Forward;
+                        if (PUB.AGV.data.Sts != 'L' || PUB.AGV.data.Speed != 'L' || PUB.AGV.data.Direction != moveDir.ToString()[0])
+                        {
+                            //셋팅이 다르다면 3초간격으로 전송한다
+                            var tsTurnSet = DateTime.Now - GotoTurnSetTime;
+                            if (tsTurnSet.TotalSeconds > 3)
+                            {
+                                PUB.AGV.AGVMoveSet(new arDev.Narumi.BunkiData
+                                {
+                                    Bunki = arDev.Narumi.eBunki.Left,
+                                    Direction = moveDir,
+                                    PBSSensor = 1,
+                                    Speed = arDev.Narumi.eMoveSpd.Low,
+                                });
+                                GotoTurnSetTime = DateTime.Now;
+                                PUB.log.Add("Turn Bunki Set");
+                            }
+                        }
+                        else
+                        {
+                            PUB.mapctl.Manager.agv.CurrentRFID.TurnOK = false;
+                            PUB.mapctl.Manager.agv.CurrentRFID.TurnStart = DateTime.Now;
+                            PUB.sm.UpdateRunStepSeq(); //셋팅이 맞으니 다음스텝으로 진행한다
+                            GotoTurnStep += 1;
+                        }
+                    }
+                }
+                else PUB.sm.UpdateRunStepSeq();
+                return false;
+            }
+            else if (PUB.sm.RunStepSeq == idx++)
+            {
+                //턴이완료되길 기다린다.
+                if (predict.ReasonCode == AGVControl.AGVActionReasonCode.NeedTurnMove ||
+                   predict.ReasonCode == AGVControl.AGVActionReasonCode.NeedTurnPoint)
+                {
+                    //(최소5초는 기다리고 판단한다)
+                    if (stepTime.TotalSeconds < 5) return false;
+
+                    //최대30초는 기다려준다
+                    if (stepTime.TotalSeconds > 30)
+                    {
+                        var ermsg = "Turn Timeout(30sec)";
+                        PUB.log.AddE(ermsg);
+                        PUB.XBE.SendError(ENIGProtocol.AGVErrorCode.TurnTimeout, ermsg);
+                        PUB.sm.SetNewRunStep(ERunStep.READY);
+                    }
+
+                    //모션이 멈추었다면 턴이완료된것이다.
+                    if (PUB.AGV.system1.agv_stop)
+                    {
+                        if (PUB.AGV.system1.Mark1_check == false && PUB.AGV.system1.Mark2_check == false)
+                        {
+                            PUB.log.AddE($"Turn 완료이나 Mark 센서가 확인되지 않았습니다");
+                        }
+                        GotoTurnStep += 1;
+                        PUB.sm.UpdateRunStepSeq();
+                    }
+                    else
+                    {
+                        //아직 이동중이므로 대기한다
+                    }
+                }
+                else PUB.sm.UpdateRunStepSeq(); //기타사항은 다음으로 넘어간다
+                return false;
+            }
+            else if (PUB.sm.RunStepSeq == idx++)
+            {
+                if (predict.ReasonCode == AGVControl.AGVActionReasonCode.NeedTurnMove ||
+                    predict.ReasonCode == AGVControl.AGVActionReasonCode.NeedTurnPoint)
+                {
+                    if (GotoTurnStep < 2)
+                    {
+                        PUB.XBE.SendError(ENIGProtocol.AGVErrorCode.TurnError, "턴시퀀스 완료 실패");
+                        PUB.log.AddE($"턴완료시퀀스가 2가아닙니다. 대기 상태로 강제 전환합니다");
+                        PUB.sm.SetNewRunStep(ERunStep.READY);
+                    }
+                    else
+                    {
+                        PUB.log.AddI("Turn Complete");
+                    }
+
+                    //방향전환용 턴이라면 이동기록을 추가해서 방향이 맞도록 처리해주자
+                    if (predict.ReasonCode == AGVControl.AGVActionReasonCode.NeedTurnMove)
+                    {
+                        var rfid = PUB.mapctl.Manager.agv.CurrentRFID;
+                        var lastHistory = PUB.mapctl.Manager.agv.MovementHistory.Last();
+                        //원래방향에서 반대로 처리한다
+                        var revDir = lastHistory.Direction == AGVControl.AgvDir.Backward ? AGVControl.AgvDir.Forward : AGVControl.AgvDir.Backward;
+                        PUB.mapctl.Manager.agv.AddToMovementHistory(rfid.Value, rfid.Location, revDir);
+                    }
+                    else
+                    {
+                        //이동용 RFID에서 턴명령이 들어있는경우였다
+                        PUB.mapctl.Manager.agv.CurrentRFID.TurnEnd = DateTime.Now;
+                        PUB.mapctl.Manager.agv.CurrentRFID.TurnOK = true;
+                    }
+
+                    PUB.sm.UpdateRunStepSeq();
+                }
+                else PUB.sm.UpdateRunStepSeq();
+                return false;
+            }
+
+
+            //좌턴이동명령 전송
+
+            //마크스탑전송
+
+            //마크스탑이 확인되면 나머지는 경로예측에 맡긴다
+
+            return true;
+        }
+    }
+}

Cs_HMI/Project/StateMachine/_AGV.cs

@@ -53,15 +53,33 @@ namespace Project
                             var chg_stop = PUB.AGV.system1.GetChanged(arDev.Narumi.SystemFlag1.eflag.agv_stop);
                             //if (chg_run && PUB.AGV.system1.agv_run) PUB.Speak("이동을 시작 합니다");
                             VAR.BOOL[eVarBool.AGVDIR_UP] = PUB.AGV.data.Direction == 'B';
-                            // PUB.AGV.signal.mark_sensor = PUB.AGV.signal.mark_sensor;
                             VAR.BOOL[eVarBool.AGV_ERROR] = PUB.AGV.error.Value > 0;
                             VAR.BOOL[eVarBool.EMERGENCY] = PUB.AGV.error.Emergency;
 
                             //모터방향 입력
                             if (PUB.AGV.data.Direction == 'B')
-                                PUB.mapctl.Manager.agv.CurrentMOTDirection = AGVControl.Direction.Backward;
+                                PUB.mapctl.Manager.agv.Current_Motor_Direction = AGVControl.AgvDir.Backward;
                             else
-                                PUB.mapctl.Manager.agv.CurrentMOTDirection = AGVControl.Direction.Forward;
+                                PUB.mapctl.Manager.agv.Current_Motor_Direction = AGVControl.AgvDir.Forward;
+
+                            //현재 속도
+                            if (PUB.AGV.data.Speed == 'H')
+                                PUB.mapctl.Manager.agv.CurrentSpeed = AGVControl.AgvSpeed.High;
+                            else if (PUB.AGV.data.Speed == 'M')
+                                PUB.mapctl.Manager.agv.CurrentSpeed = AGVControl.AgvSpeed.Middle;
+                            else if (PUB.AGV.data.Speed == 'L')
+                                PUB.mapctl.Manager.agv.CurrentSpeed = AGVControl.AgvSpeed.Low;
+                            else if (PUB.AGV.data.Speed == 'S')
+                                PUB.mapctl.Manager.agv.CurrentSpeed = AGVControl.AgvSpeed.MarkStop;
+
+                            //이동방향
+                            if (PUB.AGV.data.Sts == 'S')
+                                PUB.mapctl.Manager.agv.CurrentSTS = AGVControl.AgvSts.Straight;
+                            else if (PUB.AGV.data.Sts == 'L')
+                                PUB.mapctl.Manager.agv.CurrentSTS = AGVControl.AgvSts.Left;
+                            else if (PUB.AGV.data.Sts == 'R')
+                                PUB.mapctl.Manager.agv.CurrentSTS = AGVControl.AgvSts.Right;
+
 
                             PUB.mapctl.Manager.agv.IsMoving = PUB.AGV.system1.agv_run;
                             PUB.mapctl.Manager.agv.IsMarkCheck = PUB.AGV.system1.Mark1_check || PUB.AGV.system1.Mark2_check;

Cs_HMI/Project/StateMachine/_Xbee.cs

@@ -24,7 +24,7 @@ namespace Project
             {
                 var data = e.ReceivedPacket.Data;
                 var dataStr = System.Text.Encoding.Default.GetString(data);
-                var cmd = (ENIGProtocol.AGVCommands)e.ReceivedPacket.Command;
+                var cmd = (ENIGProtocol.AGVCommandHE)e.ReceivedPacket.Command;
                 var TargetID = 0;
                 if (dataStr.Length >= 2)
                 {
@@ -47,7 +47,7 @@ namespace Project
                 switch (cmd)
                 {
 
-                    case ENIGProtocol.AGVCommands.SetCurrent: //Set Current Position
+                    case ENIGProtocol.AGVCommandHE.SetCurrent: //Set Current Position
 
                         if (dataStr.Length == 6)
                         {
@@ -76,7 +76,7 @@ namespace Project
                         else PUB.log.AddE($"Position Param Error:{dataStr}");
 
                         break;
-                    case ENIGProtocol.AGVCommands.Goto:  //move to tag
+                    case ENIGProtocol.AGVCommandHE.Goto:  //move to tag
                         if (uint.TryParse(dataStr, out uint tagno2))
                         {
                             var currPos = PUB.mapctl.Manager.agv.CurrentRFID;///.AGVMoveToRFID(;
@@ -87,14 +87,14 @@ namespace Project
                         }
                         else PUB.log.AddE($"Path Param Error :{dataStr}");
                         break;
-                    case ENIGProtocol.AGVCommands.Stop:  //stop
+                    case ENIGProtocol.AGVCommandHE.Stop:  //stop
                         PUB.AGV.AGVMoveStop("xbee");
                         break;
-                    case ENIGProtocol.AGVCommands.Reset: //Error Reset
+                    case ENIGProtocol.AGVCommandHE.Reset: //Error Reset
                         PUB.AGV.AGVErrorReset();
                         break;
 
-                    case ENIGProtocol.AGVCommands.Manual: //Manual Move (Direction, speed, runtime)
+                    case ENIGProtocol.AGVCommandHE.Manual: //Manual Move (Direction, speed, runtime)
                         var Direction = data[0];     //0=back, 1=forward, 2=left, 3=right
                         var Speed = data[1];         //0=slow, 1=normal, 2=fast
                         var Runtime = data[2];       // running seconds
@@ -113,13 +113,13 @@ namespace Project
 
                         PUB.AGV.AGVMoveManual(opt, spd, arDev.Narumi.Sensor.PBSOn);
                         break;
-                    case ENIGProtocol.AGVCommands.MarkStop: //Set MarkStop
+                    case ENIGProtocol.AGVCommandHE.MarkStop: //Set MarkStop
                         var MarkStop = data[0];    //0=off, 1=on
 
                         //마크센서에서 멈추게 한다
                         PUB.AGV.AGVMoveStop("Xbee", arDev.Narumi.eStopOpt.MarkStop);
                         break;
-                    case ENIGProtocol.AGVCommands.LiftControl: //Lift Control
+                    case ENIGProtocol.AGVCommandHE.LiftControl: //Lift Control
                         var LiftCommand = data[0];    //0=stop, 1=up, 2=down
                         arDev.Narumi.LiftCommand LCmd = arDev.Narumi.LiftCommand.STP;
                         if (LiftCommand == 1) LCmd = arDev.Narumi.LiftCommand.UP;