diff --git a/HMI/Project/StateMachine/Step/_SM_RUN_BUFFER_OUT.cs b/HMI/Project/StateMachine/Step/_SM_RUN_BUFFER_OUT.cs index 4bbc427..605820f 100644 --- a/HMI/Project/StateMachine/Step/_SM_RUN_BUFFER_OUT.cs +++ b/HMI/Project/StateMachine/Step/_SM_RUN_BUFFER_OUT.cs @@ -56,7 +56,7 @@ namespace Project else if (PUB.sm.RunStepSeq == idx++) { //회전이없다면 오류처리한다. - if(PUB._virtualAGV.Turn != AGVNavigationCore.Models.AGVTurn.L90) + if (PUB._virtualAGV.Turn != AGVNavigationCore.Models.AGVTurn.L90) { SetRunStepError(ENIGProtocol.AGVErrorCode.BUFFER_NOT_COMPLETE, $"[{funcname}-{PUB.sm.RunStepSeq}] 버퍼에서 나오려면 버퍼진입작업이 완료되어있어야 합니다"); return false; @@ -299,6 +299,148 @@ namespace Project PUB.sm.UpdateRunStepSeq(); return false; } + else if (PUB.sm.RunStepSeq == idx++) + { + //1.전진으로 3초간 이동한다 - 방향/속도 설정 + var ret = PUB.AGV.AGVMoveSet(new arDev.Narumi.BunkiData + { + Bunki = arDev.Narumi.eBunki.Strate, + Direction = arDev.Narumi.eMoveDir.Forward, + PBSSensor = 2, + Speed = arDev.Narumi.eMoveSpd.Low, + }); + + if (ret != arDev.eNarumiCommandResult.Success) + { + PUB.log.AddE($"[{funcname}-{PUB.sm.RunStepSeq}] 얼라인 전진속도 설정 실패(AGVMoveSet)"); + SetRunStepError(ENIGProtocol.AGVErrorCode.AGV_SPEED_SET_FAIL); + return false; + } + + PUB.log.Add($"[{funcname}-{PUB.sm.RunStepSeq}] 장비 얼라인 전진 이동 설정 (Dir:Forward)"); + PUB.sm.UpdateRunStepSeq(); + return false; + } + else if (PUB.sm.RunStepSeq == idx++) + { + //전진이동 시작 + PUB.log.Add($"[{funcname}-{PUB.sm.RunStepSeq}] 장비 얼라인 전진 구동 시작"); + PUB.AGV.AGVMoveRun(arDev.Narumi.eRunOpt.Forward); + PUB.sm.UpdateRunStepSeq(); + return false; + } + else if (PUB.sm.RunStepSeq == idx++) + { + //3초간 이동한다. + if (seqtime.TotalSeconds < 3) return false; + PUB.sm.UpdateRunStepSeq(); + return false; + } + else if (PUB.sm.RunStepSeq == idx++) + { + //2.멈춘다 + PUB.AGV.AGVMoveStop(funcname); + PUB.log.Add($"[{funcname}-{PUB.sm.RunStepSeq}] 3초 전진 완료 후 정지 명령 전달"); + PUB.sm.UpdateRunStepSeq(); + return false; + } + else if (PUB.sm.RunStepSeq == idx++) + { + //AGV 멈출때까지 기다림 + if (PUB.AGV.system1.agv_run == true) + { + if (seqtime.TotalSeconds > 5) + { + PUB.AGV.AGVMoveStop(funcname + "[TIMEOUT]"); + SetRunStepError(ENIGProtocol.AGVErrorCode.AGV_STOP_FAIL); + return false; + } + return false; + } + PUB.log.Add($"[{funcname}-{PUB.sm.RunStepSeq}] AGV 전진 정지 확인 완료"); + PUB.sm.UpdateRunStepSeq(); + return false; + } + else if (PUB.sm.RunStepSeq == idx++) + { + //3. 후진으로 이동하면서 마크스탑을 설정한다 - 방향/속도 설정 + var ret = PUB.AGV.AGVMoveSet(new arDev.Narumi.BunkiData + { + Bunki = arDev.Narumi.eBunki.Strate, + Direction = arDev.Narumi.eMoveDir.Backward, + PBSSensor = 2, + Speed = arDev.Narumi.eMoveSpd.Low, + }); + + if (ret != arDev.eNarumiCommandResult.Success) + { + PUB.log.AddE($"[{funcname}-{PUB.sm.RunStepSeq}] 얼라인 후진속도 설정 실패(AGVMoveSet)"); + SetRunStepError(ENIGProtocol.AGVErrorCode.AGV_SPEED_SET_FAIL); + return false; + } + PUB.log.Add($"[{funcname}-{PUB.sm.RunStepSeq}] 장비 얼라인 후진 이동 설정 (Dir:Backward)"); + PUB.sm.UpdateRunStepSeq(); + return false; + } + else if (PUB.sm.RunStepSeq == idx++) + { + //후진 이동 시작 + PUB.AGV.AGVMoveRun(arDev.Narumi.eRunOpt.Backward); + PUB.log.Add($"[{funcname}-{PUB.sm.RunStepSeq}] 장비 얼라인 후진 구동 시작"); + PUB.sm.UpdateRunStepSeq(); + return false; + } + else if (PUB.sm.RunStepSeq == idx++) + { + //마크스탑을 설정한다. + if (seqtime.TotalSeconds < 1.0) return false; // 짧은 구동 후 마크스탑 설정 + PUB.AGV.AGVMoveStop(funcname, arDev.Narumi.eStopOpt.MarkStop); + PUB.log.Add($"[{funcname}-{PUB.sm.RunStepSeq}] 얼라인 후진 MarkStop 명령 전송"); + PUB.sm.UpdateRunStepSeq(); + return false; + } + else if (PUB.sm.RunStepSeq == idx++) + { + //마크스탑 신호 확인 + if (PUB.AGV.data.Speed != 'S') + { + if (seqtime.TotalSeconds > 10) + { + SetRunStepError(ENIGProtocol.AGVErrorCode.MARK_TIMEOUT); + return false; + } + return false; + } + PUB.log.Add($"[{funcname}-{PUB.sm.RunStepSeq}] MarkStop 신호 확인 완료"); + PUB.sm.UpdateRunStepSeq(); + return false; + } + else if (PUB.sm.RunStepSeq == idx++) + { + //4. agv정지가 확인되면 완료이다. + if (PUB.AGV.system1.agv_run == true) + { + if (seqtime.TotalSeconds > 10) + { + SetRunStepError(ENIGProtocol.AGVErrorCode.AGV_STOP_FAIL); + return false; + } + return false; + } + PUB.log.Add($"[{funcname}-{PUB.sm.RunStepSeq}] 장비 얼라인 완료 (AGV 완벽 정지 확인)"); + + //완료후 라이더를 켜기위해서 기본 값을 복구한다 + PUB.AGV.AGVMoveSet(new arDev.Narumi.BunkiData + { + Bunki = arDev.Narumi.eBunki.Strate, + Direction = arDev.Narumi.eMoveDir.Backward, + PBSSensor = 2, + Speed = arDev.Narumi.eMoveSpd.Low, + }); + + PUB.sm.UpdateRunStepSeq(); + return false; + } PUB.AddEEDB($"[{funcname}] bufferout 완료({PUB.Result.TargetPos})"); return true; diff --git a/HMI/SubProject/AGV/Command.cs b/HMI/SubProject/AGV/Command.cs index 0c71793..fbb267e 100644 --- a/HMI/SubProject/AGV/Command.cs +++ b/HMI/SubProject/AGV/Command.cs @@ -24,6 +24,7 @@ namespace arDev { public DateTime Start { get; set; } public DateTime End { get; set; } + public string Message { get; set; } public TimeSpan Runtime { get @@ -41,6 +42,13 @@ namespace arDev End = new DateTime(1982, 11, 23); State = eNarumiTurn.None; } + public bool IsTurning + { + get + { + return State == eNarumiTurn.LeftIng || State == eNarumiTurn.RightIng; + } + } } public class NarumiCommandTime { @@ -76,6 +84,7 @@ namespace arDev { public eNarmiPBSSensor PBSSensor = eNarmiPBSSensor.notset; + public NarumiTurnInfo TurnInformation { get; set; } = null; Dictionary SendCommandFailList { get; set; } = new Dictionary(); @@ -297,6 +306,38 @@ namespace arDev } } + /// + /// 턴정보를 설정한다 + /// + /// + void TurnInformationSetting(eNarumiTurn newstate) + { + if (TurnInformation == null) TurnInformation = new NarumiTurnInfo(); + TurnInformation.Start = DateTime.Now; + TurnInformation.End = new DateTime(1982, 11, 23); + if (TurnInformation.State != newstate) + { + TurnInformation.Message = $"Change {TurnInformation.State} -> {newstate}"; + TurnInformation.State = newstate; + RaiseMessage(MessageType.Normal, TurnInformation.Message); + } + } + + /// + /// 턴이진행중일경우에는 상태를 해제한다 + /// + void TurnInformationSettingCancel() + { + if (TurnInformation == null) return; + if (TurnInformation.State == eNarumiTurn.LeftIng || TurnInformation.State == eNarumiTurn.RightIng) + { + TurnInformation.End = DateTime.Now; + TurnInformation.State = eNarumiTurn.None; + TurnInformation.Message = "Turningstatus cancled"; + RaiseMessage(MessageType.Normal, TurnInformation.Message); + } + } + protected eNarumiCommandResult AddCommand(eAgvCmd command, BunkiData param) { @@ -332,10 +373,14 @@ namespace arDev cmdString = $"CST{param}"; system1.agv_run_manual = false; retval = AddCommand(cmdString); + if (retval == eNarumiCommandResult.Success) + TurnInformationSettingCancel(); break; case eAgvCmd.MoveStart: cmdString = $"CRN{param}"; retval = AddCommand(cmdString); + if (retval == eNarumiCommandResult.Success) + TurnInformationSettingCancel(); break; case eAgvCmd.ChargeOf: @@ -354,10 +399,7 @@ namespace arDev retval = AddCommand(cmdString); if (retval == eNarumiCommandResult.Success) { - if (TurnInformation == null) TurnInformation = new NarumiTurnInfo(); - TurnInformation.Start = DateTime.Now; - TurnInformation.End = new DateTime(1982, 11, 23); - TurnInformation.State = eNarumiTurn.LeftIng; + TurnInformationSetting(eNarumiTurn.LeftIng); } break; case eAgvCmd.TurnRight: @@ -365,10 +407,7 @@ namespace arDev retval = AddCommand(cmdString); if (retval == eNarumiCommandResult.Success) { - if (TurnInformation == null) TurnInformation = new NarumiTurnInfo(); - TurnInformation.Start = DateTime.Now; - TurnInformation.End = new DateTime(1982, 11, 23); - TurnInformation.State = eNarumiTurn.RightIng; + TurnInformationSetting(eNarumiTurn.RightIng); } break; case eAgvCmd.BackAndTurn: @@ -399,6 +438,8 @@ namespace arDev system1.agv_run_manual = true; cmdString = $"CRT{param}"; retval = AddCommand(cmdString); + if (retval == eNarumiCommandResult.Success) + TurnInformationSettingCancel(); break; case eAgvCmd.LiftControl: diff --git a/HMI/SubProject/AGV/Narumi.cs b/HMI/SubProject/AGV/Narumi.cs index 4d1289c..1a62339 100644 --- a/HMI/SubProject/AGV/Narumi.cs +++ b/HMI/SubProject/AGV/Narumi.cs @@ -72,7 +72,7 @@ namespace arDev /// public event EventHandler DataReceive; public event EventHandler TurnComplete; - + #endregion @@ -344,11 +344,20 @@ namespace arDev var tagnostr = rcvdNow.Substring(3); if (ushort.TryParse(tagnostr, out ushort tagnoint)) { - var Changed = !old_TagString.Equals(tagnostr); - data.TagString = tagnostr; - data.TagNo = tagnoint; - old_TagString = tagnostr; - DataReceive?.Invoke(this, new DataEventArgs(DataType.TAG)); + if (TurnInformation != null && TurnInformation.IsTurning) + { + //턴진행중에 들어온 태그는 무시하자 260226 + RaiseMessage(MessageType.Normal, $"Tag Ignore by Turn (Tag:{tagnostr})"); + } + else + { + //var Changed = !old_TagString.Equals(tagnostr); + data.TagString = tagnostr; + data.TagNo = tagnoint; + old_TagString = tagnostr; + DataReceive?.Invoke(this, new DataEventArgs(DataType.TAG)); + } + } WriteData(MakeCheckSum("ACKTAG")); @@ -476,7 +485,7 @@ namespace arDev Mid, Low, } - + private bool CheckSum(byte[] bData) {