using System; using System.Collections.Generic; using System.Drawing; using System.Linq; using System.Text; using AGVNavigationCore.Models; using AGVNavigationCore.PathFinding.Core; using AGVNavigationCore.PathFinding.Planning; 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; //자동실행모드가 아니라면 무조건 에러를 반환한다 if (VAR.BOOL[eVarBool.FLAG_AUTORUN] == false) { SetRunStepError(ENIGProtocol.AGVErrorCode.ManualMode, "현재 자동실행 모드가 아닙니다"); return; } switch (cmd) { case ENIGProtocol.AGVCommandHE.SetCurrent: //Set Current Position Resultclear(); 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) { var ermsg = ($"[{logPrefix}-SetCurrent] 노드정보를 찾을 수 없습니다 RFID:{currTag}"); SetRunStepError(ENIGProtocol.AGVErrorCode.EmptyNode, ermsg); 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); PUB.SaveLastPosition(); //위치저장 260205 } 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.PickOnEnter: // 110 case ENIGProtocol.AGVCommandHE.PickOffEnter: // 111 { Resultclear(); PUB.log.AddI($"XBEE:작업명령수신:{cmd}"); // 현재 위치 확인 (TargetNode가 아닌 CurrentNode 기준) var currNode = PUB._virtualAGV.CurrentNode; if (currNode == null) { var msg = $"[{logPrefix}-{cmd}] 현재 노드를 알 수 없습니다"; SetRunStepError(ENIGProtocol.AGVErrorCode.EmptyNode, msg); return; } var targetNode = PUB._virtualAGV.TargetNode; if (targetNode == null) { var msg = $"[{logPrefix}-{cmd}] 목표 노드를 알 수 없습니다"; SetRunStepError(ENIGProtocol.AGVErrorCode.EmptyNode, msg); return; } //버퍼의 경우 직전에 멈추기 때문에 스테이션종류를 보정해준다 var StationType = currNode.StationType; if (StationType == StationType.Normal && targetNode.StationType == StationType.Buffer) StationType = StationType.Buffer; ERunStep nextStep = ERunStep.READY; switch (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}] 해당 노드타입({StationType})은 작업을 지원하지 않습니다."); return; } //다음명령처리 PUB.NextWorkCmd = cmd; PUB.log.AddI($"작업 시작: {nextStep} (Type: {cmd})"); PUB.sm.SetNewRunStep(nextStep); } break; case ENIGProtocol.AGVCommandHE.PickOnExit: case ENIGProtocol.AGVCommandHE.PickOffExit: { Resultclear(); PUB.log.AddI($"XBEE:작업명령수신:{cmd}"); // 현재 위치 확인 (TargetNode가 아닌 CurrentNode 기준) var currNode = PUB._virtualAGV.CurrentNode; if (currNode == null) { var msg = $"[{logPrefix}-{cmd}] 현재 노드를 알 수 없습니다"; SetRunStepError(ENIGProtocol.AGVErrorCode.EmptyNode, msg); return; } var targetNode = PUB._virtualAGV.TargetNode; if (targetNode == null) { var msg = $"[{logPrefix}-{cmd}] 목표 노드를 알 수 없습니다"; SetRunStepError(ENIGProtocol.AGVErrorCode.EmptyNode, msg); return; } //버퍼의 경우 직전에 멈추기 때문에 스테이션종류를 보정해준다 var StationType = currNode.StationType; if (StationType == StationType.Normal && targetNode.StationType == StationType.Buffer) StationType = StationType.Buffer; ERunStep nextStep = ERunStep.READY; switch (StationType) { case StationType.Loader: nextStep = ERunStep.LOADER_OUT; break; case StationType.UnLoader: nextStep = ERunStep.UNLOADER_OUT; break; case StationType.Buffer: nextStep = ERunStep.BUFFER_OUT; break; case StationType.Clearner: nextStep = ERunStep.CLEANER_OUT; break; default: PUB.log.AddE($"[{logPrefix}-{cmd}] 해당 노드타입({StationType})은 작업을 지원하지 않습니다."); return; } //다음명령처리 PUB.NextWorkCmd = cmd; PUB.log.AddI($"작업 시작: {nextStep} (Type: {cmd})"); PUB.sm.SetNewRunStep(nextStep); } break; case ENIGProtocol.AGVCommandHE.Charger: // 112 { Resultclear(); 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 Resultclear(); var datalength = cmd == ENIGProtocol.AGVCommandHE.GotoAlias ? 2 : 1; 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) { SetRunStepError(ENIGProtocol.AGVErrorCode.ManualMode, $"[{logPrefix}-Goto] 자동실행상태가 아닙니다"); return; } //노드가 들어왔는데. 일반 노드라면.. 턴 정보를 제거한다. if (PUB._virtualAGV.CurrentNode != null && PUB._virtualAGV.CurrentNode.StationType != StationType.Buffer && PUB._virtualAGV.Turn != AGVTurn.None) { PUB.log.AddAT($"[{logPrefix}-Goto] 현재노드위치가 버퍼가 아니라서 턴정보를 초기화합니다{PUB._virtualAGV.Turn}"); PUB._virtualAGV.Turn = AGVTurn.None; } //s/w턴이 걸려있다면 이동 불가로한다.(버퍼 작업중이다) if (PUB._virtualAGV.Turn != AGVTurn.None) { SetRunStepError(ENIGProtocol.AGVErrorCode.BUFFER_NOT_COMPLETE, $"[{logPrefix}-Goto] 버퍼작업이 완료되지 않았습니다"); return; } //목적지 PUB._virtualAGV.TargetNode = targetNode; if (targetNode == null) { SetRunStepError(ENIGProtocol.AGVErrorCode.EmptyNode, $"[{logPrefix}-Goto] 노드정보를 찾을 수 없습니다 RFID:{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(); } PUB.SaveLastPosition(); //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.LTurn180: PUB.log.Add($"[{logPrefix}-LTurn180]"); PUB.sm.SetNewRunStep(ERunStep.REMOTECONTROL); PUB.AGV.AGVMoveLeft180Turn(); break; case ENIGProtocol.AGVCommandHE.RTurn180: PUB.log.Add($"[{logPrefix}-RTurn180]"); PUB.sm.SetNewRunStep(ERunStep.REMOTECONTROL); PUB.AGV.AGVMoveRight180Turn(); break; case ENIGProtocol.AGVCommandHE.LTurn: PUB.log.Add($"[{logPrefix}-LTurn]"); PUB.sm.SetNewRunStep(ERunStep.REMOTECONTROL); PUB.AGV.AGVMoveManual(arDev.Narumi.ManulOpt.LT, arDev.Narumi.Speed.Low, arDev.Narumi.Sensor.AllOn); break; case ENIGProtocol.AGVCommandHE.RTurn: PUB.log.Add($"[{logPrefix}-RTurn]"); PUB.sm.SetNewRunStep(ERunStep.REMOTECONTROL); PUB.AGV.AGVMoveManual(arDev.Narumi.ManulOpt.RT, arDev.Narumi.Speed.Low, arDev.Narumi.Sensor.AllOn); 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]"); ResetSystemError(); break; case ENIGProtocol.AGVCommandHE.Manual: //Manual Move (Direction, speed, runtime) Resultclear(); PUB.sm.SetNewRunStep(ERunStep.REMOTECONTROL); 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: Resultclear(); PUB.sm.SetNewRunStep(ERunStep.REMOTECONTROL); 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 Lidar = data[4]; //0=off, 1=on 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; if (Lidar == 0) bunkidata.PBSSensor = 0; else bunkidata.PBSSensor = 2; PUB.log.Add($"[{logPrefix}-AutoMove] DIR:{bunkidata.Direction}-{bunkidata.Bunki},SPD:{bunkidata.Speed}"); if (PUB.AGV.AGVMoveSet(bunkidata) != eNarumiCommandResult.Success) PUB.log.AddE($"AGV속도설정실패로 인해 자동전환이 실패되었습니다"); else { PUB.log.Add($"XBE 에서 자동모드로 전환합니다"); 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 = ( arDev.Narumi.LiftCommand)data[1]; PUB.log.Add($"[{logPrefix}-LiftControl] {LiftCommand}"); PUB.AGV.LiftControl(LiftCommand); //리프트제어 break; case ENIGProtocol.AGVCommandHE.ChargeControl: //충전을 제어한다 var chargeAction = data[1] == 1; //0= off, 1=on //충전시퀀스가 진행되지 않았다면 진행한다 if(chargeAction==true) { PUB.sm.SetNewRunStep(StateMachine.ERunStep.GOCHARGE); PUB.log.AddI($"충전을 시작합니다"); } else { PUB.sm.SetNewRunStep(ERunStep.CHARGEOFF); PUB.log.AddI($"충전을 해제 합니다"); } break; default: SetRunStepError(ENIGProtocol.AGVErrorCode.UnknownCommand, $"Unknown Command : {cmd} Sender:{e.ReceivedPacket.ID}, Target:{data[0]}"); 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); } } }