Amkor/ENIG
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

파일정리

Browse Source
This commit is contained in:
ChiKyun Kim
2026-01-29 14:03:17 +09:00
parent 00cc0ef5b7
commit 58ca67150d
440 changed files with 47236 additions and 99165 deletions
Download Patch File Download Diff File Expand all files Collapse all files

296
HMI/Project/StateMachine/Step/_SM_RUN.cs Normal file
View File

@@ -0,0 +1,296 @@
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_GOTO_HOME(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.StepMC = 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.StepMC = Device.eDocStep.ReadyForEnter;
}
else
{
//마지막위치가 아닌 다른 위치에 있으니 버퍼 작업을 할 수없다
PUB.XBE.StepMC = 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.StepMC = Device.eDocStep.ReadyForEnter;
break;
case AGVNavigationCore.Models.StationType.Clearner:
PUB.XBE.StepMC = Device.eDocStep.ReadyForEnter;
break;
case AGVNavigationCore.Models.StationType.UnLoader:
PUB.XBE.StepMC = Device.eDocStep.ReadyForEnter;
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_CHARGE_GO(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_CHARGE_CHECK(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_CHARGE_GOFF(isFirst, stepTime) == true)
{
//충전상태가 활성화되었으므로 대기상태로 전환한다
PUB.sm.ClearRunStep();
//대기상태로 전환
PUB.sm.SetNewRunStep(ERunStep.READY);
PUB.log.AddAT("충전 해제로 대기상태로 전환 합니다");
}
}
break;
case ERunStep.CLEANER_IN: //클리너도킹
case ERunStep.UNLOADER_IN: //언로더도킹
case ERunStep.LOADER_IN: //로더도킹
if (_SM_RUN_ENTER(runStepisFirst, PUB.sm.GetRunSteptime))
{
PUB.Speak(Lang.버퍼도킹이완료되었습니다);
//도킹완료상태를 업데이트한다.
PUB.XBE.StepMC = Device.eDocStep.EnterComplete;
//대기상태로 전환
PUB.sm.SetNewRunStep(ERunStep.READY);
return;
}
break;
case ERunStep.CLEANER_OUT: //클리너아웃
case ERunStep.UNLOADER_OUT: //언로더아웃
case ERunStep.LOADER_OUT: //로더아웃
if (_SM_RUN_EXIT(runStepisFirst, PUB.sm.GetRunSteptime))
{
PUB.Speak(Lang.버퍼도킹해제완료);
//도킹완료상태를 업데이트한다.
PUB.XBE.StepMC = Device.eDocStep.ExitComplete;
//대기상태로 전환
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.StepMC = Device.eDocStep.ExitComplete;
//대기상태로 전환
PUB.sm.SetNewRunStep(ERunStep.READY);
return;
}
else PUB.XBE.StepMC = Device.eDocStep.ExitIng;
break;
case ERunStep.BUFFER_IN: //버퍼도킹
if (_SM_RUN_BUFFER_IN(runStepisFirst, PUB.sm.GetRunSteptime))
{
PUB.Speak(Lang.버퍼도킹이완료되었습니다);
//도킹완료상태를 업데이트한다.
PUB.XBE.StepMC = Device.eDocStep.EnterComplete;
//대기상태로 전환
PUB.sm.SetNewRunStep(ERunStep.READY);
return;
}
else PUB.XBE.StepMC = Device.eDocStep.EnterIng;
break;
}
}
}//cvass
}

286
HMI/Project/StateMachine/Step/_SM_RUN_BUFFER_IN.cs Normal file
View File

@@ -0,0 +1,286 @@
using System;
using System.Collections.Generic;
using System.Drawing;
using System.Linq;
using System.Text;
using Project.StateMachine;
using COMM;
using AR;
using System.Windows.Media.Animation;
using System.Net.Security;
namespace Project
{
public partial class fMain
{
public Boolean _SM_RUN_BUFFER_IN(bool isFirst, TimeSpan seqtime)
{
var funcname = "_SM_RUN_BUFFER_IN";
var idx = 1;
//충전 상태가 OFF되어야 동작하게한다
if (_SM_RUN_CHARGE_GOFF(isFirst, seqtime) == false) return false;
//라이더멈춤이 설정되어있다면 음성으로 알려준다
if (CheckLiderStop() == false) return false;
/*
* 버퍼IN시퀀스
* 1. 회전이 진행되지 않았다면 회전을 진행한다.
* 2. LIFT DOWN
* 3. 후진-저속-마크다운 실행
*/
if (PUB.sm.RunStepSeq == idx++)
{
PUB.log.Add($"[{funcname}] 버퍼진입시작({PUB.NextWorkCmd}) Turn:{PUB._virtualAGV.Turn}");
PUB.Speak(Lang.진입을작합니다);
PUB.sm.UpdateRunStepSeq();
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
//모션 전후진 제어
if (PUB._virtualAGV.Turn != AGVNavigationCore.Models.AGVTurn.L90)
{
//동작중이면 동작을 멈춘다
if (PUB.AGV.system1.agv_run == true)
{
var ts = VAR.TIME.RUN(eVarTime.LastStopCommandTime);
if (ts.TotalSeconds > 3)
{
PUB.AGV.AGVMoveStop(funcname);
VAR.TIME.Update(eVarTime.LastStopCommandTime);
}
}
else PUB.sm.UpdateRunStepSeq(); //agv가 멈춰있으므로 턴을 진행해야 한다
}
else PUB.sm.UpdateRunStepSeq(); //이미완료된상태이므로 다음으로 진행한다.
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
//이미 턴이 되어있는지 확인한다 (재진입 시 중복 실행 방지)
if (PUB._virtualAGV.Turn == AGVNavigationCore.Models.AGVTurn.L90)
{
PUB.log.Add($"[{funcname}] 이미 Left Turn 완료 상태입니다. 턴 명령을 건너뜁니다.");
PUB.sm.UpdateRunStepSeq();
}
else
{
//하드웨어 상태 확인
var turnState = PUB.AGV.TurnInformation?.State ?? arDev.eNarumiTurn.None;
if (turnState == arDev.eNarumiTurn.Left || turnState == arDev.eNarumiTurn.LeftIng)
{
//이미 좌회전 중이거나 완료된 하드웨어 상태
PUB.log.Add($"[{funcname}] 하드웨어 좌회전 상태 확인됨({turnState}). 명령을 건너뜁니다.");
PUB.sm.UpdateRunStepSeq();
}
else if (turnState == arDev.eNarumiTurn.Right || turnState == arDev.eNarumiTurn.RightIng)
{
//비정상 상태 (우회전 중?)
SetRunStepError(ENIGProtocol.AGVErrorCode.TURN_FAIL, $"[{funcname}] 턴 방향 불일치(Current:{turnState}). 우회전 상태에서 좌회전을 시도할 수 없습니다.");
}
else
{
//정상 (None) -> 턴 명령 실행
PUB.AGV.AGVMoveLeft180Turn();
PUB.log.Add("AGV Left Turn");
VAR.TIME.Update(eVarTime.LastTurnCommandTime);
PUB.sm.UpdateRunStepSeq();
}
}
PUB._mapCanvas.SetAlertMessage($"턴 진행 중");
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
//이미 완료된 상태라면 대기 과정을 건너뛴다.
if (PUB._virtualAGV.Turn == AGVNavigationCore.Models.AGVTurn.L90)
{
PUB.sm.UpdateRunStepSeq();
return false;
}
//왼쪽턴이 완료되지 않은경우
if (PUB.AGV.TurnInformation.State != arDev.eNarumiTurn.Left)
{
//움직임 확인을 위해 3초간은 검증을 유예한다
if (PUB.AGV.TurnInformation.Runtime.TotalSeconds < 3) return false;
//턴 이동 상태가 확인되어야 한다.
var overtime = 30;
if (PUB.AGV.TurnInformation.Runtime.TotalSeconds > overtime)
{
//30초동안 AGV까 움직이지 않았다면 오류 처리한다.
SetRunStepError(ENIGProtocol.AGVErrorCode.TURN_FAIL, $"[{funcname}] {overtime}초이내 턴 감지 안됨");
return false;
}
else PUB._mapCanvas.SetAlertMessage($"턴 진행 중({PUB.AGV.TurnInformation.Runtime.TotalSeconds:N0}/{overtime})");
return false;
}
PUB._virtualAGV.Turn = AGVNavigationCore.Models.AGVTurn.L90; //턴완료
PUB.log.Add($"[{funcname}] Turn(left) 완료");
PUB.sm.UpdateRunStepSeq(); //이미완료된상태이므로 다음으로 진행한다.
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
//버퍼에들어갈때에는 가져다 놓을때도 가지러 갈때에도 리프트는 내려서 들어간다
var liftCmd = arDev.Narumi.LiftCommand.DN;
PUB.AGV.LiftControl(liftCmd);
PUB.log.Add($"[{funcname}] 리프트를 내립니다");
VAR.TIME.Update(eVarTime.LastTurnCommandTime);
PUB.sm.UpdateRunStepSeq();
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
//리프트 센서 확인
if (PUB.AGV.signal1.lift_down == false)
{
if (seqtime.TotalSeconds > 20)
{
PUB.log.AddAT($"[{funcname}] 리프트가 내려가지 않아 1회 재시도 합니다");
PUB.AGV.LiftControl(arDev.Narumi.LiftCommand.DN);
//재시도를 했으니 다음으로 진행하게한다
}
else
{
PUB._mapCanvas.SetAlertMessage($"리프트 하강 확인 중({seqtime.TotalSeconds:N0}/20)");
return false;
}
}
PUB.sm.UpdateRunStepSeq();
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
//리프트 센서 확인
if (PUB.AGV.signal1.lift_down == false)
{
if (seqtime.TotalSeconds > 20)
{
SetRunStepError(ENIGProtocol.AGVErrorCode.LIFT_ERROR, $"[{funcname}] 리프트가 내려가지 않습니다");
}
else PUB._mapCanvas.SetAlertMessage($"리프트 하강 확인 중({seqtime.TotalSeconds:N0}/20)");
return false;
}
PUB.sm.UpdateRunStepSeq();
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
//저속이동 (후진 진입)
// [Smart Restart] 재시작 시 안전 검사
// 이미 턴을 완료했고(L90), 현재 마크 센서가 감지된다면(ON),
// 이미 목적지(Mark 2)에 도착한 것으로 간주하여 후진을 생략한다.
if (PUB._virtualAGV.Turn == AGVNavigationCore.Models.AGVTurn.L90 && PUB.AGV.signal1.mark_sensor == true)
{
PUB.log.Add($"[{funcname}] 이미 목적지 도착 확인됨(Turn:L90, Sensor:ON). 후진 이동을 생략합니다.");
PUB.sm.UpdateRunStepSeq();
return false;
}
var ret = PUB.AGV.AGVMoveSet(new arDev.Narumi.BunkiData
{
Bunki = arDev.Narumi.eBunki.Strate,
Direction = arDev.Narumi.eMoveDir.Backward,
PBSSensor = 0,
Speed = arDev.Narumi.eMoveSpd.Low,
});
//명령이 실패되었다면 재시도를 한다
if (ret != arDev.eNarumiCommandResult.Success)
{
if (ret >= arDev.eNarumiCommandResult.Error)
{
SetRunStepError(ENIGProtocol.AGVErrorCode.AGV_SPEED_SET_FAIL);
}
return false;
}
//후진이동을한다
PUB._mapCanvas.SetAlertMessage($"도킹을 위해 후진 이동 시작");
PUB.AGV.AGVMoveRun(arDev.Narumi.eRunOpt.Backward);
PUB.sm.UpdateRunStepSeq();
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
//이동확인을 한다.
if(PUB.AGV.system1.agv_run == false)
{
if(seqtime.TotalSeconds > 3)
{
SetRunStepError(ENIGProtocol.AGVErrorCode.AGV_RUN_FAIL);
}
return false;
}
PUB.sm.UpdateRunStepSeq();
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
//마크스탑으로 이동
PUB.AGV.AGVMoveStop(funcname, arDev.Narumi.eStopOpt.MarkStop);
PUB._mapCanvas.SetAlertMessage($"도킹을 위해 후진 이동 시작");
PUB.sm.UpdateRunStepSeq();
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
//마크스탑신호가 3초이내로 들어와야 한다
if (PUB.AGV.data.Speed != 'S')
{
if (seqtime.TotalSeconds > 3)
{
SetRunStepError(ENIGProtocol.AGVErrorCode.MARK_TIMEOUT);
return false;
}
}
VAR.TIME.Update(eVarTime.LastTurnCommandTime);
PUB.sm.UpdateRunStepSeq();
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
//AGV가 멈출때까지 기다린다.
if (PUB.AGV.system1.agv_run == true)
{
var ts = VAR.TIME.RUN(eVarTime.LastTurnCommandTime);
if (ts.TotalSeconds > 10)
{
SetRunStepError(ENIGProtocol.AGVErrorCode.AGV_STOP_FAIL);
return false;
}
return false;
}
VAR.TIME.Update(eVarTime.LastTurnCommandTime);
PUB.sm.UpdateRunStepSeq();
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
if (seqtime.TotalSeconds < 2) return false;
PUB.log.Add($"[{funcname}] 작업({PUB.NextWorkCmd}) 완료. 대기 상태로 전환 (퇴출 명령 대기)");
PUB.sm.UpdateRunStepSeq();
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
//완료되었다.
PUB.log.Add($"[{funcname}] 버퍼 진입 및 작업 완료");
PUB.sm.UpdateRunStepSeq();
return false;
}
// 작업을 마치고 설비 안에 멈춰있는 상태.
// ACS가 이 상태를 확인하고 NextWorkCmd로 퇴출(Out) 명령을 보내야 함.
PUB.AddEEDB($"[{funcname}] 버퍼작업완료({PUB.Result.TargetPos})");
return true;
}
}
}

