393 lines
16 KiB
C#
393 lines
16 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.Utils;
|
|
|
|
namespace Project
|
|
{
|
|
public partial class fMain
|
|
{
|
|
|
|
bool CheckStopCondition()
|
|
{
|
|
|
|
//이머전시상태라면 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);
|
|
return false;
|
|
}
|
|
|
|
//수동충전상태라면 이동하지 못한다
|
|
if (VAR.BOOL[eVarBool.FLAG_CHARGEONM])
|
|
{
|
|
PUB.Speak("수동 충전중이라 사용할 수 없습니다");
|
|
PUB.sm.SetNewStep(eSMStep.IDLE);
|
|
return false;
|
|
}
|
|
|
|
|
|
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 (PUB._virtualAGV.CurrentNode == null || PUB._virtualAGV.PrevNode == null)
|
|
return false;
|
|
|
|
//현재위치노드 오류
|
|
var currentNode = PUB.FindByNodeID(PUB._virtualAGV.CurrentNode.Id);
|
|
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.CurrentNode.Id);
|
|
|
|
//시작노드가없다면 오류
|
|
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.HasPath() == false ||
|
|
PUB._virtualAGV.CurrentPath.DetailedPath.Where(t => t.NodeId.Equals(currentNode.Id)).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 &&
|
|
PathResult.result.Success == true &&
|
|
PathResult.result.DetailedPath.Where(t => t.NodeId == currentNode.Id).Any() == false)
|
|
{
|
|
PUB._virtualAGV.StartNode = PUB._virtualAGV.CurrentNode;
|
|
PathResult = CalcPath(PUB._virtualAGV.StartNode, PUB._virtualAGV.TargetNode);
|
|
}
|
|
|
|
|
|
if (PathResult.result == null)
|
|
{
|
|
PUB.log.AddE($"경로가 계산되지 않았습니다");
|
|
PUB.sm.SetNewRunStep(ERunStep.READY);
|
|
return false;
|
|
}
|
|
else
|
|
{
|
|
|
|
PUB._mapCanvas.CurrentPath = PathResult.result;
|
|
PUB._virtualAGV.SetPath(PathResult.result);
|
|
|
|
}
|
|
|
|
PUB.log.AddI($"경로생성 {PUB._virtualAGV.StartNode.RfidId} -> {PUB._virtualAGV.TargetNode.RfidId}");
|
|
}
|
|
|
|
//경로에 대한 무결성 검증
|
|
if (CheckPathIntegrity(PUB._virtualAGV.CurrentPath) == false)
|
|
{
|
|
if (PUB.AGV.system1.agv_run)
|
|
{
|
|
PUB.AGV.AGVMoveStop("Path Integrity Fail");
|
|
}
|
|
PUB.log.AddE($"경로 무결성 오류로 인해 경로를 삭제 합니다");
|
|
PUB._virtualAGV.SetPath(null);
|
|
VAR.I32[eVarInt32.PathValidationError] += 1;
|
|
if (VAR.I32[eVarInt32.PathValidationError] > 50)
|
|
{
|
|
PUB.log.AddE($"연속 경로 무결성 오류로 인해 중지 합니다");
|
|
PUB._mapCanvas.SetAlertMessage($"연속 경로 무결성 오류로 인해 중지 합니다");
|
|
PUB.sm.SetNewRunStep(ERunStep.ERROR);
|
|
}
|
|
return false;
|
|
}
|
|
else VAR.I32[eVarInt32.PathValidationError] = 0;
|
|
|
|
//현재위치 기준으로 재 계산하여. 더 최적화된 루트가 있다면 처리를 해준다.
|
|
if (PUB._virtualAGV.CurrentPath.DetailedPath.Count > 5)
|
|
{
|
|
var PathResult2 = CalcPath(PUB._virtualAGV.CurrentNode, PUB._virtualAGV.TargetNode);
|
|
if (PathResult2.result != null && PathResult2.result.Success)
|
|
{
|
|
//절반이상 경로가 짧을때에는 재계산을 하게한다
|
|
var halfcnt = (int)(PUB._virtualAGV.CurrentPath.DetailedPath.Count / 2.0);
|
|
if (PathResult2.result.DetailedPath.Count < halfcnt)
|
|
{
|
|
var msg = $"단축경로가 확인되었습니다. 경로를 삭제 합니다";
|
|
PUB.log.AddE(msg);
|
|
Console.WriteLine(msg);
|
|
PUB._virtualAGV.SetPath(null);
|
|
}
|
|
return false;
|
|
}
|
|
}
|
|
|
|
|
|
|
|
//predict 를 이용하여 다음 이동을 모두 확인한다.
|
|
var nextAction = PUB._virtualAGV.Predict();
|
|
if (nextAction.Reason == AGVNavigationCore.Models.eAGVCommandReason.PathOut)
|
|
{
|
|
//경로이탈
|
|
var logmessage = $"경로이탈감지 시작노드를 현재위치로 설정합니다 START:{PUB._virtualAGV.StartNode},CURRENT:{PUB._virtualAGV.CurrentNode}";
|
|
PUB.log.AddE(logmessage);
|
|
Console.WriteLine(logmessage);
|
|
PUB._virtualAGV.ClearPath();//.DetailedPath.Clear();
|
|
PUB._virtualAGV.StartNode = PUB._virtualAGV.CurrentNode;
|
|
return false;
|
|
}
|
|
|
|
|
|
//모터에서 정지를 요청했다
|
|
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)
|
|
{
|
|
if (PUB.AGV.system1.agv_run == false)
|
|
{
|
|
//목적지도착완료시
|
|
if (PUB._virtualAGV.CurrentNode.Id == PUB._virtualAGV.TargetNode.Id)
|
|
{
|
|
var node = PUB._mapCanvas.Nodes.Where(t => t.Id == PUB._virtualAGV.CurrentNodeId).FirstOrDefault();
|
|
var rfid = node?.ID2 ?? "(X)";
|
|
PUB.log.AddI($"목표 도착 및 정지 확인됨(MarkStop 완료) Node:{rfid}");
|
|
return true;
|
|
}
|
|
|
|
//목적지가 버퍼라면 그 앞에 멈춘다
|
|
if (PUB._virtualAGV.TargetNode.StationType == AGVNavigationCore.Models.StationType.Buffer &&
|
|
PUB._virtualAGV.CurrentPath != null && PUB._virtualAGV.CurrentPath.DetailedPath.Any())
|
|
{
|
|
if (PUB._virtualAGV.CurrentNode.Id == PUB._virtualAGV.CurrentPath.DetailedPath.Last().NodeId)
|
|
{
|
|
PUB.log.AddI($"목표(버퍼) 도착 및 정지 확인됨(MarkStop 완료). Node:{PUB._virtualAGV.CurrentNodeID2}");
|
|
return true;
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
//아직 멈추지 않았다면 기다린다.
|
|
}
|
|
}
|
|
|
|
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])
|
|
{
|
|
var ret = PUB.AGV.AGVMoveSet(new arDev.Narumi.BunkiData
|
|
{
|
|
Bunki = bunki,
|
|
Direction = dir,
|
|
PBSSensor = 1,
|
|
Speed = spd,
|
|
});
|
|
if (ret == arDev.eNarumiCommandResult.Success)
|
|
PUB.log.Add($"Predict Run Setting = bunki:{bunki},dir:{dir},pbs:1,spd:{spd}");
|
|
else
|
|
PUB.log.AddE($"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)
|
|
{
|
|
var ret = PUB.AGV.AGVMoveSet(new arDev.Narumi.BunkiData
|
|
{
|
|
Bunki = arDev.Narumi.eBunki.Strate,
|
|
Direction = arDev.Narumi.eMoveDir.Forward,
|
|
PBSSensor = 1,
|
|
Speed = arDev.Narumi.eMoveSpd.Low,
|
|
});
|
|
if (ret == arDev.eNarumiCommandResult.Success)
|
|
{
|
|
PUB.AGV.AGVMoveRun(arDev.Narumi.eRunOpt.Forward);//
|
|
}
|
|
|
|
tm_gocharge_command = DateTime.Now;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
//현재위치가 충전위치이고, 움직이지 않았다면 완료된 경우라 할수 있따
|
|
if (PUB._virtualAGV.CurrentNode.Id.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)
|
|
{
|
|
var ret = PUB.AGV.AGVMoveSet(new arDev.Narumi.BunkiData
|
|
{
|
|
Bunki = arDev.Narumi.eBunki.Strate,
|
|
Direction = arDev.Narumi.eMoveDir.Backward,
|
|
PBSSensor = 1,
|
|
Speed = arDev.Narumi.eMoveSpd.Low,
|
|
});
|
|
if (ret == arDev.eNarumiCommandResult.Success)
|
|
PUB.AGV.AGVMoveRun(arDev.Narumi.eRunOpt.Backward);//
|
|
LastCommandTime = DateTime.Now;
|
|
}
|
|
}
|
|
//}
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
|
|
|
|
|
|
/// <summary>
|
|
/// 경로 무결성(도킹방향 등) 검증
|
|
/// </summary>
|
|
/// <param name="pathResult"></param>
|
|
/// <returns></returns>
|
|
private bool CheckPathIntegrity(AGVNavigationCore.PathFinding.Core.AGVPathResult pathResult)
|
|
{
|
|
if (pathResult == null) return false;
|
|
|
|
// CalcPath에서 이미 DockingValidator를 수행했을 수 있음.
|
|
// 만약 수행되지 않았다면 여기서 수행.
|
|
if (pathResult.DockingValidation == null)
|
|
{
|
|
pathResult.DockingValidation = AGVNavigationCore.Utils.DockingValidator.ValidateDockingDirection(pathResult, PUB._mapCanvas.Nodes);
|
|
}
|
|
|
|
// 검증 결과 확인
|
|
if (pathResult.DockingValidation != null && pathResult.DockingValidation.IsValidationRequired)
|
|
{
|
|
if (pathResult.DockingValidation.IsValid == false)
|
|
{
|
|
PUB.log.AddE($"[경로무결성오류] {pathResult.DockingValidation.ValidationError}");
|
|
return false;
|
|
}
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
|
|
}//cvass
|
|
}
|