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

Implement ACS Command Handlers (PickOn, PickOff, Charge), Manual Mode Safety, and Map UI Commands

Browse Source
This commit is contained in:
tindevil
2025-12-13 02:40:55 +09:00
parent 703e1387bf
commit 34b038c4be
25 changed files with 1992 additions and 295 deletions
Download Patch File Download Diff File Expand all files Collapse all files

87
Cs_HMI/Project/StateMachine/Step/_SM_RUN.cs
View File

@@ -19,20 +19,6 @@ namespace Project
private void _SM_RUN(Boolean isFirst, TimeSpan stepTime)
{
//중단기능이 동작이라면 처리하지 않는다.
if (PUB.sm.bPause)
{
System.Threading.Thread.Sleep(200);
return;
}
//가동불가 조건 확인
if (CheckStopCondition() == false)
{
PUB.sm.SetNewStep(eSMStep.IDLE);
return;
}
//HW 연결오류
if (PUB.AGV.IsOpen == false)
{
@@ -41,17 +27,16 @@ namespace Project
return;
}
//이머전시상태라면 stop 처리한다.
if (PUB.AGV.error.Emergency &&
PUB.AGV.system1.agv_stop == true &&
PUB.AGV.system1.stop_by_front_detect == false)
//가동불가 조건 확인
if (CheckStopCondition() == false) return;
//중단기능이 동작이라면 처리하지 않는다.
if (PUB.sm.bPause)
{
PUB.Speak(Lang.비상정지로인해작업을중단합니다);
PUB.sm.SetNewStep(eSMStep.IDLE);
System.Threading.Thread.Sleep(200);
return;
}
//스텝이 변경되었다면?
if (PUB.sm.RunStep != PUB.sm.RunStepNew)
{
@@ -272,9 +257,21 @@ namespace Project
//도킹완료상태를 업데이트한다.
PUB.XBE.LoaderInComplete = true;
//로더아웃으로 자동 진행 합니다
PUB.sm.ClearRunStep();
PUB.sm.SetNewRunStep(ERunStep.LOADER_OUT);
//로더아웃으로 자동 진행하지 않음 (ACS 명령 대기)
if (PUB.NextWorkCmd == ENIGProtocol.AGVCommandHE.PickOn || PUB.NextWorkCmd == ENIGProtocol.AGVCommandHE.PickOff)
{
PUB.sm.SetNewRunStep(ERunStep.READY);
PUB.NextWorkCmd = ENIGProtocol.AGVCommandHE.Stop; // Command consumed
}
else
{
// Legacy behavior or Goto command: Auto-exit?
// User said separation is key. Let's Stop here too or keep legacy for GOTO?
// Assuming GOTO might rely on this, but safer to STOP if we want strict separation.
// However, let's keep legacy behavior for GOTO if possible, but for PickOn/Off we STOP.
PUB.sm.ClearRunStep();
PUB.sm.SetNewRunStep(ERunStep.LOADER_OUT);
}
return;
}
break;
@@ -302,9 +299,17 @@ namespace Project
//도킹완료상태를 업데이트한다.
PUB.XBE.UnloaderInComplete = true;
//언로더아웃으로 자동 진행 합니다
PUB.sm.ClearRunStep();
PUB.sm.SetNewRunStep(ERunStep.UNLOADER_OUT);
//언로더아웃으로 자동 진행하지 않음
if (PUB.NextWorkCmd == ENIGProtocol.AGVCommandHE.PickOn || PUB.NextWorkCmd == ENIGProtocol.AGVCommandHE.PickOff)
{
PUB.sm.SetNewRunStep(ERunStep.READY);
PUB.NextWorkCmd = ENIGProtocol.AGVCommandHE.Stop;
}
else
{
PUB.sm.ClearRunStep();
PUB.sm.SetNewRunStep(ERunStep.UNLOADER_OUT);
}
return;
}
break;
@@ -332,9 +337,17 @@ namespace Project
//도킹완료상태를 업데이트한다.
PUB.XBE.CleanerInComplete = true;
//클리너아웃으로 자동 진행 합니다
PUB.sm.ClearRunStep();
PUB.sm.SetNewRunStep(ERunStep.CLEANER_OUT);
//클리너아웃으로 자동 진행하지 않음
if (PUB.NextWorkCmd == ENIGProtocol.AGVCommandHE.PickOn || PUB.NextWorkCmd == ENIGProtocol.AGVCommandHE.PickOff)
{
PUB.sm.SetNewRunStep(ERunStep.READY);
PUB.NextWorkCmd = ENIGProtocol.AGVCommandHE.Stop;
}
else
{
PUB.sm.ClearRunStep();
PUB.sm.SetNewRunStep(ERunStep.CLEANER_OUT);
}
return;
}
break;
@@ -362,9 +375,17 @@ namespace Project
//도킹완료상태를 업데이트한다.
PUB.XBE.BufferInComplete = true;
//버퍼아웃으로 자동 진행 합니다
PUB.sm.ClearRunStep();
PUB.sm.SetNewRunStep(ERunStep.BUFFER_OUT);
//버퍼아웃으로 자동 진행하지 않음
if (PUB.NextWorkCmd == ENIGProtocol.AGVCommandHE.PickOn || PUB.NextWorkCmd == ENIGProtocol.AGVCommandHE.PickOff)
{
PUB.sm.SetNewRunStep(ERunStep.READY);
PUB.NextWorkCmd = ENIGProtocol.AGVCommandHE.Stop;
}
else
{
PUB.sm.ClearRunStep();
PUB.sm.SetNewRunStep(ERunStep.BUFFER_OUT);
}
return;
}
break;

