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

..

Browse Source
This commit is contained in:
backuppc
2026-01-27 14:01:30 +09:00
parent a04a0505d0
commit 9ee8295489
18 changed files with 644 additions and 1073 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
Cs_HMI/Project/AGV4.csproj
View File

@@ -289,24 +289,12 @@
<Compile Include="Properties\AssemblyInfo.cs" />
<Compile Include="PUB.cs" />
<Compile Include="CSetting.cs" />
<Compile Include="StateMachine\Step\_SM_RUN_CLEANER_IN.cs">
<SubType>Form</SubType>
</Compile>
<Compile Include="StateMachine\Step\_SM_RUN_CLEANER_OUT.cs">
<SubType>Form</SubType>
</Compile>
<Compile Include="StateMachine\Step\_SM_RUN_LOADER_IN.cs">
<SubType>Form</SubType>
</Compile>
<Compile Include="StateMachine\Step\_SM_RUN_LOADER_OUT.cs">
<SubType>Form</SubType>
</Compile>
<Compile Include="StateMachine\Step\_SM_RUN_UNLOADER_IN.cs">
<SubType>Form</SubType>
</Compile>
<Compile Include="StateMachine\Step\_SM_RUN_UNLOADER_OUT.cs">
<SubType>Form</SubType>
</Compile>
<Compile Include="StateMachine\Step\_Util.cs">
<SubType>Form</SubType>
</Compile>

78
Cs_HMI/Project/Device/Xbee.cs
View File

