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;
//}
//좌턴이동명령 전송

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

@@ -56,33 +56,33 @@ namespace Project
VAR.BOOL[eVarBool.AGV_ERROR] = PUB.AGV.error.Value > 0;
VAR.BOOL[eVarBool.EMERGENCY] = PUB.AGV.error.Emergency;
//모터방향 입력
if (PUB.AGV.data.Direction == 'B')
PUB.mapctl.Manager.agv.Current_Motor_Direction = AGVControl.AgvDir.Backward;
else
PUB.mapctl.Manager.agv.Current_Motor_Direction = AGVControl.AgvDir.Forward;
////모터방향 입력
//if (PUB.AGV.data.Direction == 'B')
// PUB.mapctl.Manager.agv.Current_Motor_Direction = AGVControl.AgvDir.Backward;
//else
// PUB.mapctl.Manager.agv.Current_Motor_Direction = AGVControl.AgvDir.Forward;
//현재 속도
if (PUB.AGV.data.Speed == 'H')
PUB.mapctl.Manager.agv.CurrentSpeed = AGVControl.AgvSpeed.High;
else if (PUB.AGV.data.Speed == 'M')
PUB.mapctl.Manager.agv.CurrentSpeed = AGVControl.AgvSpeed.Middle;
else if (PUB.AGV.data.Speed == 'L')
PUB.mapctl.Manager.agv.CurrentSpeed = AGVControl.AgvSpeed.Low;
else if (PUB.AGV.data.Speed == 'S')
PUB.mapctl.Manager.agv.CurrentSpeed = AGVControl.AgvSpeed.MarkStop;
////현재 속도
//if (PUB.AGV.data.Speed == 'H')
// PUB.mapctl.Manager.agv.CurrentSpeed = AGVControl.AgvSpeed.High;
//else if (PUB.AGV.data.Speed == 'M')
// PUB.mapctl.Manager.agv.CurrentSpeed = AGVControl.AgvSpeed.Middle;
//else if (PUB.AGV.data.Speed == 'L')
// PUB.mapctl.Manager.agv.CurrentSpeed = AGVControl.AgvSpeed.Low;
//else if (PUB.AGV.data.Speed == 'S')
// PUB.mapctl.Manager.agv.CurrentSpeed = AGVControl.AgvSpeed.MarkStop;
//이동방향
if (PUB.AGV.data.Sts == 'S')
PUB.mapctl.Manager.agv.CurrentSTS = AGVControl.AgvSts.Straight;
else if (PUB.AGV.data.Sts == 'L')
PUB.mapctl.Manager.agv.CurrentSTS = AGVControl.AgvSts.Left;
else if (PUB.AGV.data.Sts == 'R')
PUB.mapctl.Manager.agv.CurrentSTS = AGVControl.AgvSts.Right;
////이동방향
//if (PUB.AGV.data.Sts == 'S')
// PUB.mapctl.Manager.agv.CurrentSTS = AGVControl.AgvSts.Straight;
//else if (PUB.AGV.data.Sts == 'L')
// PUB.mapctl.Manager.agv.CurrentSTS = AGVControl.AgvSts.Left;
//else if (PUB.AGV.data.Sts == 'R')
// PUB.mapctl.Manager.agv.CurrentSTS = AGVControl.AgvSts.Right;
PUB.mapctl.Manager.agv.IsMoving = PUB.AGV.system1.agv_run;
PUB.mapctl.Manager.agv.IsMarkCheck = PUB.AGV.system1.Mark1_check || PUB.AGV.system1.Mark2_check;
//PUB.mapctl.Manager.agv.IsMoving = PUB.AGV.system1.agv_run;
//PUB.mapctl.Manager.agv.IsMarkCheck = PUB.AGV.system1.Mark1_check || PUB.AGV.system1.Mark2_check;
if (PUB.AGV.signal.mark_sensor == false)
{
@@ -184,41 +184,18 @@ namespace Project
PUB.log.AddE(logEMsg);
}
//맵데이터에서 현재 위치를 찾는다
if (PUB.mapctl.SetCurrentPosition(PUB.AGV.data.TagNo) == false)
//virtual agv setting
var CurrentNode = PUB._mapNodes.FirstOrDefault(t => t.RfidId.Equals(PUB.Result.LastTAG, StringComparison.OrdinalIgnoreCase));
if (CurrentNode == null)
{
if (VAR.BOOL[eVarBool.FLAG_AUTORUN] && PUB.AGV.system1.agv_run)
PUB.AGV.AGVMoveStop("unknown tag no");
//존재하지 않는 태그가 읽히면 관련 오류를 표시한다.
}
else
{
//위치는 찾았다 해당 위치가 내 목적지라면 mark stop기능으로 전환한다
PUB.log.AddE($"RFID:{PUB.Result.LastTAG} 의 노드를 찾을 수 없습니다");
return;
}
////자동, 상하차 모드일때 RFID 가 타겟위치에 올때는 - 멈춤을 설정해준다
//if (VAR.BOOL[eVarBool.FLAG_AUTORUN] == true &&
// PUB.Result.CurrentPos == PUB.Result.TargetPos &&
// PUB.Result.TargetPos != ePosition.NONE &&
// (PUB.sm.RunStep == ERunStep.GODOWN ||
// PUB.sm.RunStep == ERunStep.GOUP ||
// PUB.sm.RunStep == ERunStep.GOHOME ||
// PUB.sm.RunStep == ERunStep.GOCHARGE))
//{
// if (PUB.AGV.data.Sts == 'F' && dirForward == "0") //아래로 내려오고있음
// {
// PUB.AGV.AGVMoveStop("AGV_DataReceive", arDev.Narumi.eStopOpt.MarkStop);
// PUB.Speak( Lang.다음마크위치에서정지합니다);
// }
// else if (PUB.AGV.data.Sts == 'B' && dirForward == "1")
// {
// //VAR.BOOL[eVarBool.FLAG_NEXTSTOP_MARK] = true;
// PUB.AGV.AGVMoveStop("AGV_DataReceive", arDev.Narumi.eStopOpt.MarkStop);
// PUB.Speak(Lang.다음마크위치에서정지합니다);
// }
//}
//모터방향 확인해서 UI와 AGV클래스에 적용한다
var MotDireciton = PUB.AGV.data.Direction == 'B' ? AGVNavigationCore.Models.AgvDirection.Backward : AGVNavigationCore.Models.AgvDirection.Forward;
PUB._virtualAGV.SetPosition(CurrentNode, MotDireciton);
PUB._mapCanvas.SetAGVPosition("AGV", CurrentNode, MotDireciton);
}
break;
case arDev.Narumi.DataType.ACK:
@@ -231,12 +208,8 @@ namespace Project
}
//이 후 상황을 예측한다
if (PUB.mapctl != null)
{
var rlt = PUB.mapctl.Manager.PredictNextAction();
if (rlt.Changed)
Console.WriteLine($"[new] predict idx:{rlt.Idx}");
}
var command = PUB._virtualAGV.Predict();
var preditMSG = $"Motor:{command.Motor},Magnet:{command.Magnet},Speed:{command.Speed} : {command.Reason}";
}
catch (Exception ex)
{

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

@@ -25,7 +25,7 @@ namespace Project
DateTime lastbmstime = DateTime.Now;
private void Bms_Message(object sender, arDev.BMS.MessageEventArgs e)
{
if (e.MsgType == arRS232.MessageType.Error) PUB.logbms.AddE( e.Message);
if (e.MsgType == arDev.arRS232.MessageType.Error) PUB.logbms.AddE( e.Message);
else
{
var hexstr = e.Data.GetHexString().Trim();
@@ -159,9 +159,9 @@ namespace Project
private void Bms_BMSDataReceive(object sender, EventArgs e)
{
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;
//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;
if (PUB.BMS.Current_Level <= PUB.setting.ChargeStartLevel)
{
//배터리 레벨이 기준보다 낮다면 경고를 활성화 한다
@@ -180,6 +180,11 @@ namespace Project
PUB.log.AddAT("배터리 부족 경고 비활성화");
}
}
EEMStatus.MakeBMSInformation_INFO();
}
private void BMS_BMSCellDataReceive(object sender, arDev.BMSCelvoltageEventArgs e)
{
EEMStatus.MakeBMSInformation_Cell();
}
}
}