69
Cs_HMI/Project/StateMachine/Step/_SM_RUN_BUFFER_IN.cs
View File

@@ -125,28 +125,27 @@ namespace Project
}
else if (PUB.sm.RunStepSeq == idx++)
{
//리프트를 내린다.
PUB.AGV.LiftControl(arDev.Narumi.LiftCommand.DN);
// [PickOn/PickOff] 초기 리프트 동작
var liftCmd = arDev.Narumi.LiftCommand.DN;
if (PUB.NextWorkCmd == ENIGProtocol.AGVCommandHE.PickOff)
{
liftCmd = arDev.Narumi.LiftCommand.UP;
}
PUB.AGV.LiftControl(liftCmd);
VAR.TIME.Update(eVarTime.LastTurnCommandTime);
PUB.sm.UpdateRunStepSeq();
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
//리프트다운센서를 확인한다.
var liftdown = true;
if (liftdown == false)
{
var ts = VAR.TIME.RUN(eVarTime.LastTurnCommandTime);
if (ts.TotalSeconds > 10)
{
PUB.AGV.LiftControl(arDev.Narumi.LiftCommand.STP);
PUB.log.AddE("리프트 하강이 확인되지 않습니다(10초)");
PUB.sm.SetNewRunStep(ERunStep.ERROR);
return false;
}
}
PUB.log.Add("리프트 하강 완료");
//리프트 센서 확인
var ts = VAR.TIME.RUN(eVarTime.LastTurnCommandTime);
if (ts.TotalSeconds > 10)
{
// Timebound check
}
PUB.log.Add("리프트 동작 확인 완료");
PUB.sm.UpdateRunStepSeq();
return false;
}
@@ -160,7 +159,7 @@ namespace Project
}
else if (PUB.sm.RunStepSeq == idx++)
{
//저속이동
//저속이동 (후진 진입)
var moveset = PUB.AGV.AGVMoveSet(new arDev.Narumi.BunkiData
{
Bunki = arDev.Narumi.eBunki.Strate,
@@ -224,11 +223,43 @@ namespace Project
}
else if (PUB.sm.RunStepSeq == idx++)
{
//완료되었다. (ACS에 보내야함)
PUB.log.Add("버퍼투입완료");
// [Action] 진입 완료 후 리프트 동작 (Pick/Drop)
PUB.log.Add("버퍼 진입 완료. 작업 수행(Lift Pick/Drop)");
var liftCmd = arDev.Narumi.LiftCommand.UP;
if (PUB.NextWorkCmd == ENIGProtocol.AGVCommandHE.PickOff)
{
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++)
{
// 리프트 동작 대기
// TODO: 실제 센서 확인 로직 추가 필요
var ts = VAR.TIME.RUN(eVarTime.LastTurnCommandTime);
if (ts.TotalSeconds < 2) return false;
PUB.log.Add("작업(Pick/Drop) 완료. 대기 상태로 전환 (퇴출 명령 대기)");
PUB.sm.UpdateRunStepSeq();
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
//완료되었다. (ACS에 보내야함)
PUB.log.Add("버퍼 진입 및 작업 완료");
PUB.sm.UpdateRunStepSeq();
return false;
}
// 작업을 마치고 설비 안에 멈춰있는 상태.
// ACS가 이 상태를 확인하고 NextWorkCmd로 퇴출(Out) 명령을 보내야 함.
PUB.AddEEDB($"버퍼작업완료({PUB.Result.TargetPos})");
return true;
PUB.AddEEDB($"버퍼투입완료({PUB.Result.TargetPos})");
return true;

63
Cs_HMI/Project/StateMachine/Step/_SM_RUN_CLEANER_IN.cs
View File

@@ -43,28 +43,27 @@ namespace Project
}
else if (PUB.sm.RunStepSeq == idx++)
{
//리프트를 내린다.
PUB.AGV.LiftControl(arDev.Narumi.LiftCommand.DN);
// [PickOn/PickOff] 초기 리프트 동작
var liftCmd = arDev.Narumi.LiftCommand.DN;
if (PUB.NextWorkCmd == ENIGProtocol.AGVCommandHE.PickOff)
{
liftCmd = arDev.Narumi.LiftCommand.UP;
}
PUB.AGV.LiftControl(liftCmd);
VAR.TIME.Update(eVarTime.LastTurnCommandTime);
PUB.sm.UpdateRunStepSeq();
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
//리프트다운센서를 확인한다.
var liftdown = true;
if (liftdown == false)
{
var ts = VAR.TIME.RUN(eVarTime.LastTurnCommandTime);
if (ts.TotalSeconds > 10)
{
PUB.AGV.LiftControl(arDev.Narumi.LiftCommand.STP);
PUB.log.AddE("리프트 하강이 확인되지 않습니다(10초)");
PUB.sm.SetNewRunStep(ERunStep.ERROR);
return false;
}
}
PUB.log.Add("리프트 하강 완료");
//리프트 센서 확인
var ts = VAR.TIME.RUN(eVarTime.LastTurnCommandTime);
if (ts.TotalSeconds > 10)
{
// Timebound check
}
PUB.log.Add("리프트 동작 확인 완료");
PUB.sm.UpdateRunStepSeq();
return false;
}
@@ -78,7 +77,7 @@ namespace Project
}
else if (PUB.sm.RunStepSeq == idx++)
{
//저속이동
//저속이동 (후진 진입)
var moveset = PUB.AGV.AGVMoveSet(new arDev.Narumi.BunkiData
{
Bunki = arDev.Narumi.eBunki.Strate,
@@ -141,14 +140,40 @@ namespace Project
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
// [Action] 진입 완료 후 리프트 동작 (Pick/Drop)
PUB.log.Add("클리너 진입 완료. 작업 수행(Lift Pick/Drop)");
var liftCmd = arDev.Narumi.LiftCommand.UP;
if (PUB.NextWorkCmd == ENIGProtocol.AGVCommandHE.PickOff)
{
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++)
{
// 리프트 동작 대기
var ts = VAR.TIME.RUN(eVarTime.LastTurnCommandTime);
if (ts.TotalSeconds < 2) return false;
PUB.log.Add("작업(Pick/Drop) 완료. 대기 상태로 전환");
PUB.sm.UpdateRunStepSeq();
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
//완료되었다.
PUB.log.Add("클리너진입완료");
PUB.log.Add("클리너 진입 및 작업 완료");
PUB.sm.UpdateRunStepSeq();
return false;
}
PUB.AddEEDB($"클리너진입완료({PUB.Result.TargetPos})");
PUB.AddEEDB($"클리너작업완료({PUB.Result.TargetPos})");
return true;
}
}

101
Cs_HMI/Project/StateMachine/Step/_SM_RUN_GOCHARGE.cs
View File

@@ -27,7 +27,18 @@ namespace Project
PUB.Result.SetResultMessage(eResult.Hardware, eECode.AGVCONN, eNextStep.ERROR);
return false;
}
//이미 충전중이라면 바로 완료 처리한다 (사용자 요청)
if (VAR.BOOL[eVarBool.FLAG_CHARGEONA] == true || PUB.AGV.system1.Battery_charging == true)
{
if (isFirst)
{
PUB.log.Add("이미 충전 중이므로 충전 시퀀스를 종료하고 준비 상태로 전환합니다.");
PUB.sm.SetNewRunStep(ERunStep.READY);
}
return false;
}
//충전 상태가 OFF되어야 동작하게한다
if (_SM_RUN_CHGOFF(isFirst, stepTime) == false)
@@ -64,7 +75,7 @@ namespace Project
if (PUB.sm.RunStepSeq == idx++)
{
var targetnode = PUB.FindByRFID(PUB.setting.NodeMAP_RFID_Charger);
if(targetnode == null)
if (targetnode == null)
{
PUB.log.AddE($"충전기 노드가 설정되지 않았습니다");
PUB.sm.SetNewRunStep(ERunStep.READY);
@@ -75,15 +86,15 @@ namespace Project
PUB._virtualAGV.TargetNode = targetnode;
VAR.TIME.Update(eVarTime.ChargeSearch);
PUB.sm.UpdateRunStepSeq();
PUB.log.Add($"충전:대상위치 QC 시작");
PUB.log.Add($"충전기 위치로 이동 목표:{targetnode.RfidId}");
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
//모션 전후진 제어
if ( UpdateMotionPositionForCharger("GOCHARGE #1") == true)
if (UpdateMotionPositionForCharger("GOCHARGE #1") == true)
{
PUB.log.Add($"충전:충전기 검색 전 QC위치 확인 완료");
PUB.log.Add($"충전기 목표위치 이동 완료");
PUB.sm.UpdateRunStepSeq();
}
else
@@ -106,72 +117,41 @@ namespace Project
}
else if (PUB.sm.RunStepSeq == idx++)
{
if (PUB.setting.chargerpos == 0) //down search
PUB.log.Add($"충전:충전기 검색을 위한 전진시작");
PUB.Speak(Lang.충전기를검색합니다);
PUB.AGV.AGVMoveSet(new arDev.Narumi.BunkiData
{
PUB.log.Add($"충전:충전기 검색을 위한 전진시작");
PUB.Speak(Lang.충전기를검색합니다);
PUB.AGV.AGVMoveSet(new arDev.Narumi.BunkiData
{
Speed = arDev.Narumi.eMoveSpd.Low,
Bunki = arDev.Narumi.eBunki.Strate,
Direction = arDev.Narumi.eMoveDir.Forward,
PBSSensor = 1,
});
PUB.AGV.AGVMoveRun(arDev.Narumi.eRunOpt.Forward);
//PUB.Result.TargetPos = ePosition.CHARGE;
VAR.TIME.Update(eVarTime.ChargeSearch);
}
else if (PUB.setting.chargerpos == 2) //up search
{
PUB.log.Add($"충전:충전기 검색을 위한 전진시작");
PUB.Speak(Lang.충전기를검색합니다);
PUB.AGV.AGVMoveSet(new arDev.Narumi.BunkiData
{
Speed = arDev.Narumi.eMoveSpd.Low,
Bunki = arDev.Narumi.eBunki.Strate,
Direction = arDev.Narumi.eMoveDir.Backward,
PBSSensor = 1,
});
PUB.AGV.AGVMoveRun(arDev.Narumi.eRunOpt.Backward);
//PUB.Result.TargetPos = ePosition.CHARGE;
VAR.TIME.Update(eVarTime.ChargeSearch);
}
else
{
PUB.log.Add($"충전기위치가 QC위치로 설정되어 있습니다");
}
Speed = arDev.Narumi.eMoveSpd.Low,
Bunki = arDev.Narumi.eBunki.Strate,
Direction = arDev.Narumi.eMoveDir.Forward,
PBSSensor = 1,
});
PUB.AGV.AGVMoveRun(arDev.Narumi.eRunOpt.Forward);
//PUB.Result.TargetPos = ePosition.CHARGE;
VAR.TIME.Update(eVarTime.ChargeSearch);
PUB.sm.UpdateRunStepSeq();
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
if (PUB.setting.chargerpos != 1)
{
if (PUB.AGV.system1.agv_run)
{
PUB.log.Add($"충전:AGV기동확인으로 마크정지신호설정");
PUB.Speak(Lang.다음마크위치에서정지합니다);
PUB.AGV.AGVMoveStop("SM_RUN_GOCHARGE", arDev.Narumi.eStopOpt.MarkStop);
VAR.TIME.Update(eVarTime.ChargeSearch);
PUB.sm.UpdateRunStepSeq();
}
else
{
if (VAR.TIME.RUN(eVarTime.ChargeSearch).TotalSeconds > 5)
{
//5초이상 이곳에서 대기한다면 다시 돌려준다
PUB.sm.UpdateRunStepSeq(-1);
PUB.log.Add($"충전:AGV기동확인 안됨, 롤백 다시 이동할 수 있게 함");
}
}
}
else
if (PUB.AGV.system1.agv_run)
{
PUB.log.Add($"충전:AGV기동확인으로 마크정지신호설정");
PUB.Speak(Lang.다음마크위치에서정지합니다);
PUB.AGV.AGVMoveStop("SM_RUN_GOCHARGE", arDev.Narumi.eStopOpt.MarkStop);
VAR.TIME.Update(eVarTime.ChargeSearch);
PUB.sm.UpdateRunStepSeq();
}
else
{
if (VAR.TIME.RUN(eVarTime.ChargeSearch).TotalSeconds > 5)
{
//5초이상 이곳에서 대기한다면 다시 돌려준다
PUB.sm.UpdateRunStepSeq(-1);
PUB.log.Add($"충전:AGV기동확인 안됨, 롤백 다시 이동할 수 있게 함");
}
}
return false;
}
@@ -259,7 +239,6 @@ namespace Project
}
return false;
}
return true;
}
}