@@ -21,27 +21,27 @@ namespace Project.Device
/// <summary>
/// 투입준비됨
/// </summary>
InReady = 10,
ReadyForEnter = 10,
/// <summary>
/// 투입완료
/// </summary>
InComplete = 11,
EnterComplete = 11,
/// <summary>
/// 투입(진행중)
/// </summary>
InIng = 12,
EnterIng = 12,
/// <summary>
/// 진출완료
/// </summary>
OutComplete = 21,
ExitComplete = 21,
/// <summary>
/// 진출중
/// </summary>
OutIng = 22,
ExitIng = 22,
}
public class Xbee : SerialPort, arDev.ISerialComm
@@ -196,10 +196,7 @@ namespace Project.Device
Send(packet);
}
public eDocStep StepBuffer { get; set; } = eDocStep.NotSet;
public eDocStep StepUnloader { get; set; } = eDocStep.NotSet;
public eDocStep StepLoader { get; set; } = eDocStep.NotSet;
public eDocStep StepCleaner { get; set; } = eDocStep.NotSet;
public eDocStep StepMC { get; set; } = eDocStep.NotSet;
ManualResetEvent sendlock = new ManualResetEvent(true);
@@ -225,67 +222,80 @@ namespace Project.Device
*/
try
{
byte[] data = new byte[12]; // 총 12바이트 데이터
List<byte> data = new List<byte>();
byte value = 0;
// Autoron Mode
// Mode
data[0] = (byte)(VAR.BOOL[eVarBool.FLAG_AUTORUN] ? 1 : 0);
value = (byte)(VAR.BOOL[eVarBool.FLAG_AUTORUN] ? 1 : 0);
data.Add(value);
// RunSt
if (PUB.AGV.error.Emergency)
data[1] = 2; // error
value = 2; // error
else if (PUB.AGV.system1.agv_run)
data[1] = 1; // run
value = 1; // run
else
data[1] = 0; // stop
value = 0; // stop
data.Add(value);
//runstep
value = (byte)PUB.sm.RunStep;
data.Add(value);
//runstepseq
value = (byte)StepMC;
data.Add(value);
// Motor Direction
if (PUB.AGV.data.Direction == 'F')
data[2] = 0;
value = 0;
else if (PUB.AGV.data.Direction == 'B')
data[2] = 1;
value = 1;
else
data[2] = 0xff; //unknown
value = 0xff; //unknown
data.Add(value);
// Magnet Direction
if (PUB.AGV.data.Sts == 'L')
data[3] = 1; // left
value = 1; // left
else if (PUB.AGV.data.Sts == 'R')
data[3] = 2; // right
value = 2; // right
else if (PUB.AGV.data.Sts == 'S')
data[3] = 0; // straight
value = 0; // straight
else
data[3] = 0xFF; //unknown
// Inposition
data[4] = (byte)(PUB.AGV.system1.agv_stop ? 1 : 0);
value = 0xFF; //unknown
data.Add(value);
// ChargeSt
data[5] = (byte)((VAR.BOOL[eVarBool.FLAG_CHARGEONA] || VAR.BOOL[eVarBool.FLAG_CHARGEONM]) ? 1 : 0);
value = (byte)((VAR.BOOL[eVarBool.FLAG_CHARGEONA] || VAR.BOOL[eVarBool.FLAG_CHARGEONM]) ? 1 : 0);
data.Add(value);
// CartSt
if (PUB.AGV.signal2.cart_detect1 && PUB.AGV.signal2.cart_detect2)
data[6] = 1; // 센서두개가 모두 감지되는 경우
value = 1; // 센서두개가 모두 감지되는 경우
else if (PUB.AGV.signal2.cart_detect1 == false && PUB.AGV.signal2.cart_detect2 == false)
data[6] = 0; // 센서두개가 모두 감지되지 않는 경우
value = 0; // 센서두개가 모두 감지되지 않는 경우
else
data[6] = 2; // 센서하나만 감지되는 경우
value = 2; // 센서하나만 감지되는 경우
data.Add(value);
// LiftSt
if (PUB.AGV.signal1.lift_up)
data[7] = 1; // 위로 올라가는 경우
value = 1; // 위로 올라가는 경우
else if (PUB.AGV.signal1.lift_down)
data[7] = 0; // 아래로 내려가는 경우
value = 0; // 아래로 내려가는 경우
else
data[7] = 2; // unknown (기본값)
value = 2; // unknown (기본값)
data.Add(value);
// LastTag
string lastTag = PUB.AGV.data.TagNo.ToString("0000") ?? "0000";
byte[] tagBytes = Encoding.ASCII.GetBytes(lastTag.PadRight(4, '0'));
Array.Copy(tagBytes, 0, data, 8, lastTag.Length);
data.AddRange(tagBytes);
// 데이터 전송
var cmd = (byte)ENIGProtocol.AGVCommandEH.Status;
var packet = proto.CreatePacket(PUB.setting.XBE_ID, cmd, data);
var packet = proto.CreatePacket(PUB.setting.XBE_ID, cmd, data.ToArray());
if (Send(packet))
PUB.logxbee.AddI($"Send status [O] : {packet.Length} {packet.HexString()}");
else

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

@@ -115,10 +115,7 @@ namespace Project
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;
PUB.XBE.StepMC = Device.eDocStep.NotSet;
switch (target.StationType)
{
@@ -127,12 +124,12 @@ namespace Project
if (lastPath.NodeId.Equals(PUB._virtualAGV.CurrentNode.Id))
{
//버퍼진입전 노드에 도착완료했따
PUB.XBE.StepBuffer = Device.eDocStep.InReady;
PUB.XBE.StepMC = Device.eDocStep.ReadyForEnter;
}
else
{
//마지막위치가 아닌 다른 위치에 있으니 버퍼 작업을 할 수없다
PUB.XBE.StepBuffer = Device.eDocStep.NotSet;
PUB.XBE.StepMC = Device.eDocStep.NotSet;
PUB.log.AddE($"목적지가 버퍼이나 노드가 불일치 한다 오류사항");
PUB._mapCanvas.SetAlertMessage("목적지가 버퍼이나 노드 불일치 오류");
PUB.sm.SetNewRunStep(ERunStep.ERROR);
@@ -144,15 +141,15 @@ namespace Project
break;
case AGVNavigationCore.Models.StationType.Loader:
PUB.XBE.StepLoader = Device.eDocStep.InReady;
PUB.XBE.StepMC = Device.eDocStep.ReadyForEnter;
break;
case AGVNavigationCore.Models.StationType.Clearner:
PUB.XBE.StepCleaner = Device.eDocStep.InReady;
PUB.XBE.StepMC = Device.eDocStep.ReadyForEnter;
break;
case AGVNavigationCore.Models.StationType.UnLoader:
PUB.XBE.StepUnloader = Device.eDocStep.InReady;
PUB.XBE.StepMC = Device.eDocStep.ReadyForEnter;
break;
default:
@@ -230,69 +227,31 @@ namespace Project
}
}
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.CLEANER_IN: //클리너도킹
case ERunStep.UNLOADER_IN: //언로더도킹
case ERunStep.LOADER_IN: //로더도킹
if (_SM_RUN_LOADER_IN(runStepisFirst, PUB.sm.GetRunSteptime))
{
PUB.Speak(Lang.버퍼도킹이완료되었습니다);
//도킹완료상태를 업데이트한다.
PUB.XBE.StepLoader = Device.eDocStep.InComplete;
PUB.XBE.StepMC = Device.eDocStep.EnterComplete;
//대기상태로 전환
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))
case ERunStep.UNLOADER_OUT: //언로더아웃
case ERunStep.LOADER_OUT: //로더아웃
if (_SM_RUN_LOADER_OUT(runStepisFirst, PUB.sm.GetRunSteptime))
{
PUB.Speak(Lang.버퍼도킹해제완료);
//도킹완료상태를 업데이트한다.
PUB.XBE.StepCleaner = Device.eDocStep.OutComplete;
PUB.XBE.StepMC = Device.eDocStep.ExitComplete;
//대기상태로 전환
PUB.sm.SetNewRunStep(ERunStep.READY);
@@ -300,33 +259,19 @@ namespace Project
}
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.XBE.StepMC = Device.eDocStep.ExitComplete;
//대기상태로 전환
PUB.sm.SetNewRunStep(ERunStep.READY);
return;
}
else PUB.XBE.StepBuffer = Device.eDocStep.OutIng;
else PUB.XBE.StepMC = Device.eDocStep.ExitIng;
break;
case ERunStep.BUFFER_IN: //버퍼도킹
@@ -335,13 +280,13 @@ namespace Project
PUB.Speak(Lang.버퍼도킹이완료되었습니다);
//도킹완료상태를 업데이트한다.
PUB.XBE.StepBuffer = Device.eDocStep.InComplete;
PUB.XBE.StepMC = Device.eDocStep.EnterComplete;
//대기상태로 전환
PUB.sm.SetNewRunStep(ERunStep.READY);
return;
}
else PUB.XBE.StepBuffer = Device.eDocStep.InIng;
else PUB.XBE.StepMC = Device.eDocStep.EnterIng;
break;
}

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