251
HMI/Project/StateMachine/Step/_SM_RUN_BUFFER_OUT.cs Normal file
View File

@@ -0,0 +1,251 @@
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
{
public Boolean _SM_RUN_BUFFER_OUT(bool isFirst, TimeSpan seqtime)
{
var funcname = "_SM_RUN_BUFFER_IN";
var idx = 1;
//충전 상태가 OFF되어야 동작하게한다
if (_SM_RUN_CHARGE_GOFF(isFirst, seqtime) == false) return false;
//라이더멈춤이 설정되어있다면 음성으로 알려준다
if (CheckLiderStop() == false) return false;
/*
* 버퍼 OUT 시퀀스
* 1-1. PickOn 라면 리프트를 Up 한다
* 1-2. PickOff 라면 리프트를 Down 한다
* 2.전진-저속-마크다운
* 3.Turn-Right-180
*/
if (PUB.sm.RunStepSeq == idx++)
{
PUB.log.Add($"[{funcname}] 버퍼진출시작({PUB.NextWorkCmd}) Turn:{PUB._virtualAGV.Turn}");
PUB.Speak(Lang.진출을시작합니다);
PUB.sm.UpdateRunStepSeq();
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
arDev.Narumi.LiftCommand lift = PUB.NextWorkCmd == ENIGProtocol.AGVCommandHE.PickOnExit ? arDev.Narumi.LiftCommand.UP : arDev.Narumi.LiftCommand.DN;
PUB.log.Add($"[{funcname}] 리프트제어 {lift}");
PUB.AGV.LiftControl(lift);
PUB.sm.UpdateRunStepSeq();
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
//리프트 센서 확인
var checksensor = PUB.NextWorkCmd == ENIGProtocol.AGVCommandHE.PickOnExit ? PUB.AGV.signal1.lift_up : PUB.AGV.signal1.lift_down;
if (checksensor == false)
{
if (seqtime.TotalSeconds > 20)
{
PUB.log.AddAT($"[{funcname}] 리프트가 완료되지 않아 1회 재시도 합니다");
arDev.Narumi.LiftCommand lift = PUB.NextWorkCmd == ENIGProtocol.AGVCommandHE.PickOnExit ? arDev.Narumi.LiftCommand.UP : arDev.Narumi.LiftCommand.DN;
PUB.AGV.LiftControl(lift);
}
else return false;
}
PUB.sm.UpdateRunStepSeq();
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
//리프트 센서 확인
var checksensor = PUB.NextWorkCmd == ENIGProtocol.AGVCommandHE.PickOnExit ? PUB.AGV.signal1.lift_up : PUB.AGV.signal1.lift_down;
if (checksensor == false)
{
if (seqtime.TotalSeconds > 20)
{
SetRunStepError(ENIGProtocol.AGVErrorCode.LIFT_ERROR, $"[{funcname}] 리프트가 동작하지 않습니다");
}
return false;
}
PUB.sm.UpdateRunStepSeq();
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
//빈 상태로 아웃해야한다.
var ret = PUB.AGV.AGVMoveSet(new arDev.Narumi.BunkiData
{
Bunki = arDev.Narumi.eBunki.Strate,
Direction = arDev.Narumi.eMoveDir.Forward,
PBSSensor = 0,
Speed = arDev.Narumi.eMoveSpd.Low,
});
//명령이 실패되었다면 재시도를 한다
if (ret != arDev.eNarumiCommandResult.Success)
{
if (ret >= arDev.eNarumiCommandResult.Error)
{
SetRunStepError(ENIGProtocol.AGVErrorCode.AGV_SPEED_SET_FAIL);
}
return false;
}
PUB.sm.UpdateRunStepSeq();
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
//전진이동
PUB.AGV.AGVMoveRun(arDev.Narumi.eRunOpt.Forward);
PUB.sm.UpdateRunStepSeq();
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
//AGV구동을 확인하고 마크스탑을 설정한다.
if (PUB.AGV.system1.agv_run == false)
{
if (seqtime.TotalSeconds > 3)
{
//구동이확인되지 않으면 오류처리를 한다.
SetRunStepError(ENIGProtocol.AGVErrorCode.AGV_RUN_FAIL);
}
return false;
}
//마크스탑설정
PUB.AGV.AGVMoveStop(funcname, arDev.Narumi.eStopOpt.MarkStop);
PUB.sm.UpdateRunStepSeq();
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
//마크스탑신호가 3초이내로 들어와야 한다
if (PUB.AGV.data.Speed != 'S')
{
if (seqtime.TotalSeconds > 3)
{
SetRunStepError(ENIGProtocol.AGVErrorCode.MARK_TIMEOUT);
}
return false;
}
PUB.sm.UpdateRunStepSeq();
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
//AGV가 멈출때까지 기다린다.
if (PUB.AGV.system1.agv_run == true)
{
if (seqtime.TotalSeconds > 15)
{
SetRunStepError(ENIGProtocol.AGVErrorCode.AGV_STOP_FAIL);
}
return false;
}
PUB.sm.UpdateRunStepSeq();
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
//우측으로 180도 턴
//이미 턴이 되어있는지 확인한다 (재진입 시 중복 실행 방지)
if (PUB._virtualAGV.Turn == AGVNavigationCore.Models.AGVTurn.R90)
{
PUB.log.Add($"[{funcname}] 이미 Right Turn 완료 상태입니다. 턴 명령을 건너뜁니다.");
PUB.sm.UpdateRunStepSeq();
}
else
{
//하드웨어 상태 확인
var turnState = PUB.AGV.TurnInformation?.State ?? arDev.eNarumiTurn.None;
if (turnState == arDev.eNarumiTurn.Right || turnState == arDev.eNarumiTurn.RightIng)
{
//이미 우회전 중이거나 완료된 하드웨어 상태
PUB.log.Add($"[{funcname}] 하드웨어 우회전 상태 확인됨({turnState}). 명령을 건너뜁니다.");
PUB.sm.UpdateRunStepSeq();
}
else if (turnState == arDev.eNarumiTurn.Left || turnState == arDev.eNarumiTurn.LeftIng)
{
//비정상 상태 (좌회전 중?)
SetRunStepError(ENIGProtocol.AGVErrorCode.TURN_FAIL, $"[{funcname}] 턴 방향 불일치(Current:{turnState}). 좌회전 상태에서 우회전을 시도할 수 없습니다.");
}
else
{
//정상 (None) -> 턴 명령 실행
PUB.AGV.AGVMoveRight180Turn();
PUB.sm.UpdateRunStepSeq();
}
}
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
//이동확인
if (PUB.AGV.system1.agv_run == false)
{
if (seqtime.TotalSeconds > 3)
{
//만약 이미 완료된 상태라서 건너뛰었다면 run이 안될 수 있다.
//하지만 위에서 건너뜀 -> UpdateRunStepSeq -> 바로 여기로 옴.
//건너뛴 경우(Turn == R90), Run 확인을 할 필요가 없으므로...
//로직 보완: 턴 상태가 이미 완료라면 이동확인도 스킵해야 함?
//아니면 위 단계에서 완료 상태면 '다음 다음' 단계로 점프?
//간단하게: Turn이 R90이면 그냥 통과시킴.
if (PUB._virtualAGV.Turn == AGVNavigationCore.Models.AGVTurn.R90)
{
//이미 완료된 상태이므로 이동 확인 패스
PUB.sm.UpdateRunStepSeq();
return false;
}
SetRunStepError(ENIGProtocol.AGVErrorCode.TURN_FAIL);
}
return false;
}
PUB.sm.UpdateRunStepSeq();
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
//멈춤확인
if (PUB.AGV.system1.agv_run == true)
{
if (seqtime.TotalSeconds > 25)
{
SetRunStepError(ENIGProtocol.AGVErrorCode.TURN_FAIL);
}
return false;
}
//턴 완료 상태 업데이트
if (PUB._virtualAGV.Turn != AGVNavigationCore.Models.AGVTurn.R90)
{
PUB._virtualAGV.Turn = AGVNavigationCore.Models.AGVTurn.R90;
PUB.log.Add($"[{funcname}] Turn State Updated to R90");
}
PUB.sm.UpdateRunStepSeq();
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
PUB.log.Add("BufferOut Complete");
PUB.sm.UpdateRunStepSeq();
return false;
}
PUB.AddEEDB($"bufferout 완료({PUB.Result.TargetPos})");
return true;
}
}
}