9
Cs_HMI/Project/StateMachine/_SPS.cs
View File

@@ -117,10 +117,6 @@ namespace Project
Console.WriteLine($"bms connect to {PUB.setting.Port_BAT}");
PUB.BMS.PortName = PUB.setting.Port_BAT;
PUB.BMS.Open();
PUB.BMS.BMSDataReceive += Bms_BMSDataReceive;
PUB.BMS.Message += Bms_Message;
PUB.BMS.ChargeDetect += BMS_ChargeDetect;
VAR.TIME.Update(eVarTime.LastConn_BAT);
VAR.TIME.Update(eVarTime.LastConnTry_BAT);
@@ -132,9 +128,6 @@ namespace Project
if (ts.TotalSeconds > 10)
{
Console.WriteLine("bms auto disconnect");
PUB.BMS.BMSDataReceive -= Bms_BMSDataReceive;
PUB.BMS.Message -= Bms_Message;
PUB.BMS.ChargeDetect -= BMS_ChargeDetect;
PUB.BMS.Close();
VAR.TIME.Set(eVarTime.LastConn_BAT,DateTime.Now.AddSeconds(5));
}
@@ -159,7 +152,7 @@ namespace Project
{
if (PUB.BMS.lastSendTime.Year == 1982) PUB.BMS.lastSendTime = DateTime.Now.AddSeconds(1);
var ts = DateTime.Now - PUB.BMS.lastSendTime;
if (ts.TotalMilliseconds >= PUB.setting.interval_bms)
if (ts.TotalSeconds >= PUB.setting.interval_bms)
{
PUB.BMS.SendQuery();
}

3
Cs_HMI/Project/StateMachine/_TMDisplay.cs
View File

@@ -65,9 +65,6 @@ namespace Project
lbBat.CurA = PUB.BMS.Current_Amp;
lbBat.IsOpen = PUB.BMS.IsOpen;
if (PUB.mapctl != null)
PUB.mapctl.Invalidate();
//쓰레드로인해서 메인에서 진행하게한다. SPS는 메인쓰레드에서 진행 됨
//팝을 제거 혹은 표시하는 기능
if (PUB.popup.needShow) PUB.popup.showMessage();

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

@@ -60,11 +60,11 @@ namespace Project
{
if (ushort.TryParse(targstr, out ushort tagno))
{
if (PUB.mapctl.SetCurrentPosition(tagno) == true)
{
PUB.log.AddI($"Set Position:{tagno}");
}
else PUB.log.AddE($"Position Set Error:{tagno}");
//if (PUB.mapctl.SetCurrentPosition(tagno) == true)
//{
// PUB.log.AddI($"Set Position:{tagno}");
//}
//else PUB.log.AddE($"Position Set Error:{tagno}");
}
else PUB.log.AddE($"Position Param(tagstr) Error:{dataStr}");
}
@@ -79,11 +79,11 @@ namespace Project
case ENIGProtocol.AGVCommandHE.Goto: //move to tag
if (uint.TryParse(dataStr, out uint tagno2))
{
var currPos = PUB.mapctl.Manager.agv.CurrentRFID;///.AGVMoveToRFID(;
if (PUB.mapctl.SetTargetPosition(tagno2))
PUB.log.AddI($"New Target {tagno2}");
else
PUB.log.AddE($"Path Error {tagno2}");
//var currPos = PUB.mapctl.Manager.agv.CurrentRFID;///.AGVMoveToRFID(;
//if (PUB.mapctl.SetTargetPosition(tagno2))
// PUB.log.AddI($"New Target {tagno2}");
//else
// PUB.log.AddE($"Path Error {tagno2}");
}
else PUB.log.AddE($"Path Param Error :{dataStr}");
break;