@@ -29,7 +29,6 @@ namespace Project
* 3. 후진-저속-마크다운 실행
*/
if (PUB.sm.RunStepSeq == idx++)
{
PUB.log.Add($"[{funcname}] 버퍼진입시작({PUB.NextWorkCmd}) Turn:{PUB._virtualAGV.Turn}");
@@ -90,7 +89,7 @@ namespace Project
}
return false;
}
PUB._virtualAGV.Turn = AGVNavigationCore.Models.AGVTurn.L90; //턴완료
PUB.log.Add($"[{funcname}] Turn(left) 완료");
PUB.sm.UpdateRunStepSeq(); //이미완료된상태이므로 다음으로 진행한다.
return false;
@@ -139,14 +138,6 @@ namespace Project
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
//마크스탑셋팅
PUB.AGV.AGVMoveStop(funcname, arDev.Narumi.eStopOpt.MarkStop);
VAR.TIME.Update(eVarTime.LastTurnCommandTime);
PUB.sm.UpdateRunStepSeq();
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
//저속이동 (후진 진입)
var ret = PUB.AGV.AGVMoveSet(new arDev.Narumi.BunkiData
@@ -172,11 +163,13 @@ namespace Project
//후진이동을한다
PUB.AGV.AGVMoveRun(arDev.Narumi.eRunOpt.Backward);
PUB.sm.UpdateRunStepSeq();
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
//마크스탑으로 이동
PUB.AGV.AGVMoveStop(funcname, arDev.Narumi.eStopOpt.MarkStop);
VAR.TIME.Update(eVarTime.LastTurnCommandTime);
PUB.sm.UpdateRunStepSeq();
return false;
}
@@ -185,8 +178,7 @@ namespace Project
//마크스탑신호가 3초이내로 들어와야 한다
if (VAR.BOOL[eVarBool.NEXTSTOP_MARK] == false)
{
var ts = VAR.TIME.RUN(eVarTime.LastTurnCommandTime);
if (ts.TotalSeconds > 3)
if (seqtime.TotalSeconds > 3)
{
PUB.AGV.AGVMoveStop(funcname);
PUB.log.AddE($"[{funcname}] MARK STOP신호가 확인되지 않습니다");

8
Cs_HMI/Project/StateMachine/Step/_SM_RUN_BUFFER_OUT.cs
View File

@@ -39,7 +39,7 @@ namespace Project
}
else if (PUB.sm.RunStepSeq == idx++)
{
arDev.Narumi.LiftCommand lift = PUB.NextWorkCmd == ENIGProtocol.AGVCommandHE.PickOn ? arDev.Narumi.LiftCommand.UP : arDev.Narumi.LiftCommand.DN;
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();
@@ -48,13 +48,13 @@ namespace Project
else if (PUB.sm.RunStepSeq == idx++)
{
//리프트 센서 확인
var checksensor = PUB.NextWorkCmd == ENIGProtocol.AGVCommandHE.PickOn ? PUB.AGV.signal1.lift_up : PUB.AGV.signal1.lift_down;
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.PickOn ? arDev.Narumi.LiftCommand.UP : arDev.Narumi.LiftCommand.DN;
arDev.Narumi.LiftCommand lift = PUB.NextWorkCmd == ENIGProtocol.AGVCommandHE.PickOnExit ? arDev.Narumi.LiftCommand.UP : arDev.Narumi.LiftCommand.DN;
PUB.AGV.LiftControl(lift);
}
else return false;
@@ -65,7 +65,7 @@ namespace Project
else if (PUB.sm.RunStepSeq == idx++)
{
//리프트 센서 확인
var checksensor = PUB.NextWorkCmd == ENIGProtocol.AGVCommandHE.PickOn ? PUB.AGV.signal1.lift_up : PUB.AGV.signal1.lift_down;
var checksensor = PUB.NextWorkCmd == ENIGProtocol.AGVCommandHE.PickOnExit ? PUB.AGV.signal1.lift_up : PUB.AGV.signal1.lift_down;
if (checksensor == false)
{
if (seqtime.TotalSeconds > 20)

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

@@ -1,181 +0,0 @@
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_CLEANER_IN(bool isFirst, TimeSpan stepTime)
{
var funcname = "_SM_RUN_CLEANER_IN";
var idx = 1;
//충전 상태가 OFF되어야 동작하게한다
if (_SM_RUN_CHGOFF(isFirst, stepTime) == false) return false;
//라이더멈춤이 설정되어있다면 음성으로 알려준다
if (CheckLiderStop() == false) return false;
/*
* 클리너 IN 시퀀스 (버퍼 복사 - 턴 제거)
* 1. LIFT DOWN
* 2. 후진-저속-마크다운 실행
*/
if (PUB.sm.RunStepSeq == idx++)
{
PUB.log.Add("클리너진입시작");
PUB.Speak("클리너 작업을 시작합니다");
PUB.sm.UpdateRunStepSeq();
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
// [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 ts = VAR.TIME.RUN(eVarTime.LastTurnCommandTime);
if (ts.TotalSeconds > 10)
{
// Timebound check
}
PUB.log.Add("리프트 동작 확인 완료");
PUB.sm.UpdateRunStepSeq();
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
//마크스탑셋팅
PUB.AGV.AGVMoveStop(funcname, arDev.Narumi.eStopOpt.MarkStop);
VAR.TIME.Update(eVarTime.LastTurnCommandTime);
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)
{
PUB.AGV.AGVMoveStop(funcname);
PUB.log.AddE($"[{funcname}] AGV속도설정이 완료되지 않았습니다");
PUB._mapCanvas.SetAlertMessage("agv 속도 설정 오류");
PUB.sm.SetNewRunStep(ERunStep.ERROR);
}
return false;
}
PUB.AGV.AGVMoveRun(arDev.Narumi.eRunOpt.Backward);
PUB.AGV.AGVMoveStop(funcname, arDev.Narumi.eStopOpt.MarkStop);
VAR.TIME.Update(eVarTime.LastTurnCommandTime);
PUB.sm.UpdateRunStepSeq();
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
//마크스탑신호가 3초이내로 들어와야 한다
if (VAR.BOOL[eVarBool.NEXTSTOP_MARK] == false)
{
var ts = VAR.TIME.RUN(eVarTime.LastTurnCommandTime);
if (ts.TotalSeconds > 3)
{
PUB.AGV.AGVMoveStop(funcname);
PUB.log.AddE("MARK STOP신호가 확인되지 않습니다");
PUB._mapCanvas.SetAlertMessage("mark stop 신호 확인 안됨");
PUB.sm.SetNewRunStep(ERunStep.ERROR);
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)
{
PUB.AGV.AGVMoveStop(funcname);
PUB.log.AddE("AGV가 멈추지 않아 강제종료 합니다");
PUB._mapCanvas.SetAlertMessage("agv 가 멈추지 않아 강제 종료");
PUB.sm.SetNewRunStep(ERunStep.ERROR);
return false;
}
return false;
}
VAR.TIME.Update(eVarTime.LastTurnCommandTime);
PUB.sm.UpdateRunStepSeq();
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.sm.UpdateRunStepSeq();
return false;
}
PUB.AddEEDB($"클리너작업완료({PUB.Result.TargetPos})");
return true;
}
}
}