59
HMI/Project/StateMachine/Step/_SM_RUN_CHARGE_CHECK.cs Normal file
View File

@@ -0,0 +1,59 @@
using System;
using System.Collections.Generic;
using System.Drawing;
using System.Linq;
using System.Text;
using COMM;
using AR;
using Project.StateMachine;
namespace Project
{
public partial class fMain
{
public Boolean _SM_RUN_CHARGE_CHECK(bool isFirst, TimeSpan stepTime)
{
//충전상태체크를 확인합니다
//10초안에 충전 플래그가 활성화되지 않으면 QC상태로 이동한다
if (PUB.AGV.system1.Battery_charging == false || VAR.BOOL[eVarBool.FLAG_CHARGEONA] == false)
{
if (PUB.AGV.error.Charger_pos_error)
{
PUB.Speak(Lang.충전기위치오류로작업을취소합니다, true);
PUB.sm.ClearRunStep();
PUB.sm.SetNewRunStep(ERunStep.CHARGEOFF);
PUB.sm.SetNewStep(eSMStep.RUN);
}
else if (PUB.AGV.error.Charger_run_error)
{
PUB.Speak(Lang.충전기위치오류로작업을취소합니다, true);
PUB.sm.ClearRunStep();
PUB.sm.SetNewRunStep(ERunStep.CHARGEOFF);
PUB.sm.SetNewStep(eSMStep.RUN);
}
else
{
var tsCmd = DateTime.Now - LastCommandTime;
if (tsCmd.TotalSeconds >= 30)
{
PUB.log.AddE( Lang.충전기작동확인이오버되어홈으로이동조치합니다);
PUB.AGV.AGVCharge(PUB.setting.ChargerID, false);
PUB.sm.ClearRunStep();
PUB.sm.SetNewRunStep(ERunStep.CHARGEOFF);
PUB.sm.SetNewStep(eSMStep.RUN);
}
else
{
PUB.Result.result_message = string.Format("충전기 활성화 수신 확인 중({0:N1}/{1})", tsCmd.TotalSeconds, 30);
PUB.Result.result_progressmax = 15;
PUB.Result.result_progressvalue = tsCmd.TotalSeconds;
}
}
return false;
}
else return true;
}
}
}

128
HMI/Project/StateMachine/Step/_SM_RUN_CHARGE_GO.cs Normal file
View File

@@ -0,0 +1,128 @@
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 tm_gocharge_command = DateTime.Now;
public Boolean _SM_RUN_CHARGE_GO(bool isFirst, TimeSpan stepTime)
{
var funcname = "GOCHARGE";
if (runStepisFirst)
{
//홈을 찾도록 항상 위치를 지워버리자
PUB.Result.CurrentPos = ePosition.NONE;
}
//HW 연결오류
if (PUB.AGV.IsOpen == false)
{
PUB.Result.SetResultMessage(eResult.Hardware, eECode.AGVCONN, eNextStep.ERROR);
return false;
}
//이미 충전중이라면 바로 완료 처리한다 (사용자 요청)
if (VAR.BOOL[eVarBool.FLAG_CHARGEONA] == true || PUB.AGV.system1.Battery_charging == true ||
VAR.BOOL[eVarBool.FLAG_CHARGEONM] == true)
{
SetRunStepError(ENIGProtocol.AGVErrorCode.ALREADY_CHARGE);
return false;
}
//충전 상태가 OFF되어야 동작하게한다
if (_SM_RUN_CHARGE_GOFF(isFirst, stepTime) == false)
return false;
//라이더멈춤이 설정되어있다면 음성으로 알려준다 200409
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 false;
}
var idx = 1;
if (PUB.sm.RunStepSeq == idx++)
{
var targetnode = PUB.FindByRFID(PUB.setting.NodeMAP_RFID_Charger);
if (targetnode == null)
{
SetRunStepError(ENIGProtocol.AGVErrorCode.NOTSET_CHARGEPOINT);
return false;
}
//현재위치가 충전기위치이고 마크센서가 감지되었다면 충전기위치로 인지하고
//그렇지 못하면 충전위치가 아니라는 오류를 발생한다
var curnode = PUB._virtualAGV.CurrentNode;
if (PUB.AGV.signal1.mark_sensor == false || curnode.Id != targetnode.Id)
{
SetRunStepError(ENIGProtocol.AGVErrorCode.NO_CHARGEPOINT, $"충전기위치가 아니므로 충전을 시작할 수 없습니다(현재위치:{curnode.RfidId})");
return false;
}
PUB.sm.UpdateRunStepSeq();
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
//충전시작 명령을 전송한다
PUB.log.Add($"충전:충전명령전송");
PUB.Speak(Lang.충전시작명령을전송합니다);
PUB.AGV.AGVCharge(PUB.setting.ChargerID, true);
VAR.BOOL[eVarBool.WAIT_CHARGEACK] = true;
LastCommandTime = DateTime.Now;
VAR.BYTE[eVarByte.CHARGE_CMDCNT] = 0;
PUB.sm.UpdateRunStepSeq();
return false;
}
//충전시작명령의 ACK를 체크합니다
else if (PUB.sm.RunStepSeq == idx++)
{
//충전시작 명령을 전송한다
if (PUB.AGV.ACKData.Equals("CBT"))
{
PUB.log.Add($"충전명령 회신 확인");
PUB.sm.UpdateRunStepSeq();
}
else
{
var ts = DateTime.Now - LastCommandTime;
if (ts.TotalSeconds > 3)
{
if (VAR.BYTE[eVarByte.CHARGE_CMDCNT] > 5)
{
PUB.counter.CountChargeE += 1;
PUB.counter.Save();
VAR.BYTE[eVarByte.CHARGE_CMDCNT] = 0;
SetRunStepError(ENIGProtocol.AGVErrorCode.CHARGE_RETRY_OVER);
return false;
}
else
{
PUB.logagv.Add($"충전시작명령 재전송");
VAR.BYTE.Add(eVarByte.CHARGE_CMDCNT, 1);
PUB.AGV.AGVCharge(PUB.setting.ChargerID, true);
LastCommandTime = DateTime.Now;
}
}
}
return false;
}
return true;
}
}
}

78
HMI/Project/StateMachine/Step/_SM_RUN_CHARGE_GOFF.cs Normal file
View File

@@ -0,0 +1,78 @@
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
{
/// <summary>
/// 충전시퀀스를 모두 해제하고 본래위치로 이동한다.
/// </summary>
/// <param name="isFirst"></param>
/// <param name="stepTime"></param>
/// <returns></returns>
public Boolean _SM_RUN_CHARGE_GOFF(bool isFirst, TimeSpan stepTime)
{
//충전중인지 확인한다.
if (VAR.BOOL[eVarBool.FLAG_CHARGEONA] == true || PUB.AGV.system1.Battery_charging == true)
{
if (isFirst)
{
VAR.TIME.Update(eVarTime.ChargeEnd);
if (VAR.BOOL[eVarBool.FLAG_CHARGEONA])
{
PUB.Speak(Lang.충전을해제합니다);
}
}
//AGV는 충전을 해제한 상태이다
if (PUB.AGV.system1.Battery_charging == false)
{
//너무빨리 처리되지 않도록 5초후에 완료 처리를 한다
var ts1 = VAR.TIME.RUN(eVarTime.ChargeEnd);
var ts2 = VAR.TIME.RUN(eVarTime.SendChargeOff);
if (ts1.TotalSeconds >= 5 && ts2.TotalSeconds > 5) VAR.BOOL[eVarBool.FLAG_CHARGEONA] = false;
}
else
{
//OFF전송이 처음이라면 시간 설정
if (VAR.TIME.IsSet(eVarTime.SendChargeOff)==false)
VAR.TIME[eVarTime.SendChargeOff] = DateTime.Now.AddSeconds(-10);
var tsChgOff = VAR.TIME.RUN(eVarTime.SendChargeOff);
if (tsChgOff.TotalSeconds >= 3)
{
PUB.log.Add("충전 해제 전송");
PUB.AGV.AGVCharge(PUB.setting.ChargerID, false);
VAR.TIME.Update(eVarTime.SendChargeOff);
}
// 1분 타임아웃 체크
if (stepTime.TotalMinutes >= 1)
{
PUB.XBE.ErrorMessage = $"충전해제가 실패되었습니다(1분)";
PUB.log.AddE(PUB.XBE.ErrorMessage);
PUB.sm.SetNewStep(eSMStep.IDLE);
return false;
}
}
return false;
}
else
{
//중전이 해제된 상태이다
return true;
}
}
}
}

209
HMI/Project/StateMachine/Step/_SM_RUN_ENTER.cs Normal file
View File

