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

에뮬레이터 개발 전.

Browse Source
This commit is contained in:
backuppc
2025-12-12 14:29:06 +09:00
parent ee2a6f04bb
commit 07ddc0425f
35 changed files with 952 additions and 1242 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -84,7 +84,7 @@ namespace Project
if (PUB.AGV.system1.stop_by_front_detect == true)
{
var tsSpeak = DateTime.Now - LastSpeakTime;
if (tsSpeak.TotalSeconds >= PUB.setting.doorSoundTerm)
if (tsSpeak.TotalSeconds >= PUB.setting.alarmSoundTerm)
{
PUB.Speak(Lang.전방에물체가감지되었습니다);
LastSpeakTime = DateTime.Now;
@@ -96,7 +96,7 @@ namespace Project
if (PUB.AGV.error.runerror_by_no_magent_line == true)
{
var tsSpeak = DateTime.Now - LastSpeakTime;
if (tsSpeak.TotalSeconds >= PUB.setting.doorSoundTerm)
if (tsSpeak.TotalSeconds >= PUB.setting.alarmSoundTerm)
{
PUB.Speak(Lang.선로를이탈했습니다);
LastSpeakTime = DateTime.Now;
@@ -105,45 +105,18 @@ namespace Project
}
//현재위치를 모르는 상태라면 이동하여 현재 위치를 찾는다
if (PUB._virtualAGV.CurrentNodeId.isEmpty())// .cur. . CurrentPos == ePosition.NONE)
{
//이동중이지 않거나 홈이동상태가 아니라면?
if (PUB.AGV.system1.agv_run == false || PUB.sm.RunStep != ERunStep.GOHOME)
{
//현재위치를 검색해야함
PUB.sm.ClearRunStep();
PUB.sm.SetNewRunStep(ERunStep.GOHOME);
PUB.AddEEDB($"READY상태에서 현재위치를 몰라 홈으로 이동 백업스텝:{PUB.sm.RunStep}");
PUB.sm.BackupRunStep.Push(PUB.sm.RunStep); //현재 상태를 백업
return;
}
VAR.STR[eVarString.ChargeCheckMsg] = "현재 위치 모름";
}
if (_SM_RUN_POSCHK(isFirst, stepTime) == false) return;
//나머지 상황체크
switch (PUB.sm.RunStep)
{
case ERunStep.GOHOME:
if (runStepisFirst)
if (_SM_RUN_GOHOME(runStepisFirst, PUB.sm.GetRunSteptime) == true)
{
PUB.Speak(Lang.FindCurrentPisition);
}
else if (_SM_RUN_GOHOME(runStepisFirst, PUB.sm.GetRunSteptime))
{
//230601
PUB.Speak(Lang.홈이동완료대기상태로전환합니다);
VAR.TIME.Update(eVarTime.ChargeTry);
ERunStep NextStep = ERunStep.READY;
if (PUB.sm.BackupRunStep.Count > 0)
NextStep = PUB.sm.BackupRunStep.Pop();
PUB.log.Add($"현재위치 검색완료로 {NextStep} 으로 RUNSTEP값을 변경 합니다");
PUB.sm.SetNewRunStep(NextStep); //대기상태로 전환한다
return;
PUB.log.Add($"홈 이동이 완료되어 준비상태로 전환합니다");
PUB.sm.SetNewRunStep(ERunStep.READY);
}
break;
case ERunStep.GOTO: //목적지까지 이동하는 경우
if (_SM_RUN_GOTO(runStepisFirst, PUB.sm.GetRunSteptime) == true)
{
@@ -152,27 +125,27 @@ namespace Project
PUB.log.Add($"목적지({target.RfidId}) 도착완료 타입:{target.Type}, 출발지:{PUB._virtualAGV.StartNode.RfidId}");
if (target.Type == AGVNavigationCore.Models.NodeType.Buffer)
{
//현재위치가 마지막경로의 NODEID와 일치해야한다
var lastPath = PUB._virtualAGV.CurrentPath.DetailedPath.LastOrDefault();
if(lastPath.NodeId.Equals(PUB._virtualAGV.CurrentNodeId))
if (lastPath.NodeId.Equals(PUB._virtualAGV.CurrentNodeId))
{
//버퍼진입전 노드에 도착완료했따
PUB.XBE.BufferInReady = true;
PUB.XBE.BufferReadyError = false;
PUB.XBE.BufferReadyError = false;
}
else
{
//마지막위치가 아닌 다른 위치에 있으니 버퍼 작업을 할 수없다
PUB.log.AddAT("목적지 버퍼이동완료 했지만 마지막 노드가 아닙니다");
PUB.XBE.BufferInReady = false;
PUB.XBE.BufferReadyError = true;
PUB.XBE.BufferReadyError = true;
}
PUB.XBE.BufferInComplete = false;
PUB.XBE.BufferOutComplete = false;
}
else if(target.Type == AGVNavigationCore.Models.NodeType.Charging)
else if (target.Type == AGVNavigationCore.Models.NodeType.Charging)
{
}
@@ -191,7 +164,7 @@ namespace Project
else
{
//목적지다 다른 형태이다
}
PUB._virtualAGV.Turn = AGVNavigationCore.Models.AGVTurn.None;
PUB.sm.SetNewRunStep(ERunStep.READY);
@@ -271,8 +244,8 @@ namespace Project
PUB.Result.TargetPos = ePosition.QC;
}
PUB.Result.CurrentPosCW = "1";
PUB.sm.SetNewRunStep(ERunStep.GOHOME);
PUB.log.AddAT("충전 해제로 홈으로 이동");
PUB.sm.SetNewRunStep(ERunStep.READY);
PUB.log.AddAT("충전 해제로 대기상태로 전환 합니다");
}
}
break;
@@ -313,342 +286,5 @@ namespace Project
}
bool CheckStopCondition()
{
return true;
}
void CheckAGVMoveTo(eGoDir dir)
{
//계속내려간다
if (dir == eGoDir.Down)
{
var tsCmd = DateTime.Now - LastCommandTime;
if (tsCmd.TotalMilliseconds >= 1999)
{
//현재 동작중인상태에서 방향이 맞지 않다면 일단 움직임을 멈춘다
if (PUB.AGV.system1.agv_run)
{
if (PUB.AGV.data.Direction == 'B')
{
PUB.log.Add($"방향이 맞지 않아 정지 합니다({dir})");
PUB.AGV.AGVMoveStop("CheckAGVMoveTo");
}
else if (PUB.AGV.data.Speed == 'S')
{
PUB.log.Add($"마크정지를 해제하기 위해 장비를 멈춥니다");
}
}
else
{
//움직이지 않으므로 전진하도록 한다
PUB.log.Add($"AGV 기동 방향(DOWN):{dir}");
PUB.AGV.AGVMoveRun(arDev.Narumi.eRunOpt.Forward);
}
LastCommandTime = DateTime.Now;
}
}
else
{
var tsCmd = DateTime.Now - LastCommandTime;
if (tsCmd.TotalMilliseconds >= 1999)
{
if (PUB.AGV.system1.agv_run)
{
if (PUB.AGV.data.Direction == 'F')
{
PUB.log.Add($"방향이 맞지 않아 정지 합니다({dir})");
PUB.AGV.AGVMoveStop("CheckAGVMoveTo");
}
else if (PUB.AGV.data.Speed == 'S')
{
PUB.log.Add($"마크정지를 해제하기 위해 장비를 멈춥니다");
}
}
else
{
PUB.log.Add($"AGV 기동 방향(UP):{dir}");
PUB.AGV.AGVMoveRun(arDev.Narumi.eRunOpt.Backward);
}
LastCommandTime = DateTime.Now;
}
}
}
void CheckAGVStopbyMARK(string sender)
{
//계속내려간다
var tsCmd = DateTime.Now - LastCommandTime;
if (VAR.BOOL[eVarBool.NEXTSTOP_MARK] == false || tsCmd.TotalMilliseconds >= 1500)
{
PUB.AGV.AGVMoveStop("CheckAGVStopbyMARK", arDev.Narumi.eStopOpt.MarkStop);
LastCommandTime = DateTime.Now;
PUB.log.Add($"[{sender}] MARK신호에 멈춤 설정");
}
}
Boolean UpdateMotionPositionForMark(string sender)
{
//이머전시상태에서는 처리하지 않는다.
if (VAR.BOOL[eVarBool.EMERGENCY]) return false;
//DOWN 작업
// if (goDIR == eGoDir.Down)
{
//1. 현재위치 > 대상위치
if (PUB.Result.CurrentPos > PUB.Result.TargetPos)
{
//계속내려간다
if (PUB.setting.AGV_Direction_FVI_Backward)
CheckAGVMoveTo(eGoDir.Down);
else
CheckAGVMoveTo(eGoDir.Up);
}
//2. 현재위치 < 대상위치
else if (PUB.Result.CurrentPos < PUB.Result.TargetPos)
{
//올라가야한다
if (PUB.setting.AGV_Direction_FVI_Backward)
CheckAGVMoveTo(eGoDir.Up);
else
CheckAGVMoveTo(eGoDir.Down);
}
//3. 현재위치 = 대상위치
else
{
//현재위치가 확정되었는가?
var actpos = ctlPos1.GetPositionActive(PUB.Result.CurrentPos);
if (actpos == false && PUB.AGV.system1.agv_stop == true)
{
//위치확정이되지 않았다면 AGV멈춤시에 기동하게 한다.
var lastcom = DateTime.Now - LastCommandTime;
if (lastcom.TotalSeconds > 3)
{
if (PUB.Result.CurrentPosCW == "1")
{
if (PUB.setting.AGV_Direction_FVI_Backward)
PUB.AGV.AGVMoveRun(arDev.Narumi.eRunOpt.Forward);
else
PUB.AGV.AGVMoveRun(arDev.Narumi.eRunOpt.Backward);
}
else
{
if (PUB.setting.AGV_Direction_FVI_Backward)
PUB.AGV.AGVMoveRun(arDev.Narumi.eRunOpt.Backward);
else
PUB.AGV.AGVMoveRun(arDev.Narumi.eRunOpt.Forward);
}
LastCommandTime = DateTime.Now;
PUB.logagv.Add($"AGV가 멈춰있다 동작을 재개 합니다");
}
}
else
{
//마크센서가 들어와잇고, 위치가 act 되어있다면 해당 위치에 있는 것이다
if (PUB.AGV.error.Emergency == false &&
PUB.AGV.system1.agv_stop &&
PUB.AGV.signal.mark_sensor && actpos &&
PUB.Result.CurrentPos == PUB.Result.TargetPos)
{
//PUB.AGV.AGVMoveStop();
return true;
}
if (PUB.AGV.system1.agv_stop == true && PUB.AGV.system1.agv_run == false)
{
PUB.log.Add($"멈춰있으므로 이동을 시작 합니다");
if (PUB.Result.CurrentPosCW == "1")
{
if (PUB.setting.AGV_Direction_FVI_Backward)
PUB.AGV.AGVMoveRun(arDev.Narumi.eRunOpt.Backward);
else
PUB.AGV.AGVMoveRun(arDev.Narumi.eRunOpt.Forward);
}
else
{
if (PUB.setting.AGV_Direction_FVI_Backward)
PUB.AGV.AGVMoveRun(arDev.Narumi.eRunOpt.Forward);
else
PUB.AGV.AGVMoveRun(arDev.Narumi.eRunOpt.Backward);
}
}
//AGV는 아래로 FVI방향으로 내려가고 있다
if (VAR.BOOL[eVarBool.AGVDIR_UP] == false)
{
if (PUB.Result.CurrentPosCW == "0")
{
//장비가 마크센서에의해 멈췃다면 완료이다
if (PUB.AGV.error.Emergency == false && PUB.AGV.signal.mark_sensor)
{
PUB.AGV.AGVMoveStop("UPdateMotionPositionForMark");
return true;
}
else CheckAGVStopbyMARK(sender);
}
else if (PUB.Result.CurrentPosCW == "1")
{
//내려가는 작업이고 AGV는 올라가고 있는데 RFID는 위졲이 감지되었다면 아래로 내려가야한다
CheckAGVMoveTo(eGoDir.Up);
}
else
{
PUB.Result.CurrentPosCW = "1";
CheckAGVMoveTo(eGoDir.Up);
}
}
//AGV는 Qc방향으로 올라가고 있다
else
{
if (PUB.Result.CurrentPosCW == "1")
{
//네려가는 방향에서 내려가는 위치가 인식되었다면 마크에서 멈춰야 한다
if (PUB.AGV.error.Emergency == false && PUB.AGV.signal.mark_sensor)
{
PUB.AGV.AGVMoveStop("UPdateMotionPositionForMark");
return true;
}
else CheckAGVStopbyMARK(sender);
}
else if (PUB.Result.CurrentPosCW == "0")
{
//내려가는 작업이고 AGV는 올라가고 있는데 RFID는 위졲이 감지되었다면 아래로 내려가야한다
CheckAGVMoveTo(eGoDir.Down);
}
else
{
PUB.Result.CurrentPosCW = "0";
CheckAGVMoveTo(eGoDir.Down);
}
}
}
}
}
//UP 작업
//else
//{
// //1. 현재위치 > 대상위치
// if (PUB.Result.CurrentPos > PUB.Result.TargetPos)
// {
// //계속내려간다
// CheckAGVMoveTo(eGoDir.Down);
// }
// //2. 현재위치 < 대상위치
// else if (PUB.Result.CurrentPos < PUB.Result.TargetPos)
// {
// //올라가야한다
// CheckAGVMoveTo(eGoDir.Up);
// }
// //3. 현재위치 = 대상위치
// else
// {
// //AGV는 위로가고 있다
// if (VAR.BOOL[eVarBool.AGVDIR_UP] == true)
// {
// if (PUB.Result.CurrentPosCW == "0")
// {
// //장비가 마크센서에의해 멈췃다면 완료이다
// if (PUB.AGV.system1.agv_stop && PUB.AGV.system1.stop_by_front_detect == false && PUB.AGV.error.Emergency == false && PUB.AGV.signal.mark_sensor)
// {
// return true;
// }
// else CheckAGVStopbyMARK();
// }
// else if (PUB.Result.CurrentPosCW == "1")
// {
// //내려가는 작업이고 AGV는 올라가고 있는데 RFID는 위졲이 감지되었다면 아래로 내려가야한다
// CheckAGVMoveTo(eGoDir.Down);
// }
// }
// //AGV는 내려가고 있다
// else if (VAR.BOOL[eVarBool.AGVDIR_UP] == false)
// {
// if (PUB.Result.CurrentPosCW == "1")
// {
// //네려가는 방향에서 내려가는 위치가 인식되었다면 마크에서 멈춰야 한다
// if (PUB.AGV.system1.agv_stop && PUB.AGV.system1.stop_by_front_detect == false && PUB.AGV.error.Emergency == false && PUB.AGV.signal.mark_sensor)
// {
// return true;
// }
// else CheckAGVStopbyMARK();
// }
// else if (PUB.Result.CurrentPosCW == "0")
// {
// //내려가는 작업이고 AGV는 올라가고 있는데 RFID는 위졲이 감지되었다면 아래로 내려가야한다
// CheckAGVMoveTo(eGoDir.Up);
// }
// }
// }
//}
return false;
}
Boolean UpdateMotionPositionForCharger(string sender)
{
if (VAR.BOOL[eVarBool.AGVDIR_UP] == 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)
{
//PUB.PLC.Move(Device.PLC.Rundirection.Backward, "UpdateMotionPosition(" + sender + ")");
PUB.AGV.AGVMoveRun(arDev.Narumi.eRunOpt.Backward);//
tm_gocharge_command = DateTime.Now;
}
}
else
{
//CCW (역방향 )
//if (Pub.Result.RFIDPos < ePosition.CHARGE || Pub.Result.RFIDPos > ePosition.QC)
//{
// //정상적이라면 RFID값은 QC + 혹은 QC - 에 있어야 하므로 항상 차저보다 높이 있다
// //그렇지 않다면 초기화해서 QC검색부터 다시 실행하게 한다
// //여기는 비정상 위치 값이다.
// Pub.sm.SetStepSeq(1);
//}
//else
//{
//현재위치가 충전위치이고, 움직이지 않았다면 완료된 경우라 할수 있따
if (PUB.Result.CurrentPos == ePosition.CHARGE &&
PUB.AGV.signal.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)
{
//PUB.PLC.Move(Device.PLC.Rundirection.Backward, "UpdateMotionPosition #1(" + sender + ")");
PUB.AGV.AGVMoveRun(arDev.Narumi.eRunOpt.Backward);//
LastCommandTime = DateTime.Now;
}
}
//}
}
return false;
}
}//cvass
}

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