92
Cs_HMI/Project/StateMachine/Step/_SM_RUN_CLEANER_OUT.cs
View File

@@ -1,92 +0,0 @@
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_CLEANER_OUT(bool isFirst, TimeSpan stepTime)
{
var funcname = "CLEANEROUT";
//충전 상태가 OFF되어야 동작하게한다
if (_SM_RUN_CHGOFF(isFirst, stepTime) == false) return false;
//라이더멈춤이 설정되어있다면 음성으로 알려준다
if (CheckLiderStop() == false) return false;
var idx = 1;
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)
{
PUB.AGV.AGVMoveStop(funcname);
PUB.log.AddE($"[{funcname}] AGV속도설정이 완료되지 않았습니다");
PUB._mapCanvas.SetAlertMessage("agv 속도 설정 실패");
PUB.sm.SetNewRunStep(ERunStep.ERROR);
}
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++)
{
//마크스탑
PUB.AGV.AGVMoveStop("cleaner out", arDev.Narumi.eStopOpt.MarkStop);
PUB.sm.UpdateRunStepSeq();
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
//이동확인
if (PUB.AGV.system1.agv_run == false)
{
return false;
}
PUB.sm.UpdateRunStepSeq();
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
//멈춤확인
if (PUB.AGV.system1.agv_run == true)
{
return false;
}
PUB.sm.UpdateRunStepSeq();
return false;
}
return true;
}
}
}

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