@@ -0,0 +1,209 @@
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
{
/// <summary>
/// 장비로 진입한다. -
/// </summary>
public Boolean _SM_RUN_ENTER(bool isFirst, TimeSpan seqTime)
{
var idx = 1;
var funcname = PUB.sm.RunStep.ToString();
//충전 상태가 OFF되어야 동작하게한다
if (_SM_RUN_CHARGE_GOFF(isFirst, seqTime) == false) return false;
//라이더멈춤이 설정되어있다면 음성으로 알려준다
if (CheckLiderStop() == false) return false;
if (PUB.sm.RunStepSeq == idx++)
{
PUB.log.Add("IN작업-시작");
PUB.Speak("작업을 시작합니다");
PUB.AGV.AGVMoveStop(funcname);
PUB.sm.UpdateRunStepSeq();
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
//마크센서가 감지된상태여야 완전한위치로 가정한다
if(PUB.AGV.signal1.mark_sensor == false)
{
SetRunStepError(ENIGProtocol.AGVErrorCode.MARK_SENSOR_FAIL, $"[{funcname}] 마크센서가 감지되지 않습니다");
}
PUB.sm.UpdateRunStepSeq();
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
//작업상태에 따라서 카트감지여부를 확인한ㄷ.
if (PUB.NextWorkCmd == ENIGProtocol.AGVCommandHE.PickOnEnter)
{
//가지러가야하는 경우이므로 카트가 없어야한다.
if (PUB.AGV.signal2.cart_detect1 || PUB.AGV.signal2.cart_detect2)
{
SetRunStepError(ENIGProtocol.AGVErrorCode.CART_EXIST, $"[{funcname}] 카트가 존재하여 진입을 할 수 없습니다");
return false;
}
}
else if (PUB.NextWorkCmd == ENIGProtocol.AGVCommandHE.PickOffEnter)
{
//가져다 놓아야하므로 카트가 존재해야 한다
if (PUB.AGV.signal2.cart_detect1 == false || PUB.AGV.signal2.cart_detect2 == false)
{
var errmsg = $"[{funcname}] 카트감지센서가 인식되지 않았습니다";
SetRunStepError(ENIGProtocol.AGVErrorCode.CART_EXIST, errmsg);
return false;
}
}
else
{
var errmsg = $"[{funcname}] 알수없는 작업형태입니다({PUB.NextWorkCmd})";
SetRunStepError(ENIGProtocol.AGVErrorCode.UnknownCommand, errmsg);
return false;
}
PUB.sm.UpdateRunStepSeq();
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
//pick off/on 상관없이 리프트는 내려서 이동한다
var liftCmd = arDev.Narumi.LiftCommand.DN;
PUB.AGV.LiftControl(liftCmd);
VAR.TIME.Update(eVarTime.LastTurnCommandTime);
PUB.sm.UpdateRunStepSeq();
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
//리프트 센서 확인
if ((PUB.AGV.signal1.lift_down == true && PUB.AGV.signal1.lift_up == false) == false)
{
var ts = VAR.TIME.RUN(eVarTime.LastTurnCommandTime);
if (ts.TotalSeconds > 10)
{
var errmsg = $"[{funcname}] 리프트다운이 확인되지 않습니다";
SetRunStepError(ENIGProtocol.AGVErrorCode.LIFT_ERROR, errmsg);
}
return false;
}
else
{
PUB.log.Add("리프트 동작 확인 완료");
PUB.sm.UpdateRunStepSeq();
}
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
//저속이동 (후진 진입)
var ret = PUB.AGV.AGVMoveSet(new arDev.Narumi.BunkiData
{
Bunki = arDev.Narumi.eBunki.Strate,
Direction = arDev.Narumi.eMoveDir.Backward,
PBSSensor = 0,
Speed = arDev.Narumi.eMoveSpd.Low,
});
//명령이 실패되었다면 재시도를 한다
if (ret != arDev.eNarumiCommandResult.Success)
{
if (ret >= arDev.eNarumiCommandResult.Error)
{
SetRunStepError(ENIGProtocol.AGVErrorCode.AGV_SPEED_SET_FAIL);
}
return false;
}
VAR.TIME.Update(eVarTime.LastTurnCommandTime);
PUB.sm.UpdateRunStepSeq();
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
//전진이동
PUB.AGV.AGVMoveRun(arDev.Narumi.eRunOpt.Backward);
PUB.sm.UpdateRunStepSeq();
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
//AGV구동을 확인하고 마크스탑을 설정한다.
if (PUB.AGV.system1.agv_run == false)
{
if (seqTime.TotalMilliseconds > 1000)
{
//구동이확인되지 않으면 오류처리를 한다.
SetRunStepError(ENIGProtocol.AGVErrorCode.AGV_RUN_FAIL);
}
return false;
}
//마크스탑설정
PUB.AGV.AGVMoveStop(funcname, arDev.Narumi.eStopOpt.MarkStop);
PUB.sm.UpdateRunStepSeq();
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
//마크스탑신호가 3초이내로 들어와야 한다
if (PUB.AGV.data.Speed != 'S')
{
if (seqTime.TotalMilliseconds > 3000)
{
SetRunStepError(ENIGProtocol.AGVErrorCode.MARK_TIMEOUT);
}
return false;
}
PUB.sm.UpdateRunStepSeq();
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
//AGV가 멈출때까지 기다린다.
if (PUB.AGV.system1.agv_run == true)
{
if (seqTime.TotalSeconds > 10)
{
SetRunStepError(ENIGProtocol.AGVErrorCode.AGV_STOP_FAIL);
}
return false;
}
PUB.sm.UpdateRunStepSeq();
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
//마크센서입력을 확인한다.
if (PUB.AGV.signal1.mark_sensor == false)
{
if (seqTime.TotalSeconds > 5)
{
SetRunStepError(ENIGProtocol.AGVErrorCode.MARK_SENSOR_FAIL);
}
return false;
}
PUB.sm.UpdateRunStepSeq();
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
//완료되었다.
PUB.log.Add("진입 완료");
PUB.sm.UpdateRunStepSeq();
return false;
}
return true;
}
}
}

177
HMI/Project/StateMachine/Step/_SM_RUN_EXIT.cs Normal file
View File

@@ -0,0 +1,177 @@
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
{
/// <summary>
/// 장비엣 빠젼나온다.
/// </summary>
public Boolean _SM_RUN_EXIT(bool isFirst, TimeSpan seqTime)
{
var idx = 1;
var funcname = PUB.sm.RunStep.ToString();
//충전 상태가 OFF되어야 동작하게한다
if (_SM_RUN_CHARGE_GOFF(isFirst, seqTime) == false) return false;
//라이더멈춤이 설정되어있다면 음성으로 알려준다
if (CheckLiderStop() == false) return false;
if (PUB.sm.RunStepSeq == idx++)
{
PUB.log.Add("OUT작업-시작");
PUB.Speak("작업을 시작합니다");
PUB.AGV.AGVMoveStop(funcname);
PUB.sm.UpdateRunStepSeq();
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
//작업형태에 따라서. 리프트를 제어한다.
var liftCmd = arDev.Narumi.LiftCommand.DN;
if (PUB.NextWorkCmd == ENIGProtocol.AGVCommandHE.PickOnExit) liftCmd = arDev.Narumi.LiftCommand.UP;
PUB.AGV.LiftControl(liftCmd);
PUB.sm.UpdateRunStepSeq();
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
var liftCmd = arDev.Narumi.LiftCommand.DN;
if (PUB.NextWorkCmd == ENIGProtocol.AGVCommandHE.PickOnExit) liftCmd = arDev.Narumi.LiftCommand.UP;
//리프트 센서 확인
var liftdnok = (PUB.AGV.signal1.lift_down == true && PUB.AGV.signal1.lift_up == false);
var liiftupok = (PUB.AGV.signal1.lift_up == true && PUB.AGV.signal1.lift_down == false);
if (liftCmd == arDev.Narumi.LiftCommand.DN && liftdnok)
{
//정상조건
}
else if (liftCmd == arDev.Narumi.LiftCommand.UP && liftdnok)
{
//정상조건
}
else
{
var ts = VAR.TIME.RUN(eVarTime.LastTurnCommandTime);
if (ts.TotalSeconds > 10)
{
SetRunStepError(ENIGProtocol.AGVErrorCode.LIFT_ERROR, $"[{funcname}] 리프트({liftCmd})이 확인되지 않습니다");
}
return false;
}
PUB.log.Add("리프트 동작 확인 완료");
PUB.sm.UpdateRunStepSeq();
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
//빈 상태로 아웃해야한다.
var ret = PUB.AGV.AGVMoveSet(new arDev.Narumi.BunkiData
{
Bunki = arDev.Narumi.eBunki.Strate,
Direction = arDev.Narumi.eMoveDir.Forward,
PBSSensor = 0,
Speed = arDev.Narumi.eMoveSpd.Low,
});
//명령이 실패되었다면 재시도를 한다
if (ret != arDev.eNarumiCommandResult.Success)
{
if (ret >= arDev.eNarumiCommandResult.Error)
{
SetRunStepError(ENIGProtocol.AGVErrorCode.AGV_SPEED_SET_FAIL);
}
return false;
}
PUB.sm.UpdateRunStepSeq();
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
//전진이동
PUB.AGV.AGVMoveRun(arDev.Narumi.eRunOpt.Forward);
PUB.sm.UpdateRunStepSeq();
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
//AGV구동을 확인하고 마크스탑을 설정한다.
if (PUB.AGV.system1.agv_run == false)
{
if (seqTime.TotalMilliseconds > 1000)
{
//구동이확인되지 않으면 오류처리를 한다.
SetRunStepError(ENIGProtocol.AGVErrorCode.AGV_RUN_FAIL);
}
return false;
}
//마크스탑설정
PUB.AGV.AGVMoveStop(funcname, arDev.Narumi.eStopOpt.MarkStop);
PUB.sm.UpdateRunStepSeq();
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
//마크스탑신호가 3초이내로 들어와야 한다
if (PUB.AGV.data.Speed != 'S')
{
if (seqTime.TotalMilliseconds > 3000)
{
SetRunStepError(ENIGProtocol.AGVErrorCode.MARK_TIMEOUT);
}
return false;
}
PUB.sm.UpdateRunStepSeq();
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
//AGV가 멈출때까지 기다린다.
if (PUB.AGV.system1.agv_run == true)
{
if (seqTime.TotalSeconds > 10)
{
SetRunStepError(ENIGProtocol.AGVErrorCode.AGV_STOP_FAIL);
}
return false;
}
PUB.sm.UpdateRunStepSeq();
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
//마크센서입력을 확인한다.
if (PUB.AGV.signal1.mark_sensor == false)
{
if (seqTime.TotalSeconds > 5)
{
SetRunStepError(ENIGProtocol.AGVErrorCode.MARK_SENSOR_FAIL);
}
return false;
}
PUB.sm.UpdateRunStepSeq();
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
//완료되었다.
PUB.log.Add("진출 완료");
PUB.sm.UpdateRunStepSeq();
return false;
}
return true;
}
}
}

79
HMI/Project/StateMachine/Step/_SM_RUN_GOTO.cs Normal file
View File