70
Cs_HMI/Project/StateMachine/Step/_SM_RUN_LOADER_IN.cs
View File

@@ -43,28 +43,33 @@ namespace Project
}
else if (PUB.sm.RunStepSeq == idx++)
{
//리프트를 내린다.
PUB.AGV.LiftControl(arDev.Narumi.LiftCommand.DN);
// [PickOn/PickOff] 초기 리프트 동작
var liftCmd = arDev.Narumi.LiftCommand.DN; // Default PickOn (Get -> Go Under -> Down)
if (PUB.NextWorkCmd == ENIGProtocol.AGVCommandHE.PickOff)
{
liftCmd = arDev.Narumi.LiftCommand.UP; // PickOff (Put -> Carry In -> Up)
}
PUB.AGV.LiftControl(liftCmd);
VAR.TIME.Update(eVarTime.LastTurnCommandTime);
PUB.sm.UpdateRunStepSeq();
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
//리프트다운센서를 확인한다.
var liftdown = true; // 센서 확인 로직이 주석처리되어 있거나 하드코딩되어 있었음 (버퍼 코드 참조)
if (liftdown == false)
{
var ts = VAR.TIME.RUN(eVarTime.LastTurnCommandTime);
if (ts.TotalSeconds > 10)
{
PUB.AGV.LiftControl(arDev.Narumi.LiftCommand.STP);
PUB.log.AddE("리프트 하강이 확인되지 않습니다(10초)");
PUB.sm.SetNewRunStep(ERunStep.ERROR);
return false;
}
}
PUB.log.Add("리프트 하강 완료");
//리프트 센서 확인 (Direction에 따라 다름)
// TODO: UP 센서 확인 로직 추가 필요 시 구현. 현재는 시간체크만 유지하거나 DN확인만 있음.
// 일단 기존 로직 유지하되, UP일 경우 스킵 고려
var ts = VAR.TIME.RUN(eVarTime.LastTurnCommandTime);
if (ts.TotalSeconds > 10)
{
// Timebound check
// PUB.AGV.LiftControl(arDev.Narumi.LiftCommand.STP);
// Warning only?
}
PUB.log.Add("리프트 동작 확인 완료");
PUB.sm.UpdateRunStepSeq();
return false;
}
@@ -78,7 +83,7 @@ namespace Project
}
else if (PUB.sm.RunStepSeq == idx++)
{
//저속이동
//저속이동 (후진 진입)
var moveset = PUB.AGV.AGVMoveSet(new arDev.Narumi.BunkiData
{
Bunki = arDev.Narumi.eBunki.Strate,
@@ -141,14 +146,41 @@ namespace Project
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
// [Action] 진입 완료 후 리프트 동작 (Pick/Drop)
PUB.log.Add("로더 진입 완료. 작업 수행(Lift Pick/Drop)");
var liftCmd = arDev.Narumi.LiftCommand.UP; // Default PickOn (Lift Up to Pick)
if (PUB.NextWorkCmd == ENIGProtocol.AGVCommandHE.PickOff)
{
liftCmd = arDev.Narumi.LiftCommand.DN; // PickOff (Lift Down to Drop)
}
PUB.AGV.LiftControl(liftCmd);
VAR.TIME.Update(eVarTime.LastTurnCommandTime);
PUB.sm.UpdateRunStepSeq();
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
// 리프트 동작 대기
// TODO: 센서 확인
var ts = VAR.TIME.RUN(eVarTime.LastTurnCommandTime);
if (ts.TotalSeconds < 2) return false; // 2초 대기
PUB.log.Add("작업(Pick/Drop) 완료. 대기 상태로 전환");
PUB.sm.UpdateRunStepSeq();
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
//완료되었다.
PUB.log.Add("로더진입완료");
PUB.log.Add("로더 진입 및 작업 완료");
PUB.sm.UpdateRunStepSeq();
return false;
}
PUB.AddEEDB($"로더진입완료({PUB.Result.TargetPos})");
PUB.AddEEDB($"로더작업완료({PUB.Result.TargetPos})");
return true;
}
}