@@ -14,40 +14,82 @@ namespace Project
/// <summary>
/// 로더 진입
/// </summary>
public Boolean _SM_RUN_LOADER_IN(bool isFirst, TimeSpan stepTime)
public Boolean _SM_RUN_LOADER_IN(bool isFirst, TimeSpan seqTime)
{
var funcname = "_SM_RUN_LOADER_IN";
var idx = 1;
var funcname = PUB.sm.RunStep.ToString();
//충전 상태가 OFF되어야 동작하게한다
if (_SM_RUN_CHGOFF(isFirst, stepTime) == false) return false;
if (_SM_RUN_CHGOFF(isFirst, seqTime) == false) return false;
//라이더멈춤이 설정되어있다면 음성으로 알려준다
if (CheckLiderStop() == false) return false;
/*
* 로더 IN 시퀀스 (버퍼 복사 - 턴 제거)
* 1. LIFT DOWN
* 2. 후진-저속-마크다운 실행
*/
if (PUB.sm.RunStepSeq == idx++)
{
PUB.log.Add("로더진입시작");
PUB.Speak("로더 작업을 시작합니다"); // Lang resource might not exist, using string
PUB.log.Add("IN작업-시작");
PUB.Speak("작업을 시작합니다");
PUB.AGV.AGVMoveStop(funcname);
PUB.sm.UpdateRunStepSeq();
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
// [PickOn/PickOff] 초기 리프트 동작
var liftCmd = arDev.Narumi.LiftCommand.DN; // Default PickOn (Get -> Go Under -> Down)
if (PUB.NextWorkCmd == ENIGProtocol.AGVCommandHE.PickOff)
//마크센서가 감지된상태여야 완전한위치로 가정한다
if(PUB.AGV.signal1.mark_sensor == false)
{
liftCmd = arDev.Narumi.LiftCommand.UP; // PickOff (Put -> Carry In -> Up)
var errmsg = $"[{funcname}] 마크센서가 감지되지 않습니다";
PUB.AGV.AGVMoveStop(errmsg);
PUB.log.AddE(errmsg);
PUB._mapCanvas.SetAlertMessage(errmsg);
PUB.sm.SetNewRunStep(ERunStep.ERROR);
}
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)
{
var errmsg = $"[{funcname}] 카트가 존재하여 진입을 할 수 없습니다";
PUB.AGV.AGVMoveStop(errmsg);
PUB.log.AddE(errmsg);
PUB._mapCanvas.SetAlertMessage(errmsg);
PUB.sm.SetNewRunStep(ERunStep.ERROR);
}
}
else if (PUB.NextWorkCmd == ENIGProtocol.AGVCommandHE.PickOffEnter)
{
//가져다 놓아야하므로 카트가 존재해야 한다
if (PUB.AGV.signal2.cart_detect1 == false || PUB.AGV.signal2.cart_detect2 == false)
{
var errmsg = $"[{funcname}] 카트감지센서가 인식되지 않았습니다";
PUB.AGV.AGVMoveStop(errmsg);
PUB.log.AddE(errmsg);
PUB._mapCanvas.SetAlertMessage(errmsg);
PUB.sm.SetNewRunStep(ERunStep.ERROR);
}
}
else
{
var errmsg = $"[{funcname}] 알수없는 작업형태입니다({PUB.NextWorkCmd})";
PUB.AGV.AGVMoveStop(funcname);
PUB.log.AddE(errmsg);
PUB._mapCanvas.SetAlertMessage(errmsg);
PUB.sm.SetNewRunStep(ERunStep.ERROR);
}
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();
@@ -55,28 +97,24 @@ namespace Project
}
else if (PUB.sm.RunStepSeq == idx++)
{
//리프트 센서 확인 (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;
}
else if (PUB.sm.RunStepSeq == idx++)
{
//마크스탑셋팅
PUB.AGV.AGVMoveStop(funcname, arDev.Narumi.eStopOpt.MarkStop);
VAR.TIME.Update(eVarTime.LastTurnCommandTime);
PUB.sm.UpdateRunStepSeq();
//리프트 센서 확인
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}] 리프트다운이 확인되지 않습니다";
PUB.AGV.AGVMoveStop(funcname);
PUB.log.AddE(errmsg);
PUB._mapCanvas.SetAlertMessage(errmsg);
PUB.sm.SetNewRunStep(ERunStep.ERROR);
}
}
else
{
PUB.log.Add("리프트 동작 확인 완료");
PUB.sm.UpdateRunStepSeq();
}
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
@@ -89,6 +127,7 @@ namespace Project
PBSSensor = 0,
Speed = arDev.Narumi.eMoveSpd.Low,
});
//명령이 실패되었다면 재시도를 한다
if (ret != arDev.eNarumiCommandResult.Success)
{
if (ret >= arDev.eNarumiCommandResult.Error)
@@ -100,28 +139,52 @@ namespace Project
}
return false;
}
PUB.AGV.AGVMoveRun(arDev.Narumi.eRunOpt.Backward);
PUB.AGV.AGVMoveStop(funcname, arDev.Narumi.eStopOpt.MarkStop);
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)
{
//구동이확인되지 않으면 오류처리를 한다.
PUB.AGV.AGVMoveStop(funcname);
PUB.log.AddE("AGV속도설정이 완료되지 않았습니다");
PUB._mapCanvas.SetAlertMessage("agv 속도 설정 실패");
PUB.sm.SetNewRunStep(ERunStep.ERROR);
}
return false;
}
//마크스탑설정
PUB.AGV.AGVMoveStop(funcname, arDev.Narumi.eStopOpt.MarkStop);
PUB.sm.UpdateRunStepSeq();
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
//마크스탑신호가 3초이내로 들어와야 한다
if (VAR.BOOL[eVarBool.NEXTSTOP_MARK] == false)
{
var ts = VAR.TIME.RUN(eVarTime.LastTurnCommandTime);
if (ts.TotalSeconds > 3)
if (seqTime.TotalMilliseconds > 3000)
{
PUB.AGV.AGVMoveStop(funcname);
PUB.log.AddE("MARK STOP신호가 확인되지 않습니다");
PUB._mapCanvas.SetAlertMessage("mark stop 신호 확인 불가");
PUB.sm.SetNewRunStep(ERunStep.ERROR);
return false;
}
return false;
}
VAR.TIME.Update(eVarTime.LastTurnCommandTime);
PUB.sm.UpdateRunStepSeq();
return false;
}
@@ -130,57 +193,44 @@ namespace Project
//AGV가 멈출때까지 기다린다.
if (PUB.AGV.system1.agv_run == true)
{
var ts = VAR.TIME.RUN(eVarTime.LastTurnCommandTime);
if (ts.TotalSeconds > 10)
if (seqTime.TotalSeconds > 10)
{
PUB.AGV.AGVMoveStop(funcname);
PUB.log.AddE("AGV가 멈추지 않아 강제종료 합니다");
PUB._mapCanvas.SetAlertMessage("agv 가 멈추지 않아 강제 종료");
PUB.sm.SetNewRunStep(ERunStep.ERROR);
return false;
}
return false;
}
VAR.TIME.Update(eVarTime.LastTurnCommandTime);
PUB.sm.UpdateRunStepSeq();
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;
//마크센서입력을 확인한다.
if (PUB.AGV.signal1.mark_sensor == false)
{
if (seqTime.TotalSeconds > 5)
{
PUB.AGV.AGVMoveStop(funcname);
PUB.log.AddE("마크센서가 감지되지 않았습니다");
PUB._mapCanvas.SetAlertMessage("마크센서가 감지되지 않았습니다");
PUB.sm.SetNewRunStep(ERunStep.ERROR);
}
return false;
}
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})");
return true;
}
}

135
Cs_HMI/Project/StateMachine/Step/_SM_RUN_LOADER_OUT.cs
View File

