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

fix

Browse Source
This commit is contained in:
backuppc
2025-11-10 14:43:47 +09:00
parent 68745f23bb
commit 6e54633c08
57 changed files with 4432 additions and 1018 deletions
Download Patch File Download Diff File Expand all files Collapse all files

476
Cs_HMI/Project/StateMachine/Step/_SM_RUN_GOTO.cs
View File

@@ -30,259 +30,259 @@ namespace Project
}
//목적지가 설정되었는지 체크한다.
if (PUB.mapctl.Manager.agv.TargetRFID.IsEmpty)
{
//최대 5초간 설정여부를 확인하고
if (VAR.TIME.RUN(eVarTime.CheckGotoTargetSet).TotalSeconds > 5)
{
//실패시에는 READY로 전환한다.
PUB.sm.SetNewRunStep(ERunStep.READY);
PUB.Speak(Lang.목적지가없어대기상태로전환합니다);
}
return false;
}
//Z if (PUB.mapctl.Manager.agv.TargetRFID.IsEmpty)
// {
// //최대 5초간 설정여부를 확인하고
// if (VAR.TIME.RUN(eVarTime.CheckGotoTargetSet).TotalSeconds > 5)
// {
// //실패시에는 READY로 전환한다.
// PUB.sm.SetNewRunStep(ERunStep.READY);
// PUB.Speak(Lang.목적지가없어대기상태로전환합니다);
// }
// return false;
// }
var idx = 1;
var BeforePredictIdx = -1;
var predict = PUB.mapctl.Manager.PredictResult;
if (PUB.sm.RunStepSeq == idx++)
{
PUB.Speak(Lang.위치로이동합니다);
PUB.log.Add($"목적지 위치 이동시작({PUB.mapctl.Manager.agv.TargetRFID.Value})");
VAR.TIME.Update(eVarTime.CheckGotoTargetSet);
VAR.TIME.Set(eVarTime.SendGotoCommand, DateTime.Now.AddDays(-1));
PUB.sm.UpdateRunStepSeq();
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
//멈춰야하는경우
if (predict.MoveState == AGVControl.AGVMoveState.Stop)
{
if (PUB.AGV.system1.agv_run)
{
if (VAR.TIME.RUN(eVarTime.SendGotoCommand).TotalSeconds > 2)
{
PUB.Speak("AGV Stop");
PUB.AGV.AGVMoveStop("Predict", arDev.Narumi.eStopOpt.Stop);
VAR.TIME.Update(eVarTime.SendGotoCommand);
}
return false;
}
else
{
//완료되었거나 턴을진행해야한다
if (predict.ReasonCode == AGVControl.AGVActionReasonCode.Arrived ||
predict.ReasonCode == AGVControl.AGVActionReasonCode.NeedTurnMove ||
predict.ReasonCode == AGVControl.AGVActionReasonCode.NeedTurnPoint)
{
GotoTurnStep = 0;
GotoTurnSetTime = DateTime.Now.AddDays(-1);
PUB.sm.UpdateRunStepSeq();
}
return false;
}
}
else //이동해야하는 경우
{
//속도와 방향이 불일치하는 경우 다시 설정한다 (속도: H,L,M,[S]
AGVControl.AgvDir AGV_Direction = (AGVControl.AgvDir)PUB.AGV.data.Direction;
AGVControl.AgvSpeed AGV_Speed = (AGVControl.AgvSpeed)PUB.AGV.data.Speed;
AGVControl.AgvSts AGV_Sts = (AGVControl.AgvSts)PUB.AGV.data.Sts;
//var idx = 1;
//var BeforePredictIdx = -1;
//var predict = PUB.mapctl.Manager.PredictResult;
//if (PUB.sm.RunStepSeq == idx++)
//{
// PUB.Speak(Lang.위치로이동합니다);
// PUB.log.Add($"목적지 위치 이동시작({PUB.mapctl.Manager.agv.TargetRFID.Value})");
// VAR.TIME.Update(eVarTime.CheckGotoTargetSet);
// VAR.TIME.Set(eVarTime.SendGotoCommand, DateTime.Now.AddDays(-1));
// PUB.sm.UpdateRunStepSeq();
// return false;
//}
//else if (PUB.sm.RunStepSeq == idx++)
//{
// //멈춰야하는경우
// if (predict.MoveState == AGVControl.AGVMoveState.Stop)
// {
// if (PUB.AGV.system1.agv_run)
// {
// if (VAR.TIME.RUN(eVarTime.SendGotoCommand).TotalSeconds > 2)
// {
// PUB.Speak("AGV Stop");
// PUB.AGV.AGVMoveStop("Predict", arDev.Narumi.eStopOpt.Stop);
// VAR.TIME.Update(eVarTime.SendGotoCommand);
// }
// return false;
// }
// else
// {
// //완료되었거나 턴을진행해야한다
// if (predict.ReasonCode == AGVControl.AGVActionReasonCode.Arrived ||
// predict.ReasonCode == AGVControl.AGVActionReasonCode.NeedTurnMove ||
// predict.ReasonCode == AGVControl.AGVActionReasonCode.NeedTurnPoint)
// {
// GotoTurnStep = 0;
// GotoTurnSetTime = DateTime.Now.AddDays(-1);
// PUB.sm.UpdateRunStepSeq();
// }
// return false;
// }
// }
// else //이동해야하는 경우
// {
// //속도와 방향이 불일치하는 경우 다시 설정한다 (속도: H,L,M,[S]
// AGVControl.AgvDir AGV_Direction = (AGVControl.AgvDir)PUB.AGV.data.Direction;
// AGVControl.AgvSpeed AGV_Speed = (AGVControl.AgvSpeed)PUB.AGV.data.Speed;
// AGVControl.AgvSts AGV_Sts = (AGVControl.AgvSts)PUB.AGV.data.Sts;
//상태값이 바뀌었다면 전송을 해야한다
if (predict.Direction != AGV_Direction || predict.MoveSpeed != AGV_Speed || predict.MoveDiv != AGV_Sts)
{
if (VAR.TIME.RUN(eVarTime.SendGotoCommand).TotalSeconds > CommandInterval)
{
arDev.Narumi.eBunki v_bunki = arDev.Narumi.eBunki.Strate;
if (predict.MoveDiv == AGVControl.AgvSts.Straight) v_bunki = arDev.Narumi.eBunki.Strate;
else if (predict.MoveDiv == AGVControl.AgvSts.Left) v_bunki = arDev.Narumi.eBunki.Left;
else if (predict.MoveDiv == AGVControl.AgvSts.Right) v_bunki = arDev.Narumi.eBunki.Right;
// //상태값이 바뀌었다면 전송을 해야한다
// if (predict.Direction != AGV_Direction || predict.MoveSpeed != AGV_Speed || predict.MoveDiv != AGV_Sts)
// {
// if (VAR.TIME.RUN(eVarTime.SendGotoCommand).TotalSeconds > CommandInterval)
// {
// arDev.Narumi.eBunki v_bunki = arDev.Narumi.eBunki.Strate;
// if (predict.MoveDiv == AGVControl.AgvSts.Straight) v_bunki = arDev.Narumi.eBunki.Strate;
// else if (predict.MoveDiv == AGVControl.AgvSts.Left) v_bunki = arDev.Narumi.eBunki.Left;
// else if (predict.MoveDiv == AGVControl.AgvSts.Right) v_bunki = arDev.Narumi.eBunki.Right;
arDev.Narumi.eMoveDir v_dir = arDev.Narumi.eMoveDir.Backward;
if (predict.Direction == AGVControl.AgvDir.Forward) v_dir = arDev.Narumi.eMoveDir.Forward;
else if (predict.Direction == AGVControl.AgvDir.Backward) v_dir = arDev.Narumi.eMoveDir.Backward;
// arDev.Narumi.eMoveDir v_dir = arDev.Narumi.eMoveDir.Backward;
// if (predict.Direction == AGVControl.AgvDir.Forward) v_dir = arDev.Narumi.eMoveDir.Forward;
// else if (predict.Direction == AGVControl.AgvDir.Backward) v_dir = arDev.Narumi.eMoveDir.Backward;
arDev.Narumi.eMoveSpd v_spd = arDev.Narumi.eMoveSpd.Low;
if (predict.MoveSpeed == AGVControl.AgvSpeed.Middle) v_spd = arDev.Narumi.eMoveSpd.Middle;
else if (predict.MoveSpeed == AGVControl.AgvSpeed.High) v_spd = arDev.Narumi.eMoveSpd.High;
else if (predict.MoveSpeed == AGVControl.AgvSpeed.Low) v_spd = arDev.Narumi.eMoveSpd.Low;
// arDev.Narumi.eMoveSpd v_spd = arDev.Narumi.eMoveSpd.Low;
// if (predict.MoveSpeed == AGVControl.AgvSpeed.Middle) v_spd = arDev.Narumi.eMoveSpd.Middle;
// else if (predict.MoveSpeed == AGVControl.AgvSpeed.High) v_spd = arDev.Narumi.eMoveSpd.High;
// else if (predict.MoveSpeed == AGVControl.AgvSpeed.Low) v_spd = arDev.Narumi.eMoveSpd.Low;
//이동셋팅을 해준다
PUB.AGV.AGVMoveSet(new arDev.Narumi.BunkiData
{
Bunki = v_bunki,
Direction = v_dir,
PBSSensor = 1,
Speed = v_spd,
});
// //이동셋팅을 해준다
// PUB.AGV.AGVMoveSet(new arDev.Narumi.BunkiData
// {
// Bunki = v_bunki,
// Direction = v_dir,
// PBSSensor = 1,
// Speed = v_spd,
// });
if (predict.MoveSpeed == AGVControl.AgvSpeed.MarkStop)
{
PUB.AGV.AGVMoveStop("Predict", arDev.Narumi.eStopOpt.Stop);
}
VAR.TIME.Update(eVarTime.SendGotoCommand);
}
return false;
}
// if (predict.MoveSpeed == AGVControl.AgvSpeed.MarkStop)
// {
// PUB.AGV.AGVMoveStop("Predict", arDev.Narumi.eStopOpt.Stop);
// }
// VAR.TIME.Update(eVarTime.SendGotoCommand);
// }
// return false;
// }
//정지상태라면 이동 명령을 전달한다
if (PUB.AGV.system1.agv_run == false)
{
if (VAR.TIME.RUN(eVarTime.SendGotoCommand).TotalSeconds > CommandInterval)
{
PUB.Speak("AGV Start");
// //정지상태라면 이동 명령을 전달한다
// if (PUB.AGV.system1.agv_run == false)
// {
// if (VAR.TIME.RUN(eVarTime.SendGotoCommand).TotalSeconds > CommandInterval)
// {
// PUB.Speak("AGV Start");
arDev.Narumi.eRunOpt v_dir = arDev.Narumi.eRunOpt.Backward;
if (predict.Direction == AGVControl.AgvDir.Forward) v_dir = arDev.Narumi.eRunOpt.Forward;
else if (predict.Direction == AGVControl.AgvDir.Backward) v_dir = arDev.Narumi.eRunOpt.Backward;
// arDev.Narumi.eRunOpt v_dir = arDev.Narumi.eRunOpt.Backward;
// if (predict.Direction == AGVControl.AgvDir.Forward) v_dir = arDev.Narumi.eRunOpt.Forward;
// else if (predict.Direction == AGVControl.AgvDir.Backward) v_dir = arDev.Narumi.eRunOpt.Backward;
PUB.AGV.AGVMoveRun(v_dir);
VAR.TIME.Update(eVarTime.SendGotoCommand);
}
return false;
}
}
// PUB.AGV.AGVMoveRun(v_dir);
// VAR.TIME.Update(eVarTime.SendGotoCommand);
// }
// return false;
// }
// }
//예측이 업데이트되지 않으면 오류 처리해야한다
if (BeforePredictIdx == -1) BeforePredictIdx = (int)predict.Idx;
else if (BeforePredictIdx != predict.Idx) //이전사용한 IDX와 다르다면 예측이 실행된 경우이다
BeforePredictIdx = (int)predict.Idx;
else
{
//5초이상 예측값이 업데이트되지 않으면 오류 처리한다.
var tsPredict = DateTime.Now - predict.CreateTime;
if (tsPredict.TotalSeconds > 5)
{
PUB.XBE.SendError(ENIGProtocol.AGVErrorCode.PredictFix, Lang.예측값이계산되지않아이동을중단합니다);
PUB.Speak(Lang.예측값이계산되지않아이동을중단합니다);
PUB.sm.SetNewRunStep(ERunStep.READY);
}
}
// //예측이 업데이트되지 않으면 오류 처리해야한다
// if (BeforePredictIdx == -1) BeforePredictIdx = (int)predict.Idx;
// else if (BeforePredictIdx != predict.Idx) //이전사용한 IDX와 다르다면 예측이 실행된 경우이다
// BeforePredictIdx = (int)predict.Idx;
// else
// {
// //5초이상 예측값이 업데이트되지 않으면 오류 처리한다.
// var tsPredict = DateTime.Now - predict.CreateTime;
// if (tsPredict.TotalSeconds > 5)
// {
// PUB.XBE.SendError(ENIGProtocol.AGVErrorCode.PredictFix, Lang.예측값이계산되지않아이동을중단합니다);
// PUB.Speak(Lang.예측값이계산되지않아이동을중단합니다);
// PUB.sm.SetNewRunStep(ERunStep.READY);
// }
// }
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
if (predict.ReasonCode == AGVControl.AGVActionReasonCode.Arrived)
{
PUB.Speak(Lang.목적지이동이완료되었습니다);
PUB.sm.SetNewRunStep(ERunStep.READY);
PUB.sm.UpdateRunStepSeq();
}
else if (predict.ReasonCode == AGVControl.AGVActionReasonCode.NeedTurnMove ||
predict.ReasonCode == AGVControl.AGVActionReasonCode.NeedTurnPoint)
{
//턴을 해야하는 경우이다
//좌턴을 기본으로 진행하며, 좌턴이동 후 마크스탑을 입력한다
if (GotoTurnStep == 0)
{
//턴을 한적이 없으므로 턴을 먼저 진행한다
arDev.Narumi.eMoveDir moveDir = arDev.Narumi.eMoveDir.Backward;
if (predict.Direction == AGVControl.AgvDir.Forward) moveDir = arDev.Narumi.eMoveDir.Forward;
if (PUB.AGV.data.Sts != 'L' || PUB.AGV.data.Speed != 'L' || PUB.AGV.data.Direction != moveDir.ToString()[0])
{
//셋팅이 다르다면 3초간격으로 전송한다
var tsTurnSet = DateTime.Now - GotoTurnSetTime;
if (tsTurnSet.TotalSeconds > 3)
{
PUB.AGV.AGVMoveSet(new arDev.Narumi.BunkiData
{
Bunki = arDev.Narumi.eBunki.Left,
Direction = moveDir,
PBSSensor = 1,
Speed = arDev.Narumi.eMoveSpd.Low,
});
GotoTurnSetTime = DateTime.Now;
PUB.log.Add("Turn Bunki Set");
}
}
else
{
PUB.mapctl.Manager.agv.CurrentRFID.TurnOK = false;
PUB.mapctl.Manager.agv.CurrentRFID.TurnStart = DateTime.Now;
PUB.sm.UpdateRunStepSeq(); //셋팅이 맞으니 다음스텝으로 진행한다
GotoTurnStep += 1;
}
}
}
else PUB.sm.UpdateRunStepSeq();
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
//턴이완료되길 기다린다.
if (predict.ReasonCode == AGVControl.AGVActionReasonCode.NeedTurnMove ||
predict.ReasonCode == AGVControl.AGVActionReasonCode.NeedTurnPoint)
{
//(최소5초는 기다리고 판단한다)
if (stepTime.TotalSeconds < 5) return false;
// return false;
//}
//else if (PUB.sm.RunStepSeq == idx++)
//{
// if (predict.ReasonCode == AGVControl.AGVActionReasonCode.Arrived)
// {
// PUB.Speak(Lang.목적지이동이완료되었습니다);
// PUB.sm.SetNewRunStep(ERunStep.READY);
// PUB.sm.UpdateRunStepSeq();
// }
// else if (predict.ReasonCode == AGVControl.AGVActionReasonCode.NeedTurnMove ||
// predict.ReasonCode == AGVControl.AGVActionReasonCode.NeedTurnPoint)
// {
// //턴을 해야하는 경우이다
// //좌턴을 기본으로 진행하며, 좌턴이동 후 마크스탑을 입력한다
// if (GotoTurnStep == 0)
// {
// //턴을 한적이 없으므로 턴을 먼저 진행한다
// arDev.Narumi.eMoveDir moveDir = arDev.Narumi.eMoveDir.Backward;
// if (predict.Direction == AGVControl.AgvDir.Forward) moveDir = arDev.Narumi.eMoveDir.Forward;
// if (PUB.AGV.data.Sts != 'L' || PUB.AGV.data.Speed != 'L' || PUB.AGV.data.Direction != moveDir.ToString()[0])
// {
// //셋팅이 다르다면 3초간격으로 전송한다
// var tsTurnSet = DateTime.Now - GotoTurnSetTime;
// if (tsTurnSet.TotalSeconds > 3)
// {
// PUB.AGV.AGVMoveSet(new arDev.Narumi.BunkiData
// {
// Bunki = arDev.Narumi.eBunki.Left,
// Direction = moveDir,
// PBSSensor = 1,
// Speed = arDev.Narumi.eMoveSpd.Low,
// });
// GotoTurnSetTime = DateTime.Now;
// PUB.log.Add("Turn Bunki Set");
// }
// }
// else
// {
// PUB.mapctl.Manager.agv.CurrentRFID.TurnOK = false;
// PUB.mapctl.Manager.agv.CurrentRFID.TurnStart = DateTime.Now;
// PUB.sm.UpdateRunStepSeq(); //셋팅이 맞으니 다음스텝으로 진행한다
// GotoTurnStep += 1;
// }
// }
// }
// else PUB.sm.UpdateRunStepSeq();
// return false;
//}
//else if (PUB.sm.RunStepSeq == idx++)
//{
// //턴이완료되길 기다린다.
// if (predict.ReasonCode == AGVControl.AGVActionReasonCode.NeedTurnMove ||
// predict.ReasonCode == AGVControl.AGVActionReasonCode.NeedTurnPoint)
// {
// //(최소5초는 기다리고 판단한다)
// if (stepTime.TotalSeconds < 5) return false;
//최대30초는 기다려준다
if (stepTime.TotalSeconds > 30)
{
var ermsg = "Turn Timeout(30sec)";
PUB.log.AddE(ermsg);
PUB.XBE.SendError(ENIGProtocol.AGVErrorCode.TurnTimeout, ermsg);
PUB.sm.SetNewRunStep(ERunStep.READY);
}
// //최대30초는 기다려준다
// if (stepTime.TotalSeconds > 30)
// {
// var ermsg = "Turn Timeout(30sec)";
// PUB.log.AddE(ermsg);
// PUB.XBE.SendError(ENIGProtocol.AGVErrorCode.TurnTimeout, ermsg);
// PUB.sm.SetNewRunStep(ERunStep.READY);
// }
//모션이 멈추었다면 턴이완료된것이다.
if (PUB.AGV.system1.agv_stop)
{
if (PUB.AGV.system1.Mark1_check == false && PUB.AGV.system1.Mark2_check == false)
{
PUB.log.AddE($"Turn 완료이나 Mark 센서가 확인되지 않았습니다");
}
GotoTurnStep += 1;
PUB.sm.UpdateRunStepSeq();
}
else
{
//아직 이동중이므로 대기한다
}
}
else PUB.sm.UpdateRunStepSeq(); //기타사항은 다음으로 넘어간다
return false;
}
else if (PUB.sm.RunStepSeq == idx++)
{
if (predict.ReasonCode == AGVControl.AGVActionReasonCode.NeedTurnMove ||
predict.ReasonCode == AGVControl.AGVActionReasonCode.NeedTurnPoint)
{
if (GotoTurnStep < 2)
{
PUB.XBE.SendError(ENIGProtocol.AGVErrorCode.TurnError, "턴시퀀스 완료 실패");
PUB.log.AddE($"턴완료시퀀스가 2가아닙니다. 대기 상태로 강제 전환합니다");
PUB.sm.SetNewRunStep(ERunStep.READY);
}
else
{
PUB.log.AddI("Turn Complete");
}
// //모션이 멈추었다면 턴이완료된것이다.
// if (PUB.AGV.system1.agv_stop)
// {
// if (PUB.AGV.system1.Mark1_check == false && PUB.AGV.system1.Mark2_check == false)
// {
// PUB.log.AddE($"Turn 완료이나 Mark 센서가 확인되지 않았습니다");
// }
// GotoTurnStep += 1;
// PUB.sm.UpdateRunStepSeq();
// }
// else
// {
// //아직 이동중이므로 대기한다
// }
// }
// else PUB.sm.UpdateRunStepSeq(); //기타사항은 다음으로 넘어간다
// return false;
//}
//else if (PUB.sm.RunStepSeq == idx++)
//{
// if (predict.ReasonCode == AGVControl.AGVActionReasonCode.NeedTurnMove ||
// predict.ReasonCode == AGVControl.AGVActionReasonCode.NeedTurnPoint)
// {
// if (GotoTurnStep < 2)
// {
// PUB.XBE.SendError(ENIGProtocol.AGVErrorCode.TurnError, "턴시퀀스 완료 실패");
// PUB.log.AddE($"턴완료시퀀스가 2가아닙니다. 대기 상태로 강제 전환합니다");
// PUB.sm.SetNewRunStep(ERunStep.READY);
// }
// else
// {
// PUB.log.AddI("Turn Complete");
// }
//방향전환용 턴이라면 이동기록을 추가해서 방향이 맞도록 처리해주자
if (predict.ReasonCode == AGVControl.AGVActionReasonCode.NeedTurnMove)
{
var rfid = PUB.mapctl.Manager.agv.CurrentRFID;
var lastHistory = PUB.mapctl.Manager.agv.MovementHistory.Last();
//원래방향에서 반대로 처리한다
var revDir = lastHistory.Direction == AGVControl.AgvDir.Backward ? AGVControl.AgvDir.Forward : AGVControl.AgvDir.Backward;
PUB.mapctl.Manager.agv.AddToMovementHistory(rfid.Value, rfid.Location, revDir);
}
else
{
//이동용 RFID에서 턴명령이 들어있는경우였다
PUB.mapctl.Manager.agv.CurrentRFID.TurnEnd = DateTime.Now;
PUB.mapctl.Manager.agv.CurrentRFID.TurnOK = true;
}
// //방향전환용 턴이라면 이동기록을 추가해서 방향이 맞도록 처리해주자
// if (predict.ReasonCode == AGVControl.AGVActionReasonCode.NeedTurnMove)
// {
// var rfid = PUB.mapctl.Manager.agv.CurrentRFID;
// var lastHistory = PUB.mapctl.Manager.agv.MovementHistory.Last();
// //원래방향에서 반대로 처리한다
// var revDir = lastHistory.Direction == AGVControl.AgvDir.Backward ? AGVControl.AgvDir.Forward : AGVControl.AgvDir.Backward;
// PUB.mapctl.Manager.agv.AddToMovementHistory(rfid.Value, rfid.Location, revDir);
// }
// else
// {
// //이동용 RFID에서 턴명령이 들어있는경우였다
// PUB.mapctl.Manager.agv.CurrentRFID.TurnEnd = DateTime.Now;
// PUB.mapctl.Manager.agv.CurrentRFID.TurnOK = true;
// }
PUB.sm.UpdateRunStepSeq();
}
else PUB.sm.UpdateRunStepSeq();
return false;
}
// PUB.sm.UpdateRunStepSeq();
// }
// else PUB.sm.UpdateRunStepSeq();
// return false;
//}
//좌턴이동명령 전송