Amkor/ENIG
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

프로토콜 조정

Browse Source
This commit is contained in:
backuppc
2025-11-14 15:03:51 +09:00
parent 028e9ab35f
commit 97b2a3076f
14 changed files with 612 additions and 251 deletions
Download Patch File Download Diff File Expand all files Collapse all files

51
Cs_HMI/Project/Device/Xbee.cs
View File

@@ -173,7 +173,7 @@ namespace Project.Device
*/
try
{
byte[] data = new byte[13]; // 총 13바이트 데이터
byte[] data = new byte[12]; // 총 12바이트 데이터
// Mode
data[0] = (byte)(VAR.BOOL[eVarBool.FLAG_AUTORUN] ? 1 : 0);
@@ -186,47 +186,56 @@ namespace Project.Device
else
data[1] = 0; // stop
// Direction
if (PUB.AGV.system1.stop_by_front_detect)
data[2] = 3; // markstop
else if (VAR.BOOL[eVarBool.FLAG_LEFT_RUN])
data[2] = 1; // left
else if (VAR.BOOL[eVarBool.FLAG_RIGHT_RUN])
data[2] = 2; // right
// Motor Direction
if (PUB.AGV.data.Direction == 'F')
data[2] = 0;
else if (PUB.AGV.data.Direction == 'B')
data[2] = 1;
else
data[2] = 0; // straight
data[2] = 0xff; //unknown
// Magnet Direction
if (PUB.AGV.data.Sts == 'L')
data[3] = 1; // left
else if (PUB.AGV.data.Sts == 'R')
data[3] = 2; // right
else if (PUB.AGV.data.Sts == 'S')
data[3] = 0; // straight
else
data[3] = 0xFF; //unknown
// Inposition
data[3] = (byte)(PUB.AGV.system1.agv_stop ? 1 : 0);
data[4] = (byte)(PUB.AGV.system1.agv_stop ? 1 : 0);
// ChargeSt
data[4] = (byte)((VAR.BOOL[eVarBool.FLAG_CHARGEONA] || VAR.BOOL[eVarBool.FLAG_CHARGEONM]) ? 1 : 0);
data[5] = (byte)((VAR.BOOL[eVarBool.FLAG_CHARGEONA] || VAR.BOOL[eVarBool.FLAG_CHARGEONM]) ? 1 : 0);
// CartSt
if (PUB.AGV.signal.cart_detect1 && PUB.AGV.signal.cart_detect2)
data[5] = 1; // 센서두개가 모두 감지되는 경우
data[6] = 1; // 센서두개가 모두 감지되는 경우
else if (PUB.AGV.signal.cart_detect1 == false && PUB.AGV.signal.cart_detect2 == false)
data[5] = 0; // 센서두개가 모두 감지되지 않는 경우
data[6] = 0; // 센서두개가 모두 감지되지 않는 경우
else
data[5] = 2; // 센서하나만 감지되는 경우
data[6] = 2; // 센서하나만 감지되는 경우
// LiftSt
if (PUB.AGV.signal.lift_up)
data[6] = 1; // 위로 올라가는 경우
data[7] = 1; // 위로 올라가는 경우
else if (PUB.AGV.signal.lift_down)
data[6] = 0; // 아래로 내려가는 경우
data[7] = 0; // 아래로 내려가는 경우
else
data[6] = 2; // unknown (기본값)
data[7] = 2; // unknown (기본값)
// LastTag
string lastTag = PUB.AGV.data.TagNo.ToString("000000") ?? "000000";
byte[] tagBytes = Encoding.ASCII.GetBytes(lastTag.PadRight(6, '0'));
Array.Copy(tagBytes, 0, data, 7, 6);
string lastTag = PUB.AGV.data.TagNo.ToString("0000") ?? "0000";
byte[] tagBytes = Encoding.ASCII.GetBytes(lastTag.PadRight(4, '0'));
Array.Copy(tagBytes, 0, data, 8, lastTag.Length);
// 데이터 전송
var cmd = (byte)ENIGProtocol.AGVCommandEH.Status;
var packet = proto.CreatePacket(PUB.setting.XBE_ID, cmd, data);
Send(packet);
if (Send(packet))
PUB.logxbee.AddI($"Send status {packet.Length} {packet.HexString()}");
LastStatusSendTime = DateTime.Now;
}
catch (Exception ex)

2
Cs_HMI/Project/StateMachine/_AGV.cs
View File

@@ -195,7 +195,7 @@ namespace Project
//모터방향 확인해서 UI와 AGV클래스에 적용한다
var MotDireciton = PUB.AGV.data.Direction == 'B' ? AGVNavigationCore.Models.AgvDirection.Backward : AGVNavigationCore.Models.AgvDirection.Forward;
PUB._virtualAGV.SetPosition(CurrentNode, MotDireciton);
PUB._mapCanvas.SetAGVPosition("AGV", CurrentNode, MotDireciton);
PUB._mapCanvas.SetAGVPosition(PUB.setting.MCID, CurrentNode, MotDireciton);
}
break;
case arDev.Narumi.DataType.ACK:

204
Cs_HMI/Project/StateMachine/_Xbee.cs
View File