@@ -14,17 +14,71 @@ namespace Project
/// <summary>
/// 로더 배출
/// </summary>
public Boolean _SM_RUN_LOADER_OUT(bool isFirst, TimeSpan stepTime)
public Boolean _SM_RUN_LOADER_OUT(bool isFirst, TimeSpan seqTime)
{
var funcname = "LOADEROUT";
var idx = 1;
var funcname = PUB.sm.RunStep.ToString();
//충전 상태가 OFF되어야 동작하게한다
if (_SM_RUN_CHGOFF(isFirst, stepTime) == false) return false;
if (_SM_RUN_CHGOFF(isFirst, seqTime) == false) return false;
//라이더멈춤이 설정되어있다면 음성으로 알려준다
if (CheckLiderStop() == false) return false;
var idx = 1;
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)
{
var errmsg = $"[{funcname}] 리프트({liftCmd})이 확인되지 않습니다";
PUB.AGV.AGVMoveStop(funcname);
PUB.log.AddE(errmsg);
PUB._mapCanvas.SetAlertMessage(errmsg);
PUB.sm.SetNewRunStep(ERunStep.ERROR);
}
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
@@ -47,7 +101,6 @@ namespace Project
return false;
}
PUB.sm.UpdateRunStepSeq();
return false;
}
@@ -60,16 +113,36 @@ namespace Project
}
else if (PUB.sm.RunStepSeq == idx++)
{
//마크스탑
PUB.AGV.AGVMoveStop("loader out", arDev.Narumi.eStopOpt.MarkStop);
PUB.sm.UpdateRunStepSeq();
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
//이동확인
//AGV구동을 확인하고 마크스탑을 설정한다.
if (PUB.AGV.system1.agv_run == false)
{
if (seqTime.TotalMilliseconds > 1000)
{
//구동이확인되지 않으면 오류처리를 한다.
PUB.AGV.AGVMoveStop(funcname);
PUB.log.AddE("AGV속도설정이 완료되지 않았습니다");
PUB._mapCanvas.SetAlertMessage("agv 속도 설정 실패");
PUB.sm.SetNewRunStep(ERunStep.ERROR);
}
return false;
}
//마크스탑설정
PUB.AGV.AGVMoveStop(funcname, arDev.Narumi.eStopOpt.MarkStop);
PUB.sm.UpdateRunStepSeq();
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
//마크스탑신호가 3초이내로 들어와야 한다
if (VAR.BOOL[eVarBool.NEXTSTOP_MARK] == false)
{
if (seqTime.TotalMilliseconds > 3000)
{
PUB.AGV.AGVMoveStop(funcname);
PUB.log.AddE("MARK STOP신호가 확인되지 않습니다");
PUB._mapCanvas.SetAlertMessage("mark stop 신호 확인 불가");
PUB.sm.SetNewRunStep(ERunStep.ERROR);
}
return false;
}
PUB.sm.UpdateRunStepSeq();
@@ -77,16 +150,46 @@ namespace Project
}
else if (PUB.sm.RunStepSeq == idx++)
{
//멈춤확인
//AGV가 멈출때까지 기다린다.
if (PUB.AGV.system1.agv_run == true)
{
if (seqTime.TotalSeconds > 10)
{
PUB.AGV.AGVMoveStop(funcname);
PUB.log.AddE("AGV가 멈추지 않아 강제종료 합니다");
PUB._mapCanvas.SetAlertMessage("agv 가 멈추지 않아 강제 종료");
PUB.sm.SetNewRunStep(ERunStep.ERROR);
}
return false;
}
PUB.sm.UpdateRunStepSeq();
return false;
}
// 턴 제거됨
else if (PUB.sm.RunStepSeq == idx++)
{
//마크센서입력을 확인한다.
if (PUB.AGV.signal1.mark_sensor == false)
{
if (seqTime.TotalSeconds > 5)
{
PUB.AGV.AGVMoveStop(funcname);
PUB.log.AddE("마크센서가 감지되지 않았습니다");
PUB._mapCanvas.SetAlertMessage("마크센서가 감지되지 않았습니다");
PUB.sm.SetNewRunStep(ERunStep.ERROR);
}
return false;
}
PUB.sm.UpdateRunStepSeq();
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
//완료되었다.
PUB.log.Add("진출 완료");
PUB.sm.UpdateRunStepSeq();
return false;
}
return true;
}
}

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

@@ -1,183 +0,0 @@
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_UNLOADER_IN(bool isFirst, TimeSpan stepTime)
{
var funcname = "_SM_RUN_UNLOADER_IN";
var idx = 1;
//충전 상태가 OFF되어야 동작하게한다
if (_SM_RUN_CHGOFF(isFirst, stepTime) == false) return false;
//라이더멈춤이 설정되어있다면 음성으로 알려준다
if (CheckLiderStop() == false) return false;
/*
* 언로더 IN 시퀀스 (버퍼 복사 - 턴 제거)
* 1. LIFT DOWN
* 2. 후진-저속-마크다운 실행
*/
if (PUB.sm.RunStepSeq == idx++)
{
PUB.log.Add("언로더진입시작");
PUB.Speak("언로더 작업을 시작합니다");
PUB.sm.UpdateRunStepSeq();
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
// [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 ts = VAR.TIME.RUN(eVarTime.LastTurnCommandTime);
if (ts.TotalSeconds > 10)
{
// Timebound check
}
PUB.log.Add("리프트 동작 확인 완료");
PUB.sm.UpdateRunStepSeq();
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
//마크스탑셋팅
PUB.AGV.AGVMoveStop(funcname, arDev.Narumi.eStopOpt.MarkStop);
VAR.TIME.Update(eVarTime.LastTurnCommandTime);
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)
{
PUB.AGV.AGVMoveStop(funcname);
PUB.log.AddE($"[{funcname}] AGV속도설정이 완료되지 않았습니다");
PUB._mapCanvas.SetAlertMessage("agv속도설정오류");
PUB.sm.SetNewRunStep(ERunStep.ERROR);
}
return false;
}
PUB.AGV.AGVMoveRun(arDev.Narumi.eRunOpt.Backward);
PUB.AGV.AGVMoveStop(funcname, arDev.Narumi.eStopOpt.MarkStop);
VAR.TIME.Update(eVarTime.LastTurnCommandTime);
PUB.sm.UpdateRunStepSeq();
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
//마크스탑신호가 3초이내로 들어와야 한다
if (VAR.BOOL[eVarBool.NEXTSTOP_MARK] == false)
{
var ts = VAR.TIME.RUN(eVarTime.LastTurnCommandTime);
if (ts.TotalSeconds > 3)
{
PUB.AGV.AGVMoveStop(funcname);
PUB.log.AddE("MARK STOP신호가 확인되지 않습니다");
PUB._mapCanvas.SetAlertMessage("mark stop 신호 확인 불가");
PUB.sm.SetNewRunStep(ERunStep.ERROR);
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)
{
PUB.AGV.AGVMoveStop(funcname);
PUB.log.AddE("AGV가 멈추지 않아 강제종료 합니다");
PUB._mapCanvas.SetAlertMessage("agv 가 멈추지 않아 강제 종료 합니다");
PUB.sm.SetNewRunStep(ERunStep.ERROR);
return false;
}
return false;
}
VAR.TIME.Update(eVarTime.LastTurnCommandTime);
PUB.sm.UpdateRunStepSeq();
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.sm.UpdateRunStepSeq();
return false;
}
PUB.AddEEDB($"언로더작업완료({PUB.Result.TargetPos})");
return true;
}
}
}

