352 lines
15 KiB
C#
352 lines
15 KiB
C#
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
|
|
{
|
|
DateTime LastSpeakTime = DateTime.Now;
|
|
DateTime CoverControlTime = DateTime.Now;
|
|
DateTime LastCommandTime = DateTime.Now;
|
|
DateTime LastCommandTimeNextStop = DateTime.Now;
|
|
bool runStepisFirst = false;
|
|
|
|
private void _SM_RUN(Boolean isFirst, TimeSpan stepTime)
|
|
{
|
|
//HW 연결오류
|
|
if (PUB.AGV.IsOpen == false)
|
|
{
|
|
PUB.Result.SetResultMessage(eResult.Hardware, eECode.AGVCONN, eNextStep.ERROR);
|
|
PUB.sm.SetNewStep(eSMStep.IDLE);
|
|
return;
|
|
}
|
|
|
|
//가동불가 조건 확인
|
|
if (CheckStopCondition() == false) return;
|
|
|
|
//중단기능이 동작이라면 처리하지 않는다.
|
|
if (PUB.sm.bPause)
|
|
{
|
|
System.Threading.Thread.Sleep(200);
|
|
return;
|
|
}
|
|
|
|
//스텝이 변경되었다면?
|
|
if (PUB.sm.RunStep != PUB.sm.RunStepNew)
|
|
{
|
|
runStepisFirst = true;
|
|
PUB.sm.ApplyRunStep();
|
|
}
|
|
else runStepisFirst = false;
|
|
|
|
//처음시작이라면 시작시간을 설정한다
|
|
if (isFirst)
|
|
{
|
|
if (PUB.sm.RunStep == ERunStep.READY)
|
|
VAR.TIME.Update(eVarTime.ReadyStart);
|
|
else
|
|
VAR.TIME.Update(eVarTime.RunStart);
|
|
VAR.I32[eVarInt32.PathValidationError] = 0;
|
|
}
|
|
|
|
//자동모드에서 대기상태 (추가동작없음)
|
|
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.alarmSoundTerm)
|
|
{
|
|
PUB.Speak(Lang.전방에물체가감지되었습니다);
|
|
LastSpeakTime = DateTime.Now;
|
|
}
|
|
return;
|
|
}
|
|
|
|
//선로이탈감지
|
|
if (PUB.AGV.error.runerror_by_no_magent_line == true)
|
|
{
|
|
var tsSpeak = DateTime.Now - LastSpeakTime;
|
|
if (tsSpeak.TotalSeconds >= PUB.setting.alarmSoundTerm)
|
|
{
|
|
PUB.Speak(Lang.선로를이탈했습니다);
|
|
LastSpeakTime = DateTime.Now;
|
|
}
|
|
return;
|
|
}
|
|
|
|
//현재위치를 모르는 상태라면 이동하여 현재 위치를 찾는다
|
|
if (_SM_RUN_POSCHK(isFirst, stepTime) == false)
|
|
{
|
|
PUB.Result.result_message = "현재 위치 확인 중";
|
|
PUB.Result.result_progressmax = 0;
|
|
return;
|
|
}
|
|
|
|
//나머지 상황체크
|
|
switch (PUB.sm.RunStep)
|
|
{
|
|
case ERunStep.GOHOME:
|
|
if (_SM_RUN_GOHOME(runStepisFirst, PUB.sm.GetRunSteptime) == true)
|
|
{
|
|
PUB.log.Add($"홈 이동이 완료되어 준비상태로 전환합니다");
|
|
PUB.sm.SetNewRunStep(ERunStep.READY);
|
|
}
|
|
break;
|
|
case ERunStep.GOTO: //목적지까지 이동하는 경우
|
|
if (_SM_RUN_GOTO(runStepisFirst, PUB.sm.GetRunSteptime) == true)
|
|
{
|
|
//목적지가 BUFFER라면 버퍼투입대기위치까지 완료했다는 시그널을 보낸다.
|
|
var target = PUB._virtualAGV.TargetNode;
|
|
PUB.log.Add($"목적지({target.RfidId}) 도착완료 타입:{target.Type}, 출발지:{PUB._virtualAGV.StartNode.RfidId}");
|
|
|
|
PUB.XBE.StepLoader = Device.eDocStep.NotSet;
|
|
PUB.XBE.StepCleaner = Device.eDocStep.NotSet;
|
|
PUB.XBE.StepUnloader = Device.eDocStep.NotSet;
|
|
PUB.XBE.StepBuffer = Device.eDocStep.NotSet;
|
|
|
|
switch (target.StationType)
|
|
{
|
|
case AGVNavigationCore.Models.StationType.Buffer:
|
|
var lastPath = PUB._virtualAGV.CurrentPath.DetailedPath.LastOrDefault();
|
|
if (lastPath.NodeId.Equals(PUB._virtualAGV.CurrentNode.Id))
|
|
{
|
|
//버퍼진입전 노드에 도착완료했따
|
|
PUB.XBE.StepBuffer = Device.eDocStep.InReady;
|
|
}
|
|
else
|
|
{
|
|
//마지막위치가 아닌 다른 위치에 있으니 버퍼 작업을 할 수없다
|
|
PUB.XBE.StepBuffer = Device.eDocStep.NotSet;
|
|
PUB.log.AddE($"목적지가 버퍼이나 노드가 불일치 한다 오류사항");
|
|
PUB._mapCanvas.SetAlertMessage("목적지가 버퍼이나 노드 불일치 오류");
|
|
PUB.sm.SetNewRunStep(ERunStep.ERROR);
|
|
}
|
|
break;
|
|
case AGVNavigationCore.Models.StationType.Charger1:
|
|
case AGVNavigationCore.Models.StationType.Charger2:
|
|
|
|
break;
|
|
|
|
case AGVNavigationCore.Models.StationType.Loader:
|
|
PUB.XBE.StepLoader = Device.eDocStep.InReady;
|
|
break;
|
|
|
|
case AGVNavigationCore.Models.StationType.Clearner:
|
|
PUB.XBE.StepCleaner = Device.eDocStep.InReady;
|
|
break;
|
|
|
|
case AGVNavigationCore.Models.StationType.UnLoader:
|
|
PUB.XBE.StepUnloader = Device.eDocStep.InReady;
|
|
break;
|
|
|
|
default:
|
|
PUB.log.Add($"정의되지 않은 스테이션 입니다({target.StationType}) ");
|
|
break;
|
|
}
|
|
|
|
PUB._virtualAGV.Turn = AGVNavigationCore.Models.AGVTurn.None;
|
|
PUB.sm.SetNewRunStep(ERunStep.READY);
|
|
}
|
|
break;
|
|
|
|
case ERunStep.MARKSTOPB: //후진방향으로 마크스탑
|
|
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)
|
|
{
|
|
VAR.TIME[eVarTime.ChargeTry] = DateTime.Now;
|
|
PUB.sm.ResetRunStepSeq();
|
|
PUB.log.Add("충전 명령 시작");
|
|
}
|
|
else if (_SM_RUN_GOCHARGE(runStepisFirst, PUB.sm.GetRunSteptime))
|
|
{
|
|
PUB.Speak(Lang.충전을시작합니다);
|
|
PUB.sm.SetNewRunStep(ERunStep.CHARGECHECK);
|
|
return;
|
|
}
|
|
break;
|
|
|
|
case ERunStep.CHARGECHECK: //충전중
|
|
if (runStepisFirst)
|
|
{
|
|
VAR.TIME.Update(eVarTime.ChargeStart);
|
|
LastCommandTime = DateTime.Now;
|
|
}
|
|
else if (_SM_RUN_GOCHARGECHECK(runStepisFirst, PUB.sm.GetRunSteptime))
|
|
{
|
|
//충전상태가 활성화되었으므로 대기상태로 전환한다
|
|
PUB.sm.SetNewRunStep(ERunStep.READY);
|
|
PUB.log.AddAT("충전상태 확인 완료로 인해 대기 합니다");
|
|
}
|
|
break;
|
|
|
|
case ERunStep.CHARGEOFF:
|
|
if (runStepisFirst)
|
|
{
|
|
VAR.TIME.Update(eVarTime.ChargeEnd);
|
|
LastCommandTime = DateTime.Now;
|
|
}
|
|
else
|
|
{
|
|
//충전 상태가 OFF되어야 동작하게한다
|
|
if (_SM_RUN_CHGOFF(isFirst, stepTime) == true)
|
|
{
|
|
//충전상태가 활성화되었으므로 대기상태로 전환한다
|
|
PUB.sm.ClearRunStep();
|
|
|
|
//대기상태로 전환
|
|
PUB.sm.SetNewRunStep(ERunStep.READY);
|
|
PUB.log.AddAT("충전 해제로 대기상태로 전환 합니다");
|
|
}
|
|
}
|
|
break;
|
|
case ERunStep.LOADER_OUT: //로더아웃
|
|
if (_SM_RUN_LOADER_OUT(runStepisFirst, PUB.sm.GetRunSteptime))
|
|
{
|
|
PUB.Speak(Lang.버퍼도킹해제완료);
|
|
|
|
//도킹완료상태를 업데이트한다.
|
|
PUB.XBE.StepLoader = Device.eDocStep.OutComplete;
|
|
|
|
//대기상태로 전환
|
|
PUB.sm.SetNewRunStep(ERunStep.READY);
|
|
return;
|
|
}
|
|
break;
|
|
|
|
case ERunStep.LOADER_IN: //로더도킹
|
|
if (_SM_RUN_LOADER_IN(runStepisFirst, PUB.sm.GetRunSteptime))
|
|
{
|
|
PUB.Speak(Lang.버퍼도킹이완료되었습니다);
|
|
|
|
//도킹완료상태를 업데이트한다.
|
|
PUB.XBE.StepLoader = Device.eDocStep.InComplete;
|
|
|
|
//대기상태로 전환
|
|
PUB.sm.SetNewRunStep(ERunStep.READY);
|
|
return;
|
|
}
|
|
break;
|
|
|
|
case ERunStep.UNLOADER_OUT: //언로더아웃
|
|
if (_SM_RUN_UNLOADER_OUT(runStepisFirst, PUB.sm.GetRunSteptime))
|
|
{
|
|
PUB.Speak(Lang.버퍼도킹해제완료);
|
|
|
|
//도킹완료상태를 업데이트한다.
|
|
PUB.XBE.StepUnloader = Device.eDocStep.OutComplete;
|
|
|
|
//대기상태로 전환
|
|
PUB.sm.SetNewRunStep(ERunStep.READY);
|
|
return;
|
|
}
|
|
break;
|
|
|
|
case ERunStep.UNLOADER_IN: //언로더도킹
|
|
if (_SM_RUN_UNLOADER_IN(runStepisFirst, PUB.sm.GetRunSteptime))
|
|
{
|
|
PUB.Speak(Lang.버퍼도킹이완료되었습니다);
|
|
|
|
//도킹완료상태를 업데이트한다.
|
|
PUB.XBE.StepUnloader = Device.eDocStep.InComplete;
|
|
|
|
//대기상태로 전환
|
|
PUB.sm.SetNewRunStep(ERunStep.READY);
|
|
return;
|
|
}
|
|
break;
|
|
|
|
case ERunStep.CLEANER_OUT: //클리너아웃
|
|
if (_SM_RUN_CLEANER_OUT(runStepisFirst, PUB.sm.GetRunSteptime))
|
|
{
|
|
PUB.Speak(Lang.버퍼도킹해제완료);
|
|
|
|
//도킹완료상태를 업데이트한다.
|
|
PUB.XBE.StepCleaner = Device.eDocStep.OutComplete;
|
|
|
|
//대기상태로 전환
|
|
PUB.sm.SetNewRunStep(ERunStep.READY);
|
|
return;
|
|
}
|
|
break;
|
|
|
|
case ERunStep.CLEANER_IN: //클리너도킹
|
|
if (_SM_RUN_CLEANER_IN(runStepisFirst, PUB.sm.GetRunSteptime))
|
|
{
|
|
PUB.Speak(Lang.버퍼도킹이완료되었습니다);
|
|
|
|
//도킹완료상태를 업데이트한다.
|
|
PUB.XBE.StepCleaner = Device.eDocStep.InComplete;
|
|
|
|
//클리너아웃으로 자동 진행하지 않음
|
|
PUB.sm.SetNewRunStep(ERunStep.READY);
|
|
return;
|
|
}
|
|
break;
|
|
|
|
case ERunStep.BUFFER_OUT: //버퍼아웃
|
|
if (_SM_RUN_BUFFER_OUT(runStepisFirst, PUB.sm.GetRunSteptime))
|
|
{
|
|
PUB.Speak(Lang.버퍼도킹해제완료);
|
|
|
|
//도킹완료상태를 업데이트한다.
|
|
PUB.XBE.StepBuffer = Device.eDocStep.OutComplete;
|
|
|
|
//대기상태로 전환
|
|
PUB.sm.SetNewRunStep(ERunStep.READY);
|
|
return;
|
|
}
|
|
else PUB.XBE.StepBuffer = Device.eDocStep.OutIng;
|
|
break;
|
|
|
|
case ERunStep.BUFFER_IN: //버퍼도킹
|
|
if (_SM_RUN_BUFFER_IN(runStepisFirst, PUB.sm.GetRunSteptime))
|
|
{
|
|
PUB.Speak(Lang.버퍼도킹이완료되었습니다);
|
|
|
|
//도킹완료상태를 업데이트한다.
|
|
PUB.XBE.StepBuffer = Device.eDocStep.InComplete;
|
|
|
|
//대기상태로 전환
|
|
PUB.sm.SetNewRunStep(ERunStep.READY);
|
|
return;
|
|
}
|
|
else PUB.XBE.StepBuffer = Device.eDocStep.InIng;
|
|
break;
|
|
}
|
|
|
|
}
|
|
|
|
}//cvass
|
|
}
|