@@ -3,6 +3,8 @@ 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;
@@ -22,82 +24,88 @@ namespace Project
//ACS 수신 데이터 처리(타 장비는 확인하지 않는다)
if (e.ReceivedPacket.ID == 0)
{
var logPrefix = "ACS";
var data = e.ReceivedPacket.Data;
var dataStr = System.Text.Encoding.Default.GetString(data);
var cmd = (ENIGProtocol.AGVCommandHE)e.ReceivedPacket.Command;
var TargetID = 0;
if (dataStr.Length >= 2)
{
//대상디바이스
TargetID = Convert.ToByte(dataStr.Substring(0, 2), 16);
//데이터영역을 다시 설정
if (dataStr.Length > 2) dataStr = dataStr.Substring(2);
else dataStr = string.Empty;
}
else
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 (PUB.setting.XBE_ID != TargetID) return;
switch (cmd)
{
case ENIGProtocol.AGVCommandHE.SetCurrent: //Set Current Position
if (dataStr.Length == 6)
if (data.Length > 4)
{
var targID = dataStr.Substring(0, 2);
var targstr = dataStr.Substring(2);
if (byte.TryParse(targID, out byte tID))
var currTag = System.Text.Encoding.Default.GetString(data, 1, data.Length - 1);
var node = PUB._mapNodes.FirstOrDefault(t => t.RfidId == currTag);
if (node == null)
{
if (PUB.setting.XBE_ID == tID)
{
if (ushort.TryParse(targstr, out ushort tagno))
{
//if (PUB.mapctl.SetCurrentPosition(tagno) == true)
//{
// PUB.log.AddI($"Set Position:{tagno}");
//}
//else PUB.log.AddE($"Position Set Error:{tagno}");
}
else PUB.log.AddE($"Position Param(tagstr) Error:{dataStr}");
}
else PUB.log.AddI($"Another Target {tID}:{PUB.setting.XBE_ID}");
PUB.log.AddE($"[{logPrefix}-SetCurrent] 노드정보를 찾을 수 없습니다 RFID:{currTag}");
PUB.XBE.SendError(ENIGProtocol.AGVErrorCode.EmptyNode, $"{currTag}");
return;
}
else PUB.log.AddE($"Position Param(targetid) Error:{dataStr}");
PUB._mapCanvas.SetAGVPosition(PUB.setting.MCID, node, PUB._virtualAGV.CurrentDirection);
}
else PUB.log.AddE($"Position Param Error:{dataStr}");
else PUB.log.AddE($"[{logPrefix}-SetCurrent] TagString Lenght Errorr:{data.Length}");
break;
case ENIGProtocol.AGVCommandHE.Goto: //move to tag
if (uint.TryParse(dataStr, out uint tagno2))
if (data.Length > 4)
{
//var currPos = PUB.mapctl.Manager.agv.CurrentRFID;///.AGVMoveToRFID(;
//if (PUB.mapctl.SetTargetPosition(tagno2))
// PUB.log.AddI($"New Target {tagno2}");
//else
// PUB.log.AddE($"Path Error {tagno2}");
var currTag = System.Text.Encoding.Default.GetString(data, 1, data.Length - 1);
var targetNode = PUB._mapNodes.FirstOrDefault(t => t.RfidId == currTag);
if (targetNode == null)
{
PUB.log.AddE($"[{logPrefix}-Goto] 노드정보를 찾을 수 없습니다 RFID:{currTag}");
PUB.XBE.SendError(ENIGProtocol.AGVErrorCode.EmptyNode, $"{currTag}");
return;
}
var startNode = PUB._mapNodes.FirstOrDefault(t => t.RfidId == PUB._virtualAGV.CurrentNodeId);
if (startNode == null)
{
PUB.log.AddE($"[{logPrefix}-Goto] 노드정보를 찾을 수 없습니다 RFID:{PUB._virtualAGV.CurrentNodeId}");
PUB.XBE.SendError(ENIGProtocol.AGVErrorCode.EmptyNode, $"{PUB._virtualAGV.CurrentNodeId}");
return;
}
var rltGoto = CalcPath(startNode, targetNode);
if (rltGoto.result == false)
{
PUB.log.AddE($"[{logPrefix}-Goto] {rltGoto.message}");
PUB.XBE.SendError(ENIGProtocol.AGVErrorCode.Goto, rltGoto.message);
return;
}
//Move to
PUB.log.Add($"[{logPrefix}-Goto] {startNode.RfidId} -> {targetNode.RfidId}");
}
else PUB.log.AddE($"Path Param Error :{dataStr}");
else PUB.log.AddE($"[{logPrefix}-Goto] TagString Lenght Errorr:{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[0]; //0=back, 1=forward, 2=left, 3=right
var Speed = data[1]; //0=slow, 1=normal, 2=fast
var Runtime = data[2]; // running seconds
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;
@@ -111,27 +119,63 @@ namespace Project
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.MarkStop: //Set MarkStop
var MarkStop = data[0]; //0=off, 1=on
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.Middle;
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();
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[0]; //0=stop, 1=up, 2=down
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.AGV.LiftControl(LCmd);
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;
}
}
}
@@ -142,6 +186,60 @@ namespace Project
else PUB.log.Add(e.Message);
}
AGVNavigationCore.PathFinding.Planning.AGVPathfinder _advancedPathfinder = null;
(bool result, string message) CalcPath(MapNode startNode, MapNode targetNode)
{
var _mapNodes = PUB._mapNodes;
// 시작 RFID가 없으면 AGV 현재 위치로 설정
if (startNode == null || targetNode == null)
return (false, "시작 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();
if (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);
return (true, string.Empty);
}
else
{
// 경로 실패시 디버깅 정보 초기화
//_pathDebugLabel.Text = $"경로: 실패 - {advancedResult.ErrorMessage}";
return (false, $"경로를 찾을 수 없습니다:\n{advancedResult.ErrorMessage}");
}
}
}
}