@@ -0,0 +1,79 @@
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;
var funcName = "_SM_RUN_GOTO";
//충전 상태가 OFF되어야 동작하게한다
if (_SM_RUN_CHARGE_GOFF(isFirst, stepTime) == false)
return false;
//최초시작이라면 시간변수 초기화
if (isFirst)
{
PUB.log.Add($"[>>] _SM_RUN_GOTO");
VAR.TIME.Update(eVarTime.CheckGotoTargetSet);
}
//라이더멈춤이 설정되어있다면 음성으로 알려준다 200409
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 false;
}
var idx = 1;
if (PUB.sm.RunStepSeq == idx++)
{
if(PUB._virtualAGV.TargetNode == null)
{
PUB.log.Add($"대상노드가 없어 이동을할 수 없습니다");
PUB.sm.SetNewRunStep(ERunStep.READY);
return false;
}
PUB.sm.UpdateRunStepSeq();
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
//모션 전후진 제어
if (UpdateMotionPositionForMark(funcName))
{
PUB.AGV.AGVMoveStop(funcName);
PUB.sm.UpdateRunStepSeq();
}
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
//QC까지 모두 완료되었다.(완전히 정차할때까지 기다린다)
PUB.Speak(Lang.이동완료, true);
PUB.AddEEDB($"이동완료({PUB._virtualAGV.TargetNode.ID2})");
PUB.sm.UpdateRunStepSeq();
return false;
}
return true;
}
}
}

84
HMI/Project/StateMachine/Step/_SM_RUN_GOTO_HOME.cs Normal file
View File

@@ -0,0 +1,84 @@
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
{
public Boolean _SM_RUN_GOTO_HOME(bool isFirst, TimeSpan stepTime)
{
var funcName = "_SM_RUN_GOHOME";
if (runStepisFirst)
{
VAR.BOOL[eVarBool.FLAG_NEXTSTOP_ALIGN] = false;//);
}
//HW 연결오류
if (PUB.AGV.IsOpen == false)
{
PUB.Result.SetResultMessage(eResult.Hardware, eECode.AGVCONN, eNextStep.ERROR);
return false;
}
//충전 상태가 OFF되어야 동작하게한다
if (_SM_RUN_CHARGE_GOFF(isFirst, stepTime) == false)
return false;
//라이더멈춤이 설정되어있다면 음성으로 알려준다 200409
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 false;
}
var idx = 1;
if (PUB.sm.RunStepSeq == idx++)
{
PUB.Speak(Lang.홈으로이동합니다);
var homenode = PUB.FindByRFID(PUB.setting.NodeMAP_RFID_Home);
if (homenode == null)
{
PUB.log.Add($"홈위치의 노드맵핑(환경설정)이 없습니다");
PUB.sm.SetNewRunStep(ERunStep.READY);
return false;
}
PUB._virtualAGV.TargetNode = homenode;
PUB.AddEEDB($"홈검색시작({homenode.RfidId})");
PUB.sm.UpdateRunStepSeq();
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
//모션 전후진 제어
if (UpdateMotionPositionForMark(funcName))
{
PUB.AGV.AGVMoveStop(funcName);
PUB.sm.UpdateRunStepSeq();
}
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
//QC까지 모두 완료되었다.(완전히 정차할때까지 기다린다)
PUB.Speak(Lang.이동완료, true);
PUB.AddEEDB($"이동완료({PUB.Result.TargetPos})");
PUB.sm.UpdateRunStepSeq();
return false;
}
return true;
}
}
}

120
HMI/Project/StateMachine/Step/_SM_RUN_INIT.cs Normal file
View File

@@ -0,0 +1,120 @@
using System;
using System.Collections.Generic;
using System.Drawing;
using System.Linq;
using System.Text;
namespace Project
{
public partial class fMain
{
/// <summary>
/// 웨이퍼를 집기 위해 지정된 위치로 이동을 합니다.
/// </summary>
/// <returns></returns>
private bool _SM_RUN_INIT(Boolean firstRun, TimeSpan RunTime)
{
//result message
int progress = 0;
var ProgressMax = 13;
//setting socket
UpdateProgressStatus(++progress, ProgressMax, "소켓 초기화"); System.Threading.Thread.Sleep(5);
if (PUB.sock_debug.Listen(PUB.setting.listenPort))
PUB.log.AddI("Socket Listen : " + PUB.setting.listenPort);
else
PUB.log.AddE("Socket Listen : " + PUB.sock_debug.errorMessage);
//UpdateProgressStatus(++progress, ProgressMax, "PLC 확인"); System.Threading.Thread.Sleep(5);
//if (PUB.setting.Port_PLC != "")
//{
// if (PUB.PLC.Init(PUB.setting.Port_PLC, PUB.setting.Baud_PLC))
// {
// //VAR.BOOL[eVarBool.FLAG_SETUP] = true;
// PUB.log.Add("Entering Setup Mode (Main)");
// }
// else PUB.log.AddE("PLC Init error : " + PUB.PLC.GetErrorMessage());
//}
//UpdateProgressStatus("PLC #2 확인", ++progress, ProgressMax); System.Threading.Thread.Sleep(5);
//if (Pub.setting.Port_PLC2 != "")
//{
// if (Pub.plcS.Init(Pub.setting.Port_PLC2, Pub.setting.Baud_plc2))
// {
// Pub.plcS.SetFlag(Device.PLC2.PLCFlag.FLAG_SETUP, true);
// Pub.log.Add("Entering Setup Mode (Sub)");
// }
// else Pub.log.AddE("DIO #S Init error : " + Pub.plcS.GetErrorMessage());
//}
//setting Port_Xbee
//UpdateProgressStatus(++progress, ProgressMax, "Port_Xbee 확인"); System.Threading.Thread.Sleep(5);
//if (PUB.setting.Port_XBE.isEmpty() == false)
//{
// PUB.XBE.PortName = PUB.setting.Port_XBE;
// if (PUB.XBE.Open() == false)
// PUB.log.AddE(string.Format("Barcode Port({0}) Open Error: {1}", PUB.setting.Port_XBE, PUB.XBE.errorMessage));
//}
//else
//{
// PUB.log.AddAT("NOT-SET Port_Xbee PORTNAME");
// PUB.XBE.errorMessage = "NOT-SET Port_Xbee PORTNAME";
//}
////setting Port_Xbee
//UpdateProgressStatus(++progress, ProgressMax, "Port_AGV 확인"); System.Threading.Thread.Sleep(5);
//if (PUB.setting.Port_AGV.isEmpty() == false)
//{
// PUB.AGV.PortName = PUB.setting.Port_AGV;
// PUB.AGV.BaudRate = PUB.setting.Baud_AGV;
// if (PUB.AGV.Open() == false)
// PUB.log.AddE(string.Format("RFID Host:{0} Open Error: {1}", PUB.setting.Port_AGV, PUB.AGV.errorMessage));
//}
//else
//{
// PUB.log.AddAT("NOT-SET Port_AGV PORTNAME");
// PUB.AGV.errorMessage = "NOT-SET Port_AGV PORTNAME";
//}
////setting Port_Xbee
//UpdateProgressStatus(++progress, ProgressMax, "Port_BMS 확인"); System.Threading.Thread.Sleep(5);
//if (!PUB.setting.Port_BAT.isEmpty())
//{
// PUB.BAT.PortName = PUB.setting.Port_BAT;//.Port_BMS;
// PUB.BAT.BaudRate = PUB.setting.Baud_BAT;//
// PUB.BAT.ScanInterval = 1000;// Pub.setting.interval_bms;
// var binit_bms = PUB.BAT.Open();
// if (binit_bms == false) PUB.log.AddE(string.Format("binit_bms Port({0}) Open Error: {1}", PUB.setting.Port_BAT, PUB.BAT.errorMessage));
//}
//else
//{
// PUB.log.AddAT("NOT-SET binit_bms PORTNAME");
//}
//남은 공간
UpdateProgressStatus(++progress, ProgressMax, "공간 확인"); System.Threading.Thread.Sleep(5);
CheckFreeSpace(); //181225
//시스템 오류가 있다면 경고 메시지를 표싷ㄴ다.
UpdateProgressStatus(++progress, ProgressMax, "오류 수집"); System.Threading.Thread.Sleep(5);
//var AlertMessage = new System.Text.StringBuilder();
//if (Pub.Xbee.IsInit == false) AlertMessage.AppendLine(string.Format("BARCODE : {0}", Pub.Xbee.errorMessage));
if (freeSpaceRate < PUB.setting.AutoDeleteThreshold)
{
PUB.Speak(string.Format("디스크 용량이 {0:N0}% 남았습니다", freeSpaceRate));
}
//if (AlertMessage.Length > 1)
//{
// AlertMessage.Insert(0, "SYSTEM ALERT\n");
// Pub.popup.setMessage(AlertMessage.ToString());
//}
UpdateProgressStatus(++progress, ProgressMax, "--"); System.Threading.Thread.Sleep(5);
PUB.log.Add("초기화 완료");
return true;
}
}
}

44
HMI/Project/StateMachine/Step/_SM_RUN_POSCHK.cs Normal file
View File

@@ -0,0 +1,44 @@
using System;
using System.Collections.Generic;
using System.Drawing;
using System.Linq;
using System.Text;
using AR;
using COMM;
namespace Project
{
public partial class fMain
{
public Boolean _SM_RUN_POSCHK(bool isFirst, TimeSpan stepTime)
{
//현재위치가 설정되어있는지 확인한다, 현재위치값이 있는 경우 True 를 반환
if (PUB._virtualAGV.CurrentNode != null && PUB._virtualAGV.PrevNode != null)
return true;
//이동을 하지 않고있다면 전진을 진행한다
if (PUB.AGV.system1.agv_run == false)
{
var ts = VAR.TIME.RUN(eVarTime.LastRunCommandTime);
if (ts.TotalSeconds > 5)
{
PUB.log.Add($"현재위치를 몰라 전진 이동 합니다");
var ret = PUB.AGV.AGVMoveSet(new arDev.Narumi.BunkiData
{
Bunki = arDev.Narumi.eBunki.Strate,
Direction = arDev.Narumi.eMoveDir.Forward,
PBSSensor = 1,
Speed = arDev.Narumi.eMoveSpd.Low,
});
if(ret == arDev.eNarumiCommandResult.Success)
{
PUB.AGV.AGVMoveRun(arDev.Narumi.eRunOpt.Forward);
}
VAR.TIME.Update(eVarTime.LastRunCommandTime);
}
}
VAR.STR[eVarString.StatusMessage] = "현재 위치를 알 수 없습니다";
return false;
}
}
}

