123 lines
4.4 KiB
C#
123 lines
4.4 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
|
|
{
|
|
byte GotoTurnStep = 0;
|
|
DateTime GotoTurnSetTime = DateTime.Now;
|
|
public Boolean _SM_RUN_GOTO(bool isFirst, TimeSpan seqtime)
|
|
{
|
|
///명령어 재전송 간격(기본 2초)
|
|
var CommandInterval = 2;
|
|
var funcname = "_SM_RUN_GOTO";
|
|
|
|
//충전 상태가 OFF되어야 동작하게한다
|
|
if (_SM_RUN_CHARGE_GOFF(isFirst, seqtime) == false)
|
|
return false;
|
|
|
|
//최초시작이라면 시간변수 초기화
|
|
if (isFirst)
|
|
{
|
|
PUB.log.Add($"[>>] _SM_RUN_GOTO");
|
|
VAR.TIME.Update(eVarTime.CheckGotoTargetSet);
|
|
}
|
|
|
|
//라이더멈춤이 설정되어있다면 음성으로 알려준다 200409
|
|
if (PUB.AGV.system1.stop_by_front_detect == true)
|
|
{
|
|
var tsSpeak = DateTime.Now - LastSpeakTime;
|
|
if (tsSpeak.TotalSeconds >= PUB.setting.alarmSoundTerm)
|
|
{
|
|
PUB.Speak(Lang.전방에물체가감지되었습니다);
|
|
LastSpeakTime = DateTime.Now;
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
var idx = 1;
|
|
if (PUB.sm.RunStepSeq == idx++)
|
|
{
|
|
SpeedSetRetry = false;
|
|
PUB.sm.UpdateRunStepSeq();
|
|
return false;
|
|
}
|
|
else if (PUB.sm.RunStepSeq == idx++)
|
|
{
|
|
////빈 상태로 아웃해야한다.
|
|
//if (SpeedSetRetry)
|
|
//{
|
|
// if (seqtime.TotalSeconds < 2)
|
|
// return false;
|
|
//}
|
|
//var ret = PUB.AGV.AGVMoveSet(new arDev.Narumi.BunkiData
|
|
//{
|
|
// Bunki = arDev.Narumi.eBunki.Strate,
|
|
// Direction = arDev.Narumi.eMoveDir.Forward,
|
|
// PBSSensor = 2,
|
|
// Speed = arDev.Narumi.eMoveSpd.Low,
|
|
//});
|
|
////명령이 실패되었다면 재시도를 한다
|
|
//if (ret != arDev.eNarumiCommandResult.Success)
|
|
//{
|
|
// if (SpeedSetRetry == true || ret >= arDev.eNarumiCommandResult.Error)
|
|
// {
|
|
// SetRunStepError(ENIGProtocol.AGVErrorCode.AGV_SPEED_SET_FAIL);
|
|
// }
|
|
// else SpeedSetRetry = true;
|
|
// return false;
|
|
//}
|
|
|
|
|
|
PUB.sm.UpdateRunStepSeq();
|
|
return false;
|
|
}
|
|
else if (PUB.sm.RunStepSeq == idx++)
|
|
{
|
|
if (PUB._virtualAGV.TargetNode == null)
|
|
{
|
|
PUB.log.Add($"대상노드가 없어 이동을할 수 없습니다");
|
|
PUB.sm.SetNewRunStep(ERunStep.READY);
|
|
return false;
|
|
}
|
|
PUB.log.Add($"[GOTO] Step {idx - 1}: TargetNode Checked ({PUB._virtualAGV.TargetNode.ID2})");
|
|
PUB.sm.UpdateRunStepSeq();
|
|
return false;
|
|
}
|
|
else if (PUB.sm.RunStepSeq == idx++)
|
|
{
|
|
//모션 전후진 제어
|
|
if (UpdateMotionPositionForMark(funcname))
|
|
{
|
|
PUB.log.Add($"[GOTO] Step {idx - 1}: UpdateMotionPositionForMark Completed. Stopping AGV.");
|
|
PUB.AGV.AGVMoveStop(funcname);
|
|
PUB.sm.UpdateRunStepSeq();
|
|
}
|
|
return false;
|
|
}
|
|
else if (PUB.sm.RunStepSeq == idx++)
|
|
{
|
|
//QC까지 모두 완료되었다.(완전히 정차할때까지 기다린다)
|
|
PUB.log.Add($"[GOTO] Step {idx - 1}: Movement Finished. Waiting for full stop.");
|
|
PUB.Speak(Lang.이동완료, true);
|
|
PUB.AddEEDB($"이동완료({PUB._virtualAGV.TargetNode.ID2})");
|
|
|
|
//ㅇ목적지 완료신호를 ACS에전송한다.
|
|
PUB.XBE.SendMoveComplete(PUB._virtualAGV.TargetNode.RfidId.ToString("0000"));
|
|
PUB.sm.UpdateRunStepSeq();
|
|
return false;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
}
|
|
}
|