✨ feat(runtime): 完善 FANUC 命令参数与状态通道重连
* 在 FanucCommandProtocol/Client 中补齐速度倍率、TCP 位姿和 IO 的封包/解析，并引入 FanucIoTypes 字符串到枚举映射 * FanucControllerRuntime 在非仿真模式下接入真机命令通道，本地 缓存仅作为兜底，TCP 操作扩展为 7 维 Pose * FanucStateClient 增加帧超时检测、退避自动重连和诊断状态接口, 超时或重连期间不再把陈旧帧当作当前机器人状态 * FanucStateProtocol 锁定 90B 帧字段为 pose[6]、joint[6]、 external_axes[3] 和 raw_tail_words[4],并保留状态字诊断槽位 * ICspPlanner 增加 global_scale > 1.0 失败判定,self-adapt-icsp 内部禁用该判定以保留补点重试链路 * 同步更新 README/AGENTS/计划文档的 todo 状态和实现说明
This commit is contained in:
@@ -59,6 +59,54 @@ public static class FanucCommandMessageIds
|
||||
public const uint SetIo = 0x2209;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 定义旧 ControllerClient 公开的 FANUC IO 类型枚举值。
|
||||
/// </summary>
|
||||
public static class FanucIoTypes
|
||||
{
|
||||
/// <summary>
|
||||
/// FANUC 数字输入 DI。
|
||||
/// </summary>
|
||||
public const uint DigitalInput = 1;
|
||||
|
||||
/// <summary>
|
||||
/// FANUC 数字输出 DO。
|
||||
/// </summary>
|
||||
public const uint DigitalOutput = 2;
|
||||
|
||||
/// <summary>
|
||||
/// FANUC 机器人输入 RI。
|
||||
/// </summary>
|
||||
public const uint RobotInput = 8;
|
||||
|
||||
/// <summary>
|
||||
/// FANUC 机器人输出 RO。
|
||||
/// </summary>
|
||||
public const uint RobotOutput = 9;
|
||||
|
||||
/// <summary>
|
||||
/// 将 HTTP/兼容层传入的 IO 类型字符串转换为 FANUC 命令通道枚举值。
|
||||
/// </summary>
|
||||
/// <param name="ioType">IO 类型字符串，例如 DI、DO、RI、RO。</param>
|
||||
/// <returns>命令通道使用的 IO 类型数值。</returns>
|
||||
public static uint FromName(string ioType)
|
||||
{
|
||||
if (string.IsNullOrWhiteSpace(ioType))
|
||||
{
|
||||
throw new ArgumentException("IO 类型不能为空。", nameof(ioType));
|
||||
}
|
||||
|
||||
return ioType.Trim().ToUpperInvariant() switch
|
||||
{
|
||||
"DI" or "KIOTYPEDI" => DigitalInput,
|
||||
"DO" or "KIOTYPEDO" => DigitalOutput,
|
||||
"RI" or "KIOTYPERI" => RobotInput,
|
||||
"RO" or "KIOTYPERO" => RobotOutput,
|
||||
_ => throw new ArgumentOutOfRangeException(nameof(ioType), ioType, "未知 IO 类型。")
|
||||
};
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 表示 FANUC TCP 10012 命令通道中只携带结果码的响应。
|
||||
/// </summary>
|
||||
@@ -91,6 +139,140 @@ public sealed class FanucCommandResultResponse
|
||||
public bool IsSuccess => ResultCode == 0;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 表示 FANUC TCP 10012 速度倍率响应。
|
||||
/// </summary>
|
||||
public sealed class FanucSpeedRatioResponse
|
||||
{
|
||||
/// <summary>
|
||||
/// 初始化速度倍率响应。
|
||||
/// </summary>
|
||||
/// <param name="messageId">响应对应的消息号。</param>
|
||||
/// <param name="ratioInt">控制器返回的整数百分比。</param>
|
||||
/// <param name="resultCode">控制器返回的结果码。</param>
|
||||
public FanucSpeedRatioResponse(uint messageId, uint ratioInt, uint resultCode)
|
||||
{
|
||||
MessageId = messageId;
|
||||
RatioInt = ratioInt;
|
||||
ResultCode = resultCode;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取响应对应的消息号。
|
||||
/// </summary>
|
||||
public uint MessageId { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取控制器返回的整数百分比。
|
||||
/// </summary>
|
||||
public uint RatioInt { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取控制器返回的结果码。
|
||||
/// </summary>
|
||||
public uint ResultCode { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取转换后的 0.0 到 1.0 速度倍率。
|
||||
/// </summary>
|
||||
public double Ratio => RatioInt / 100.0;
|
||||
|
||||
/// <summary>
|
||||
/// 获取当前响应是否表示成功。
|
||||
/// </summary>
|
||||
public bool IsSuccess => ResultCode == 0;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 表示 FANUC TCP 10012 TCP 位姿响应。
|
||||
/// </summary>
|
||||
public sealed class FanucTcpResponse
|
||||
{
|
||||
/// <summary>
|
||||
/// 初始化 TCP 位姿响应。
|
||||
/// </summary>
|
||||
/// <param name="messageId">响应对应的消息号。</param>
|
||||
/// <param name="resultCode">控制器返回的结果码。</param>
|
||||
/// <param name="tcpId">控制器返回的 TCP ID。</param>
|
||||
/// <param name="pose">7 维 TCP 位姿。</param>
|
||||
public FanucTcpResponse(uint messageId, uint resultCode, uint tcpId, IReadOnlyList<double> pose)
|
||||
{
|
||||
MessageId = messageId;
|
||||
ResultCode = resultCode;
|
||||
TcpId = tcpId;
|
||||
Pose = pose.ToArray();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取响应对应的消息号。
|
||||
/// </summary>
|
||||
public uint MessageId { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取控制器返回的结果码。
|
||||
/// </summary>
|
||||
public uint ResultCode { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取控制器返回的 TCP ID。
|
||||
/// </summary>
|
||||
public uint TcpId { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取 7 维 TCP 位姿。
|
||||
/// </summary>
|
||||
public IReadOnlyList<double> Pose { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取当前响应是否表示成功。
|
||||
/// </summary>
|
||||
public bool IsSuccess => ResultCode == 0;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 表示 FANUC TCP 10012 IO 读取响应。
|
||||
/// </summary>
|
||||
public sealed class FanucIoResponse
|
||||
{
|
||||
/// <summary>
|
||||
/// 初始化 IO 读取响应。
|
||||
/// </summary>
|
||||
/// <param name="messageId">响应对应的消息号。</param>
|
||||
/// <param name="resultCode">控制器返回的结果码。</param>
|
||||
/// <param name="numericValue">控制器返回的 float IO 数值。</param>
|
||||
public FanucIoResponse(uint messageId, uint resultCode, double numericValue)
|
||||
{
|
||||
MessageId = messageId;
|
||||
ResultCode = resultCode;
|
||||
NumericValue = numericValue;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取响应对应的消息号。
|
||||
/// </summary>
|
||||
public uint MessageId { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取控制器返回的结果码。
|
||||
/// </summary>
|
||||
public uint ResultCode { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取控制器返回的原始数值。
|
||||
/// </summary>
|
||||
public double NumericValue { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取按布尔 IO 解释后的值。
|
||||
/// </summary>
|
||||
public bool Value => Math.Abs(NumericValue) > double.Epsilon;
|
||||
|
||||
/// <summary>
|
||||
/// 获取当前响应是否表示成功。
|
||||
/// </summary>
|
||||
public bool IsSuccess => ResultCode == 0;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 表示 FANUC TCP 10012 程序状态响应。
|
||||
/// </summary>
|
||||
@@ -166,6 +348,109 @@ public static class FanucCommandProtocol
|
||||
return PackFrame(messageId, body);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 封装读取速度倍率命令。
|
||||
/// </summary>
|
||||
/// <returns>可直接写入命令通道 Socket 的完整帧。</returns>
|
||||
public static byte[] PackGetSpeedRatioCommand()
|
||||
{
|
||||
return PackEmptyCommand(FanucCommandMessageIds.GetSpeedRatio);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 封装设置速度倍率命令，按旧系统逻辑转换为 0..100 的整数百分比。
|
||||
/// </summary>
|
||||
/// <param name="ratio">目标速度倍率。</param>
|
||||
/// <returns>可直接写入命令通道 Socket 的完整帧。</returns>
|
||||
public static byte[] PackSetSpeedRatioCommand(double ratio)
|
||||
{
|
||||
if (double.IsNaN(ratio) || double.IsInfinity(ratio))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(ratio), "ratio 必须是有限数值。");
|
||||
}
|
||||
|
||||
var ratioInt = (uint)Math.Clamp((int)(ratio * 100.0), 0, 100);
|
||||
var body = new byte[sizeof(uint)];
|
||||
BinaryPrimitives.WriteUInt32BigEndian(body, ratioInt);
|
||||
return PackFrame(FanucCommandMessageIds.SetSpeedRatio, body);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 封装读取 TCP 位姿命令。
|
||||
/// </summary>
|
||||
/// <param name="tcpId">目标 TCP ID。</param>
|
||||
/// <returns>可直接写入命令通道 Socket 的完整帧。</returns>
|
||||
public static byte[] PackGetTcpCommand(uint tcpId)
|
||||
{
|
||||
var body = new byte[sizeof(uint)];
|
||||
BinaryPrimitives.WriteUInt32BigEndian(body, tcpId);
|
||||
return PackFrame(FanucCommandMessageIds.GetTcp, body);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 封装设置 TCP 位姿命令。
|
||||
/// </summary>
|
||||
/// <param name="tcpId">目标 TCP ID。</param>
|
||||
/// <param name="pose">7 维 TCP 位姿。</param>
|
||||
/// <returns>可直接写入命令通道 Socket 的完整帧。</returns>
|
||||
public static byte[] PackSetTcpCommand(uint tcpId, IReadOnlyList<double> pose)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(pose);
|
||||
if (pose.Count != 7)
|
||||
{
|
||||
throw new ArgumentException("TCP 位姿必须包含 7 个数值。", nameof(pose));
|
||||
}
|
||||
|
||||
var body = new byte[sizeof(uint) + sizeof(float) * 7];
|
||||
BinaryPrimitives.WriteUInt32BigEndian(body.AsSpan(0, sizeof(uint)), tcpId);
|
||||
for (int i = 0; i < 7; i++)
|
||||
{
|
||||
BinaryPrimitives.WriteSingleBigEndian(body.AsSpan(sizeof(uint) + i * sizeof(float), sizeof(float)), (float)pose[i]);
|
||||
}
|
||||
|
||||
return PackFrame(FanucCommandMessageIds.SetTcp, body);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 封装读取 IO 命令，字段顺序为 io_type 后接 io_index。
|
||||
/// </summary>
|
||||
/// <param name="ioType">IO 类型数值。</param>
|
||||
/// <param name="ioIndex">IO 索引。</param>
|
||||
/// <returns>可直接写入命令通道 Socket 的完整帧。</returns>
|
||||
public static byte[] PackGetIoCommand(uint ioType, int ioIndex)
|
||||
{
|
||||
if (ioIndex < 0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(ioIndex), "IO 索引不能为负数。");
|
||||
}
|
||||
|
||||
var body = new byte[sizeof(uint) * 2];
|
||||
BinaryPrimitives.WriteUInt32BigEndian(body.AsSpan(0, sizeof(uint)), ioType);
|
||||
BinaryPrimitives.WriteUInt32BigEndian(body.AsSpan(sizeof(uint), sizeof(uint)), (uint)ioIndex);
|
||||
return PackFrame(FanucCommandMessageIds.GetIo, body);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 封装设置 IO 命令，字段顺序为 io_type、io_index、float io_value。
|
||||
/// </summary>
|
||||
/// <param name="ioType">IO 类型数值。</param>
|
||||
/// <param name="ioIndex">IO 索引。</param>
|
||||
/// <param name="value">目标 IO 布尔值。</param>
|
||||
/// <returns>可直接写入命令通道 Socket 的完整帧。</returns>
|
||||
public static byte[] PackSetIoCommand(uint ioType, int ioIndex, bool value)
|
||||
{
|
||||
if (ioIndex < 0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(ioIndex), "IO 索引不能为负数。");
|
||||
}
|
||||
|
||||
var body = new byte[sizeof(uint) * 2 + sizeof(float)];
|
||||
BinaryPrimitives.WriteUInt32BigEndian(body.AsSpan(0, sizeof(uint)), ioType);
|
||||
BinaryPrimitives.WriteUInt32BigEndian(body.AsSpan(sizeof(uint), sizeof(uint)), (uint)ioIndex);
|
||||
BinaryPrimitives.WriteSingleBigEndian(body.AsSpan(sizeof(uint) * 2, sizeof(float)), value ? 1.0f : 0.0f);
|
||||
return PackFrame(FanucCommandMessageIds.SetIo, body);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 解析只携带结果码的 TCP 10012 响应帧。
|
||||
/// </summary>
|
||||
@@ -185,6 +470,70 @@ public static class FanucCommandProtocol
|
||||
BinaryPrimitives.ReadUInt32BigEndian(body[..sizeof(uint)]));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 解析 GetSpeedRatio 的 TCP 10012 响应帧。
|
||||
/// </summary>
|
||||
/// <param name="frame">完整响应帧。</param>
|
||||
/// <returns>速度倍率响应。</returns>
|
||||
public static FanucSpeedRatioResponse ParseSpeedRatioResponse(ReadOnlySpan<byte> frame)
|
||||
{
|
||||
var messageId = ValidateAndReadMessageId(frame);
|
||||
var body = GetBody(frame);
|
||||
if (body.Length < sizeof(uint) * 2)
|
||||
{
|
||||
throw new InvalidDataException("FANUC 速度倍率响应体长度不足。");
|
||||
}
|
||||
|
||||
// GetSpeedRatio 的字段顺序特殊：ratio_int 在前，result_code 在后。
|
||||
var ratioInt = BinaryPrimitives.ReadUInt32BigEndian(body[..sizeof(uint)]);
|
||||
var resultCode = BinaryPrimitives.ReadUInt32BigEndian(body.Slice(sizeof(uint), sizeof(uint)));
|
||||
return new FanucSpeedRatioResponse(messageId, ratioInt, resultCode);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 解析 GetTCP 的 TCP 10012 响应帧。
|
||||
/// </summary>
|
||||
/// <param name="frame">完整响应帧。</param>
|
||||
/// <returns>TCP 位姿响应。</returns>
|
||||
public static FanucTcpResponse ParseTcpResponse(ReadOnlySpan<byte> frame)
|
||||
{
|
||||
var messageId = ValidateAndReadMessageId(frame);
|
||||
var body = GetBody(frame);
|
||||
if (body.Length < sizeof(uint) * 2 + sizeof(float) * 7)
|
||||
{
|
||||
throw new InvalidDataException("FANUC TCP 响应体长度不足。");
|
||||
}
|
||||
|
||||
var resultCode = BinaryPrimitives.ReadUInt32BigEndian(body[..sizeof(uint)]);
|
||||
var tcpId = BinaryPrimitives.ReadUInt32BigEndian(body.Slice(sizeof(uint), sizeof(uint)));
|
||||
var pose = new double[7];
|
||||
for (int i = 0; i < pose.Length; i++)
|
||||
{
|
||||
pose[i] = BinaryPrimitives.ReadSingleBigEndian(body.Slice(sizeof(uint) * 2 + i * sizeof(float), sizeof(float)));
|
||||
}
|
||||
|
||||
return new FanucTcpResponse(messageId, resultCode, tcpId, pose);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 解析 GetIO 的 TCP 10012 响应帧。
|
||||
/// </summary>
|
||||
/// <param name="frame">完整响应帧。</param>
|
||||
/// <returns>IO 读取响应。</returns>
|
||||
public static FanucIoResponse ParseIoResponse(ReadOnlySpan<byte> frame)
|
||||
{
|
||||
var messageId = ValidateAndReadMessageId(frame);
|
||||
var body = GetBody(frame);
|
||||
if (body.Length < sizeof(uint) + sizeof(float))
|
||||
{
|
||||
throw new InvalidDataException("FANUC IO 响应体长度不足。");
|
||||
}
|
||||
|
||||
var resultCode = BinaryPrimitives.ReadUInt32BigEndian(body[..sizeof(uint)]);
|
||||
var ioValue = BinaryPrimitives.ReadSingleBigEndian(body.Slice(sizeof(uint), sizeof(float)));
|
||||
return new FanucIoResponse(messageId, resultCode, ioValue);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 解析 GetProgStatus 的 TCP 10012 响应帧。
|
||||
/// </summary>
|
||||
|
||||
Reference in New Issue
Block a user