92
Cs_HMI/Project/StateMachine/Step/_SM_RUN_UNLOADER_OUT.cs
View File

@@ -1,92 +0,0 @@
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_UNLOADER_OUT(bool isFirst, TimeSpan stepTime)
{
var funcname = "UNLOADEROUT";
//충전 상태가 OFF되어야 동작하게한다
if (_SM_RUN_CHGOFF(isFirst, stepTime) == false) return false;
//라이더멈춤이 설정되어있다면 음성으로 알려준다
if (CheckLiderStop() == false) return false;
var idx = 1;
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)
{
PUB.AGV.AGVMoveStop(funcname);
PUB.log.AddE($"[{funcname}] AGV속도설정이 완료되지 않았습니다");
PUB._mapCanvas.SetAlertMessage($"[{funcname}] AGV속도설정이 완료되지 않았습니다");
PUB.sm.SetNewRunStep(ERunStep.ERROR);
}
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++)
{
//마크스탑
PUB.AGV.AGVMoveStop("unloader out", arDev.Narumi.eStopOpt.MarkStop);
PUB.sm.UpdateRunStepSeq();
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
//이동확인
if (PUB.AGV.system1.agv_run == false)
{
return false;
}
PUB.sm.UpdateRunStepSeq();
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
//멈춤확인
if (PUB.AGV.system1.agv_run == true)
{
return false;
}
PUB.sm.UpdateRunStepSeq();
return false;
}
return true;
}
}
}

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

@@ -110,30 +110,17 @@ namespace Project
PUB.AGV.AGVMoveStop("경로재생성");
}
var PathResult = CalcPath(PUB._virtualAGV.StartNode, PUB._virtualAGV.TargetNode);
if(PathResult.result !=null &&
PathResult.result.Success == true &&
PathResult.result.DetailedPath.Where(t => t.NodeId == currentNode.Id).Any() == false)
{
PUB._virtualAGV.StartNode = PUB._virtualAGV.CurrentNode;
PathResult = CalcPath(PUB._virtualAGV.StartNode, PUB._virtualAGV.TargetNode);
}
if (PathResult.result == null)
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;
}
else
{
PUB._mapCanvas.CurrentPath = PathResult.result;
PUB._virtualAGV.SetPath(PathResult.result);
}
PUB._mapCanvas.CurrentPath = PathResult;
PUB._virtualAGV.SetPath(PathResult);
PUB.log.AddI($"경로생성 {PUB._virtualAGV.StartNode.RfidId} -> {PUB._virtualAGV.TargetNode.RfidId}");
}
@@ -161,23 +148,22 @@ namespace Project
if (PUB._virtualAGV.CurrentPath.DetailedPath.Count > 5)
{
var PathResult2 = CalcPath(PUB._virtualAGV.CurrentNode, PUB._virtualAGV.TargetNode);
if (PathResult2.result != null && PathResult2.result.Success)
if (PathResult2 != null && PathResult2.Success)
{
//절반이상 경로가 짧을때에는 재계산을 하게한다
var halfcnt = (int)(PUB._virtualAGV.CurrentPath.DetailedPath.Count / 2.0);
if (PathResult2.result.DetailedPath.Count < halfcnt)
if (PathResult2.DetailedPath.Count < halfcnt)
{
var msg = $"단축경로가 확인되었습니다. 경로를 삭제 합니다";
PUB.log.AddE(msg);
Console.WriteLine(msg);
PUB._virtualAGV.SetPath(null);
return false;
}
return false;
}
}
//predict 를 이용하여 다음 이동을 모두 확인한다.
var nextAction = PUB._virtualAGV.Predict();
if (nextAction.Reason == AGVNavigationCore.Models.eAGVCommandReason.PathOut)

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

