244 lines
10 KiB
C#
244 lines
10 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.Drawing;
|
|
using System.Linq;
|
|
using System.Text;
|
|
using Project.StateMachine;
|
|
using COMM;
|
|
using AR;
|
|
using AGVNavigationCore.Models;
|
|
|
|
namespace Project
|
|
{
|
|
public partial class fMain
|
|
{
|
|
/// <summary>
|
|
/// 장비엣 빠젼나온다.
|
|
/// </summary>
|
|
public Boolean _SM_RUN_EXIT(bool isFirst, TimeSpan seqtime)
|
|
{
|
|
var idx = 1;
|
|
var funcname = $"[EXIT-{PUB.sm.RunStep}]";
|
|
|
|
//충전 상태가 OFF되어야 동작하게한다
|
|
if (_SM_RUN_CHARGE_OFF(isFirst, seqtime) == false) return false;
|
|
|
|
//라이더멈춤이 설정되어있다면 음성으로 알려준다
|
|
if (CheckLiderStop() == false) return false;
|
|
|
|
if (PUB.sm.RunStepSeq == idx++)
|
|
{
|
|
PUB.log.Add($"[{funcname}-{PUB.sm.RunStepSeq}] OUT작업-시작");
|
|
PUB.Speak("작업을 시작합니다(안전을위해 AGVSTOP신호를 전송합니다)");
|
|
PUB.AGV.AGVMoveStop(funcname);
|
|
VAR.I32[eVarInt32.RetryLift] = 0;
|
|
PUB.sm.UpdateRunStepSeq();
|
|
return false;
|
|
}
|
|
else if (PUB.sm.RunStepSeq == idx++)
|
|
{
|
|
//작업형태에 따라서. 리프트를 제어한다.
|
|
var liftCmd = LiftCommand.DN;
|
|
if (PUB.NextWorkCmd == ENIGProtocol.AGVCommandHE.PickOnExit) liftCmd = LiftCommand.UP;
|
|
VAR.I32[eVarInt32.RetryLift] += 1;
|
|
PUB.AGV.LiftControl(liftCmd);
|
|
PUB.sm.UpdateRunStepSeq();
|
|
return false;
|
|
}
|
|
else if (PUB.sm.RunStepSeq == idx++)
|
|
{
|
|
var liftCmd = LiftCommand.DN;
|
|
if (PUB.NextWorkCmd == ENIGProtocol.AGVCommandHE.PickOnExit) liftCmd = 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 == LiftCommand.DN && liftdnok)
|
|
{
|
|
//정상조건
|
|
}
|
|
else if (liftCmd == LiftCommand.UP && liftdnok)
|
|
{
|
|
//정상조건
|
|
}
|
|
else
|
|
{
|
|
if (seqtime.TotalSeconds > 20)
|
|
{
|
|
if (VAR.I32[eVarInt32.RetryLift] < 3)
|
|
{
|
|
PUB.log.AddAT($"[{funcname}-{PUB.sm.RunStepSeq}] 리프트가 동작({liftCmd})하지 않아 재시도 합니다");
|
|
PUB.sm.UpdateRunStepSeq(-1);
|
|
}
|
|
else
|
|
{
|
|
SetRunStepError(ENIGProtocol.AGVErrorCode.LIFT_ERROR,
|
|
$"[{funcname}-{PUB.sm.RunStepSeq}] 리프트가 동작({liftCmd})하지 않습니다");
|
|
VAR.I32[eVarInt32.RetryLift] = 0;
|
|
}
|
|
}
|
|
else PUB._mapCanvas.SetAlertMessage($"리프트 하강 확인 중({seqtime.TotalSeconds:N0}/20)");
|
|
return false;
|
|
}
|
|
VAR.I32[eVarInt32.RetryMoveset] = 0;
|
|
PUB.log.Add($"[{funcname}-{PUB.sm.RunStepSeq}] 리프트 동작 확인 완료");
|
|
PUB.sm.UpdateRunStepSeq();
|
|
return false;
|
|
}
|
|
else if (PUB.sm.RunStepSeq == idx++)
|
|
{
|
|
//라이더 끈상태로 빠져나와야 한다
|
|
VAR.I32[eVarInt32.RetryMoveset] += 1;
|
|
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)
|
|
PUB.log.AddAT($"[{funcname}-{PUB.sm.RunStepSeq}] moveset return fail:{ret}");
|
|
|
|
PUB.sm.UpdateRunStepSeq();
|
|
return false;
|
|
}
|
|
else if (PUB.sm.RunStepSeq == idx++)
|
|
{
|
|
//moveset 결과확인
|
|
if (PUB.AGV.data.Speed != 'L' ||
|
|
PUB.AGV.data.Sts != 'S' ||
|
|
PUB.AGV.data.Direction != 'F')
|
|
{
|
|
if (seqtime.TotalSeconds > 5)
|
|
{
|
|
if (VAR.I32[eVarInt32.RetryMoveset] < 3)
|
|
{
|
|
PUB.log.AddAT($"[{funcname}-{PUB.sm.RunStepSeq}] MoveSet이 확인되지 않아 재시도 합니다");
|
|
PUB.sm.UpdateRunStepSeq(-1);
|
|
}
|
|
else
|
|
{
|
|
SetRunStepError(ENIGProtocol.AGVErrorCode.AGV_SPEED_SET_FAIL,
|
|
$"[{funcname}-{PUB.sm.RunStepSeq}] MoveSet 실패");
|
|
VAR.I32[eVarInt32.RetryMoveset] = 0;
|
|
}
|
|
}
|
|
else PUB._mapCanvas.SetAlertMessage($"이동설정 확인 중({seqtime.TotalSeconds:N0}/20)");
|
|
return false;
|
|
}
|
|
VAR.I32[eVarInt32.RetryMove] = 0;
|
|
PUB.sm.UpdateRunStepSeq();
|
|
return false;
|
|
}
|
|
else if (PUB.sm.RunStepSeq == idx++)
|
|
{
|
|
//전진이동
|
|
PUB.log.Add($"[{funcname}-{PUB.sm.RunStepSeq}] 전진이동");
|
|
VAR.I32[eVarInt32.RetryMove] += 1;
|
|
PUB.AGV.AGVMoveRun(arDev.Narumi.eRunOpt.Forward);
|
|
PUB.sm.UpdateRunStepSeq();
|
|
return false;
|
|
}
|
|
else if (PUB.sm.RunStepSeq == idx++)
|
|
{
|
|
//AGV구동을 확인하고 마크스탑을 설정한다.
|
|
if (PUB.AGV.system1.agv_run == false)
|
|
{
|
|
if (seqtime.TotalSeconds > 3)
|
|
{
|
|
if (VAR.I32[eVarInt32.RetryMove] < 3)
|
|
{
|
|
PUB.log.AddAT($"[{funcname}-{PUB.sm.RunStepSeq}] AGV전진이 확인되지 않아 재시도 합니다");
|
|
PUB.sm.UpdateRunStepSeq(-1);
|
|
}
|
|
else
|
|
{
|
|
SetRunStepError(ENIGProtocol.AGVErrorCode.AGV_RUN_FAIL,
|
|
$"[{funcname}-{PUB.sm.RunStepSeq}] agv 구동확인 안됨");
|
|
VAR.I32[eVarInt32.RetryMove] = 0;
|
|
}
|
|
}
|
|
else PUB._mapCanvas.SetAlertMessage($"AGV RUN 확인 중({seqtime.TotalSeconds:N0}/20)");
|
|
return false;
|
|
}
|
|
|
|
//마크스탑설정
|
|
VAR.I32[eVarInt32.RetryMarkStop] = 0;
|
|
PUB.sm.UpdateRunStepSeq();
|
|
return false;
|
|
}
|
|
else if (PUB.sm.RunStepSeq == idx++)
|
|
{
|
|
PUB.log.Add($"[{funcname}-{PUB.sm.RunStepSeq}] 마크스탑신호를 전달합니다");
|
|
VAR.I32[eVarInt32.RetryMarkStop] += 1;
|
|
PUB.AGV.AGVMoveStop(funcname, arDev.Narumi.eStopOpt.MarkStop);
|
|
PUB.sm.UpdateRunStepSeq();
|
|
return false;
|
|
}
|
|
else if (PUB.sm.RunStepSeq == idx++)
|
|
{
|
|
//마크스탑신호가 3초이내로 들어와야 한다
|
|
if (PUB.AGV.data.Speed != 'S')
|
|
{
|
|
if (seqtime.TotalSeconds > 20)
|
|
{
|
|
if (VAR.I32[eVarInt32.RetryMarkStop] < 3)
|
|
{
|
|
PUB.log.AddAT($"[{funcname}-{PUB.sm.RunStepSeq}] 마크스탑이 확인되지 않아 재시도 합니다");
|
|
PUB.sm.UpdateRunStepSeq(-1);
|
|
}
|
|
else
|
|
{
|
|
SetRunStepError(ENIGProtocol.AGVErrorCode.MARK_STOP_FAIL,
|
|
$"[{funcname}-{PUB.sm.RunStepSeq}] 마크스탑이 확인되지 않습니다");
|
|
VAR.I32[eVarInt32.RetryMarkStop] = 0;
|
|
}
|
|
}
|
|
else PUB._mapCanvas.SetAlertMessage($"리프트 하강 확인 중({seqtime.TotalSeconds:N0}/20)");
|
|
return false;
|
|
}
|
|
PUB.sm.UpdateRunStepSeq();
|
|
return false;
|
|
}
|
|
else if (PUB.sm.RunStepSeq == idx++)
|
|
{
|
|
//AGV가 멈출때까지 기다린다.
|
|
if (PUB.AGV.system1.agv_run == true)
|
|
{
|
|
if (seqtime.TotalSeconds > 10)
|
|
{
|
|
SetRunStepError(ENIGProtocol.AGVErrorCode.AGV_STOP_FAIL);
|
|
}
|
|
return false;
|
|
}
|
|
PUB.sm.UpdateRunStepSeq();
|
|
return false;
|
|
}
|
|
else if (PUB.sm.RunStepSeq == idx++)
|
|
{
|
|
//마크센서입력을 확인한다.
|
|
if (PUB.AGV.signal1.mark_sensor == false)
|
|
{
|
|
if (seqtime.TotalSeconds > 5)
|
|
{
|
|
SetRunStepError(ENIGProtocol.AGVErrorCode.MARK_SENSOR_FAIL);
|
|
}
|
|
return false;
|
|
}
|
|
PUB.sm.UpdateRunStepSeq();
|
|
return false;
|
|
}
|
|
else if (PUB.sm.RunStepSeq == idx++)
|
|
{
|
|
//완료되었다.
|
|
PUB.log.Add("진출 완료");
|
|
PUB.sm.UpdateRunStepSeq();
|
|
return false;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
}
|
|
}
|