77
HMI/Project/StateMachine/Step/_SM_RUN_READY.cs Normal file
View File

@@ -0,0 +1,77 @@
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using AR;
using COMM;
using Project.StateMachine;
namespace Project
{
public partial class fMain
{
public Boolean _SM_RUN_READY(bool isFirst, TimeSpan stepTime)
{
////이동 불가 조건이 걸려있다면 충전을 하지 못하게 한다.
//Boolean bAutoChageOn = true;
//if (PUB.AGV.system1.stop_by_front_detect) bAutoChageOn = false;
//else if (PUB.AGV.error.Emergency) bAutoChageOn = false;
//else if (VAR.BOOL[eVarBool.FLAG_CHARGEONA]) bAutoChageOn = false;
//else if (VAR.BOOL[eVarBool.FLAG_CHARGEONM]) bAutoChageOn = false;
//자동 충전 중이라면 최대 충전시간과 레벨을 체크한다
if (VAR.BOOL[eVarBool.FLAG_CHARGEONA] == true)
{
if (VAR.TIME.IsSet(eVarTime.ChargeStart) == false)
VAR.TIME.Update(eVarTime.ChargeStart);
//충전중이라면 최대 충전 시간을 체크한다.
var tsChargeRunTime = VAR.TIME.RUN(eVarTime.ChargeStart);
if (PUB.BMS.BMSInformation.rsoc >= PUB.setting.ChargeMaxLevel)
{
PUB.AGV.AGVCharge(PUB.setting.ChargerID, false);
PUB.sm.ClearRunStep();
PUB.sm.SetNewRunStep(ERunStep.CHARGEOFF);
PUB.sm.SetNewStep(eSMStep.RUN);
PUB.log.Add($"최대충전레벨 OVER로 인한 자동 해제(레벨:{PUB.setting.ChargeMaxLevel})");
PUB.AddEEDB($"최대충전레벨 OVER로 인한 자동 해제(레벨:{PUB.setting.ChargeMaxLevel})");
PUB.Speak(Lang.충전완료로해제합니다);
}
else if (PUB.setting.ChargeMaxTime > 1 && tsChargeRunTime.TotalSeconds >= PUB.setting.ChargeMaxTime)
{
PUB.AGV.AGVCharge(PUB.setting.ChargerID, false);
PUB.sm.ClearRunStep();
PUB.sm.SetNewRunStep(ERunStep.CHARGEOFF);
PUB.sm.SetNewStep(eSMStep.RUN);
PUB.log.Add($"최대충전시간 OVER로 인한 자동 해제(시간:{PUB.setting.ChargeMaxTime})");
PUB.AddEEDB($"최대충전시간 OVER로 인한 자동 해제(시간:{PUB.setting.ChargeMaxTime})");
PUB.Speak(Lang.충전완료로해제합니다);
}
VAR.STR[eVarString.ChargeCheckMsg] = "자동 충전 중";
}
else if (VAR.BOOL[eVarBool.FLAG_CHARGEONM] == true)
{
if (VAR.TIME.IsSet(eVarTime.ChargeStart) == false)
VAR.TIME.Update(eVarTime.ChargeStart);
VAR.STR[eVarString.ChargeCheckMsg] = "수동 충전";
}
//대기모드에서는 움직이지 않게 한다
if(PUB.AGV.system1.agv_run)
{
var ts = VAR.TIME.RUN(eVarTime.IdleStopTime);
if(ts.TotalSeconds > 1)
{
PUB.log.Add($"대기상태에서는 정차");
PUB.AGV.AGVMoveStop("대기상태에서는 정차");
VAR.TIME.Update(eVarTime.IdleStopTime);
}
}
return false;
}
}
}

159
HMI/Project/StateMachine/Step/_SM_RUN_RESET.cs Normal file
View File

@@ -0,0 +1,159 @@
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
{
public Boolean _SM_RUN_RESET(bool isFirst, TimeSpan stepTime)
{
if (runStepisFirst)
{
PUB.Speak(Lang.장비상태초기화);
//충전기검색시퀀스 OFF
//PUB.flag.set(EFlag.FLAG_GO_CHAGER_TEMP, false);
}
var idx = 1;
if (PUB.sm.RunStepSeq == idx++)
{
//충전중이라면 충전off코드를 넣는다
PUB.AGV.AGVCharge(PUB.setting.ChargerID, false,PUB.setting.ChargetWaitSec);
LastCommandTime = DateTime.Now;
PUB.AddEEDB($"초기화");
PUB.sm.UpdateRunStepSeq();
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
UpdateProgressStatus(stepTime.TotalSeconds, 5, "SYNC : 수동 모드 확인 중");
if (VAR.BOOL[eVarBool.FLAG_AUTORUN] == false)
PUB.sm.UpdateRunStepSeq();
else
{
//일정시간동안 명령을 재전송한다
var tsCmd = DateTime.Now - LastCommandTime;
if (tsCmd.TotalMilliseconds >= 1999)
{
VAR.BOOL[eVarBool.FLAG_AUTORUN] = false;
LastCommandTime = DateTime.Now;
}
}
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
//모션을 멈춥니다
//PUB.PLC.Move(Device.PLC.Rundirection.Stop,"RESET");
PUB.AGV.AGVMoveStop("_sm_run_reset");
LastCommandTime = DateTime.Now;
PUB.sm.UpdateRunStepSeq();
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
//모션이 멈출때까지 기다린다
if (PUB.AGV.system1.agv_stop == false)
{
PUB.sm.UpdateRunStepSeq();
}
else
{
//모션이 멈추지 않는다면 멈춤코드를 계속 넣는다
var tsCmd = DateTime.Now - LastCommandTime;
if (tsCmd.TotalMilliseconds >= 1999)
{
//PUB.PLC.Move(Device.PLC.Rundirection.Stop,"RESET(Re)");
PUB.AGV.AGVMoveStop("_sm_run_reset");
LastCommandTime = DateTime.Now;
}
}
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
//대상위치 초기화
PUB.Result.TargetPos = ePosition.NONE;
PUB.Result.CurrentPos = ePosition.NONE;
PUB.Result.CurrentPosCW = "0";
PUB.sm.UpdateRunStepSeq();
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
//자동 스탑 플래그 해제
//VAR.BOOL[eVarBool.FLAG_NEXTSTOP_ALIGN] = false;//"reset #6");
VAR.BOOL[eVarBool.FLAG_NEXTSTOP_ALIGN] = false;
LastCommandTime = DateTime.Now;
PUB.sm.UpdateRunStepSeq();
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
UpdateProgressStatus(stepTime.TotalSeconds, 5, "자동멈춤해제(ALIGN)");
if (VAR.BOOL[eVarBool.FLAG_NEXTSTOP_ALIGN] == false)
PUB.sm.UpdateRunStepSeq();
else
{
//일정시간동안 명령을 재전송한다
var tsCmd = DateTime.Now - LastCommandTime;
if (tsCmd.TotalMilliseconds >= 1999)
{
VAR.BOOL[eVarBool.FLAG_NEXTSTOP_ALIGN] = false;// "reset #7");
//VAR.BOOL[eVarBool.FLAG_NEXTSTOP_ALIGN] = false;//
LastCommandTime = DateTime.Now;
}
}
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
//자동 스탑 플래그 해제
//VAR.BOOL[eVarBool.FLAG_NEXTSTOP_MARK] = false;// "reset #8");
LastCommandTime = DateTime.Now;
PUB.sm.UpdateRunStepSeq();
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
//설정모드가 되어있다면 해제 해준다
VAR.BOOL[eVarBool.FLAG_SETUP] = false;
VAR.BOOL[eVarBool.ITEMON] = false;
VAR.BOOL[eVarBool.WAIT_COVER_DOWN] = false;
VAR.BOOL[eVarBool.WAIT_COVER_UP] = false;
LastCommandTime = DateTime.Now;
PUB.sm.UpdateRunStepSeq();
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
UpdateProgressStatus(stepTime.TotalSeconds, 5, "설정모드해제(MARK)");
if (VAR.BOOL[eVarBool.FLAG_SETUP] == false)
PUB.sm.UpdateRunStepSeq();
else
{
//일정시간동안 명령을 재전송한다
var tsCmd = DateTime.Now - LastCommandTime;
if (tsCmd.TotalMilliseconds >= 1999)
{
VAR.BOOL[eVarBool.FLAG_SETUP] = false;
LastCommandTime = DateTime.Now;
}
}
return false;
}
return true;
}
}
}

171
HMI/Project/StateMachine/Step/_SM_RUN_SYNC.cs Normal file
View File

