xsxiaosa/FlyShotHost
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

✨ feat(fanuc): 添加协议编解码与状态页" -m "* 固化 10010 状态帧、10012 命令帧和 60015 J519 包编解码

Browse Source 
* 扩展 ControllerClient 兼容层的执行参数和运行时编排
  * 新增 /status 页面与 /api/status/snapshot 状态快照接口
  * 补充 FANUC 协议、客户端和状态接口的最小验证测试
  * 更新 README、兼容要求和真机 Socket 通信实现计划
This commit is contained in:
yunxiao.zhu
2026-04-24 21:26:25 +08:00
parent 8a20d9f507
commit a78e6761cb
25 changed files with 3773 additions and 55 deletions
Download Patch File Download Diff File Expand all files Collapse all files

386
src/Flyshot.Runtime.Fanuc/Protocol/FanucJ519Protocol.cs Normal file
View File

@@ -0,0 +1,386 @@
using System.Buffers.Binary;
namespace Flyshot.Runtime.Fanuc.Protocol;
/// <summary>
/// 表示 FANUC UDP 60015 J519/ICSP 伺服流中的一帧命令数据。
/// </summary>
public sealed class FanucJ519Command
{
private readonly double[] _targetJoints;
/// <summary>
/// 初始化 J519 命令数据。
/// </summary>
/// <param name="sequence">命令序号。</param>
/// <param name="targetJoints">目标关节或扩展轴数据，最多 9 个槽位。</param>
/// <param name="lastData">是否为最后一帧数据。</param>
/// <param name="readIoType">读取 IO 类型。</param>
/// <param name="readIoIndex">读取 IO 起始索引。</param>
/// <param name="readIoMask">读取 IO 掩码。</param>
/// <param name="dataStyle">目标数据类型。</param>
/// <param name="writeIoType">写入 IO 类型。</param>
/// <param name="writeIoIndex">写入 IO 起始索引。</param>
/// <param name="writeIoMask">写入 IO 掩码。</param>
/// <param name="writeIoValue">写入 IO 数值。</param>
public FanucJ519Command(
uint sequence,
IReadOnlyList<double> targetJoints,
byte lastData = 0,
byte readIoType = 2,
ushort readIoIndex = 1,
ushort readIoMask = 255,
byte dataStyle = 1,
byte writeIoType = 2,
ushort writeIoIndex = 1,
ushort writeIoMask = 0,
ushort writeIoValue = 0)
{
ArgumentNullException.ThrowIfNull(targetJoints);
if (targetJoints.Count is <= 0 or > 9)
{
throw new ArgumentOutOfRangeException(nameof(targetJoints), "J519 目标数据必须包含 1 到 9 个槽位。");
}
Sequence = sequence;
LastData = lastData;
ReadIoType = readIoType;
ReadIoIndex = readIoIndex;
ReadIoMask = readIoMask;
DataStyle = dataStyle;
WriteIoType = writeIoType;
WriteIoIndex = writeIoIndex;
WriteIoMask = writeIoMask;
WriteIoValue = writeIoValue;
_targetJoints = targetJoints.ToArray();
}
/// <summary>
/// 获取命令序号。
/// </summary>
public uint Sequence { get; }
/// <summary>
/// 获取是否为最后一帧数据。
/// </summary>
public byte LastData { get; }
/// <summary>
/// 获取读取 IO 类型。
/// </summary>
public byte ReadIoType { get; }
/// <summary>
/// 获取读取 IO 起始索引。
/// </summary>
public ushort ReadIoIndex { get; }
/// <summary>
/// 获取读取 IO 掩码。
/// </summary>
public ushort ReadIoMask { get; }
/// <summary>
/// 获取目标数据类型。
/// </summary>
public byte DataStyle { get; }
/// <summary>
/// 获取写入 IO 类型。
/// </summary>
public byte WriteIoType { get; }
/// <summary>
/// 获取写入 IO 起始索引。
/// </summary>
public ushort WriteIoIndex { get; }
/// <summary>
/// 获取写入 IO 掩码。
/// </summary>
public ushort WriteIoMask { get; }
/// <summary>
/// 获取写入 IO 数值。
/// </summary>
public ushort WriteIoValue { get; }
/// <summary>
/// 获取目标关节或扩展轴数据。
/// </summary>
public IReadOnlyList<double> TargetJoints => _targetJoints;
}
/// <summary>
/// 表示 FANUC UDP 60015 J519/ICSP 伺服流中的一帧响应数据。
/// </summary>
public sealed class FanucJ519Response
{
private readonly double[] _pose;
private readonly double[] _externalAxes;
private readonly double[] _jointDegrees;
private readonly double[] _motorCurrents;
/// <summary>
/// 初始化 J519 响应数据。
/// </summary>
/// <param name="messageType">响应类型。</param>
/// <param name="version">协议版本。</param>
/// <param name="sequence">响应序号。</param>
/// <param name="status">状态位集合。</param>
/// <param name="readIoType">读取 IO 类型。</param>
/// <param name="readIoIndex">读取 IO 起始索引。</param>
/// <param name="readIoMask">读取 IO 掩码。</param>
/// <param name="readIoValue">读取 IO 数值。</param>
/// <param name="timestamp">控制器时间戳。</param>
/// <param name="pose">TCP 笛卡尔位姿。</param>
/// <param name="externalAxes">扩展轴反馈。</param>
/// <param name="jointDegrees">关节角度反馈。</param>
/// <param name="motorCurrents">电机电流反馈。</param>
public FanucJ519Response(
uint messageType,
uint version,
uint sequence,
byte status,
byte readIoType,
ushort readIoIndex,
ushort readIoMask,
ushort readIoValue,
uint timestamp,
IEnumerable<double> pose,
IEnumerable<double> externalAxes,
IEnumerable<double> jointDegrees,
IEnumerable<double> motorCurrents)
{
MessageType = messageType;
Version = version;
Sequence = sequence;
Status = status;
ReadIoType = readIoType;
ReadIoIndex = readIoIndex;
ReadIoMask = readIoMask;
ReadIoValue = readIoValue;
Timestamp = timestamp;
_pose = pose?.ToArray() ?? throw new ArgumentNullException(nameof(pose));
_externalAxes = externalAxes?.ToArray() ?? throw new ArgumentNullException(nameof(externalAxes));
_jointDegrees = jointDegrees?.ToArray() ?? throw new ArgumentNullException(nameof(jointDegrees));
_motorCurrents = motorCurrents?.ToArray() ?? throw new ArgumentNullException(nameof(motorCurrents));
}
/// <summary>
/// 获取响应类型。
/// </summary>
public uint MessageType { get; }
/// <summary>
/// 获取协议版本。
/// </summary>
public uint Version { get; }
/// <summary>
/// 获取响应序号。
/// </summary>
public uint Sequence { get; }
/// <summary>
/// 获取状态位集合。
/// </summary>
public byte Status { get; }
/// <summary>
/// 获取读取 IO 类型。
/// </summary>
public byte ReadIoType { get; }
/// <summary>
/// 获取读取 IO 起始索引。
/// </summary>
public ushort ReadIoIndex { get; }
/// <summary>
/// 获取读取 IO 掩码。
/// </summary>
public ushort ReadIoMask { get; }
/// <summary>
/// 获取读取 IO 数值。
/// </summary>
public ushort ReadIoValue { get; }
/// <summary>
/// 获取控制器时间戳。
/// </summary>
public uint Timestamp { get; }
/// <summary>
/// 获取 TCP 笛卡尔位姿。
/// </summary>
public IReadOnlyList<double> Pose => _pose;
/// <summary>
/// 获取扩展轴反馈。
/// </summary>
public IReadOnlyList<double> ExternalAxes => _externalAxes;
/// <summary>
/// 获取关节角度反馈。
/// </summary>
public IReadOnlyList<double> JointDegrees => _jointDegrees;
/// <summary>
/// 获取电机电流反馈。
/// </summary>
public IReadOnlyList<double> MotorCurrents => _motorCurrents;
/// <summary>
/// 获取控制器是否接受命令。
/// </summary>
public bool AcceptsCommand => (Status & 0b0001) != 0;
/// <summary>
/// 获取控制器是否已收到命令。
/// </summary>
public bool ReceivedCommand => (Status & 0b0010) != 0;
/// <summary>
/// 获取控制器系统是否就绪。
/// </summary>
public bool SystemReady => (Status & 0b0100) != 0;
/// <summary>
/// 获取机器人是否处于运动中。
/// </summary>
public bool RobotInMotion => (Status & 0b1000) != 0;
}
/// <summary>
/// 提供 FANUC UDP 60015 J519/ICSP 伺服流的基础封包与响应解析能力。
/// </summary>
public static class FanucJ519Protocol
{
/// <summary>
/// J519 初始化和结束控制包长度。
/// </summary>
public const int ControlPacketLength = 8;
/// <summary>
/// J519 命令包长度。
/// </summary>
public const int CommandPacketLength = 64;
/// <summary>
/// J519 响应包长度。
/// </summary>
public const int ResponsePacketLength = 132;
/// <summary>
/// 封装 J519 初始化包。
/// </summary>
/// <returns>初始化包。</returns>
public static byte[] PackInitPacket()
{
return PackControlPacket(0);
}
/// <summary>
/// 封装 J519 结束包。
/// </summary>
/// <returns>结束包。</returns>
public static byte[] PackEndPacket()
{
return PackControlPacket(2);
}
/// <summary>
/// 封装 J519 64 字节命令包。
/// </summary>
/// <param name="command">命令数据。</param>
/// <returns>命令包。</returns>
public static byte[] PackCommandPacket(FanucJ519Command command)
{
ArgumentNullException.ThrowIfNull(command);
var packet = new byte[CommandPacketLength];
BinaryPrimitives.WriteUInt32BigEndian(packet.AsSpan(0x00, sizeof(uint)), 1);
BinaryPrimitives.WriteUInt32BigEndian(packet.AsSpan(0x04, sizeof(uint)), 1);
BinaryPrimitives.WriteUInt32BigEndian(packet.AsSpan(0x08, sizeof(uint)), command.Sequence);
packet[0x0c] = command.LastData;
packet[0x0d] = command.ReadIoType;
BinaryPrimitives.WriteUInt16BigEndian(packet.AsSpan(0x0e, sizeof(ushort)), command.ReadIoIndex);
BinaryPrimitives.WriteUInt16BigEndian(packet.AsSpan(0x10, sizeof(ushort)), command.ReadIoMask);
packet[0x12] = command.DataStyle;
packet[0x13] = command.WriteIoType;
BinaryPrimitives.WriteUInt16BigEndian(packet.AsSpan(0x14, sizeof(ushort)), command.WriteIoIndex);
BinaryPrimitives.WriteUInt16BigEndian(packet.AsSpan(0x16, sizeof(ushort)), command.WriteIoMask);
BinaryPrimitives.WriteUInt16BigEndian(packet.AsSpan(0x18, sizeof(ushort)), command.WriteIoValue);
BinaryPrimitives.WriteUInt16BigEndian(packet.AsSpan(0x1a, sizeof(ushort)), 0);
// J519 命令包固定保留 9 个 f32 目标槽位，少于 9 个时剩余槽位补零。
for (var index = 0; index < 9; index++)
{
var value = index < command.TargetJoints.Count ? command.TargetJoints[index] : 0.0;
BinaryPrimitives.WriteSingleBigEndian(packet.AsSpan(0x1c + (index * sizeof(float)), sizeof(float)), (float)value);
}
return packet;
}
/// <summary>
/// 解析 J519 132 字节响应包。
/// </summary>
/// <param name="packet">响应包。</param>
/// <returns>响应解析结果。</returns>
public static FanucJ519Response ParseResponse(ReadOnlySpan<byte> packet)
{
if (packet.Length != ResponsePacketLength)
{
throw new InvalidDataException("FANUC J519 响应包长度不正确。");
}
return new FanucJ519Response(
BinaryPrimitives.ReadUInt32BigEndian(packet.Slice(0x00, sizeof(uint))),
BinaryPrimitives.ReadUInt32BigEndian(packet.Slice(0x04, sizeof(uint))),
BinaryPrimitives.ReadUInt32BigEndian(packet.Slice(0x08, sizeof(uint))),
packet[0x0c],
packet[0x0d],
BinaryPrimitives.ReadUInt16BigEndian(packet.Slice(0x0e, sizeof(ushort))),
BinaryPrimitives.ReadUInt16BigEndian(packet.Slice(0x10, sizeof(ushort))),
BinaryPrimitives.ReadUInt16BigEndian(packet.Slice(0x12, sizeof(ushort))),
BinaryPrimitives.ReadUInt32BigEndian(packet.Slice(0x14, sizeof(uint))),
ReadFloatArray(packet, 0x18, 6),
ReadFloatArray(packet, 0x30, 3),
ReadFloatArray(packet, 0x3c, 9),
ReadFloatArray(packet, 0x60, 9));
}
/// <summary>
/// 封装 J519 控制包。
/// </summary>
/// <param name="packetType">控制包类型。</param>
/// <returns>控制包。</returns>
private static byte[] PackControlPacket(uint packetType)
{
var packet = new byte[ControlPacketLength];
BinaryPrimitives.WriteUInt32BigEndian(packet.AsSpan(0, sizeof(uint)), packetType);
BinaryPrimitives.WriteUInt32BigEndian(packet.AsSpan(sizeof(uint), sizeof(uint)), 1);
return packet;
}
/// <summary>
/// 从响应包中读取固定长度 f32 数组。
/// </summary>
/// <param name="packet">响应包。</param>
/// <param name="offset">数组起始偏移。</param>
/// <param name="count">数组元素数量。</param>
/// <returns>转换成 double 的数值数组。</returns>
private static double[] ReadFloatArray(ReadOnlySpan<byte> packet, int offset, int count)
{
var values = new double[count];
for (var index = 0; index < count; index++)
{
values[index] = BinaryPrimitives.ReadSingleBigEndian(packet.Slice(offset + (index * sizeof(float)), sizeof(float)));
}
return values;
}
}