@@ -4,6 +4,8 @@ using System.Drawing;
using System.Linq;
using System.Text;
using AGVNavigationCore.Models;
using AGVNavigationCore.PathFinding.Core;
using AGVNavigationCore.PathFinding.Planning;
using AGVNavigationCore.Utils;
using AR;
using arDev;
@@ -70,8 +72,8 @@ namespace Project
else PUB.log.AddE($"[{logPrefix}-SetCurrent] TagString Lenght Errorr:{data.Length}");
break;
case ENIGProtocol.AGVCommandHE.PickOn: // 110
case ENIGProtocol.AGVCommandHE.PickOff: // 111
case ENIGProtocol.AGVCommandHE.PickOnEnter: // 110
case ENIGProtocol.AGVCommandHE.PickOffEnter: // 111
{
PUB.log.AddI($"XBEE:작업명령수신:{cmd}");
@@ -109,6 +111,7 @@ namespace Project
return;
}
//다음명령처리
PUB.NextWorkCmd = cmd;
PUB.log.AddI($"작업 시작: {nextStep} (Type: {cmd})");
PUB.sm.SetNewRunStep(nextStep);
@@ -307,18 +310,12 @@ namespace Project
else PUB.log.Add(e.Message);
}
AGVNavigationCore.PathFinding.Planning.AGVPathfinder _advancedPathfinder = null;
(AGVNavigationCore.PathFinding.Core.AGVPathResult result, string message) CalcPath(MapNode startNode, MapNode targetNode)
AGVPathResult CalcPath(MapNode startNode, MapNode targetNode)
{
var nodes = PUB._mapCanvas.Nodes;
var _simulatorCanvas = PUB._mapCanvas;
var _mapNodes = PUB._mapCanvas.Nodes;
// 시작 RFID가 없으면 AGV 현재 위치로 설정
if (startNode == null || targetNode == null)
return (null, "시작 RFID와 목표 RFID를 선택해주세요.");
//경로계산기확인
if (_advancedPathfinder == null)
_advancedPathfinder = new AGVNavigationCore.PathFinding.Planning.AGVPathfinder(_mapNodes);
// 현재 AGV 방향 가져오기
var selectedAGV = PUB._virtualAGV;
var currentDirection = selectedAGV.CurrentDirection;
@@ -327,55 +324,15 @@ namespace Project
var prevNode = selectedAGV.PrevNode;
var prevDir = selectedAGV.PrevDirection;
// 고급 경로 계획 사용 (노드 객체 직접 전달)
var advancedResult = _advancedPathfinder.FindBasicPath(startNode, targetNode, prevNode, prevDir);
// Core Logic으로 이관됨
var pathFinder = new AGVPathfinder(nodes);
var result = pathFinder.CalculatePath(startNode, targetNode, prevNode, prevDir);
var _simulatorCanvas = PUB._mapCanvas;
_simulatorCanvas.FitToNodes();
string Message = string.Empty;
if (advancedResult != null && advancedResult.Success)
{
// 도킹 검증이 없는 경우 추가 검증 수행
if (advancedResult.DockingValidation == null || !advancedResult.DockingValidation.IsValidationRequired)
advancedResult.DockingValidation = DockingValidator.ValidateDockingDirection(advancedResult, _mapNodes);
//마지막대상이 버퍼라면 시퀀스처리를 해야한다
if (targetNode.StationType == StationType.Buffer && advancedResult.DetailedPath.Any())
{
var lastDetailPath = advancedResult.DetailedPath.Last();
if (lastDetailPath.NodeId == targetNode.Id) //마지막노드 재확인
{
//버퍼에 도킹할때에는 마지막 노드에서 멈추고 시퀀스를 적용해야한다
advancedResult.DetailedPath = advancedResult.DetailedPath.Take(advancedResult.DetailedPath.Count - 1).ToList();
Console.WriteLine("최종위치가 버퍼이므로 마지막 RFID에서 멈추도록 합니다");
}
}
//게이트웨이노드를 하이라이트강조 한단
_simulatorCanvas.HighlightNodeId = (result.Gateway?.Id ?? string.Empty);
return result;
_simulatorCanvas.CurrentPath = advancedResult;
//_pathLengthLabel.Text = $"경로 길이: {advancedResult.TotalDistance:F1}";
//_statusLabel.Text = $"경로 계산 완료 ({advancedResult.CalculationTimeMs}ms)";
// 🔥 VirtualAGV에도 경로 설정 (Predict()가 동작하려면 필요)
selectedAGV.SetPath(advancedResult);
// 도킹 검증 결과 확인 및 UI 표시
//CheckAndDisplayDockingValidation(advancedResult);
// 고급 경로 디버깅 정보 표시
//UpdateAdvancedPathDebugInfo(advancedResult);
}
else if (advancedResult != null)
{
// 경로 실패시 디버깅 정보 초기화
//_pathDebugLabel.Text = $"경로: 실패 - {advancedResult.ErrorMessage}";
Message = $"경로를 찾을 수 없습니다:\n{advancedResult.Message}";
advancedResult = null;
}
return (advancedResult, Message);
}

8
Cs_HMI/Project/ViewForm/fAuto.cs
View File

@@ -62,12 +62,12 @@ namespace Project.ViewForm
// PickOn
var pickOn = new ToolStripMenuItem("Pick On (Move & Pick)");
pickOn.Click += (s, args) => ExecuteManualCommand(mapnode, ENIGProtocol.AGVCommandHE.PickOn);
pickOn.Click += (s, args) => ExecuteManualCommand(mapnode, ENIGProtocol.AGVCommandHE.PickOnEnter);
menu.Items.Add(pickOn);
// PickOff
var pickOff = new ToolStripMenuItem("Pick Off (Move & Drop)");
pickOff.Click += (s, args) => ExecuteManualCommand(mapnode, ENIGProtocol.AGVCommandHE.PickOff);
pickOff.Click += (s, args) => ExecuteManualCommand(mapnode, ENIGProtocol.AGVCommandHE.PickOffEnter);
menu.Items.Add(pickOff);
// Charge
@@ -246,11 +246,11 @@ namespace Project.ViewForm
ContextMenuStrip menu = new ContextMenuStrip();
var pickOn = new ToolStripMenuItem("Pick On (Move & Pick)");
pickOn.Click += (s, args) => ExecuteManualCommand(targetNode, ENIGProtocol.AGVCommandHE.PickOn);
pickOn.Click += (s, args) => ExecuteManualCommand(targetNode, ENIGProtocol.AGVCommandHE.PickOnEnter);
menu.Items.Add(pickOn);
var pickOff = new ToolStripMenuItem("Pick Off (Move & Drop)");
pickOff.Click += (s, args) => ExecuteManualCommand(targetNode, ENIGProtocol.AGVCommandHE.PickOff);
pickOff.Click += (s, args) => ExecuteManualCommand(targetNode, ENIGProtocol.AGVCommandHE.PickOffEnter);
menu.Items.Add(pickOff);
menu.Show(Cursor.Position);