에뮬레이터 개발 전.
This commit is contained in:
backuppc
228
Cs_HMI/Project/StateMachine/Step/_Util.cs
Normal file
228
Cs_HMI/Project/StateMachine/Step/_Util.cs
Normal file
@@ -0,0 +1,228 @@
|
||||
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>
|
||||
/// 설정된 목적지까지 이동을 완료 한 후 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
|
||||
}
|
||||
Reference in New Issue
Block a user