@@ -0,0 +1,171 @@
using System;
using System.Collections.Generic;
using System.Drawing;
using System.Linq;
using System.Text;
using Project.StateMachine;
using COMM;
using System.Diagnostics;
using AR;
namespace Project
{
public partial class fMain
{
Dictionary<string, string> synlist;
int synidx;
public Boolean _SM_RUN_SYNC(bool isFirst, TimeSpan stepTime)
{
var idx = 1;
if (PUB.sm.RunStepSeq == idx++)
{
// return true;
UpdateProgressStatus(stepTime.TotalSeconds, 5, "SYNC : 장치 연결(MAIN) 확인 중");
if (PUB.AGV.IsOpen == false)
{
//agv connect
var rlt = ConnectSerialPort(PUB.AGV, PUB.setting.Port_AGV, PUB.setting.Baud_AGV,
eVarTime.LastConn_AGV, eVarTime.LastConnTry_AGV, eVarTime.LastRecv_AGV);
if (rlt == false)
{
//존재하지 않는 포트라면 sync를 벗어난다
PUB.log.AddE($"AGV포트({PUB.setting.Port_AGV}) 가 존재하지않아 SYNC를 중단합니다");
PUB.sm.SetNewStep(eSMStep.IDLE);
}
}
else if (PUB.AGV.IsValid == true)
{
//설정모드 진입
VAR.BOOL[eVarBool.FLAG_SETUP] = true;
LastCommandTime = DateTime.Now;
PUB.sm.UpdateRunStepSeq();
}
else
{
UpdateProgressStatus(stepTime.TotalMilliseconds, 10000.0, "wait");
if (stepTime.TotalSeconds >= 10)
{
PUB.log.AddE("시간초과로 인해 Sync 작업이 실패되었습니다");
PUB.sm.SetNewStep(eSMStep.IDLE);
}
}
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
//동기화 명령 생성
synlist = new Dictionary<string, string>();
synlist.Add("SSH", PUB.setting.SPD_H.ToString("0000")); // PUB.AGV.AGVCommand(item.Key, item.Value);
synlist.Add("SSM", PUB.setting.SPD_M.ToString("0000"));
synlist.Add("SSL", PUB.setting.SPD_L.ToString("0000"));
synlist.Add("SSS", PUB.setting.SPD_S.ToString("0000"));
synlist.Add("SRS", PUB.setting.SPD_R.ToString("0000"));
synlist.Add("SPK", PUB.setting.PID_PH.ToString("0000"));
synlist.Add("SPM", PUB.setting.PID_PM.ToString("0000"));
synlist.Add("SPL", PUB.setting.PID_PL.ToString("0000"));
synlist.Add("SIK", PUB.setting.PID_IH.ToString("0000"));
synlist.Add("SIM", PUB.setting.PID_IM.ToString("0000"));
synlist.Add("SIL", PUB.setting.PID_IL.ToString("0000"));
synlist.Add("SDK", PUB.setting.PID_DH.ToString("0000"));
synlist.Add("SDM", PUB.setting.PID_DM.ToString("0000"));
synlist.Add("SDL", PUB.setting.PID_DL.ToString("0000"));
synlist.Add("SPS", PUB.setting.PID_PS.ToString("0000"));
synlist.Add("SIS", PUB.setting.PID_IS.ToString("0000"));
synlist.Add("SDS", PUB.setting.PID_DS.ToString("0000"));
//추가 230110
synlist.Add("SCK", PUB.setting.SCK.ToString("0000"));
synlist.Add("SSK", PUB.setting.SSK.ToString("0000"));
synlist.Add("STT", PUB.setting.STT.ToString("0000"));
//synlist.Add("SBN", PUB.setting.ChargerID.ToString("0000"));
synlist.Add("SGS", PUB.setting.GDSValue.ToString("0000"));
VAR.I32[eVarInt32.SyncItemCount] = synlist.Count;
PUB.AddEEDB($"SYNC시작({PUB.Result.TargetPos})");
synidx = 0;
PUB.sm.UpdateRunStepSeq();
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
//잠시 대기
var ts = DateTime.Now - LastCommandTime;
if (ts.TotalSeconds > 0.15)
{
PUB.sm.UpdateRunStepSeq();
}
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
//통신 확인되었으므로 설정값들을
if (synidx < synlist.Count)
{
var item = synlist.ElementAt(synidx);
UpdateProgressStatus(stepTime.TotalSeconds, 5, $"SYNC :{item.Key}");
PUB.AGV.AGVCommand(item.Key, item.Value);
// 캔버스에 동기화 상태 표시
if (PUB._mapCanvas != null)
{
float progress = (float)synidx / VAR.I32[eVarInt32.SyncItemCount];
PUB._mapCanvas.SetSyncStatus("장비 설정 동기화 중...", progress, $"항목: {item.Key} ({synidx + 1}/{synlist.Count})");
}
}
LastCommandTime = DateTime.Now;
PUB.sm.UpdateRunStepSeq();
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
if (synidx < synlist.Count)
{
var ts = DateTime.Now - LastCommandTime;
var item = synlist.ElementAt(synidx);
UpdateProgressStatus(stepTime.TotalSeconds, 5, $"SYNC 변경 확인 중 :{item.Key}");
if (PUB.AGV.ACKData.Equals(item.Key))
{
synidx += 1;
if (ts.TotalSeconds < 0.15) PUB.sm.UpdateRunStepSeq(-2); //싱크중에 추가 지연시간 확보
else PUB.sm.UpdateRunStepSeq(-1);
LastCommandTime = DateTime.Now;
}
else
{
if (ts.TotalSeconds > 1)
{
PUB.AGV.AGVCommand(item.Key, item.Value);
LastCommandTime = DateTime.Now;
Console.WriteLine($"RE.send Sync : {item.Key}:{item.Value}");
}
}
}
else PUB.sm.UpdateRunStepSeq();
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
PUB.AddEEDB($"SYNC완료({PUB.Result.TargetPos})");
UpdateProgressStatus(stepTime.TotalSeconds, 5, "SYNC : 완료");
// 동기화 완료 시 캔버스 모드 복귀
if (PUB._mapCanvas != null)
PUB._mapCanvas.SetSyncStatus("동기화 완료!", 1.0f, "잠시 후 메인 화면으로 이동합니다.");
LastCommandTime = DateTime.Now;
PUB.sm.UpdateRunStepSeq();
return false;
}
return true;
}
}
}

56
HMI/Project/StateMachine/Step/_STEP_CLOSE.cs Normal file
View File

@@ -0,0 +1,56 @@
using System;
using System.Collections.Generic;
using System.Drawing;
using System.Linq;
using System.Text;
using Project.StateMachine;
namespace Project
{
public partial class fMain
{
/// <summary>
/// 프로그램을 닫을때 1회 실행되는 함수
/// File : /Step/_Step_Close.cs
/// </summary>
private void _STEP_CLOSING_START(eSMStep step)
{
PUB.bShutdown = true;
PUB.AddEEDB("프로그램 종료");
PUB.log.Add("Program Close");
PUB.LogFlush();
}
public StepResult _STEP_CLOSING(eSMStep step, TimeSpan stepTime, TimeSpan seqTime)
{
//############################
//#### 사용자 전용 코드
//############################
//############################
//#### 개발자 권장코드
//############################
PUB.sm.SetNewStep(eSMStep.CLOSED);
return StepResult.Complete;
}
public void _STEP_CLOSED_START(eSMStep step)
{
try
{
this.BeginInvoke(new Action(() =>
{
//화면을 닫는다
PUB.sm.Stop();
this.Close();
}));
}
catch { }
}
public StepResult _STEP_CLOSED(eSMStep step, TimeSpan stepTime, TimeSpan seqTime)
{
return StepResult.Wait;
}
}
}

432
HMI/Project/StateMachine/Step/_Util.cs Normal file
View File

