540 lines
24 KiB
C#
540 lines
24 KiB
C#
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
|
|
{
|
|
DateTime LastSpeakTime = DateTime.Now;
|
|
DateTime CoverControlTime = DateTime.Now;
|
|
DateTime LastCommandTime = DateTime.Now;
|
|
DateTime LastCommandTimeNextStop = DateTime.Now;
|
|
bool runStepisFirst = false;
|
|
|
|
private void _SM_RUN(Boolean isFirst, TimeSpan stepTime)
|
|
{
|
|
//중단기능이 동작이라면 처리하지 않는다.
|
|
if (PUB.sm.bPause)
|
|
{
|
|
System.Threading.Thread.Sleep(200);
|
|
return;
|
|
}
|
|
|
|
//가동불가 조건 확인
|
|
if (CheckStopCondition() == false)
|
|
{
|
|
PUB.sm.SetNewStep(eSMStep.PAUSE);
|
|
return;
|
|
}
|
|
|
|
//이머전시상태라면 stop 처리한다.
|
|
if (PUB.AGV.error.Emergency && PUB.AGV.system1.agv_stop == true &&
|
|
PUB.AGV.system1.stop_by_front_detect == false)
|
|
{
|
|
PUB.Speak(Lang.비상정지로인해작업을중단합니다);
|
|
PUB.sm.SetNewStep(eSMStep.IDLE);
|
|
}
|
|
|
|
//스텝이 변경되었다면?
|
|
if (PUB.sm.RunStep != PUB.sm.RunStepNew)
|
|
{
|
|
runStepisFirst = true;
|
|
PUB.sm.ApplyRunStep();
|
|
}
|
|
else runStepisFirst = false;
|
|
|
|
if (isFirst)
|
|
{
|
|
if (PUB.sm.RunStep == ERunStep.READY)
|
|
VAR.TIME.Update(eVarTime.ReadyStart);
|
|
else
|
|
VAR.TIME.Update(eVarTime.RunStart);
|
|
}
|
|
|
|
switch (PUB.sm.RunStep)
|
|
{
|
|
case ERunStep.READY:
|
|
_SM_RUN_READY(runStepisFirst, PUB.sm.GetRunSteptime);
|
|
break;
|
|
|
|
case ERunStep.GOCHARGE: //충전위치로 이동
|
|
if (runStepisFirst)
|
|
{
|
|
VAR.TIME[eVarTime.ChargeTry] = DateTime.Now;
|
|
PUB.sm.ResetRunStepSeq();
|
|
PUB.log.Add("충전 명령 시작");
|
|
}
|
|
else if (_SM_RUN_GOCHARGE(runStepisFirst, PUB.sm.GetRunSteptime))
|
|
{
|
|
//230601
|
|
//if (PUB.Result != null && PUB.sm != null)
|
|
// EEMStatus.AddEEDBSQL(PUB.sm.Step, PUB.sm.RunStep.ToString(), PUB.Result.TargetPos.ToString());
|
|
|
|
PUB.Speak(Lang.충전을시작합니다);
|
|
PUB.sm.SetNewRunStep(ERunStep.CHARGECHECK);
|
|
return;
|
|
}
|
|
break;
|
|
|
|
case ERunStep.CHARGECHECK: //충전중
|
|
if (runStepisFirst)
|
|
{
|
|
VAR.TIME.Update(eVarTime.ChargeStart);
|
|
LastCommandTime = DateTime.Now;
|
|
}
|
|
else if (_SM_RUN_GOCHARGECHECK(runStepisFirst, PUB.sm.GetRunSteptime))
|
|
{
|
|
//충전상태가 활성화되었으므로 대기상태로 전환한다
|
|
PUB.sm.SetNewRunStep(ERunStep.READY);
|
|
PUB.log.AddAT("충전상태 확인 완료로 인해 대기 합니다");
|
|
}
|
|
break;
|
|
|
|
case ERunStep.CHARGEOFF:
|
|
if (runStepisFirst)
|
|
{
|
|
VAR.TIME.Update(eVarTime.ChargeEnd);
|
|
LastCommandTime = DateTime.Now;
|
|
}
|
|
else
|
|
{
|
|
//충전 상태가 OFF되어야 동작하게한다
|
|
if (_SM_RUN_CHGOFF(isFirst, stepTime) == true)
|
|
{
|
|
//충전상태가 활성화되었으므로 대기상태로 전환한다
|
|
PUB.sm.ClearRunStep();
|
|
|
|
//충전기위치에서 OFF를 한 경우이다
|
|
if (PUB.Result.CurrentPos != ePosition.CHARGE || PUB.Result.TargetPos != ePosition.CHARGE)
|
|
{
|
|
PUB.Result.CurrentPos = ePosition.NONE;
|
|
}
|
|
else
|
|
{
|
|
PUB.Result.TargetPos = ePosition.QC;
|
|
}
|
|
PUB.Result.CurrentPosCW = "1";
|
|
PUB.sm.SetNewRunStep(ERunStep.GOHOME);
|
|
PUB.log.AddAT("충전 해제로 홈으로 이동");
|
|
}
|
|
}
|
|
break;
|
|
case ERunStep.GOUP: //상차이동
|
|
if (_SM_RUN_GOUP(runStepisFirst, PUB.sm.GetRunSteptime))
|
|
{
|
|
PUB.Speak(Lang.상차작업이완료되었습니다);
|
|
|
|
//230601
|
|
// if (PUB.Result != null && PUB.sm != null)
|
|
// EEMStatus.AddEEDBSQL(PUB.sm.Step, PUB.sm.RunStep.ToString(), PUB.Result.TargetPos.ToString());
|
|
|
|
//QA를 제외한 경우에는 기본 QC로 이동한다
|
|
if (PUB.Result.NextPos == ePosition.QA)
|
|
PUB.Result.TargetPos = ePosition.QA;
|
|
else
|
|
PUB.Result.TargetPos = ePosition.QC;
|
|
|
|
|
|
PUB.sm.SetNewRunStep(ERunStep.GODOWN); //하차작업으로 전환
|
|
return;
|
|
}
|
|
break;
|
|
|
|
case ERunStep.GODOWN: //하차이동
|
|
if (_SM_RUN_GODOWN(runStepisFirst, PUB.sm.GetRunSteptime))
|
|
{
|
|
PUB.Speak(Lang.하차작업이완료되었습니다);
|
|
VAR.TIME.Update(eVarTime.ChargeTry);
|
|
PUB.AGV.LiftControl(arDev.Narumi.LiftCommand.DN);// (Device.PLC.ZMotDirection.Down); //하차작업이 완료되면 커버를 내려서 바로 작업할 수 있게 한다.
|
|
|
|
//230601
|
|
//if (PUB.Result != null && PUB.sm != null)
|
|
// EEMStatus.AddEEDBSQL(PUB.sm.Step, PUB.sm.RunStep.ToString(), PUB.Result.TargetPos.ToString());
|
|
|
|
//하차가 완료되면 충전대기시간을 30초 남겨두고 없데이트한다
|
|
//충전이 필요할 경우 바로 될수있도록 220118
|
|
VAR.TIME[eVarTime.ChargeTry] = DateTime.Now.AddSeconds(-1 * PUB.setting.ChargeRetryTerm + 30);
|
|
|
|
if (PUB.Result.CurrentPos == ePosition.QC)
|
|
{
|
|
PUB.Speak(Lang.대기상태로전환합니다);
|
|
PUB.sm.SetNewRunStep(ERunStep.READY);
|
|
}
|
|
else
|
|
{
|
|
//홈 이동 명령처리
|
|
PUB.sm.ClearRunStep();
|
|
PUB.Result.TargetPos = ePosition.QC;
|
|
PUB.sm.SetNewRunStep(ERunStep.GOHOME);
|
|
PUB.Speak(Lang.홈위치로이동합니다);
|
|
}
|
|
return;
|
|
}
|
|
break;
|
|
|
|
case ERunStep.GOHOME:
|
|
if (runStepisFirst)
|
|
{
|
|
PUB.Speak(Lang.홈검색을시작합니다);
|
|
}
|
|
else if (_SM_RUN_GOHOME(runStepisFirst, PUB.sm.GetRunSteptime))
|
|
{
|
|
//230601
|
|
// if (PUB.Result != null && PUB.sm != null)
|
|
// EEMStatus.AddEEDBSQL(PUB.sm.Step, PUB.sm.RunStep.ToString(), PUB.Result.TargetPos.ToString());
|
|
|
|
PUB.Speak(Lang.홈이동완료대기상태로전환합니다);
|
|
VAR.TIME.Update(eVarTime.ChargeTry);
|
|
PUB.sm.SetNewRunStep(ERunStep.READY); //대기상태로 전환한다
|
|
return;
|
|
}
|
|
break;
|
|
}
|
|
}
|
|
|
|
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
|
|
}
|