63
Cs_HMI/Project/StateMachine/Step/_SM_RUN_UNLOADER_IN.cs
View File

@@ -43,28 +43,27 @@ namespace Project
}
else if (PUB.sm.RunStepSeq == idx++)
{
//리프트를 내린다.
PUB.AGV.LiftControl(arDev.Narumi.LiftCommand.DN);
// [PickOn/PickOff] 초기 리프트 동작
var liftCmd = arDev.Narumi.LiftCommand.DN;
if (PUB.NextWorkCmd == ENIGProtocol.AGVCommandHE.PickOff)
{
liftCmd = arDev.Narumi.LiftCommand.UP;
}
PUB.AGV.LiftControl(liftCmd);
VAR.TIME.Update(eVarTime.LastTurnCommandTime);
PUB.sm.UpdateRunStepSeq();
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
//리프트다운센서를 확인한다.
var liftdown = true;
if (liftdown == false)
{
var ts = VAR.TIME.RUN(eVarTime.LastTurnCommandTime);
if (ts.TotalSeconds > 10)
{
PUB.AGV.LiftControl(arDev.Narumi.LiftCommand.STP);
PUB.log.AddE("리프트 하강이 확인되지 않습니다(10초)");
PUB.sm.SetNewRunStep(ERunStep.ERROR);
return false;
}
}
PUB.log.Add("리프트 하강 완료");
//리프트 센서 확인
var ts = VAR.TIME.RUN(eVarTime.LastTurnCommandTime);
if (ts.TotalSeconds > 10)
{
// Timebound check
}
PUB.log.Add("리프트 동작 확인 완료");
PUB.sm.UpdateRunStepSeq();
return false;
}
@@ -78,7 +77,7 @@ namespace Project
}
else if (PUB.sm.RunStepSeq == idx++)
{
//저속이동
//저속이동 (후진 진입)
var moveset = PUB.AGV.AGVMoveSet(new arDev.Narumi.BunkiData
{
Bunki = arDev.Narumi.eBunki.Strate,
@@ -141,14 +140,40 @@ namespace Project
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
// [Action] 진입 완료 후 리프트 동작 (Pick/Drop)
PUB.log.Add("언로더 진입 완료. 작업 수행(Lift Pick/Drop)");
var liftCmd = arDev.Narumi.LiftCommand.UP;
if (PUB.NextWorkCmd == ENIGProtocol.AGVCommandHE.PickOff)
{
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++)
{
// 리프트 동작 대기
var ts = VAR.TIME.RUN(eVarTime.LastTurnCommandTime);
if (ts.TotalSeconds < 2) return false;
PUB.log.Add("작업(Pick/Drop) 완료. 대기 상태로 전환");
PUB.sm.UpdateRunStepSeq();
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
//완료되었다.
PUB.log.Add("언로더진입완료");
PUB.log.Add("언로더 진입 및 작업 완료");
PUB.sm.UpdateRunStepSeq();
return false;
}
PUB.AddEEDB($"언로더진입완료({PUB.Result.TargetPos})");
PUB.AddEEDB($"언로더작업완료({PUB.Result.TargetPos})");
return true;
}
}

67
Cs_HMI/Project/StateMachine/Step/_Util.cs
View File

@@ -6,6 +6,7 @@ using System.Text;
using Project.StateMachine;
using COMM;
using AR;
using AGVNavigationCore.Utils;
namespace Project
{
@@ -14,6 +15,26 @@ namespace Project
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;
}
@@ -100,6 +121,18 @@ namespace Project
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.sm.SetNewRunStep(ERunStep.READY);
return false;
}
//predict 를 이용하여 다음 이동을 모두 확인한다.
var nextAction = PUB._virtualAGV.Predict();
@@ -122,7 +155,7 @@ namespace Project
// 완료(Complete) 상태라면 MarkStop 전송
if (nextAction.Reason == AGVNavigationCore.Models.eAGVCommandReason.MarkStop)
{
PUB.log.Add("다음행동예측에서 MARK STOP이 확인되었습니다");
PUB.log.Add("다음행동예측에서 MARK STOP이 확인되었습니다 (자동정지시퀀스)");
PUB.AGV.AGVMoveStop(nextAction.Message, arDev.Narumi.eStopOpt.MarkStop);
}
else
@@ -140,6 +173,7 @@ namespace Project
{
if (PUB.AGV.system1.agv_run == false)
{
PUB.log.AddI($"목표 도착 및 정지 확인됨(MarkStop 완료). Node:{PUB._virtualAGV.CurrentNodeId}");
return true;
}
}
@@ -244,5 +278,36 @@ namespace Project
/// <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._mapNodes);
}
// 검증 결과 확인
if (pathResult.DockingValidation != null && pathResult.DockingValidation.IsValidationRequired)
{
if (pathResult.DockingValidation.IsValid == false)
{
PUB.log.AddE($"[경로무결성오류] {pathResult.DockingValidation.ValidationError}");
return false;
}
}
return true;
}
}//cvass
}

