333 lines
17 KiB
C#
333 lines
17 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.Drawing;
|
|
using System.Linq;
|
|
using System.Text;
|
|
using AGVNavigationCore.Models;
|
|
using AGVNavigationCore.Utils;
|
|
using AR;
|
|
using arDev;
|
|
using COMM;
|
|
using Project.StateMachine;
|
|
|
|
namespace Project
|
|
{
|
|
public partial class fMain
|
|
{
|
|
|
|
private void XBE_ProtocReceived(object sender, ENIG.EEProtocol.DataEventArgs e)
|
|
{
|
|
//TODO : 기능 처리필요 (XBee 메세지 데이터처리)
|
|
//PUB.CheckManualChargeMode() : 수동충전확인
|
|
//VAR.BOOL[eVarBool.FLAG_AUTORUN] : 자동실행
|
|
//PUB.Speak("현재 위치는 QA 입니다.") : 음성출력
|
|
|
|
//ACS 수신 데이터 처리(타 장비는 확인하지 않는다)
|
|
if (e.ReceivedPacket.ID == 0)
|
|
{
|
|
var logPrefix = "ACS";
|
|
var data = e.ReceivedPacket.Data;
|
|
var cmd = (ENIGProtocol.AGVCommandHE)e.ReceivedPacket.Command;
|
|
if (data.Length < 1)
|
|
{
|
|
PUB.log.Add($"ACS 데이터에서 TARGET ID가 없습니다(data : first byte)");
|
|
return;
|
|
}
|
|
|
|
//대상디바이스
|
|
var TargetID = data[0];
|
|
|
|
//해당 패킷의 대상이 나라면 처리한다.
|
|
if (PUB.setting.XBE_ID != TargetID) return;
|
|
|
|
switch (cmd)
|
|
{
|
|
|
|
case ENIGProtocol.AGVCommandHE.SetCurrent: //Set Current Position
|
|
|
|
if (data.Length > 4)
|
|
{
|
|
var currTag = System.Text.Encoding.Default.GetString(data, 1, data.Length - 1);
|
|
if (ushort.TryParse(currTag, out ushort currtagValue))
|
|
{
|
|
var node = PUB._mapCanvas.Nodes.FirstOrDefault(t => t.RfidId == currtagValue);
|
|
if (node == null)
|
|
{
|
|
PUB.log.AddE($"[{logPrefix}-SetCurrent] 노드정보를 찾을 수 없습니다 RFID:{currTag}");
|
|
PUB.XBE.SendError(ENIGProtocol.AGVErrorCode.EmptyNode, $"{currTag}");
|
|
return;
|
|
}
|
|
else
|
|
{
|
|
PUB.log.AddI($"XBEE:현재위치설정:[{node.RfidId}]{node.Id}");
|
|
}
|
|
|
|
PUB._mapCanvas.SetAGVPosition(PUB.setting.MCID, node, PUB._virtualAGV.CurrentDirection);
|
|
PUB._virtualAGV.SetPosition(node, PUB._virtualAGV.CurrentDirection);
|
|
}
|
|
else PUB.log.AddE($"[{logPrefix}-SetCurrent] TagString Value Errorr:{data}");
|
|
}
|
|
else PUB.log.AddE($"[{logPrefix}-SetCurrent] TagString Lenght Errorr:{data.Length}");
|
|
break;
|
|
|
|
case ENIGProtocol.AGVCommandHE.PickOn: // 110
|
|
case ENIGProtocol.AGVCommandHE.PickOff: // 111
|
|
{
|
|
PUB.log.AddI($"XBEE:작업명령수신:{cmd}");
|
|
|
|
// 현재 위치 확인 (TargetNode가 아닌 CurrentNode 기준)
|
|
var currNode = PUB._virtualAGV.CurrentNode;
|
|
if (currNode == null)
|
|
{
|
|
PUB.log.AddE($"[{logPrefix}-{cmd}] 현재 노드를 알 수 없습니다 NodeID:{PUB._virtualAGV.CurrentNode.Id}");
|
|
PUB.XBE.SendError(ENIGProtocol.AGVErrorCode.EmptyNode, "Unknown Node");
|
|
return;
|
|
}
|
|
|
|
PUB.NextWorkCmd = cmd;
|
|
ERunStep nextStep = ERunStep.READY;
|
|
|
|
switch (currNode.StationType)
|
|
{
|
|
case StationType.Loader: nextStep = ERunStep.LOADER_IN; break;
|
|
case StationType.UnLoader: nextStep = ERunStep.UNLOADER_IN; break;
|
|
case StationType.Buffer: nextStep = ERunStep.BUFFER_IN; break;
|
|
case StationType.Clearner: nextStep = ERunStep.CLEANER_IN; break;
|
|
default:
|
|
PUB.log.AddE($"[{logPrefix}-{cmd}] 해당 노드타입({currNode.Type})은 작업을 지원하지 않습니다.");
|
|
return;
|
|
}
|
|
|
|
PUB.log.AddI($"작업 시작: {nextStep} (Type: {cmd})");
|
|
PUB.sm.SetNewRunStep(nextStep);
|
|
}
|
|
break;
|
|
|
|
case ENIGProtocol.AGVCommandHE.Charger: // 112
|
|
{
|
|
PUB.log.AddI($"XBEE:충전명령수신");
|
|
PUB.NextWorkCmd = ENIGProtocol.AGVCommandHE.Charger;
|
|
PUB.sm.SetNewRunStep(ERunStep.GOCHARGE);
|
|
}
|
|
break;
|
|
|
|
case ENIGProtocol.AGVCommandHE.GotoAlias:
|
|
case ENIGProtocol.AGVCommandHE.Goto: //move to tag
|
|
var datalength = cmd == ENIGProtocol.AGVCommandHE.GotoAlias ? 1 : 4;
|
|
if (data.Length > datalength)
|
|
{
|
|
var currTag = System.Text.Encoding.Default.GetString(data, 1, data.Length - 1).Trim();
|
|
MapNode targetNode = null;
|
|
if(cmd == ENIGProtocol.AGVCommandHE.GotoAlias)
|
|
{
|
|
targetNode = PUB._mapCanvas.Nodes.FirstOrDefault(t => t.AliasName == currTag);
|
|
}
|
|
else
|
|
{
|
|
if (ushort.TryParse(currTag, out ushort currtagvalue))
|
|
targetNode = PUB._mapCanvas.Nodes.FirstOrDefault(t => t.RfidId == currtagvalue);
|
|
else PUB.log.Add($"targstring 이 숫자가 아니라서 대상을 설정할 수 없습니다 값:{currTag}");
|
|
}
|
|
|
|
if (targetNode != null)
|
|
{
|
|
//자동상태가아니라면 처리하지 않는다.
|
|
if (VAR.BOOL[eVarBool.FLAG_AUTORUN] == false)
|
|
{
|
|
PUB.log.AddE($"[{logPrefix}-Goto] 자동실행상태가 아닙니다");
|
|
PUB.XBE.SendError(ENIGProtocol.AGVErrorCode.ManualMode, $"{currTag}");
|
|
return;
|
|
}
|
|
|
|
//목적지
|
|
PUB._virtualAGV.TargetNode = targetNode;
|
|
if (targetNode == null)
|
|
{
|
|
PUB.log.AddE($"[{logPrefix}-Goto] 노드정보를 찾을 수 없습니다 RFID:{currTag}");
|
|
PUB.XBE.SendError(ENIGProtocol.AGVErrorCode.EmptyNode, $"{currTag}");
|
|
return;
|
|
}
|
|
|
|
///출발지
|
|
var startNode = PUB._mapCanvas.Nodes.FirstOrDefault(t => t.RfidId == PUB._virtualAGV.CurrentNode.RfidId);
|
|
PUB._virtualAGV.StartNode = startNode;
|
|
if (startNode == null)
|
|
{
|
|
PUB.log.AddE($"[{logPrefix}-Goto] 시작노드가 없습니다(현재위치 없음) NodeID:{PUB._virtualAGV.CurrentNode.Id}");
|
|
}
|
|
|
|
//대상이동으로 처리한다.
|
|
if(PUB.sm.RunStep != ERunStep.GOTO)
|
|
{
|
|
PUB.sm.SetNewRunStep(StateMachine.ERunStep.GOTO);
|
|
PUB.sm.ResetRunStepSeq();
|
|
}
|
|
|
|
|
|
//Move to
|
|
PUB.log.Add($"[{logPrefix}-{cmd}] {startNode.RfidId} -> {targetNode.RfidId}");
|
|
}
|
|
else PUB.log.AddE($"[{logPrefix}-{cmd}] 대상노드가 없습니다 {data}");
|
|
}
|
|
else PUB.log.AddE($"[{logPrefix}-{cmd}] Length Error:{data.Length}");
|
|
break;
|
|
|
|
case ENIGProtocol.AGVCommandHE.Stop: //stop
|
|
PUB.log.Add($"[{logPrefix}-Stop]");
|
|
PUB.AGV.AGVMoveStop("xbee");
|
|
break;
|
|
case ENIGProtocol.AGVCommandHE.Reset: //Error Reset
|
|
PUB.log.Add($"[{logPrefix}-Reset]");
|
|
PUB.AGV.AGVErrorReset();
|
|
break;
|
|
|
|
case ENIGProtocol.AGVCommandHE.Manual: //Manual Move (Direction, speed, runtime)
|
|
var Direction = data[1]; //0=back, 1=forward, 2=left, 3=right
|
|
var Speed = data[2]; //0=slow, 1=normal, 2=fast
|
|
|
|
arDev.Narumi.ManulOpt opt = arDev.Narumi.ManulOpt.BS;
|
|
arDev.Narumi.Speed spd = arDev.Narumi.Speed.Low;
|
|
if (Speed == 1) spd = arDev.Narumi.Speed.Mid;
|
|
else if (Speed == 2) spd = arDev.Narumi.Speed.High;
|
|
|
|
//0.자동모드가 아니라면 실행하지 않는다
|
|
//1.입력된 파라미터로 AGV를 이동한다
|
|
if (Direction == 0) opt = arDev.Narumi.ManulOpt.BS;
|
|
else if (Direction == 1) opt = arDev.Narumi.ManulOpt.FS;
|
|
else if (Direction == 2) opt = arDev.Narumi.ManulOpt.LT;
|
|
else if (Direction == 3) opt = arDev.Narumi.ManulOpt.RT;
|
|
|
|
PUB.log.Add($"[{logPrefix}-Manual] DIR:{opt},SPD:{spd}");
|
|
PUB.AGV.AGVMoveManual(opt, spd, arDev.Narumi.Sensor.PBSOn);
|
|
break;
|
|
|
|
case ENIGProtocol.AGVCommandHE.AutoMove:
|
|
var MotDirection = data[1]; //0=back, 1=forward
|
|
var MagDirection = data[2]; //0=straight, 1=left, 2=right
|
|
var AutSpeed = data[3]; //0=slow, 1=normal, 2=fast
|
|
|
|
var bunkidata = new arDev.Narumi.BunkiData();
|
|
|
|
//speed;
|
|
if (AutSpeed == 1) bunkidata.Speed = arDev.Narumi.eMoveSpd.Mid;
|
|
else if (AutSpeed == 2) bunkidata.Speed = arDev.Narumi.eMoveSpd.High;
|
|
else bunkidata.Speed = arDev.Narumi.eMoveSpd.Low;
|
|
|
|
//motor direction
|
|
if (MotDirection == 0) bunkidata.Direction = arDev.Narumi.eMoveDir.Backward;
|
|
else bunkidata.Direction = arDev.Narumi.eMoveDir.Forward;
|
|
|
|
if (MagDirection == 2) bunkidata.Bunki = arDev.Narumi.eBunki.Right;
|
|
else if (MagDirection == 1) bunkidata.Bunki = arDev.Narumi.eBunki.Left;
|
|
else bunkidata.Bunki = arDev.Narumi.eBunki.Strate;
|
|
|
|
PUB.log.Add($"[{logPrefix}-AutoMove] DIR:{bunkidata.Direction}-{bunkidata.Bunki},SPD:{bunkidata.Speed}");
|
|
PUB.AGV.AGVMoveSet(bunkidata);
|
|
PUB.AGV.AGVMoveRun((MotDirection == 0 ? arDev.Narumi.eRunOpt.Backward : arDev.Narumi.eRunOpt.Forward));
|
|
break;
|
|
|
|
case ENIGProtocol.AGVCommandHE.MarkStop: //Set MarkStop
|
|
//마크센서에서 멈추게 한다
|
|
PUB.log.Add($"[{logPrefix}-MarkStop]");
|
|
PUB.AGV.AGVMoveStop("Xbee", arDev.Narumi.eStopOpt.MarkStop);
|
|
break;
|
|
|
|
case ENIGProtocol.AGVCommandHE.LiftControl: //Lift Control
|
|
var LiftCommand = data[1]; //0=stop, 1=up, 2=down
|
|
|
|
arDev.Narumi.LiftCommand LCmd = arDev.Narumi.LiftCommand.STP;
|
|
if (LiftCommand == 1) LCmd = arDev.Narumi.LiftCommand.UP;
|
|
else if (LiftCommand == 2) LCmd = arDev.Narumi.LiftCommand.DN;
|
|
|
|
PUB.log.Add($"[{logPrefix}-LiftControl] {LCmd}");
|
|
PUB.AGV.LiftControl(LCmd); //리프트제어
|
|
break;
|
|
|
|
case ENIGProtocol.AGVCommandHE.ChargeControl: //충전을 제어한다
|
|
var chargeAction = data[1] == 1; //0= off, 1=on
|
|
|
|
//충전시퀀스가 진행되지 않았다면 진행한다
|
|
if (PUB.sm.RunStep == StateMachine.ERunStep.GOCHARGE && PUB.sm.RunStepNew != StateMachine.ERunStep.GOCHARGE)
|
|
{
|
|
PUB.sm.SetNewRunStep(StateMachine.ERunStep.GOCHARGE);
|
|
PUB.log.AddI($"충전을 시작합니다");
|
|
}
|
|
break;
|
|
|
|
default:
|
|
PUB.logagv.AddE($"Unknown Command : {cmd}");
|
|
PUB.XBE.SendError(ENIGProtocol.AGVErrorCode.UnknownCommand, $"{cmd}");
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
private void XBE_MessageReceived(object sender, Device.Xbee.MessageArgs e)
|
|
{
|
|
if (e.IsError) PUB.log.AddE(e.Message);
|
|
else PUB.log.Add(e.Message);
|
|
}
|
|
|
|
AGVNavigationCore.PathFinding.Planning.AGVPathfinder _advancedPathfinder = null;
|
|
(AGVNavigationCore.PathFinding.Core.AGVPathResult result, string message) CalcPath(MapNode startNode, MapNode targetNode)
|
|
{
|
|
var _mapNodes = PUB._mapCanvas.Nodes;
|
|
// 시작 RFID가 없으면 AGV 현재 위치로 설정
|
|
if (startNode == null || targetNode == null)
|
|
return (null, "시작 RFID와 목표 RFID를 선택해주세요.");
|
|
|
|
//경로계산기확인
|
|
if (_advancedPathfinder == null)
|
|
_advancedPathfinder = new AGVNavigationCore.PathFinding.Planning.AGVPathfinder(_mapNodes);
|
|
|
|
// 현재 AGV 방향 가져오기
|
|
var selectedAGV = PUB._virtualAGV;
|
|
var currentDirection = selectedAGV.CurrentDirection;
|
|
|
|
// AGV의 이전 위치에서 가장 가까운 노드 찾기
|
|
var prevNode = selectedAGV.PrevNode;
|
|
var prevDir = selectedAGV.PrevDirection;
|
|
|
|
// 고급 경로 계획 사용 (노드 객체 직접 전달)
|
|
var advancedResult = _advancedPathfinder.FindPath(startNode, targetNode, prevNode, prevDir, currentDirection);
|
|
|
|
var _simulatorCanvas = PUB._mapCanvas;
|
|
|
|
_simulatorCanvas.FitToNodes();
|
|
string Message = string.Empty;
|
|
if (advancedResult != null && advancedResult.Success)
|
|
{
|
|
// 도킹 검증이 없는 경우 추가 검증 수행
|
|
if (advancedResult.DockingValidation == null || !advancedResult.DockingValidation.IsValidationRequired)
|
|
advancedResult.DockingValidation = DockingValidator.ValidateDockingDirection(advancedResult, _mapNodes);
|
|
|
|
_simulatorCanvas.CurrentPath = advancedResult;
|
|
//_pathLengthLabel.Text = $"경로 길이: {advancedResult.TotalDistance:F1}";
|
|
//_statusLabel.Text = $"경로 계산 완료 ({advancedResult.CalculationTimeMs}ms)";
|
|
|
|
// 🔥 VirtualAGV에도 경로 설정 (Predict()가 동작하려면 필요)
|
|
selectedAGV.SetPath(advancedResult);
|
|
|
|
// 도킹 검증 결과 확인 및 UI 표시
|
|
//CheckAndDisplayDockingValidation(advancedResult);
|
|
|
|
// 고급 경로 디버깅 정보 표시
|
|
//UpdateAdvancedPathDebugInfo(advancedResult);
|
|
|
|
}
|
|
else if(advancedResult != null)
|
|
{
|
|
// 경로 실패시 디버깅 정보 초기화
|
|
//_pathDebugLabel.Text = $"경로: 실패 - {advancedResult.ErrorMessage}";
|
|
Message = $"경로를 찾을 수 없습니다:\n{advancedResult.ErrorMessage}";
|
|
advancedResult = null;
|
|
}
|
|
|
|
return (advancedResult, Message);
|
|
}
|
|
|
|
|
|
}
|
|
}
|
|
|