@@ -0,0 +1,432 @@
using System;
using System.Collections.Generic;
using System.Drawing;
using System.Linq;
using System.Text;
using Project.StateMachine;
using COMM;
using AR;
using AGVNavigationCore.Utils;
using AGVNavigationCore.PathFinding.Core;
using AGVNavigationCore.Models;
using AGVNavigationCore.PathFinding.Planning;
namespace Project
{
public partial class fMain
{
bool CheckStopCondition()
{
//이머전시상태라면 stop 처리한다.
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 false;
}
//수동충전상태라면 이동하지 못한다
if (VAR.BOOL[eVarBool.FLAG_CHARGEONM])
{
PUB.Speak("수동 충전중이라 사용할 수 없습니다");
PUB.sm.SetNewStep(eSMStep.IDLE);
return false;
}
return true;
}
AGVPathResult CalcPath(MapNode startNode, MapNode targetNode)
{
var nodes = PUB._mapCanvas.Nodes;
var _simulatorCanvas = PUB._mapCanvas;
var _mapNodes = PUB._mapCanvas.Nodes;
// 현재 AGV 방향 가져오기
var selectedAGV = PUB._virtualAGV;
var currentDirection = selectedAGV.CurrentDirection;
// AGV의 이전 위치에서 가장 가까운 노드 찾기
var prevNode = selectedAGV.PrevNode;
var prevDir = selectedAGV.PrevDirection;
// Core Logic으로 이관됨
var pathFinder = new AGVPathfinder(nodes);
var result = pathFinder.CalculatePath(startNode, targetNode, prevNode, prevDir);
//게이트웨이노드를 하이라이트강조 한단
_simulatorCanvas.HighlightNodeId = (result.Gateway?.Id ?? string.Empty);
return result;
}
/// <summary>
/// 실행스텝을 오류로 전환합니다.
/// 로그메세지(에러)가 추가됩니다.
/// 맵캔버스오류경고메세지가 설정됩니다
/// </summary>
/// <param name="ermsg"></param>
/// <param name="ecode"></param>
public void SetRunStepError(ENIGProtocol.AGVErrorCode ecode, string errmsg = "")
{
if (string.IsNullOrEmpty(errmsg))
{
errmsg = ENIGProtocol.AGVUtility.GetAGVErrorMessage(ecode);
}
PUB.AGV.AGVMoveStop(errmsg);
PUB.log.AddE(errmsg);
PUB._mapCanvas.SetAlertMessage(errmsg);
PUB.Result.RunStepErrorCode = ecode;
PUB.sm.SetNewRunStep(ERunStep.ERROR);
}
/// <summary>
/// 라이더멈춤이 설정되어있다면 음성으로 알려준다
/// </summary>
/// <returns></returns>
public bool CheckLiderStop()
{
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 false;
}
return true;
}
/// <summary>
/// 설정된 목적지까지 이동을 완료 한 후 True를 반환합니다.
/// 목적지 : PUB._virtualAGV.TargetNode
///
/// </summary>
/// <param name="sender"></param>
/// <returns></returns>
Boolean UpdateMotionPositionForMark(string sender)
{
////현재위치를 모르는 상태라면 처리하지 않는다
if (PUB._virtualAGV.CurrentNode == null || PUB._virtualAGV.PrevNode == null)
return false;
//현재위치노드 오류
var currentNode = PUB.FindByNodeID(PUB._virtualAGV.CurrentNode.Id);
if (currentNode == null)
{
PUB.log.AddE($"현재위치노드가 없습니다");
PUB.sm.SetNewRunStep(ERunStep.READY);
return false;
}
//시작노드값이 없다면 현재위치를 노드로 결정한다
if (PUB._virtualAGV.StartNode == null)
PUB._virtualAGV.StartNode = PUB.FindByNodeID(PUB._virtualAGV.CurrentNode.Id);
//시작노드가없다면 오류
if (PUB._virtualAGV.StartNode == null)
{
PUB.log.AddE($"경로시작노드가 없습니다");
PUB.sm.SetNewRunStep(ERunStep.READY);
return false;
}
//대상노드가 없다면 오류
if (PUB._virtualAGV.TargetNode == null)
{
PUB.log.AddE($"경로종료노드가 없습니다");
PUB.sm.SetNewRunStep(ERunStep.READY);
return false;
}
//경로 생성(경로정보가 없거나 현재노드가 경로에 없는경우)
if (PUB._virtualAGV.HasPath() == false ||
PUB._virtualAGV.CurrentPath.DetailedPath.Where(t => t.NodeId.Equals(currentNode.Id)).Any() == false)
{
if (PUB.AGV.system1.agv_run)
{
PUB.log.Add($"경로 재생성으로 인해 구동을 멈춥니다");
PUB.AGV.AGVMoveStop("경로재생성");
}
PUB._virtualAGV.StartNode = PUB._virtualAGV.CurrentNode;
var PathResult = CalcPath(currentNode, PUB._virtualAGV.TargetNode);
if (PathResult.Success == false)
{
PUB.log.AddE($"경로가 계산되지 않았습니다");
PUB.sm.SetNewRunStep(ERunStep.READY);
return false;
}
PUB._mapCanvas.CurrentPath = PathResult;
PUB._virtualAGV.SetPath(PathResult);
PUB.log.AddI($"경로생성 {PUB._virtualAGV.StartNode.RfidId} -> {PUB._virtualAGV.TargetNode.RfidId}");
}
//경로에 대한 무결성 검증
if (CheckPathIntegrity(PUB._virtualAGV.CurrentPath) == false)
{
if (PUB.AGV.system1.agv_run)
{
PUB.AGV.AGVMoveStop("Path Integrity Fail");
}
PUB.log.AddE($"경로 무결성 오류로 인해 경로를 삭제 합니다");
PUB._virtualAGV.SetPath(null);
VAR.I32[eVarInt32.PathValidationError] += 1;
if (VAR.I32[eVarInt32.PathValidationError] > 50)
{
SetRunStepError(ENIGProtocol.AGVErrorCode.PATH_INTEGRITY_FAIL, $"연속 경로 무결성 오류로 인해 중지 합니다");
}
return false;
}
else VAR.I32[eVarInt32.PathValidationError] = 0;
//현재위치 기준으로 재 계산하여. 더 최적화된 루트가 있다면 처리를 해준다.
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 < halfcnt)
{
var msg = $"단축경로가 확인되었습니다. 경로를 삭제 합니다";
PUB.log.AddE(msg);
Console.WriteLine(msg);
PUB._virtualAGV.SetPath(null);
return false;
}
}
}
//predict 를 이용하여 다음 이동을 모두 확인한다.
var nextAction = PUB._virtualAGV.Predict();
if (nextAction.Reason == AGVNavigationCore.Models.eAGVCommandReason.PathOut)
{
//경로이탈
var logmessage = $"경로이탈감지 시작노드를 현재위치로 설정합니다 START:{PUB._virtualAGV.StartNode},CURRENT:{PUB._virtualAGV.CurrentNode}";
PUB.log.AddE(logmessage);
Console.WriteLine(logmessage);
PUB._virtualAGV.ClearPath();//.DetailedPath.Clear();
PUB._virtualAGV.StartNode = PUB._virtualAGV.CurrentNode;
return false;
}
//모터에서 정지를 요청했다
if (nextAction.Motor == AGVNavigationCore.Models.MotorCommand.Stop)
{
if (PUB.AGV.system1.agv_run)
{
// 완료(Complete) 상태라면 MarkStop 전송
if (nextAction.Reason == AGVNavigationCore.Models.eAGVCommandReason.MarkStop)
{
PUB.log.Add("다음행동예측에서 MARK STOP이 확인되었습니다 (자동정지시퀀스)");
PUB.AGV.AGVMoveStop(nextAction.Message, arDev.Narumi.eStopOpt.MarkStop);
}
else
{
PUB.log.Add($"다음행동예측에서 장비 멈춤이 확인되었습니다({nextAction.Reason})");
PUB.AGV.AGVMoveStop(nextAction.Message);
}
}
// 목적지 도착 여부 확인
// 현재 노드가 타겟 노드와 같고, 위치가 확정된 상태라면 도착으로 간주
// 단, AGV가 실제로 멈췄는지 확인 (agv_run == false)
if (PUB._virtualAGV.IsPositionConfirmed)
{
if (PUB.AGV.system1.agv_run == false)
{
// 경로가 존재한다면, 경로의 마지막 노드에 도착했는지 확인한다.
if (PUB._virtualAGV.CurrentPath != null && PUB._virtualAGV.CurrentPath.DetailedPath.Any())
{
var lastInfo = PUB._virtualAGV.CurrentPath.DetailedPath.Last();
// 위치와 방향이 모두 일치해야 완료된 것으로 본다.
if (PUB._virtualAGV.CurrentNode.Id == lastInfo.NodeId &&
PUB._virtualAGV.CurrentDirection == lastInfo.MotorDirection)
{
var node = PUB._mapCanvas.Nodes.Where(t => t.Id == PUB._virtualAGV.CurrentNodeId).FirstOrDefault();
var rfid = node?.ID2 ?? "(X)";
PUB.log.AddI($"목표 도착 및 정지 확인됨(MarkStop 완료) Node:{rfid}, Dir:{PUB._virtualAGV.CurrentDirection}");
return true;
}
}
else
{
// 경로 정보가 없다면 단순히 목적지 ID와 비교한다 (Fallback)
if (PUB._virtualAGV.CurrentNode.Id == PUB._virtualAGV.TargetNode.Id)
{
var node = PUB._mapCanvas.Nodes.Where(t => t.Id == PUB._virtualAGV.CurrentNodeId).FirstOrDefault();
var rfid = node?.ID2 ?? "(X)";
PUB.log.AddI($"목표 도착 및 정지 확인됨(MarkStop 완료, No Path Info) Node:{rfid}");
return true;
}
}
}
else
{
//아직 멈추지 않았다면 기다린다.
}
}
return false;
}
else
{
// 이동 명령 변환 (AGVNavigationCore -> arDev.Narumi)
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;
var dir = arDev.Narumi.eMoveDir.Forward;
if (nextAction.Motor == AGVNavigationCore.Models.MotorCommand.Backward) dir = arDev.Narumi.eMoveDir.Backward;
var spd = arDev.Narumi.eMoveSpd.Low;
if (nextAction.Speed == AGVNavigationCore.Models.SpeedLevel.M) spd = arDev.Narumi.eMoveSpd.Mid;
else if (nextAction.Speed == AGVNavigationCore.Models.SpeedLevel.H) spd = arDev.Narumi.eMoveSpd.High;
// 명령 설정
// 현재 상태와 다를 때만 전송 (불필요한 통신 부하 방지)
if (PUB.AGV.data.Sts != bunki.ToString()[0] ||
PUB.AGV.data.Direction != dir.ToString()[0] ||
PUB.AGV.data.Speed != spd.ToString()[0])
{
var ret = PUB.AGV.AGVMoveSet(new arDev.Narumi.BunkiData
{
Bunki = bunki,
Direction = dir,
PBSSensor = 1,
Speed = spd,
});
if (ret == arDev.eNarumiCommandResult.Success)
PUB.log.Add($"Predict Run Setting = bunki:{bunki},dir:{dir},pbs:1,spd:{spd}");
else
PUB.log.AddE($"Predict Run Setting = bunki:{bunki},dir:{dir},pbs:1,spd:{spd}");
}
// AGV가 정지 상태라면 구동 시작
if (PUB.AGV.system1.agv_run == false)
{
var runOpt = (dir == arDev.Narumi.eMoveDir.Forward) ? arDev.Narumi.eRunOpt.Forward : arDev.Narumi.eRunOpt.Backward;
PUB.AGV.AGVMoveRun(runOpt);
}
return false;
}
}
/// <summary>
/// 충전기검색시퀀스
/// </summary>
/// <param name="sender"></param>
/// <returns></returns>
Boolean UpdateMotionPositionForCharger(string sender)
{
if (VAR.BOOL[eVarBool.AGVDIR_BACK] == false)// PUB.flag.get(EFlag.FLAG_DIR_BW) == true)
{
//충전기 검색은 항상 앞으로 검색한다
var tsCmd = DateTime.Now - tm_gocharge_command;
if (tsCmd.TotalMilliseconds >= 1999 &&
PUB.AGV.error.Emergency == false &&
PUB.AGV.system1.agv_run == false)
{
var ret = PUB.AGV.AGVMoveSet(new arDev.Narumi.BunkiData
{
Bunki = arDev.Narumi.eBunki.Strate,
Direction = arDev.Narumi.eMoveDir.Forward,
PBSSensor = 1,
Speed = arDev.Narumi.eMoveSpd.Low,
});
if (ret == arDev.eNarumiCommandResult.Success)
{
PUB.AGV.AGVMoveRun(arDev.Narumi.eRunOpt.Forward);//
}
tm_gocharge_command = DateTime.Now;
}
}
else
{
//현재위치가 충전위치이고, 움직이지 않았다면 완료된 경우라 할수 있따
if (PUB._virtualAGV.CurrentNode.Id.Equals(PUB.setting.NodeMAP_RFID_Charger) &&
VAR.BOOL[eVarBool.MARK_SENSOR] == true)
{
PUB.log.AddI("충전위치 검색 완료");
return true;
}
else
{
//이동중이지 않다면 항상 이동을 해줘야한다
var tsCmd = DateTime.Now - LastCommandTime;
if (tsCmd.TotalMilliseconds >= 1999 &&
PUB.AGV.error.Emergency == false &&
PUB.AGV.system1.agv_run == false)
{
var ret = PUB.AGV.AGVMoveSet(new arDev.Narumi.BunkiData
{
Bunki = arDev.Narumi.eBunki.Strate,
Direction = arDev.Narumi.eMoveDir.Backward,
PBSSensor = 1,
Speed = arDev.Narumi.eMoveSpd.Low,
});
if (ret == arDev.eNarumiCommandResult.Success)
PUB.AGV.AGVMoveRun(arDev.Narumi.eRunOpt.Backward);//
LastCommandTime = DateTime.Now;
}
}
//}
}
return false;
}
/// <summary>
/// 경로 무결성(도킹방향 등) 검증
/// </summary>
/// <param name="pathResult"></param>
/// <returns></returns>
private bool CheckPathIntegrity(AGVNavigationCore.PathFinding.Core.AGVPathResult pathResult)
{
if (pathResult == null) return false;
// CalcPath에서 이미 DockingValidator를 수행했을 수 있음.
// 만약 수행되지 않았다면 여기서 수행.
if (pathResult.DockingValidation == null)
{
pathResult.DockingValidation = AGVNavigationCore.Utils.DockingValidator.ValidateDockingDirection(pathResult, PUB._mapCanvas.Nodes);
}
// 검증 결과 확인
if (pathResult.DockingValidation != null && pathResult.DockingValidation.IsValidationRequired)
{
if (pathResult.DockingValidation.IsValid == false)
{
PUB.log.AddE($"[경로무결성오류] {pathResult.DockingValidation.ValidationError}");
return false;
}
}
return true;
}
}//cvass
}