@@ -45,7 +45,7 @@ namespace Project
if (PUB.AGV.system1.stop_by_front_detect == true)
{
var tsSpeak = DateTime.Now - LastSpeakTime;
if (tsSpeak.TotalSeconds >= PUB.setting.doorSoundTerm)
if (tsSpeak.TotalSeconds >= PUB.setting.alarmSoundTerm)
{
PUB.Speak(Lang.전방에물체가감지되었습니다);
LastSpeakTime = DateTime.Now;
@@ -214,7 +214,7 @@ namespace Project
else if (PUB.sm.RunStepSeq == idx++)
{
//마크스탑신호가 3초이내로 들어와야 한다
if (PUB.AGV.signal.mark_sensor == false)
if (VAR.BOOL[eVarBool.NEXTSTOP_MARK] == false)
{
var ts = VAR.TIME.RUN(eVarTime.LastTurnCommandTime);
if (ts.TotalSeconds > 3)

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

@@ -34,7 +34,7 @@ namespace Project
if (PUB.AGV.system1.stop_by_front_detect == true)
{
var tsSpeak = DateTime.Now - LastSpeakTime;
if (tsSpeak.TotalSeconds >= PUB.setting.doorSoundTerm)
if (tsSpeak.TotalSeconds >= PUB.setting.alarmSoundTerm)
{
PUB.Speak(Lang.전방에물체가감지되었습니다);
LastSpeakTime = DateTime.Now;

15
Cs_HMI/Project/StateMachine/Step/_SM_RUN_CHGOFF.cs
View File

@@ -14,6 +14,8 @@ namespace Project
public Boolean _SM_RUN_CHGOFF(bool isFirst, TimeSpan stepTime)
{
//충전중인지 확인한다.
if (VAR.BOOL[eVarBool.FLAG_CHARGEONA] == true || PUB.AGV.system1.Battery_charging == true)
{
@@ -25,7 +27,7 @@ namespace Project
PUB.Speak(Lang.충전을해제합니다);
}
}
//AGV는 충전을 해제한 상태이다
if (PUB.AGV.system1.Battery_charging == false)
{
@@ -47,6 +49,17 @@ namespace Project
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;
}

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

@@ -38,7 +38,7 @@ namespace Project
if (PUB.AGV.system1.stop_by_front_detect == true)
{
var tsSpeak = DateTime.Now - LastSpeakTime;
if (tsSpeak.TotalSeconds >= PUB.setting.doorSoundTerm)
if (tsSpeak.TotalSeconds >= PUB.setting.alarmSoundTerm)
{
PUB.Speak(Lang.전방에물체가감지되었습니다);
LastSpeakTime = DateTime.Now;
@@ -63,8 +63,16 @@ namespace Project
var idx = 1;
if (PUB.sm.RunStepSeq == idx++)
{
PUB.Speak(Lang.충전을위해홈위치로이동합니다);
PUB.Result.TargetPos = ePosition.QC;
var targetnode = PUB.FindByRFID(PUB.setting.NodeMAP_RFID_Charger);
if(targetnode == null)
{
PUB.log.AddE($"충전기 노드가 설정되지 않았습니다");
PUB.sm.SetNewRunStep(ERunStep.READY);
return false;
}
//충전기로 대상을 설정한다
PUB._virtualAGV.TargetNode = targetnode;
VAR.TIME.Update(eVarTime.ChargeSearch);
PUB.sm.UpdateRunStepSeq();
PUB.log.Add($"충전:대상위치 QC 시작");
@@ -73,7 +81,7 @@ namespace Project
else if (PUB.sm.RunStepSeq == idx++)
{
//모션 전후진 제어
if (UpdateMotionPositionForMark("GOCHARGE #1") == true)
if ( UpdateMotionPositionForCharger("GOCHARGE #1") == true)
{
PUB.log.Add($"충전:충전기 검색 전 QC위치 확인 완료");
PUB.sm.UpdateRunStepSeq();
@@ -200,9 +208,6 @@ namespace Project
PUB.Result.CurrentPos = ePosition.CHARGE; //현재위치를 충전기로 한다
PUB.Result.TargetPos = ePosition.CHARGE;
PUB.Result.CurrentPosCW = "1";
ctlPos1.SetPosition(ePosition.CHARGE);
ctlPos1.SetPositionActive(ePosition.CHARGE);
ctlPos1.Invalidate();
PUB.sm.UpdateRunStepSeq();
return false;
}

35
Cs_HMI/Project/StateMachine/Step/_SM_RUN_GOHOME.cs
View File

@@ -13,27 +13,18 @@ namespace Project
{
public Boolean _SM_RUN_GOHOME(bool isFirst, TimeSpan stepTime)
{
var funcName = "_SM_RUN_GOHOME";
if (runStepisFirst)
{
// PUB.flag.set(EFlag.FLAG_NEXTSTOP_ALIGN, false);
//VAR.BOOL[eVarBool.FLAG_NEXTSTOP_MARK] = false;//);
VAR.BOOL[eVarBool.FLAG_NEXTSTOP_ALIGN] = false;//);
}
//220629
// if(PUB.flag.get(EFlag.FLAG_GO_CHAGER_TEMP))
//{
// PUB.flag.set(EFlag.FLAG_GO_CHAGER_TEMP, false);
//}
//HW 연결오류
if (PUB.AGV.IsOpen == false)
{
PUB.Result.SetResultMessage(eResult.Hardware, eECode.AGVCONN, eNextStep.ERROR);
return false;
}
//충전 상태가 OFF되어야 동작하게한다
if (_SM_RUN_CHGOFF(isFirst, stepTime) == false)
@@ -43,7 +34,7 @@ namespace Project
if (PUB.AGV.system1.stop_by_front_detect == true)
{
var tsSpeak = DateTime.Now - LastSpeakTime;
if (tsSpeak.TotalSeconds >= PUB.setting.doorSoundTerm)
if (tsSpeak.TotalSeconds >= PUB.setting.alarmSoundTerm)
{
PUB.Speak(Lang.전방에물체가감지되었습니다);
LastSpeakTime = DateTime.Now;
@@ -51,27 +42,29 @@ namespace Project
return false;
}
//현재 위치가 결정되어있는지 체크한다
if (_SM_RUN_POSCHK(isFirst, stepTime) == false)
return false;
var idx = 1;
if (PUB.sm.RunStepSeq == idx++)
{
PUB.Speak(Lang.홈으로이동합니다);
//홈은 무조건 QC위치로 간다
PUB.AddEEDB($"홈검색시작({PUB.Result.TargetPos})");
PUB.Result.TargetPos = ePosition.QC;
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("GOHOME"))
if (UpdateMotionPositionForMark(funcName))
{
PUB.AGV.AGVMoveStop("SM_RUN_GOHOME");
PUB.AGV.AGVMoveStop(funcName);
PUB.sm.UpdateRunStepSeq();
}
return false;
@@ -79,7 +72,7 @@ namespace Project
else if (PUB.sm.RunStepSeq == idx++)
{
//QC까지 모두 완료되었다.(완전히 정차할때까지 기다린다)
PUB.Speak(Lang.홈검색완료, true);
PUB.Speak(Lang.홈검색완료, true);
PUB.AddEEDB($"홈검색완료({PUB.Result.TargetPos})");
PUB.sm.UpdateRunStepSeq();
return false;

55
Cs_HMI/Project/StateMachine/Step/_SM_RUN_POSCHK.cs
View File

@@ -3,6 +3,8 @@ using System.Collections.Generic;
using System.Drawing;
using System.Linq;
using System.Text;
using AR;
using COMM;
namespace Project
{
@@ -10,45 +12,30 @@ namespace Project
{
public Boolean _SM_RUN_POSCHK(bool isFirst, TimeSpan stepTime)
{
//현재 위치가 찾아지지 않았다면. 먼저 위치를 찾는다. 위로 이동시켜서 찾는다.
if (PUB.Result.CurrentPos == ePosition.NONE)
//현재위치가 설정되어있는지 확인한다, 현재위치값이 있는 경우 True 를 반환
var currentnode = PUB.FindByNodeID(PUB._virtualAGV.CurrentNodeId);
if (currentnode != null) return true;
//이동을 하지 않고있다면 전진을 진행한다
if (PUB.AGV.system1.agv_run == false)
{
if (isFirst)
var ts = VAR.TIME.RUN(eVarTime.LastRunCommandTime);
if (ts.TotalSeconds > 5)
{
PUB.Speak( Lang.현재위치를검색합니다);
}
if (PUB.AGV.system1.agv_run)
{
//방향을 체크한다
var basedir = PUB.setting.AGV_Direction_FVI_Backward ? 'F' : 'B';
if (PUB.AGV.data.Direction == basedir)
PUB.log.Add($"현재위치를 몰라 전진 이동 합니다");
PUB.AGV.AGVMoveSet(new arDev.Narumi.BunkiData
{
var ts = DateTime.Now - LastCommandTime;
if (ts.TotalMilliseconds >= 1999)
{
PUB.AGV.AGVMoveStop("SM_RUN_POSCHK",arDev.Narumi.eStopOpt.Stop);
LastCommandTime = DateTime.Now;
}
}
Bunki = arDev.Narumi.eBunki.Strate,
Direction = arDev.Narumi.eMoveDir.Forward,
PBSSensor = 1,
Speed = arDev.Narumi.eMoveSpd.Low,
});
PUB.AGV.AGVMoveRun();
VAR.TIME.Update(eVarTime.LastRunCommandTime);
}
else if (PUB.AGV.system1.agv_run == false)
{
//전진이동한다
var ts = DateTime.Now - LastCommandTime;
if (ts.TotalMilliseconds >= 1999)
{
if (PUB.setting.AGV_Direction_FVI_Backward)
PUB.AGV.AGVMoveRun(arDev.Narumi.eRunOpt.Backward);//
else
PUB.AGV.AGVMoveRun(arDev.Narumi.eRunOpt.Forward);//
LastCommandTime = DateTime.Now;
}
}
return false;
}
else return true;
VAR.STR[eVarString.StatusMessage] = "현재 위치를 알 수 없습니다";
return false;
}
}
}

5
Cs_HMI/Project/StateMachine/Step/_SM_RUN_RESET.cs
View File

@@ -83,11 +83,6 @@ namespace Project
PUB.Result.TargetPos = ePosition.NONE;
PUB.Result.CurrentPos = ePosition.NONE;
PUB.Result.CurrentPosCW = "0";
ctlPos1.SetPositionDeActive();
ctlPos1.SetPosition(ePosition.NONE);
ctlPos1.SetDirection("0");
ctlPos1.Invalidate();
PUB.sm.UpdateRunStepSeq();
return false;
}

6
Cs_HMI/Project/StateMachine/Step/_SM_RUN_SYNC.cs
View File

@@ -80,7 +80,7 @@ namespace Project
//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})");
@@ -101,7 +101,7 @@ namespace Project
}
else if (PUB.sm.RunStepSeq == idx++)
{
//통신 확인되었으므로 스피드를 전송한다.
//통신 확인되었으므로 설정값들을
if (synidx < synlist.Count)
{
var item = synlist.ElementAt(synidx);
@@ -111,7 +111,7 @@ namespace Project
// 캔버스에 동기화 상태 표시
if (PUB._mapCanvas != null)
{
float progress = (float)synidx / synlist.Count;
float progress = (float)synidx / VAR.I32[eVarInt32.SyncItemCount];
PUB._mapCanvas.SetSyncStatus("장비 설정 동기화 중...", progress, $"항목: {item.Key} ({synidx + 1}/{synlist.Count})");
}
}

228
Cs_HMI/Project/StateMachine/Step/_Util.cs Normal file
View File

@@ -0,0 +1,228 @@
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
{
bool CheckStopCondition()
{
return true;
}
/// <summary>
/// 설정된 목적지까지 이동을 완료 한 후 True를 반환합니다.
/// 목적지 : PUB._virtualAGV.TargetNode
///
/// </summary>
/// <param name="sender"></param>
/// <returns></returns>
Boolean UpdateMotionPositionForMark(string sender)
{
//현재위치를 모르는 상태라면 이동하여 현재 위치를 찾는다
if (_SM_RUN_POSCHK(false, new TimeSpan()) == false) return false;
//현재위치노드 오류
var currentNode = PUB.FindByNodeID(PUB._virtualAGV.CurrentNodeId);
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.CurrentNodeId);
//시작노드가없다면 오류
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.CurrentPath == null ||
PUB._virtualAGV.CurrentPath.DetailedPath.Any() == false ||
PUB._virtualAGV.CurrentPath.DetailedPath.Where(t => t.NodeId.Equals(currentNode.NodeId)).Any() == false)
{
if (PUB.AGV.system1.agv_run)
{
PUB.log.Add($"경로 재생성으로 인해 구동을 멈춥니다");
PUB.AGV.AGVMoveStop("경로재생성");
}
var PathResult = CalcPath(PUB._virtualAGV.StartNode, PUB._virtualAGV.TargetNode);
if (PathResult.result == null)
{
PUB.log.AddE($"경로가 계산되지 않았습니다");
PUB.sm.SetNewRunStep(ERunStep.READY);
return false;
}
PUB.log.AddI($"경로생성 {PUB._virtualAGV.StartNode.RfidId} -> {PUB._virtualAGV.TargetNode.RfidId}");
}
//predict 를 이용하여 다음 이동을 모두 확인한다.
var nextAction = PUB._virtualAGV.Predict();
var message = $"[다음 행동 예측]\n\n" +
$"모터: {nextAction.Motor}\n" +
$"마그넷: {nextAction.Magnet}\n" +
$"속도: {nextAction.Speed}\n" +
$"이유: {nextAction.Message}\n\n" +
$"---\n" +
$"현재 상태: {PUB._virtualAGV.CurrentState}\n" +
$"현재 방향: {PUB._virtualAGV.CurrentDirection}\n" +
$"위치 확정: {PUB._virtualAGV.IsPositionConfirmed} (RFID {PUB._virtualAGV.DetectedRfidCount}개)\n" +
$"현재 노드: {PUB._virtualAGV.CurrentNodeId ?? "없음"}";
//모터에서 정지를 요청했다
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("다음행동예측에서 장비 멈춤이 확인되었습니다");
PUB.AGV.AGVMoveStop(nextAction.Message);
}
}
// 목적지 도착 여부 확인
// 현재 노드가 타겟 노드와 같고, 위치가 확정된 상태라면 도착으로 간주
// 단, AGV가 실제로 멈췄는지 확인 (agv_run == false)
if (PUB._virtualAGV.IsPositionConfirmed &&
PUB._virtualAGV.CurrentNodeId == PUB._virtualAGV.TargetNode.NodeId)
{
if (PUB.AGV.system1.agv_run == false)
{
return true;
}
}
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])
{
PUB.AGV.AGVMoveSet(new arDev.Narumi.BunkiData
{
Bunki = bunki,
Direction = dir,
PBSSensor = 1,
Speed = spd,
});
PUB.log.Add($"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)
{
PUB.AGV.AGVMoveSet(new arDev.Narumi.BunkiData
{
Bunki = arDev.Narumi.eBunki.Strate,
Direction = arDev.Narumi.eMoveDir.Forward,
PBSSensor = 1,
Speed = arDev.Narumi.eMoveSpd.Low,
});
PUB.AGV.AGVMoveRun();//
tm_gocharge_command = DateTime.Now;
}
}
else
{
//현재위치가 충전위치이고, 움직이지 않았다면 완료된 경우라 할수 있따
if (PUB._virtualAGV.CurrentNodeId.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)
{
//PUB.PLC.Move(Device.PLC.Rundirection.Backward, "UpdateMotionPosition #1(" + sender + ")");
PUB.AGV.AGVMoveRun(arDev.Narumi.eRunOpt.Backward);//
LastCommandTime = DateTime.Now;
}
}
//}
}
return false;
}
}//cvass
}