249 lines
10 KiB
C#
249 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;
|
|
|
|
namespace Project
|
|
{
|
|
public partial class fMain
|
|
{
|
|
|
|
bool CheckStopCondition()
|
|
{
|
|
return true;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 라이더멈춤이 설정되어있다면 음성으로 알려준다
|
|
/// </summary>
|
|
/// <returns></returns>
|
|
public bool CheckLiderStop()
|
|
{
|
|
|
|
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;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 설정된 목적지까지 이동을 완료 한 후 True를 반환합니다.
|
|
/// 목적지 : PUB._virtualAGV.TargetNode
|
|
///
|
|
/// </summary>
|
|
/// <param name="sender"></param>
|
|
/// <returns></returns>
|
|
Boolean UpdateMotionPositionForMark(string sender)
|
|
{
|
|
//현재위치를 모르는 상태라면 이동하여 현재 위치를 찾는다
|
|
if (_SM_RUN_POSCHK(false, new TimeSpan()) == false) return false;
|
|
|
|
//현재위치노드 오류
|
|
var currentNode = PUB.FindByNodeID(PUB._virtualAGV.CurrentNodeId);
|
|
if (currentNode == null)
|
|
{
|
|
PUB.log.AddE($"현재위치노드가 없습니다");
|
|
PUB.sm.SetNewRunStep(ERunStep.READY);
|
|
return false;
|
|
}
|
|
|
|
//시작노드값이 없다면 현재위치를 노드로 결정한다
|
|
if (PUB._virtualAGV.StartNode == null)
|
|
PUB._virtualAGV.StartNode = PUB.FindByNodeID(PUB._virtualAGV.CurrentNodeId);
|
|
|
|
//시작노드가없다면 오류
|
|
if (PUB._virtualAGV.StartNode == null)
|
|
{
|
|
PUB.log.AddE($"경로시작노드가 없습니다");
|
|
PUB.sm.SetNewRunStep(ERunStep.READY);
|
|
return false;
|
|
}
|
|
|
|
//대상노드가 없다면 오류
|
|
if (PUB._virtualAGV.TargetNode == null)
|
|
{
|
|
PUB.log.AddE($"경로종료노드가 없습니다");
|
|
PUB.sm.SetNewRunStep(ERunStep.READY);
|
|
return false;
|
|
}
|
|
|
|
//경로 생성(경로정보가 없거나 현재노드가 경로에 없는경우)
|
|
if (PUB._virtualAGV.CurrentPath == null ||
|
|
PUB._virtualAGV.CurrentPath.DetailedPath.Any() == false ||
|
|
PUB._virtualAGV.CurrentPath.DetailedPath.Where(t => t.NodeId.Equals(currentNode.NodeId)).Any() == false)
|
|
{
|
|
if (PUB.AGV.system1.agv_run)
|
|
{
|
|
PUB.log.Add($"경로 재생성으로 인해 구동을 멈춥니다");
|
|
PUB.AGV.AGVMoveStop("경로재생성");
|
|
}
|
|
|
|
var PathResult = CalcPath(PUB._virtualAGV.StartNode, PUB._virtualAGV.TargetNode);
|
|
if (PathResult.result == null)
|
|
{
|
|
PUB.log.AddE($"경로가 계산되지 않았습니다");
|
|
PUB.sm.SetNewRunStep(ERunStep.READY);
|
|
return false;
|
|
}
|
|
|
|
PUB.log.AddI($"경로생성 {PUB._virtualAGV.StartNode.RfidId} -> {PUB._virtualAGV.TargetNode.RfidId}");
|
|
}
|
|
|
|
//predict 를 이용하여 다음 이동을 모두 확인한다.
|
|
var nextAction = PUB._virtualAGV.Predict();
|
|
|
|
var message = $"[다음 행동 예측]\n\n" +
|
|
$"모터: {nextAction.Motor}\n" +
|
|
$"마그넷: {nextAction.Magnet}\n" +
|
|
$"속도: {nextAction.Speed}\n" +
|
|
$"이유: {nextAction.Message}\n\n" +
|
|
$"---\n" +
|
|
$"현재 상태: {PUB._virtualAGV.CurrentState}\n" +
|
|
$"현재 방향: {PUB._virtualAGV.CurrentDirection}\n" +
|
|
$"위치 확정: {PUB._virtualAGV.IsPositionConfirmed} (RFID {PUB._virtualAGV.DetectedRfidCount}개)\n" +
|
|
$"현재 노드: {PUB._virtualAGV.CurrentNodeId ?? "없음"}";
|
|
|
|
//모터에서 정지를 요청했다
|
|
if (nextAction.Motor == AGVNavigationCore.Models.MotorCommand.Stop)
|
|
{
|
|
if (PUB.AGV.system1.agv_run)
|
|
{
|
|
// 완료(Complete) 상태라면 MarkStop 전송
|
|
if (nextAction.Reason == AGVNavigationCore.Models.eAGVCommandReason.MarkStop)
|
|
{
|
|
PUB.log.Add("다음행동예측에서 MARK STOP이 확인되었습니다");
|
|
PUB.AGV.AGVMoveStop(nextAction.Message, arDev.Narumi.eStopOpt.MarkStop);
|
|
}
|
|
else
|
|
{
|
|
PUB.log.Add("다음행동예측에서 장비 멈춤이 확인되었습니다");
|
|
PUB.AGV.AGVMoveStop(nextAction.Message);
|
|
}
|
|
}
|
|
|
|
// 목적지 도착 여부 확인
|
|
// 현재 노드가 타겟 노드와 같고, 위치가 확정된 상태라면 도착으로 간주
|
|
// 단, AGV가 실제로 멈췄는지 확인 (agv_run == false)
|
|
if (PUB._virtualAGV.IsPositionConfirmed &&
|
|
PUB._virtualAGV.CurrentNodeId == PUB._virtualAGV.TargetNode.NodeId)
|
|
{
|
|
if (PUB.AGV.system1.agv_run == false)
|
|
{
|
|
return true;
|
|
}
|
|
}
|
|
|
|
return false;
|
|
}
|
|
else
|
|
{
|
|
// 이동 명령 변환 (AGVNavigationCore -> arDev.Narumi)
|
|
var bunki = arDev.Narumi.eBunki.Strate;
|
|
if (nextAction.Magnet == AGVNavigationCore.Models.MagnetPosition.L) bunki = arDev.Narumi.eBunki.Left;
|
|
else if (nextAction.Magnet == AGVNavigationCore.Models.MagnetPosition.R) bunki = arDev.Narumi.eBunki.Right;
|
|
|
|
var dir = arDev.Narumi.eMoveDir.Forward;
|
|
if (nextAction.Motor == AGVNavigationCore.Models.MotorCommand.Backward) dir = arDev.Narumi.eMoveDir.Backward;
|
|
|
|
var spd = arDev.Narumi.eMoveSpd.Low;
|
|
if (nextAction.Speed == AGVNavigationCore.Models.SpeedLevel.M) spd = arDev.Narumi.eMoveSpd.Mid;
|
|
else if (nextAction.Speed == AGVNavigationCore.Models.SpeedLevel.H) spd = arDev.Narumi.eMoveSpd.High;
|
|
|
|
// 명령 설정
|
|
// 현재 상태와 다를 때만 전송 (불필요한 통신 부하 방지)
|
|
if (PUB.AGV.data.Sts != bunki.ToString()[0] ||
|
|
PUB.AGV.data.Direction != dir.ToString()[0] ||
|
|
PUB.AGV.data.Speed != spd.ToString()[0])
|
|
{
|
|
PUB.AGV.AGVMoveSet(new arDev.Narumi.BunkiData
|
|
{
|
|
Bunki = bunki,
|
|
Direction = dir,
|
|
PBSSensor = 1,
|
|
Speed = spd,
|
|
});
|
|
PUB.log.Add($"Predict Run Setting = bunki:{bunki},dir:{dir},pbs:1,spd:{spd}");
|
|
}
|
|
|
|
// AGV가 정지 상태라면 구동 시작
|
|
if (PUB.AGV.system1.agv_run == false)
|
|
{
|
|
var runOpt = (dir == arDev.Narumi.eMoveDir.Forward) ? arDev.Narumi.eRunOpt.Forward : arDev.Narumi.eRunOpt.Backward;
|
|
PUB.AGV.AGVMoveRun(runOpt);
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
}
|
|
|
|
/// <summary>
|
|
/// 충전기검색시퀀스
|
|
/// </summary>
|
|
/// <param name="sender"></param>
|
|
/// <returns></returns>
|
|
Boolean UpdateMotionPositionForCharger(string sender)
|
|
{
|
|
if (VAR.BOOL[eVarBool.AGVDIR_BACK] == 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.AGV.AGVMoveSet(new arDev.Narumi.BunkiData
|
|
{
|
|
Bunki = arDev.Narumi.eBunki.Strate,
|
|
Direction = arDev.Narumi.eMoveDir.Forward,
|
|
PBSSensor = 1,
|
|
Speed = arDev.Narumi.eMoveSpd.Low,
|
|
});
|
|
PUB.AGV.AGVMoveRun();//
|
|
tm_gocharge_command = DateTime.Now;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
//현재위치가 충전위치이고, 움직이지 않았다면 완료된 경우라 할수 있따
|
|
if (PUB._virtualAGV.CurrentNodeId.Equals(PUB.setting.NodeMAP_RFID_Charger) &&
|
|
VAR.BOOL[eVarBool.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
|
|
}
|