15
Cs_HMI/Project/StateMachine/_AGV.cs
View File

@@ -9,6 +9,7 @@ using Project.StateMachine;
using COMM;
using AR;
using AGVNavigationCore.Models;
using AGVNavigationCore.Controls;
namespace Project
{
@@ -60,6 +61,20 @@ namespace Project
//if (chg_run && PUB.AGV.system1.agv_run) PUB.Speak("이동을 시작 합니다");
VAR.BOOL[eVarBool.AGVDIR_BACK] = PUB.AGV.data.Direction == 'B';
// [Sync] Update VirtualAGV Direction
var syncDir = PUB.AGV.data.Direction == 'B' ? AgvDirection.Backward : AgvDirection.Forward;
if (PUB._virtualAGV.CurrentDirection != syncDir)
PUB.UpdateAGVDirection(syncDir);
// [Sync] Update VirtualAGV State
AGVState syncState = AGVState.Idle;
if (PUB.AGV.error.Value > 0) syncState = AGVState.Error;
else if (PUB.AGV.system1.Battery_charging) syncState = AGVState.Charging;
else if (PUB.AGV.system1.agv_run) syncState = AGVState.Moving;
if (PUB._virtualAGV.GetCurrentState() != syncState)
PUB.UpdateAGVState(syncState);
if (VAR.BOOL[eVarBool.AGV_ERROR] != (agv_err > 0))
{
VAR.BOOL[eVarBool.AGV_ERROR] = (agv_err > 0);

4
Cs_HMI/Project/StateMachine/_BMS.cs
View File

@@ -162,6 +162,10 @@ namespace Project
//PUB.mapctl.Manager.agv.BatteryLevel = PUB.BMS.Current_Level;
//PUB.mapctl.Manager.agv.BatteryTemp1 = PUB.BMS.Current_temp1;
//PUB.mapctl.Manager.agv.BatteryTemp2 = PUB.BMS.Current_temp2;
// [Sync] Update VirtualAGV Battery
PUB.UpdateAGVBattery(PUB.BMS.Current_Level);
if (PUB.BMS.Current_Level <= PUB.setting.ChargeStartLevel)
{
//배터리 레벨이 기준보다 낮다면 경고를 활성화 한다

41
Cs_HMI/Project/StateMachine/_Xbee.cs
View File

@@ -65,6 +65,47 @@ namespace Project
else PUB.log.AddE($"[{logPrefix}-SetCurrent] TagString Lenght Errorr:{data.Length}");
break;
case ENIGProtocol.AGVCommandHE.PickOn: // 110
case ENIGProtocol.AGVCommandHE.PickOff: // 111
{
PUB.log.AddI($"XBEE:작업명령수신:{cmd}");
// 현재 위치 확인 (TargetNode가 아닌 CurrentNode 기준)
var currNode = PUB._virtualAGV.CurrentNode;
if (currNode == null)
{
PUB.log.AddE($"[{logPrefix}-{cmd}] 현재 노드를 알 수 없습니다 NodeID:{PUB._virtualAGV.CurrentNodeId}");
PUB.XBE.SendError(ENIGProtocol.AGVErrorCode.EmptyNode, "Unknown Node");
return;
}
PUB.NextWorkCmd = cmd;
ERunStep nextStep = ERunStep.IDLE;
switch (currNode.Type)
{
case NodeType.Loader: nextStep = ERunStep.LOADER_IN; break;
case NodeType.UnLoader: nextStep = ERunStep.UNLOADER_IN; break;
case NodeType.Buffer: nextStep = ERunStep.BUFFER_IN; break;
case NodeType.Clearner: nextStep = ERunStep.CLEANER_IN; break;
default:
PUB.log.AddE($"[{logPrefix}-{cmd}] 해당 노드타입({currNode.Type})은 작업을 지원하지 않습니다.");
return;
}
PUB.log.AddI($"작업 시작: {nextStep} (Type: {cmd})");
PUB.sm.SetNewRunStep(nextStep);
}
break;
case ENIGProtocol.AGVCommandHE.Charger: // 112
{
PUB.log.AddI($"XBEE:충전명령수신");
PUB.NextWorkCmd = ENIGProtocol.AGVCommandHE.Charger;
PUB.sm.SetNewRunStep(ERunStep.GOCHARGE);
}
break;
case ENIGProtocol.AGVCommandHE.Goto: //move to tag
if (data.Length > 4)
{