1705 lines
70 KiB
C#
1705 lines
70 KiB
C#
using RobotNet10.CANOpen;
|
|
using RobotNet10.CANOpen.CiA402.Enums;
|
|
using RobotNet10.CANOpen.CiA402.Models;
|
|
using RobotNet10.CANOpen.Enums;
|
|
using RobotNet10.CANOpen.Exceptions;
|
|
using RobotNet10.CANOpen.Models;
|
|
using RobotNet10.CANOpen.Services;
|
|
using RobotNet10.RobotApp.Client.Shared.Devices;
|
|
using RobotNet10.RobotApp.Devices;
|
|
using RobotNet10.RobotApp.Drivers.PhenikaaX;
|
|
using System.Collections.Concurrent;
|
|
|
|
namespace RobotNet10.RobotApp.Drivers.ZLAC;
|
|
|
|
/// <summary>
|
|
/// Trục của motor trên driver ZLAC8015D (1 node CAN điều khiển 2 bánh)
|
|
/// </summary>
|
|
public enum Zlac8015dAxis
|
|
{
|
|
Left,
|
|
Right
|
|
}
|
|
|
|
/// <summary>
|
|
/// Cấu hình cho ZLAC8015D driver
|
|
/// </summary>
|
|
internal class Zlac8015dConfig
|
|
{
|
|
public string CanInterface { get; set; } = string.Empty;
|
|
public string NodeId { get; set; } = string.Empty;
|
|
/// <summary>
|
|
/// Trục motor mà instance này đại diện: "Left" hoặc "Right"
|
|
/// </summary>
|
|
public string Axis { get; set; } = string.Empty;
|
|
/// <summary>
|
|
/// Số count encoder trên một vòng bánh (ZLAC8015D mặc định 4096)
|
|
/// Dùng để quy đổi counts/s (interface ICiA402Servo) sang r/min (đơn vị của drive)
|
|
/// </summary>
|
|
public int CountsPerRevolution { get; set; } = 4096;
|
|
/// <summary>
|
|
/// Độ phân giải tốc độ đặt (object 0x2026 sub 06): đơn vị lệnh 0x60FF = 1/N r/min.
|
|
/// N = 1..10. Mặc định 10 (0.1 rpm). Nếu firmware không hỗ trợ, driver tự fallback về 1.
|
|
/// </summary>
|
|
public int SpeedResolution { get; set; } = 10;
|
|
/// <summary>
|
|
/// Thời gian tăng tốc (0x6083, ms, 0-32767). ZLAC dùng đơn vị thời gian,
|
|
/// không quy đổi được từ counts/s² nên cấu hình trực tiếp tại đây.
|
|
/// </summary>
|
|
public ushort AccelTimeMs { get; set; } = 200;
|
|
/// <summary>
|
|
/// Thời gian giảm tốc (0x6084, ms, 0-32767)
|
|
/// </summary>
|
|
public ushort DecelTimeMs { get; set; } = 200;
|
|
public bool? AutoReconnectEnabled { get; set; }
|
|
public int? ReconnectDelayMs { get; set; }
|
|
public int? MaxReconnectAttempts { get; set; }
|
|
public int? PdoConfigRetryCount { get; set; }
|
|
public int? PdoConfigRetryTimeoutMs { get; set; }
|
|
public bool? UseHeartbeatCheck { get; set; }
|
|
}
|
|
|
|
/// <summary>
|
|
/// Driver cho ZLAC8015D dual-wheel hub servo (CANopen CiA402).
|
|
/// Đặc thù: MỘT node CAN điều khiển HAI bánh, các object trục dùng sub-index 01 (trái) / 02 (phải);
|
|
/// Controlword/Statusword/Mode (0x6040/0x6041/0x6060) dùng chung sub 0 cho cả hai bánh.
|
|
///
|
|
/// Để giữ nguyên mô hình "mỗi bánh một ICiA402Servo" của DifferentialDrive/OdometryService,
|
|
/// mỗi instance của driver này là một "facade" cho MỘT trục (Axis = Left/Right);
|
|
/// hai instance (trái + phải) trỏ cùng CanInterface + NodeId sẽ dùng chung một CanOpenDevice
|
|
/// (ICanOpenManager cache theo interface + nodeId).
|
|
///
|
|
/// Đơn vị: interface nhận/trả counts và counts/s (giống driver PhenikaaX);
|
|
/// driver quy đổi sang r/min của ZLAC theo CountsPerRevolution.
|
|
/// Lệnh 0x60FF ghi theo độ phân giải 1/SpeedResolution r/min (0x2026:06);
|
|
/// feedback 0x606C cố định 0.1 r/min; feedback 0x6064 là counts (không quy đổi).
|
|
///
|
|
/// Feedback qua TPDO. Firmware ZLAC8015D phát TPDO khi DỮ LIỆU THAY ĐỔI (bỏ qua event
|
|
/// timer 0x18xx:05 — đo thực tế: TPDO đặt statusword ở đầu không bao giờ phát dù position
|
|
/// phía sau đổi liên tục, còn TPDO velocity phát ~240 lần/s theo mỗi thay đổi). Vì vậy:
|
|
/// TPDO1 = 0x6064:01 + 0x6064:02 (position 2 bánh - đổi khi lăn bánh -> có event)
|
|
/// TPDO2 = 0x6041:00 (statusword - phát ngay khi đổi, vd fault)
|
|
/// TPDO3 = 0x606C:01 + 0x606C:02 (velocity 2 bánh)
|
|
/// Mỗi TPDO đặt InhibitTime 50ms để firmware không phát dồn dập theo từng thay đổi.
|
|
/// Hai instance trái/phải đăng ký mapping GIỐNG HỆT nhau (idempotent), mỗi instance
|
|
/// chỉ đọc phần bit của trục mình. Khi TPDO không phát (đứng yên), poll SDO nền làm tươi.
|
|
/// </summary>
|
|
[Device(DeviceType.CiA402Servo, "ZLAC", "ZLAC8015D", "1.0.0", Description = "ZLAC8015D Dual Wheel Hub Servo Driver (CANopen CiA402, 1 node / 2 bánh)")]
|
|
public class Zlac8015dServo : DeviceBase, ICiA402Servo, IDualAxisVelocityCommand
|
|
{
|
|
// Object dictionary ZLAC8015D
|
|
private const ushort ObjControlword = 0x6040; // sub 0, chung 2 bánh
|
|
private const ushort ObjStatusword = 0x6041; // sub 0, chung 2 bánh
|
|
private const ushort ObjModesOfOperation = 0x6060; // sub 0, chung 2 bánh
|
|
private const ushort ObjModesOfOperationDisplay = 0x6061;
|
|
private const ushort ObjPositionActual = 0x6064; // sub 1/2, counts
|
|
private const ushort ObjVelocityActual = 0x606C; // sub 1/2, 0.1 r/min
|
|
private const ushort ObjTargetTorque = 0x6071; // sub 1/2, mA
|
|
private const ushort ObjTargetPosition = 0x607A; // sub 1/2, counts
|
|
private const ushort ObjProfileVelocity = 0x6081; // sub 1/2, r/min (position mode)
|
|
private const ushort ObjProfileAcceleration = 0x6083; // sub 1/2, ms
|
|
private const ushort ObjProfileDeceleration = 0x6084; // sub 1/2, ms
|
|
private const ushort ObjTargetVelocity = 0x60FF; // sub 1/2, 1/SpeedResolution r/min
|
|
private const ushort ObjProducerHeartbeatTime = 0x1017; // sub 0, ms - node tự phát heartbeat
|
|
private const ushort HeartbeatProducerTimeMs = 500; // phải nhỏ hơn HeartbeatConsumerTimeoutMs
|
|
private const int HeartbeatConsumerTimeoutMs = 2000;
|
|
private const int HeartbeatStartupWaitMs = 1500; // ~3 chu kỳ producer
|
|
private const int EnableTimeoutMs = 3000; // tổng thời gian đưa drive lên OperationEnabled
|
|
private const int StateStepDelayMs = 100; // chờ giữa mỗi bước chuyển trạng thái
|
|
// RPDO gộp target velocity của CẢ HAI trục vào MỘT khung: 0x60FF:01 (byte 0-3) +
|
|
// 0x60FF:02 (byte 4-7). Một frame lệnh cả hai bánh cùng lúc -> đồng bộ tuyệt đối,
|
|
// và không đi qua semaphore SDO nên không bị poll feedback chen ngang.
|
|
private const byte DualVelocityRpdoNumber = 1; // RPDO1, COB-ID 0x200 + nodeId
|
|
private const ushort ObjSyncControlFlag = 0x200F; // 0 = asynchronous control
|
|
private const ushort ObjMotorParams = 0x2026; // sub 6 = given speed resolution (1..10)
|
|
private const byte SubSpeedResolution = 0x06;
|
|
|
|
// Feedback 0x606C luôn là 0.1 r/min bất kể 0x2026:06
|
|
private const double FeedbackUnitRpm = 0.1;
|
|
private const int MaxRpm = 1000;
|
|
|
|
private readonly ICanOpenManager? _canOpenManager;
|
|
private readonly string _canInterface;
|
|
private readonly byte _nodeId;
|
|
private readonly Zlac8015dAxis _axis;
|
|
private readonly byte _axisSub; // sub-index của trục: 1 = trái, 2 = phải
|
|
private readonly int _countsPerRevolution;
|
|
private readonly int _configuredSpeedResolution;
|
|
private int _speedResolution; // hiệu lực thực tế (fallback = 1 nếu drive từ chối 0x2026:06)
|
|
private readonly ushort _accelTimeMs;
|
|
private readonly ushort _decelTimeMs;
|
|
private readonly int _pdoConfigRetryCount;
|
|
private readonly int _pdoConfigRetryTimeoutMs;
|
|
private readonly bool _useHeartbeatCheck;
|
|
// Trạng thái runtime của heartbeat check: tắt nếu không ghi được 0x1017 (drive không phát heartbeat)
|
|
private volatile bool _heartbeatCheckActive;
|
|
private readonly ILogger _logger;
|
|
private CanOpenDevice? _canOpenDevice;
|
|
|
|
// TPDO layout dùng chung cho cả 2 trục (xem doc comment của class)
|
|
private const byte PositionTpdoNumber = 1; // 0x6064:01 + 0x6064:02
|
|
private const byte StatuswordTpdoNumber = 2; // 0x6041:00
|
|
private const byte VelocityTpdoNumber = 3; // 0x606C:01 + 0x606C:02
|
|
// Firmware phát TPDO theo thay đổi dữ liệu, inhibit time chặn phát dồn dập (đơn vị 100µs)
|
|
private const ushort TpdoInhibitTime100Us = 500; // 50ms
|
|
|
|
// Registry mapping TPDO của riêng trục này: (index, subIndex) -> info
|
|
private readonly ConcurrentDictionary<(ushort Index, byte SubIndex), PdoMappingInfo> _pdoMappingRegistry = new();
|
|
|
|
// Kế hoạch parse TPDO tính sẵn theo PdoNumber. OnPdoReceived chạy đồng bộ trên thread
|
|
// nhận frame CAN (PdoManager.OnFrameReceived) nên tuyệt đối không LINQ/cấp phát mỗi frame.
|
|
private Dictionary<byte, (PdoMappingInfo[] Mappings, int RequiredBytes)> _tpdoParsePlan = new();
|
|
|
|
// Thread-safe cached values (đơn vị interface: counts, counts/s)
|
|
private readonly Lock _lock = new();
|
|
private Statusword _cachedStatusword;
|
|
private int _cachedPosition;
|
|
private int _cachedVelocity;
|
|
|
|
// Thời điểm TPDO cập nhật từng giá trị lần cuối. Chỉ tin cache khi TPDO còn "tươi";
|
|
// nếu TPDO không về (mapping lỗi/chưa Operational) thì đọc thẳng SDO. Không có
|
|
// fallback này thì Position/Velocity đứng yên ở 0 vĩnh viễn dù drive vẫn chạy.
|
|
private DateTime _statuswordUpdatedAt = DateTime.MinValue;
|
|
private DateTime _positionUpdatedAt = DateTime.MinValue;
|
|
private DateTime _velocityUpdatedAt = DateTime.MinValue;
|
|
private static readonly TimeSpan TpdoCacheTtl = TimeSpan.FromMilliseconds(200);
|
|
|
|
// Poll nền làm tươi feedback khi TPDO không về (xem StartFeedbackPolling)
|
|
private const int FeedbackPollIntervalMs = 50;
|
|
private static readonly TimeSpan TpdoAbsentGracePeriod = TimeSpan.FromSeconds(3);
|
|
private CancellationTokenSource? _feedbackPollCts;
|
|
private Task? _feedbackPollTask;
|
|
private volatile bool _feedbackPollWarned;
|
|
private DateTime? _tpdoAbsentSinceUtc;
|
|
private short _cachedTorque;
|
|
private int _targetPosition;
|
|
|
|
public event EventHandler<StatuswordChangedEventArgs>? StatuswordChanged;
|
|
public event EventHandler<PositionChangedEventArgs>? PositionChanged;
|
|
public event EventHandler<VelocityChangedEventArgs>? VelocityChanged;
|
|
|
|
public Zlac8015dServo(string deviceId, string deviceName, IConfigurationSection connection, IServiceProvider serviceProvider)
|
|
: base(deviceId, deviceName, DeviceType.CiA402Servo)
|
|
{
|
|
var config = new Zlac8015dConfig();
|
|
connection.Bind(config);
|
|
|
|
if (string.IsNullOrWhiteSpace(config.CanInterface))
|
|
throw new InvalidOperationException("CanInterface is required in connection configuration");
|
|
|
|
if (string.IsNullOrWhiteSpace(config.NodeId))
|
|
throw new InvalidOperationException("NodeId is required in connection configuration");
|
|
|
|
if (!Enum.TryParse(config.Axis, ignoreCase: true, out _axis))
|
|
throw new InvalidOperationException($"Invalid Axis: '{config.Axis}'. Must be 'Left' or 'Right'");
|
|
|
|
_canInterface = config.CanInterface;
|
|
|
|
if (!byte.TryParse(config.NodeId, out _nodeId) || _nodeId == 0 || _nodeId > 127)
|
|
throw new InvalidOperationException($"Invalid NodeId: {config.NodeId}. Must be between 1 and 127");
|
|
|
|
if (config.CountsPerRevolution <= 0)
|
|
throw new InvalidOperationException($"Invalid CountsPerRevolution: {config.CountsPerRevolution}. Must be greater than 0");
|
|
|
|
if (config.SpeedResolution < 1 || config.SpeedResolution > 10)
|
|
throw new InvalidOperationException($"Invalid SpeedResolution: {config.SpeedResolution}. Must be between 1 and 10");
|
|
|
|
_countsPerRevolution = config.CountsPerRevolution;
|
|
_configuredSpeedResolution = config.SpeedResolution;
|
|
_speedResolution = config.SpeedResolution;
|
|
_accelTimeMs = config.AccelTimeMs;
|
|
_decelTimeMs = config.DecelTimeMs;
|
|
|
|
_axisSub = _axis == Zlac8015dAxis.Left ? (byte)1 : (byte)2;
|
|
|
|
_pdoConfigRetryCount = config.PdoConfigRetryCount ?? 3;
|
|
_pdoConfigRetryTimeoutMs = config.PdoConfigRetryTimeoutMs ?? 1000;
|
|
_useHeartbeatCheck = config.UseHeartbeatCheck ?? true;
|
|
|
|
AutoReconnectEnabled = config.AutoReconnectEnabled ?? true;
|
|
ReconnectDelayMs = config.ReconnectDelayMs ?? 3000;
|
|
MaxReconnectAttempts = config.MaxReconnectAttempts ?? 0;
|
|
|
|
SetProperty("CanInterface", _canInterface);
|
|
SetProperty("NodeId", _nodeId.ToString());
|
|
SetProperty("Axis", _axis.ToString());
|
|
SetProperty("UseHeartbeatCheck", _useHeartbeatCheck.ToString());
|
|
|
|
_canOpenManager = serviceProvider.GetRequiredService<ICanOpenManager>();
|
|
_logger = serviceProvider.GetRequiredService<ILoggerFactory>().CreateLogger<Zlac8015dServo>();
|
|
|
|
_cachedStatusword = new Statusword(0);
|
|
}
|
|
|
|
protected override IEnumerable<PropertyDescription> CreatePropertyDescriptions()
|
|
{
|
|
yield return new PropertyDescription("CanInterface", "CAN Interface", "Tên CAN interface (ví dụ: can0, can1)")
|
|
{
|
|
DataType = "string",
|
|
IsReadOnly = true,
|
|
DisplayOrder = 1,
|
|
Category = "Kết nối",
|
|
DefaultValue = ""
|
|
};
|
|
|
|
yield return new PropertyDescription("NodeId", "Node ID", "CANOpen Node ID (1-127), chung cho cả 2 bánh")
|
|
{
|
|
DataType = "number",
|
|
IsReadOnly = true,
|
|
DisplayOrder = 2,
|
|
Category = "Kết nối",
|
|
DefaultValue = "1"
|
|
};
|
|
|
|
yield return new PropertyDescription("Axis", "Trục", "Trục motor: Left hoặc Right")
|
|
{
|
|
DataType = "string",
|
|
IsReadOnly = true,
|
|
DisplayOrder = 3,
|
|
Category = "Kết nối",
|
|
DefaultValue = "Left"
|
|
};
|
|
|
|
yield return new PropertyDescription("UseHeartbeatCheck", "Use Heartbeat Check", "Bật/tắt kiểm tra heartbeat")
|
|
{
|
|
DataType = "boolean",
|
|
IsReadOnly = true,
|
|
DisplayOrder = 4,
|
|
Category = "Cấu hình",
|
|
DefaultValue = "true"
|
|
};
|
|
|
|
yield return new PropertyDescription("Statusword", "Statusword", "Trạng thái servo (hex, chung 2 bánh)")
|
|
{
|
|
DataType = "string",
|
|
IsReadOnly = true,
|
|
DisplayOrder = 5,
|
|
Category = "Trạng thái",
|
|
DefaultValue = "0x0000"
|
|
};
|
|
|
|
yield return new PropertyDescription("Position", "Vị trí", "Vị trí hiện tại (encoder counts)")
|
|
{
|
|
DataType = "number",
|
|
IsReadOnly = true,
|
|
DisplayOrder = 6,
|
|
Category = "Trạng thái",
|
|
DefaultValue = "0"
|
|
};
|
|
|
|
yield return new PropertyDescription("Velocity", "Vận tốc", "Vận tốc hiện tại (encoder counts/s)")
|
|
{
|
|
DataType = "number",
|
|
IsReadOnly = true,
|
|
DisplayOrder = 7,
|
|
Category = "Trạng thái",
|
|
DefaultValue = "0"
|
|
};
|
|
|
|
yield return new PropertyDescription("DriveState", "Drive State", "Trạng thái drive")
|
|
{
|
|
DataType = "string",
|
|
IsReadOnly = true,
|
|
DisplayOrder = 8,
|
|
Category = "Trạng thái",
|
|
DefaultValue = "Unknown"
|
|
};
|
|
}
|
|
|
|
protected override async Task OnInitializeAsync(CancellationToken cancellationToken)
|
|
{
|
|
if (_canOpenManager == null)
|
|
{
|
|
throw new InvalidOperationException("ICanOpenManager service is required. Make sure to register it in DI container.");
|
|
}
|
|
|
|
// CanOpenManager cache theo (interface, nodeId) nên 2 instance trái/phải nhận cùng một CanOpenDevice
|
|
_canOpenDevice = await _canOpenManager.GetOrCreateDeviceAsync(_canInterface, _nodeId, cancellationToken);
|
|
|
|
_canOpenDevice.PdoReceived += OnPdoReceived;
|
|
|
|
ConfigurePdos();
|
|
UpdateProperties();
|
|
}
|
|
|
|
protected override async Task OnConnectAsync(CancellationToken cancellationToken)
|
|
{
|
|
if (_canOpenDevice == null)
|
|
throw new InvalidOperationException("Device not initialized. Call InitializeAsync first.");
|
|
|
|
if (_canOpenManager != null)
|
|
{
|
|
var canBus = _canOpenManager.GetCanBus(_canInterface);
|
|
if (canBus == null || !canBus.IsConnected)
|
|
{
|
|
throw new InvalidOperationException($"ICanBus for interface '{_canInterface}' is not connected.");
|
|
}
|
|
}
|
|
|
|
// Hai facade trái/phải dùng chung node vật lý: giữ lock để trình tự
|
|
// PDO config + NMT + tham số của trục này không xen kẽ với trục kia
|
|
await _canOpenDevice.ConfigurationLock.WaitAsync(cancellationToken);
|
|
try
|
|
{
|
|
await ConfigureDevicePdosAsync(cancellationToken);
|
|
|
|
var currentState = _canOpenDevice.State;
|
|
if (currentState != NmtState.PreOperational && currentState != NmtState.Stopped)
|
|
{
|
|
await _canOpenDevice.SendNmtCommandAsync(NmtCommand.PreOperational, cancellationToken);
|
|
await Task.Delay(100, cancellationToken);
|
|
}
|
|
|
|
// Bật heartbeat SAU khi cấu hình PDO: PdoManager gửi NMT Reset Communication,
|
|
// lệnh này reset vùng object 0x1000-0x1FFF về mặc định (0x1017 = 0) nên nếu ghi
|
|
// trước thì heartbeat producer bị xóa và node không bao giờ phát heartbeat.
|
|
if (_useHeartbeatCheck)
|
|
{
|
|
await EnableHeartbeatAsync(cancellationToken);
|
|
}
|
|
|
|
await _canOpenDevice.StartNodeAsync(cancellationToken);
|
|
await VerifyOperationalAsync(cancellationToken);
|
|
|
|
await ConfigureDriveParametersAsync(cancellationToken);
|
|
|
|
bool rpdoReady = _canOpenDevice.Pdo.IsRpdoConfigured(DualVelocityRpdoNumber);
|
|
_logger.LogInformation(
|
|
"ZLAC8015D Node {NodeId} axis {Axis}: lệnh vận tốc đồng bộ qua RPDO1 = {State}. {Detail}",
|
|
_nodeId, _axis, rpdoReady ? "BẬT" : "TẮT",
|
|
rpdoReady
|
|
? "Hai bánh nhận lệnh trong cùng một khung CAN."
|
|
: "Firmware không nhận RPDO -> fallback ghi SDO tuần tự (có thể lệch nhẹ).");
|
|
}
|
|
finally
|
|
{
|
|
_canOpenDevice.ConfigurationLock.Release();
|
|
}
|
|
|
|
StartFeedbackPolling();
|
|
UpdateProperties();
|
|
}
|
|
|
|
/// <summary>
|
|
/// Ghi 0x1017 để drive tự phát heartbeat, bật monitor phía consumer rồi chờ heartbeat đầu tiên về.
|
|
/// Nếu drive không phát (firmware không hỗ trợ 0x1017, hoặc bị Reset Communication xóa) thì
|
|
/// tắt heartbeat check và fallback sang đọc statusword — nếu không, IsAlive không bao giờ true
|
|
/// và device sẽ fail connect vĩnh viễn.
|
|
/// </summary>
|
|
private async Task EnableHeartbeatAsync(CancellationToken ct)
|
|
{
|
|
try
|
|
{
|
|
await _canOpenDevice!.WriteUInt16Async(ObjProducerHeartbeatTime, 0, HeartbeatProducerTimeMs, ct);
|
|
_canOpenDevice.AutoVerifyStateViaHeartbeat = true;
|
|
_canOpenDevice.EnableHeartbeatMonitoring(timeoutMs: HeartbeatConsumerTimeoutMs);
|
|
|
|
if (await WaitForFirstHeartbeatAsync(ct))
|
|
{
|
|
_heartbeatCheckActive = true;
|
|
return;
|
|
}
|
|
|
|
_heartbeatCheckActive = false;
|
|
_canOpenDevice.AutoVerifyStateViaHeartbeat = false;
|
|
_canOpenDevice.DisableHeartbeatMonitoring();
|
|
_logger.LogWarning(
|
|
"ZLAC8015D Node {NodeId}: đã ghi 0x1017 = {TimeMs}ms nhưng không nhận được heartbeat sau {WaitMs}ms. " +
|
|
"Tắt kiểm tra heartbeat, fallback đọc statusword.",
|
|
_nodeId, HeartbeatProducerTimeMs, HeartbeatStartupWaitMs);
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_heartbeatCheckActive = false;
|
|
_logger.LogWarning(ex,
|
|
"ZLAC8015D Node {NodeId}: không ghi được 0x1017 (producer heartbeat = {TimeMs}ms), tắt kiểm tra heartbeat, fallback đọc statusword",
|
|
_nodeId, HeartbeatProducerTimeMs);
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Xác nhận node thật sự vào Operational sau NMT Start. TPDO CHỈ được phát ở Operational,
|
|
/// nên nếu lệnh Start bị bỏ qua thì mọi cấu hình TPDO đúng đến đâu cũng vô nghĩa.
|
|
/// Trạng thái NMT lấy từ heartbeat (byte dữ liệu của frame 0x700+NodeId).
|
|
/// </summary>
|
|
private async Task VerifyOperationalAsync(CancellationToken ct)
|
|
{
|
|
if (!_heartbeatCheckActive)
|
|
return; // Không có heartbeat thì không quan sát được state, bỏ qua
|
|
|
|
const int maxWaitMs = 1500;
|
|
int waited = 0;
|
|
|
|
while (waited < maxWaitMs)
|
|
{
|
|
await Task.Delay(100, ct);
|
|
waited += 100;
|
|
|
|
var nodeInfo = _canOpenDevice!.Heartbeat.GetNodeInfo(_nodeId);
|
|
if (nodeInfo?.LastState == NmtState.Operational)
|
|
{
|
|
_logger.LogInformation("ZLAC8015D Node {NodeId} axis {Axis}: node đã vào Operational, TPDO sẽ bắt đầu phát.",
|
|
_nodeId, _axis);
|
|
return;
|
|
}
|
|
}
|
|
|
|
var lastState = _canOpenDevice!.Heartbeat.GetNodeInfo(_nodeId)?.LastState;
|
|
_logger.LogWarning(
|
|
"ZLAC8015D Node {NodeId} axis {Axis}: sau NMT Start, heartbeat vẫn báo trạng thái {State} (không phải Operational) sau {MaxWaitMs}ms. " +
|
|
"TPDO sẽ không phát — feedback sẽ chạy bằng fallback SDO.",
|
|
_nodeId, _axis, lastState?.ToString() ?? "không rõ", maxWaitMs);
|
|
}
|
|
|
|
private async Task<bool> WaitForFirstHeartbeatAsync(CancellationToken ct)
|
|
{
|
|
int waited = 0;
|
|
while (waited < HeartbeatStartupWaitMs)
|
|
{
|
|
await Task.Delay(50, ct);
|
|
waited += 50;
|
|
|
|
var nodeInfo = _canOpenDevice!.Heartbeat.GetNodeInfo(_nodeId);
|
|
if (nodeInfo != null && nodeInfo.IsAlive)
|
|
return true;
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Ghi các tham số vận hành của ZLAC8015D sau khi node Operational:
|
|
/// chế độ điều khiển bất đồng bộ, độ phân giải tốc độ, thời gian tăng/giảm tốc.
|
|
/// Hai instance trái/phải ghi trùng các object chung (0x200F, 0x2026) — idempotent, không sao.
|
|
/// </summary>
|
|
private async Task ConfigureDriveParametersAsync(CancellationToken ct)
|
|
{
|
|
if (_canOpenDevice == null) return;
|
|
|
|
// 0x200F = 0: asynchronous control (mỗi bánh nhận lệnh độc lập, không chờ frame sync)
|
|
await _canOpenDevice.WriteUInt16Async(ObjSyncControlFlag, 0, 0, ct);
|
|
|
|
// 0x2026:06 - độ phân giải tốc độ đặt (1/N r/min). Firmware cũ có thể không hỗ trợ.
|
|
// QUAN TRỌNG: object này là CHUNG cho cả node nhưng _speedResolution là bản riêng của
|
|
// từng instance trái/phải. Nếu một instance ghi lỗi mà tự đoán fallback = 1 trong khi
|
|
// instance kia đã ghi 10 thành công, thì mọi lệnh SDO của trục lệch sẽ nhỏ hơn 10 lần
|
|
// -> một bánh luôn chạy chậm hơn bánh kia. Vì vậy luôn ĐỌC LẠI giá trị thật từ drive
|
|
// để hai instance và drive thống nhất một độ phân giải.
|
|
try
|
|
{
|
|
await _canOpenDevice.WriteUInt16Async(ObjMotorParams, SubSpeedResolution, (ushort)_configuredSpeedResolution, ct);
|
|
_speedResolution = _configuredSpeedResolution;
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogWarning(ex,
|
|
"ZLAC8015D Node {NodeId} axis {Axis}: không ghi được 0x2026:06 (speed resolution = {Resolution}), sẽ đọc lại giá trị thật từ drive",
|
|
_nodeId, _axis, _configuredSpeedResolution);
|
|
}
|
|
|
|
try
|
|
{
|
|
ushort actual = await _canOpenDevice.ReadUInt16Async(ObjMotorParams, SubSpeedResolution, ct);
|
|
if (actual >= 1 && actual <= 10)
|
|
{
|
|
if (actual != _speedResolution)
|
|
{
|
|
_logger.LogWarning(
|
|
"ZLAC8015D Node {NodeId} axis {Axis}: 0x2026:06 trên drive = {Actual} khác giá trị dự kiến {Expected}, dùng giá trị của drive",
|
|
_nodeId, _axis, actual, _speedResolution);
|
|
}
|
|
_speedResolution = actual;
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
// Không đọc được (firmware không hỗ trợ 0x2026:06) -> chắc chắn không ghi được,
|
|
// drive chạy đơn vị mặc định 1 r/min cho cả hai trục nên fallback 1 là nhất quán.
|
|
_speedResolution = 1;
|
|
_logger.LogWarning(ex,
|
|
"ZLAC8015D Node {NodeId} axis {Axis}: không đọc được 0x2026:06, fallback đơn vị lệnh 1 r/min",
|
|
_nodeId, _axis);
|
|
}
|
|
|
|
_logger.LogInformation("ZLAC8015D Node {NodeId} axis {Axis}: speed resolution hiệu lực = 1/{Resolution} r/min",
|
|
_nodeId, _axis, _speedResolution);
|
|
|
|
// Thời gian tăng/giảm tốc (ms). Ghi cho CẢ HAI trục chứ không chỉ trục mình: khi
|
|
// instance kia connect SAU, bước cấu hình PDO của nó gửi NMT Reset Communication làm
|
|
// drive khôi phục mặc định (đo thực tế: trục đã ghi 200ms bị trả về 10ms) -> hai bánh
|
|
// tăng/giảm tốc lệch nhau 20 lần, một bánh luôn "ì" hơn ở pha quá độ. Hai instance
|
|
// dùng chung AccelTimeMs/DecelTimeMs trong config nên ghi trùng là vô hại.
|
|
for (byte sub = 1; sub <= 2; sub++)
|
|
{
|
|
await _canOpenDevice.WriteUInt32Async(ObjProfileAcceleration, sub, _accelTimeMs, ct);
|
|
await _canOpenDevice.WriteUInt32Async(ObjProfileDeceleration, sub, _decelTimeMs, ct);
|
|
}
|
|
}
|
|
|
|
protected override async Task OnDisconnectAsync(CancellationToken cancellationToken)
|
|
{
|
|
StopFeedbackPolling();
|
|
|
|
if (_canOpenDevice != null)
|
|
{
|
|
// Dừng bánh của trục này trước khi ngắt (node CAN dùng chung với trục còn lại nên không stop node ở đây)
|
|
try
|
|
{
|
|
await _canOpenDevice.WriteInt32Async(ObjTargetVelocity, _axisSub, 0, cancellationToken);
|
|
}
|
|
catch
|
|
{
|
|
// Ignore - bus có thể đã mất
|
|
}
|
|
_canOpenDevice.DisableHeartbeatMonitoring();
|
|
}
|
|
}
|
|
|
|
protected override async Task OnResetAsync(CancellationToken cancellationToken)
|
|
{
|
|
try
|
|
{
|
|
await DisableAsync(cancellationToken);
|
|
await Task.Delay(100, cancellationToken);
|
|
|
|
var state = await GetStateAsync(cancellationToken);
|
|
if (state == DriveState.Fault)
|
|
{
|
|
await FaultResetAsync(cancellationToken);
|
|
await Task.Delay(100, cancellationToken);
|
|
}
|
|
|
|
await ShutdownAsync(cancellationToken);
|
|
await Task.Delay(100, cancellationToken);
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogWarning(ex, "Error occurred during reset for Node {NodeId} axis {Axis}", _nodeId, _axis);
|
|
}
|
|
|
|
UpdateProperties();
|
|
}
|
|
|
|
protected override async Task<bool> OnCheckConnectionAsync(CancellationToken cancellationToken)
|
|
{
|
|
if (_canOpenDevice == null)
|
|
return false;
|
|
|
|
try
|
|
{
|
|
if (_heartbeatCheckActive)
|
|
{
|
|
var nodeInfo = _canOpenDevice.Heartbeat.GetNodeInfo(_nodeId);
|
|
if (nodeInfo == null || !nodeInfo.IsAlive)
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
|
|
await GetStatuswordAsync(cancellationToken);
|
|
return true;
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "Error checking connection for Node {NodeId} axis {Axis}", _nodeId, _axis);
|
|
return false;
|
|
}
|
|
}
|
|
|
|
#region PDO Configuration
|
|
|
|
/// <summary>
|
|
/// Đăng ký TPDO feedback cho trục này lên CanOpenDevice dùng chung.
|
|
/// ZLAC8015D chỉ có 3 TPDO (0x1A03 không tồn tại trên firmware). Firmware phát TPDO
|
|
/// theo THAY ĐỔI dữ liệu chứ không theo event timer (đo thực tế trên bus), nên mỗi TPDO
|
|
/// chỉ chở một loại dữ liệu và object hay thay đổi phải đứng đầu:
|
|
/// TPDO1 = position CẢ HAI trục (0x6064:01 + 0x6064:02) = 8 byte
|
|
/// TPDO2 = statusword (0x6041:00) = 2 byte
|
|
/// TPDO3 = velocity CẢ HAI trục (0x606C:01 + 0x606C:02) = 8 byte
|
|
/// (layout cũ TPDO1/2 = statusword + position từng trục KHÔNG BAO GIỜ phát vì object
|
|
/// đầu tiên - statusword - hầu như không đổi.)
|
|
/// Hai instance cùng đăng ký mapping GIỐNG HỆT nhau (idempotent), mỗi instance chỉ đọc
|
|
/// phần bit của trục mình: trái bit 0-31, phải bit 32-63.
|
|
/// </summary>
|
|
private void ConfigurePdos()
|
|
{
|
|
if (_canOpenDevice == null)
|
|
return;
|
|
|
|
_pdoMappingRegistry.Clear();
|
|
|
|
// TPDO1 position: 0x6064:01 (bit 0-31) + 0x6064:02 (bit 32-63), counts
|
|
var positionTpdo = new PdoConfiguration(PositionTpdoNumber, (uint)(0x180 + (PositionTpdoNumber - 1) * 0x100 + _nodeId))
|
|
{
|
|
EventTimer = 50,
|
|
InhibitTime = TpdoInhibitTime100Us
|
|
};
|
|
positionTpdo.AddMapping(new PdoMapping(ObjPositionActual, 1, 32));
|
|
positionTpdo.AddMapping(new PdoMapping(ObjPositionActual, 2, 32));
|
|
_canOpenDevice.Pdo.ConfigureTPDO(positionTpdo);
|
|
|
|
_pdoMappingRegistry[(ObjPositionActual, _axisSub)] = new PdoMappingInfo
|
|
{
|
|
IsRPDO = false,
|
|
PdoNumber = PositionTpdoNumber,
|
|
BitOffset = (_axisSub - 1) * 32,
|
|
BitLength = 32,
|
|
ObjectIndex = ObjPositionActual,
|
|
SubIndex = _axisSub
|
|
};
|
|
|
|
// TPDO2 statusword: 0x6041:00 (16 bit, chung 2 trục). Statusword ít đổi nên TPDO này
|
|
// hiếm khi phát — nhưng khi đổi (vd fault) sẽ phát ngay lập tức; giữa các lần đổi,
|
|
// poll SDO nền vẫn làm tươi định kỳ.
|
|
var statuswordTpdo = new PdoConfiguration(StatuswordTpdoNumber, (uint)(0x180 + (StatuswordTpdoNumber - 1) * 0x100 + _nodeId))
|
|
{
|
|
EventTimer = 50,
|
|
InhibitTime = TpdoInhibitTime100Us
|
|
};
|
|
statuswordTpdo.AddMapping(new PdoMapping(ObjStatusword, 0, 16));
|
|
_canOpenDevice.Pdo.ConfigureTPDO(statuswordTpdo);
|
|
|
|
_pdoMappingRegistry[(ObjStatusword, (byte)0)] = new PdoMappingInfo
|
|
{
|
|
IsRPDO = false,
|
|
PdoNumber = StatuswordTpdoNumber,
|
|
BitOffset = 0,
|
|
BitLength = 16,
|
|
ObjectIndex = ObjStatusword,
|
|
SubIndex = 0
|
|
};
|
|
|
|
// TPDO3 velocity: 0x606C:01 (bit 0-31) + 0x606C:02 (bit 32-63), đơn vị 0.1 r/min.
|
|
// InhibitTime 50ms để firmware không phát theo từng thay đổi (đo được 240 frame/s khi chạy).
|
|
var velocityTpdo = new PdoConfiguration(VelocityTpdoNumber, (uint)(0x180 + (VelocityTpdoNumber - 1) * 0x100 + _nodeId))
|
|
{
|
|
EventTimer = 50,
|
|
InhibitTime = TpdoInhibitTime100Us
|
|
};
|
|
velocityTpdo.AddMapping(new PdoMapping(ObjVelocityActual, 1, 32));
|
|
velocityTpdo.AddMapping(new PdoMapping(ObjVelocityActual, 2, 32));
|
|
_canOpenDevice.Pdo.ConfigureTPDO(velocityTpdo);
|
|
|
|
_pdoMappingRegistry[(ObjVelocityActual, _axisSub)] = new PdoMappingInfo
|
|
{
|
|
IsRPDO = false,
|
|
PdoNumber = VelocityTpdoNumber,
|
|
BitOffset = (_axisSub - 1) * 32,
|
|
BitLength = 32,
|
|
ObjectIndex = ObjVelocityActual,
|
|
SubIndex = _axisSub
|
|
};
|
|
|
|
// RPDO1 lệnh vận tốc đồng bộ: 0x60FF:01 (bit 0-31) + 0x60FF:02 (bit 32-63).
|
|
// Cả hai instance đăng ký y hệt nhau (idempotent). Nếu firmware không nhận RPDO
|
|
// này thì TrySetBothTargetVelocitiesAsync trả false và tự fallback về SDO tuần tự.
|
|
var dualVelocityRpdo = new PdoConfiguration(DualVelocityRpdoNumber, (uint)(0x200 + _nodeId));
|
|
dualVelocityRpdo.AddMapping(new PdoMapping(ObjTargetVelocity, 1, 32));
|
|
dualVelocityRpdo.AddMapping(new PdoMapping(ObjTargetVelocity, 2, 32));
|
|
_canOpenDevice.Pdo.ConfigureRPDO(dualVelocityRpdo);
|
|
|
|
// Tính sẵn kế hoạch parse cho OnPdoReceived. RequiredBytes theo bit CUỐI CÙNG cần đọc,
|
|
// không phải tổng độ dài: trục phải đọc velocity ở bit 32-63 của TPDO3 nên cần đủ
|
|
// 8 byte dù chỉ map 32 bit.
|
|
_tpdoParsePlan = _pdoMappingRegistry.Values
|
|
.Where(m => !m.IsRPDO)
|
|
.GroupBy(m => m.PdoNumber)
|
|
.ToDictionary(
|
|
g => g.Key,
|
|
g =>
|
|
{
|
|
var mappings = g.OrderBy(m => m.BitOffset).ToArray();
|
|
int lastBit = mappings.Max(m => m.BitOffset + m.BitLength);
|
|
return (mappings, (lastBit + 7) / 8);
|
|
});
|
|
}
|
|
|
|
/// <summary>
|
|
/// Ghi PDO configuration xuống device qua SDO (retry giống driver PhenikaaX)
|
|
/// </summary>
|
|
private async Task ConfigureDevicePdosAsync(CancellationToken ct)
|
|
{
|
|
Exception? lastException = null;
|
|
for (int attempt = 1; attempt <= _pdoConfigRetryCount; attempt++)
|
|
{
|
|
try
|
|
{
|
|
await EnsurePreOperationalStateAsync(ct);
|
|
await _canOpenDevice!.WriteAllPdoConfigurationsToDeviceAsync(null, ct);
|
|
return;
|
|
}
|
|
catch (CanOpenTimeoutException timeoutEx)
|
|
{
|
|
lastException = timeoutEx;
|
|
|
|
_logger.LogWarning("PDO configuration attempt {Attempt}/{MaxRetries} failed for Node {NodeId} axis {Axis}: {Error}",
|
|
attempt, _pdoConfigRetryCount, _nodeId, _axis, timeoutEx.Message);
|
|
|
|
if (attempt < _pdoConfigRetryCount)
|
|
{
|
|
int delayMs = _pdoConfigRetryTimeoutMs * attempt;
|
|
await Task.Delay(delayMs, ct);
|
|
|
|
try
|
|
{
|
|
await _canOpenDevice!.SendNmtCommandAsync(NmtCommand.PreOperational, ct);
|
|
await Task.Delay(200, ct);
|
|
}
|
|
catch
|
|
{
|
|
// Ignore errors
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
throw new InvalidOperationException(
|
|
$"Failed to configure PDOs after {_pdoConfigRetryCount} attempts for Node {_nodeId} axis {_axis}. " +
|
|
$"Last error: {lastException?.Message}", lastException);
|
|
}
|
|
|
|
private async Task EnsurePreOperationalStateAsync(CancellationToken ct)
|
|
{
|
|
var currentState = _canOpenDevice!.State;
|
|
if (currentState != NmtState.PreOperational)
|
|
{
|
|
await _canOpenDevice.SendNmtCommandAsync(NmtCommand.PreOperational, ct);
|
|
await Task.Delay(100, ct);
|
|
}
|
|
}
|
|
|
|
#endregion
|
|
|
|
#region PDO Receive
|
|
|
|
private void OnPdoReceived(object? sender, PdoReceivedEventArgs e)
|
|
{
|
|
var data = e.Data;
|
|
|
|
// Chỉ parse TPDO thuộc trục này (instance của trục kia tự parse TPDO của nó)
|
|
if (!_tpdoParsePlan.TryGetValue(data.PdoNumber, out var plan))
|
|
return;
|
|
|
|
if (data.Data.Length < plan.RequiredBytes)
|
|
{
|
|
_logger.LogWarning("TPDO{Number} data length ({Length}) is less than expected ({Expected})",
|
|
data.PdoNumber, data.Data.Length, plan.RequiredBytes);
|
|
return;
|
|
}
|
|
|
|
// Cập nhật cache trong lock, nhưng bắn event NGOÀI lock: handler này chạy đồng bộ
|
|
// trên thread nhận frame CAN, subscriber chậm (UI/odometry) mà chạy trong lock sẽ
|
|
// chặn cả đường nhận CAN (SDO response, heartbeat, TPDO trục kia) -> trễ dây chuyền
|
|
// cả điều khiển lẫn hiển thị.
|
|
StatuswordChangedEventArgs? statuswordArgs = null;
|
|
PositionChangedEventArgs? positionArgs = null;
|
|
VelocityChangedEventArgs? velocityArgs = null;
|
|
ushort oldStatuswordValue = 0;
|
|
|
|
lock (_lock)
|
|
{
|
|
foreach (var mappingInfo in plan.Mappings)
|
|
{
|
|
try
|
|
{
|
|
byte[] valueBytes = CiA402Helper.ExtractValueFromPdoData(data.Data, mappingInfo.BitOffset, mappingInfo.BitLength);
|
|
|
|
switch (mappingInfo.ObjectIndex)
|
|
{
|
|
case ObjStatusword when valueBytes.Length >= 2:
|
|
var newStatusword = new Statusword(BitConverter.ToUInt16(valueBytes, 0));
|
|
_statuswordUpdatedAt = DateTime.UtcNow;
|
|
|
|
if (newStatusword.Value != _cachedStatusword.Value)
|
|
{
|
|
oldStatuswordValue = _cachedStatusword.Value;
|
|
statuswordArgs = new StatuswordChangedEventArgs(
|
|
newStatusword, _cachedStatusword.GetState(), newStatusword.GetState());
|
|
_cachedStatusword = newStatusword;
|
|
}
|
|
break;
|
|
|
|
case ObjPositionActual when valueBytes.Length >= 4:
|
|
var position = BitConverter.ToInt32(valueBytes, 0);
|
|
_positionUpdatedAt = DateTime.UtcNow;
|
|
|
|
if (position != _cachedPosition)
|
|
{
|
|
positionArgs = new PositionChangedEventArgs(position, _cachedPosition);
|
|
_cachedPosition = position;
|
|
}
|
|
break;
|
|
|
|
case ObjVelocityActual when valueBytes.Length >= 4:
|
|
var velocity = FeedbackRawToCountsPerSec(BitConverter.ToInt32(valueBytes, 0));
|
|
_velocityUpdatedAt = DateTime.UtcNow;
|
|
|
|
if (velocity != _cachedVelocity)
|
|
{
|
|
velocityArgs = new VelocityChangedEventArgs(velocity, _cachedVelocity);
|
|
_cachedVelocity = velocity;
|
|
}
|
|
break;
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "Error processing TPDO{Number} mapping for object 0x{Index:X4}:{SubIndex}",
|
|
mappingInfo.PdoNumber, mappingInfo.ObjectIndex, mappingInfo.SubIndex);
|
|
}
|
|
}
|
|
}
|
|
|
|
if (statuswordArgs != null)
|
|
{
|
|
if (statuswordArgs.NewState != statuswordArgs.OldState)
|
|
{
|
|
_logger.LogInformation(
|
|
"ZLAC8015D Node {NodeId} axis {Axis}: DriveState {OldState} -> {NewState} (statusword 0x{Old:X4} -> 0x{New:X4})",
|
|
_nodeId, _axis, statuswordArgs.OldState, statuswordArgs.NewState,
|
|
oldStatuswordValue, statuswordArgs.Statusword.Value);
|
|
}
|
|
|
|
StatuswordChanged?.Invoke(this, statuswordArgs);
|
|
}
|
|
|
|
if (positionArgs != null)
|
|
PositionChanged?.Invoke(this, positionArgs);
|
|
|
|
if (velocityArgs != null)
|
|
VelocityChanged?.Invoke(this, velocityArgs);
|
|
|
|
// Chỉ tốn công cập nhật property khi thực sự có giá trị thay đổi
|
|
if (statuswordArgs != null || positionArgs != null || velocityArgs != null)
|
|
UpdateProperties();
|
|
}
|
|
|
|
#endregion
|
|
|
|
#region Unit Conversion
|
|
|
|
/// <summary>
|
|
/// counts/s (đơn vị interface) -> giá trị lệnh 0x60FF (1/SpeedResolution r/min), kẹp ±1000 r/min
|
|
/// </summary>
|
|
private int CountsPerSecToCommandRaw(int countsPerSec)
|
|
{
|
|
double rpm = (double)countsPerSec * 60.0 / _countsPerRevolution;
|
|
double clamped = Math.Clamp(rpm, -MaxRpm, MaxRpm);
|
|
return (int)Math.Round(clamped * _speedResolution);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Giá trị feedback 0x606C (0.1 r/min) -> counts/s (đơn vị interface)
|
|
/// </summary>
|
|
private int FeedbackRawToCountsPerSec(int feedbackRaw)
|
|
{
|
|
return (int)Math.Round(feedbackRaw * FeedbackUnitRpm / 60.0 * _countsPerRevolution);
|
|
}
|
|
|
|
/// <summary>
|
|
/// counts/s -> r/min nguyên (dùng cho 0x6081 profile velocity, range 1-1000)
|
|
/// </summary>
|
|
private uint CountsPerSecToRpm(uint countsPerSec)
|
|
{
|
|
double rpm = (double)countsPerSec * 60.0 / _countsPerRevolution;
|
|
return (uint)Math.Clamp(Math.Round(rpm), 1, MaxRpm);
|
|
}
|
|
|
|
#endregion
|
|
|
|
private void UpdateProperties()
|
|
{
|
|
try
|
|
{
|
|
lock (_lock)
|
|
{
|
|
SetProperty("Statusword", $"0x{_cachedStatusword.Value:X4}");
|
|
SetProperty("Position", _cachedPosition.ToString());
|
|
SetProperty("Velocity", _cachedVelocity.ToString());
|
|
|
|
var state = _cachedStatusword.GetState();
|
|
SetProperty("DriveState", state.ToString());
|
|
}
|
|
}
|
|
catch
|
|
{
|
|
// Ignore errors
|
|
}
|
|
}
|
|
|
|
private CanOpenDevice RequireDevice()
|
|
{
|
|
return _canOpenDevice ?? throw new InvalidOperationException("Device not initialized");
|
|
}
|
|
|
|
#region ICiA402Servo Implementation
|
|
|
|
public Statusword CachedStatusword
|
|
{
|
|
get { lock (_lock) { return _cachedStatusword; } }
|
|
}
|
|
|
|
public int CachedPosition
|
|
{
|
|
get { lock (_lock) { return _cachedPosition; } }
|
|
}
|
|
|
|
public int CachedVelocity
|
|
{
|
|
get { lock (_lock) { return _cachedVelocity; } }
|
|
}
|
|
|
|
public short CachedTorque
|
|
{
|
|
get { lock (_lock) { return _cachedTorque; } }
|
|
}
|
|
|
|
// Statusword & Controlword (0x6040/0x6041 sub 0 - chung cho cả 2 bánh)
|
|
public async Task<Statusword> GetStatuswordAsync(CancellationToken ct = default)
|
|
{
|
|
var device = RequireDevice();
|
|
|
|
// Chỉ dùng cache khi TPDO vừa cập nhật trong TTL. Trước đây cache được dùng
|
|
// mãi mãi khi != 0, nên nếu TPDO ngừng về thì GetStateAsync trả giá trị cũ
|
|
// vĩnh viễn và EnableAsync không bao giờ thấy trạng thái tiến triển.
|
|
lock (_lock)
|
|
{
|
|
if (DateTime.UtcNow - _statuswordUpdatedAt < TpdoCacheTtl)
|
|
return _cachedStatusword;
|
|
}
|
|
|
|
return await ReadStatuswordViaSdoAsync(device, ct);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Đọc statusword trực tiếp qua SDO, bỏ qua cache TPDO. State machine dùng hàm này
|
|
/// để mỗi bước chuyển trạng thái đều thấy giá trị mới nhất từ drive.
|
|
/// </summary>
|
|
private async Task<Statusword> ReadStatuswordViaSdoAsync(CanOpenDevice device, CancellationToken ct)
|
|
{
|
|
var value = await device.ReadUInt16Async(ObjStatusword, 0, ct);
|
|
var statusword = new Statusword(value);
|
|
|
|
bool changed;
|
|
Statusword previous;
|
|
lock (_lock)
|
|
{
|
|
previous = _cachedStatusword;
|
|
changed = statusword.Value != previous.Value;
|
|
_cachedStatusword = statusword;
|
|
// Không cập nhật _statuswordUpdatedAt: mốc đó chỉ dành cho TPDO,
|
|
// để lần đọc sau vẫn lấy giá trị tươi nếu TPDO chưa hoạt động.
|
|
}
|
|
|
|
if (changed)
|
|
{
|
|
var oldState = previous.GetState();
|
|
var newState = statusword.GetState();
|
|
|
|
if (newState != oldState)
|
|
{
|
|
_logger.LogInformation(
|
|
"ZLAC8015D Node {NodeId} axis {Axis}: DriveState {OldState} -> {NewState} (statusword 0x{Old:X4} -> 0x{New:X4}, đọc qua SDO)",
|
|
_nodeId, _axis, oldState, newState, previous.Value, statusword.Value);
|
|
}
|
|
|
|
StatuswordChanged?.Invoke(this, new StatuswordChangedEventArgs(statusword, oldState, newState));
|
|
}
|
|
|
|
return statusword;
|
|
}
|
|
|
|
private async Task<DriveState> ReadStateViaSdoAsync(CancellationToken ct)
|
|
{
|
|
var statusword = await ReadStatuswordViaSdoAsync(RequireDevice(), ct);
|
|
return statusword.GetState();
|
|
}
|
|
|
|
public async Task SetControlwordAsync(Controlword controlword, CancellationToken ct = default)
|
|
{
|
|
await RequireDevice().WriteUInt16Async(ObjControlword, 0, controlword.Value, ct);
|
|
}
|
|
|
|
public async Task<DriveState> GetStateAsync(CancellationToken ct = default)
|
|
{
|
|
var statusword = await GetStatuswordAsync(ct);
|
|
return statusword.GetState();
|
|
}
|
|
|
|
// Operation Mode (0x6060 sub 0 - chung cho cả 2 bánh)
|
|
public async Task SetOperationModeAsync(OperationMode mode, CancellationToken ct = default)
|
|
{
|
|
await RequireDevice().WriteUInt8Async(ObjModesOfOperation, 0, (byte)mode, ct);
|
|
}
|
|
|
|
public async Task<OperationMode> GetOperationModeAsync(CancellationToken ct = default)
|
|
{
|
|
var value = await RequireDevice().ReadUInt8Async(ObjModesOfOperationDisplay, 0, ct);
|
|
return (OperationMode)(sbyte)value;
|
|
}
|
|
|
|
// State Machine Control (chung 2 bánh - enable một trục là enable cả drive)
|
|
public async Task FaultResetAsync(CancellationToken ct = default)
|
|
{
|
|
await SetControlwordAsync(Controlword.FaultResetCmd, ct);
|
|
await Task.Delay(100, ct);
|
|
await SetControlwordAsync(new Controlword(0x0000), ct);
|
|
}
|
|
|
|
public async Task ShutdownAsync(CancellationToken ct = default)
|
|
{
|
|
await SetControlwordAsync(Controlword.Shutdown, ct);
|
|
}
|
|
|
|
public async Task SwitchOnAsync(CancellationToken ct = default)
|
|
{
|
|
await SetControlwordAsync(Controlword.SwitchOnCmd, ct);
|
|
}
|
|
|
|
public async Task EnableOperationAsync(CancellationToken ct = default)
|
|
{
|
|
await SetControlwordAsync(Controlword.EnableOperationCmd, ct);
|
|
}
|
|
|
|
public async Task DisableOperationAsync(CancellationToken ct = default)
|
|
{
|
|
await SetControlwordAsync(Controlword.DisableOperation, ct);
|
|
}
|
|
|
|
public async Task QuickStopAsync(CancellationToken ct = default)
|
|
{
|
|
await SetControlwordAsync(Controlword.QuickStopCmd, ct);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Đưa drive lên OperationEnabled theo state machine CiA402.
|
|
/// Chạy vòng lặp cho tới khi đạt trạng thái hoặc hết thời gian, mỗi vòng đọc statusword
|
|
/// tươi qua SDO (không tin cache TPDO), và ném exception kèm statusword nếu thất bại —
|
|
/// bản cũ chạy một lượt tuyến tính rồi im lặng bỏ cuộc nếu drive chưa kịp chuyển trạng thái.
|
|
/// </summary>
|
|
public async Task EnableAsync(CancellationToken ct = default)
|
|
{
|
|
var deadline = DateTime.UtcNow + TimeSpan.FromMilliseconds(EnableTimeoutMs);
|
|
DriveState state = DriveState.Unknown;
|
|
|
|
while (DateTime.UtcNow < deadline)
|
|
{
|
|
state = await ReadStateViaSdoAsync(ct);
|
|
|
|
switch (state)
|
|
{
|
|
case DriveState.OperationEnabled:
|
|
return;
|
|
|
|
case DriveState.NotReadyToSwitchOn:
|
|
// Trạng thái tự động của CiA402: drive đang tự khởi tạo, KHÔNG nhận lệnh
|
|
// controlword nào. Chỉ được chờ nó tự chuyển sang SwitchOnDisabled.
|
|
await Task.Delay(StateStepDelayMs, ct);
|
|
break;
|
|
|
|
case DriveState.Fault:
|
|
await FaultResetAsync(ct);
|
|
await Task.Delay(StateStepDelayMs, ct);
|
|
break;
|
|
|
|
case DriveState.SwitchOnDisabled:
|
|
await ShutdownAsync(ct); // 0x0006 -> ReadyToSwitchOn
|
|
await Task.Delay(StateStepDelayMs, ct);
|
|
break;
|
|
|
|
case DriveState.ReadyToSwitchOn:
|
|
await SwitchOnAsync(ct); // 0x0007 -> SwitchedOn
|
|
await Task.Delay(StateStepDelayMs, ct);
|
|
break;
|
|
|
|
case DriveState.SwitchedOn:
|
|
await EnableOperationAsync(ct); // 0x000F -> OperationEnabled
|
|
await Task.Delay(StateStepDelayMs, ct);
|
|
break;
|
|
|
|
default:
|
|
await Task.Delay(StateStepDelayMs, ct);
|
|
break;
|
|
}
|
|
}
|
|
|
|
var statusword = await ReadStatuswordViaSdoAsync(RequireDevice(), ct);
|
|
throw new InvalidOperationException(
|
|
$"ZLAC8015D Node {_nodeId} axis {_axis}: không đạt được OperationEnabled sau {EnableTimeoutMs}ms. " +
|
|
$"Trạng thái cuối: {state}, statusword = 0x{statusword.Value:X4}. " +
|
|
(statusword.Value == 0
|
|
? "Statusword = 0 nghĩa là drive không trả về trạng thái hợp lệ — kiểm tra nguồn động lực (motor power) và dây encoder."
|
|
: "Kiểm tra fault code và nguồn động lực của drive."));
|
|
}
|
|
|
|
public async Task DisableAsync(CancellationToken ct = default)
|
|
{
|
|
// Đưa tốc độ trục này về 0 trước; controlword là chung nên chỉ disable
|
|
// khi caller thực sự muốn tắt cả drive (DifferentialDrive luôn disable cả 2 bánh)
|
|
try
|
|
{
|
|
await RequireDevice().WriteInt32Async(ObjTargetVelocity, _axisSub, 0, ct);
|
|
}
|
|
catch
|
|
{
|
|
// Ignore - vẫn tiếp tục disable
|
|
}
|
|
await DisableOperationAsync(ct);
|
|
}
|
|
|
|
// Position Control (0x607A sub theo trục, counts)
|
|
/// <summary>
|
|
/// Vị trí thực tế (counts). Ưu tiên cache TPDO khi còn tươi, ngược lại đọc SDO 0x6064:sub.
|
|
/// Trước đây hàm này CHỈ trả cache TPDO nên khi TPDO không về (mapping lỗi) thì
|
|
/// Position đứng yên ở 0 mãi mãi dù động cơ vẫn quay.
|
|
/// </summary>
|
|
public async Task<int> GetActualPositionAsync(CancellationToken ct = default)
|
|
{
|
|
var device = RequireDevice();
|
|
|
|
lock (_lock)
|
|
{
|
|
if (DateTime.UtcNow - _positionUpdatedAt < TpdoCacheTtl)
|
|
return _cachedPosition;
|
|
}
|
|
|
|
var position = await device.ReadInt32Async(ObjPositionActual, _axisSub, ct);
|
|
|
|
bool changed;
|
|
int oldPosition;
|
|
lock (_lock)
|
|
{
|
|
oldPosition = _cachedPosition;
|
|
changed = position != _cachedPosition;
|
|
_cachedPosition = position;
|
|
}
|
|
|
|
if (changed)
|
|
{
|
|
PositionChanged?.Invoke(this, new PositionChangedEventArgs(position, oldPosition));
|
|
UpdateProperties();
|
|
}
|
|
|
|
return position;
|
|
}
|
|
|
|
public async Task SetTargetPositionAsync(int position, CancellationToken ct = default)
|
|
{
|
|
lock (_lock)
|
|
{
|
|
_targetPosition = position;
|
|
}
|
|
|
|
await RequireDevice().WriteInt32Async(ObjTargetPosition, _axisSub, position, ct);
|
|
}
|
|
|
|
public async Task SetProfileSpeedAsync(uint velocity, CancellationToken ct = default)
|
|
{
|
|
// 0x6081 sub theo trục: max speed trong position mode, đơn vị r/min (1-1000)
|
|
await RequireDevice().WriteUInt32Async(ObjProfileVelocity, _axisSub, CountsPerSecToRpm(velocity), ct);
|
|
}
|
|
|
|
public async Task SetProfileVelocityAsync(uint velocity, CancellationToken ct = default)
|
|
{
|
|
// Giống driver PhenikaaX: profile velocity ghi vào target velocity (0x60FF)
|
|
await RequireDevice().WriteInt32Async(ObjTargetVelocity, _axisSub, CountsPerSecToCommandRaw((int)velocity), ct);
|
|
}
|
|
|
|
public async Task SetProfileAccelerationAsync(uint acceleration, CancellationToken ct = default)
|
|
{
|
|
// ZLAC dùng thời gian tăng tốc (ms), không quy đổi được từ counts/s²
|
|
// -> luôn ghi giá trị AccelTimeMs từ config, bỏ qua tham số
|
|
_logger.LogTrace("ZLAC8015D axis {Axis}: SetProfileAcceleration({Value}) -> ghi AccelTimeMs={AccelMs}ms từ config",
|
|
_axis, acceleration, _accelTimeMs);
|
|
await RequireDevice().WriteUInt32Async(ObjProfileAcceleration, _axisSub, _accelTimeMs, ct);
|
|
}
|
|
|
|
public async Task SetProfileDecelerationAsync(uint deceleration, CancellationToken ct = default)
|
|
{
|
|
_logger.LogTrace("ZLAC8015D axis {Axis}: SetProfileDeceleration({Value}) -> ghi DecelTimeMs={DecelMs}ms từ config",
|
|
_axis, deceleration, _decelTimeMs);
|
|
await RequireDevice().WriteUInt32Async(ObjProfileDeceleration, _axisSub, _decelTimeMs, ct);
|
|
}
|
|
|
|
public async Task<uint> GetProfileSpeedAsync(CancellationToken ct = default)
|
|
{
|
|
return await RequireDevice().ReadUInt32Async(ObjProfileVelocity, _axisSub, ct);
|
|
}
|
|
|
|
public async Task<uint> GetProfileAccelerationAsync(CancellationToken ct = default)
|
|
{
|
|
return await RequireDevice().ReadUInt32Async(ObjProfileAcceleration, _axisSub, ct);
|
|
}
|
|
|
|
public async Task<uint> GetProfileDecelerationAsync(CancellationToken ct = default)
|
|
{
|
|
return await RequireDevice().ReadUInt32Async(ObjProfileDeceleration, _axisSub, ct);
|
|
}
|
|
|
|
public async Task StartPositionMoveAsync(CancellationToken ct = default)
|
|
{
|
|
var newSetpoint = new Controlword(0x001F);
|
|
var resetSetpoint = new Controlword(0x000F);
|
|
await SetControlwordAsync(newSetpoint, ct);
|
|
await Task.Delay(50, ct);
|
|
await SetControlwordAsync(resetSetpoint, ct);
|
|
}
|
|
|
|
public async Task MoveToPositionAsync(int position, uint velocity = 1000, uint acceleration = 5000, uint deceleration = 5000, CancellationToken ct = default)
|
|
{
|
|
_logger.LogInformation("Zlac8015dServo[{DeviceId}] axis {Axis}: MoveToPositionAsync position={Position}, velocity={Velocity}",
|
|
DeviceId, _axis, position, velocity);
|
|
|
|
OperationMode currentMode;
|
|
DriveState currentState;
|
|
try
|
|
{
|
|
currentMode = await GetOperationModeAsync(ct);
|
|
currentState = await GetStateAsync(ct);
|
|
}
|
|
catch
|
|
{
|
|
currentMode = OperationMode.ProfilePosition;
|
|
currentState = DriveState.SwitchedOn;
|
|
}
|
|
|
|
bool alreadyReady = currentMode == OperationMode.ProfilePosition && currentState == DriveState.OperationEnabled;
|
|
|
|
if (!alreadyReady)
|
|
{
|
|
await DisableOperationAsync(ct);
|
|
await SetOperationModeAsync(OperationMode.ProfilePosition, ct);
|
|
await Task.Delay(50, ct);
|
|
await EnableAsync(ct);
|
|
await Task.Delay(50, ct);
|
|
}
|
|
|
|
await SetProfileSpeedAsync(velocity, ct);
|
|
await SetProfileAccelerationAsync(acceleration, ct);
|
|
await SetProfileDecelerationAsync(deceleration, ct);
|
|
await SetTargetPositionAsync(position, ct);
|
|
await Task.Delay(150, ct);
|
|
await StartPositionMoveAsync(ct);
|
|
}
|
|
|
|
// Velocity Control (0x60FF sub theo trục)
|
|
/// <summary>
|
|
/// Vận tốc thực tế (counts/s). Ưu tiên cache TPDO khi còn tươi, ngược lại đọc SDO 0x606C:sub.
|
|
/// </summary>
|
|
public async Task<int> GetActualVelocityAsync(CancellationToken ct = default)
|
|
{
|
|
var device = RequireDevice();
|
|
|
|
lock (_lock)
|
|
{
|
|
if (DateTime.UtcNow - _velocityUpdatedAt < TpdoCacheTtl)
|
|
return _cachedVelocity;
|
|
}
|
|
|
|
var raw = await device.ReadInt32Async(ObjVelocityActual, _axisSub, ct);
|
|
var velocity = FeedbackRawToCountsPerSec(raw);
|
|
|
|
bool changed;
|
|
int oldVelocity;
|
|
lock (_lock)
|
|
{
|
|
oldVelocity = _cachedVelocity;
|
|
changed = velocity != _cachedVelocity;
|
|
_cachedVelocity = velocity;
|
|
}
|
|
|
|
if (changed)
|
|
{
|
|
VelocityChanged?.Invoke(this, new VelocityChangedEventArgs(velocity, oldVelocity));
|
|
UpdateProperties();
|
|
}
|
|
|
|
return velocity;
|
|
}
|
|
|
|
public async Task SetTargetVelocityAsync(int velocity, CancellationToken ct = default)
|
|
{
|
|
await RequireDevice().WriteInt32Async(ObjTargetVelocity, _axisSub, CountsPerSecToCommandRaw(velocity), ct);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Gửi vận tốc CẢ HAI trục trong MỘT khung RPDO để hai bánh nhận lệnh đồng thời.
|
|
/// Vì 0x60FF:01 và 0x60FF:02 dùng chung một node vật lý, hai lệnh SDO tuần tự sẽ tới
|
|
/// lệch nhau (nặng hơn khi feedback poll đang chiếm kênh SDO), gây hai bánh chạy lệch.
|
|
/// Nếu RPDO không có, fallback ghi SDO THEO LÔ (giữ kênh SDO liên tục cho cả hai lệnh):
|
|
/// độ lệch giữa hai bánh chỉ còn đúng một vòng SDO (~ms), không bị poll feedback chen
|
|
/// vào giữa như khi caller tự ghi hai lệnh SetTargetVelocityAsync riêng lẻ.
|
|
/// Trả false chỉ khi cả hai đường đều thất bại -> caller fallback về SDO tuần tự.
|
|
/// Đơn vị leftCountsPerSec/rightCountsPerSec giống SetTargetVelocityAsync (đã gồm IsReversed).
|
|
/// </summary>
|
|
public async Task<bool> TrySetBothTargetVelocitiesAsync(int leftCountsPerSec, int rightCountsPerSec, CancellationToken ct = default)
|
|
{
|
|
var device = RequireDevice();
|
|
|
|
// Conversion counts/s -> raw là như nhau cho cả hai trục (cùng countsPerRevolution
|
|
// và speedResolution), nên tính cả hai từ một instance là chính xác.
|
|
int leftRaw = CountsPerSecToCommandRaw(leftCountsPerSec);
|
|
int rightRaw = CountsPerSecToCommandRaw(rightCountsPerSec);
|
|
|
|
if (device.Pdo.IsRpdoConfigured(DualVelocityRpdoNumber))
|
|
{
|
|
var frame = new byte[8];
|
|
BitConverter.TryWriteBytes(frame.AsSpan(0, 4), leftRaw); // 0x60FF:01
|
|
BitConverter.TryWriteBytes(frame.AsSpan(4, 4), rightRaw); // 0x60FF:02
|
|
|
|
try
|
|
{
|
|
await device.Pdo.SendRPDOAsync(DualVelocityRpdoNumber, frame, ct);
|
|
return true;
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogWarning(ex, "ZLAC8015D Node {NodeId}: gửi RPDO vận tốc gộp thất bại, thử fallback SDO theo lô", _nodeId);
|
|
}
|
|
}
|
|
|
|
try
|
|
{
|
|
await device.WriteInt32BatchAsync(
|
|
[
|
|
(ObjTargetVelocity, (byte)1, leftRaw),
|
|
(ObjTargetVelocity, (byte)2, rightRaw)
|
|
], ct);
|
|
return true;
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogWarning(ex, "ZLAC8015D Node {NodeId}: ghi SDO theo lô vận tốc hai trục thất bại", _nodeId);
|
|
return false;
|
|
}
|
|
}
|
|
|
|
public async Task TargetVelocityAsync(int targetVelocity, uint acceleration = 5000, uint deceleration = 5000, CancellationToken ct = default)
|
|
{
|
|
await SetOperationModeAsync(OperationMode.ProfileVelocity, ct);
|
|
await EnableAsync(ct);
|
|
await SetProfileAccelerationAsync(acceleration, ct);
|
|
await SetProfileDecelerationAsync(deceleration, ct);
|
|
await SetTargetVelocityAsync(targetVelocity, ct);
|
|
}
|
|
|
|
public async Task ProfileVelocityAsync(int targetVelocity, uint acceleration = 5000, uint deceleration = 5000, CancellationToken ct = default)
|
|
{
|
|
await SetOperationModeAsync(OperationMode.ProfileVelocity, ct);
|
|
await Task.Delay(50, ct);
|
|
await EnableAsync(ct);
|
|
await SetProfileAccelerationAsync(acceleration, ct);
|
|
await SetProfileDecelerationAsync(deceleration, ct);
|
|
await SetTargetVelocityAsync(targetVelocity, ct);
|
|
}
|
|
|
|
// Torque Control (0x6071 sub theo trục, đơn vị mA)
|
|
public async Task<short> GetActualTorqueAsync(CancellationToken ct = default)
|
|
{
|
|
var value = await RequireDevice().ReadInt16Async(ObjTargetTorque, _axisSub, ct);
|
|
lock (_lock)
|
|
{
|
|
_cachedTorque = value;
|
|
return _cachedTorque;
|
|
}
|
|
}
|
|
|
|
public async Task SetTargetTorqueAsync(short torque, CancellationToken ct = default)
|
|
{
|
|
await RequireDevice().WriteInt16Async(ObjTargetTorque, _axisSub, torque, ct);
|
|
}
|
|
|
|
public async Task RunTorqueAsync(short torque, CancellationToken ct = default)
|
|
{
|
|
await SetOperationModeAsync(OperationMode.ProfileTorque, ct);
|
|
await Task.Delay(50, ct);
|
|
await SetTargetTorqueAsync(torque, ct);
|
|
}
|
|
|
|
// Homing - ZLAC8015D là drive bánh xe hub, không hỗ trợ homing
|
|
public Task SetHomingMethodAsync(byte method, CancellationToken ct = default)
|
|
=> throw new NotSupportedException("ZLAC8015D không hỗ trợ homing");
|
|
|
|
public Task SetHomingSpeedAsync(int speed, CancellationToken ct = default)
|
|
=> throw new NotSupportedException("ZLAC8015D không hỗ trợ homing");
|
|
|
|
public Task SetHomingOffsetAsync(int offset, CancellationToken ct = default)
|
|
=> throw new NotSupportedException("ZLAC8015D không hỗ trợ homing");
|
|
|
|
public Task<byte> GetHomingMethodAsync(CancellationToken ct = default)
|
|
=> throw new NotSupportedException("ZLAC8015D không hỗ trợ homing");
|
|
|
|
public Task<int> GetHomingSpeedAsync(CancellationToken ct = default)
|
|
=> throw new NotSupportedException("ZLAC8015D không hỗ trợ homing");
|
|
|
|
public Task<int> GetHomingOffsetAsync(CancellationToken ct = default)
|
|
=> throw new NotSupportedException("ZLAC8015D không hỗ trợ homing");
|
|
|
|
public Task StartHomingAsync(byte method, int speed, CancellationToken ct = default)
|
|
=> throw new NotSupportedException("ZLAC8015D không hỗ trợ homing");
|
|
|
|
// Target Position Checking
|
|
public bool IsAtTarget(int tolerance = 100, bool useStatusword = true)
|
|
{
|
|
if (useStatusword)
|
|
{
|
|
lock (_lock)
|
|
{
|
|
return _cachedStatusword.TargetReached;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
lock (_lock)
|
|
{
|
|
return Math.Abs(_cachedPosition - _targetPosition) <= tolerance;
|
|
}
|
|
}
|
|
}
|
|
|
|
public async Task WaitUntilAtTargetAsync(int tolerance = 100, bool useStatusword = true, int checkIntervalMs = 10, CancellationToken ct = default)
|
|
{
|
|
while (!ct.IsCancellationRequested)
|
|
{
|
|
if (IsAtTarget(tolerance, useStatusword))
|
|
return;
|
|
|
|
await Task.Delay(checkIntervalMs, ct);
|
|
}
|
|
|
|
throw new OperationCanceledException("Wait for servo at target was cancelled", ct);
|
|
}
|
|
|
|
// Error and Status Information
|
|
public async Task<bool> IsInFaultStateAsync(CancellationToken ct = default)
|
|
{
|
|
var state = await GetStateAsync(ct);
|
|
return state == DriveState.Fault;
|
|
}
|
|
|
|
public async Task<byte> GetErrorRegisterAsync(CancellationToken ct = default)
|
|
{
|
|
try
|
|
{
|
|
return await RequireDevice().ReadUInt8Async(0x1001, 0, ct);
|
|
}
|
|
catch
|
|
{
|
|
return 0;
|
|
}
|
|
}
|
|
|
|
public async Task<ushort[]> GetErrorHistoryAsync(CancellationToken ct = default)
|
|
{
|
|
try
|
|
{
|
|
var device = RequireDevice();
|
|
byte errorCount = await device.ReadUInt8Async(0x1003, 0, ct);
|
|
|
|
if (errorCount == 0) return [];
|
|
|
|
var errors = new List<ushort>();
|
|
for (byte i = 1; i <= errorCount && i <= 8; i++)
|
|
{
|
|
try
|
|
{
|
|
ushort errorCode = await device.ReadUInt16Async(0x1003, i, ct);
|
|
errors.Add(errorCode);
|
|
}
|
|
catch
|
|
{
|
|
break;
|
|
}
|
|
}
|
|
|
|
return [.. errors];
|
|
}
|
|
catch
|
|
{
|
|
return [];
|
|
}
|
|
}
|
|
|
|
public async Task<ushort> GetLatestErrorCodeAsync(CancellationToken ct = default)
|
|
{
|
|
var errorHistory = await GetErrorHistoryAsync(ct);
|
|
return errorHistory.Length > 0 ? errorHistory[0] : (ushort)0x0000;
|
|
}
|
|
|
|
public async Task<bool> TryFaultResetAsync(CancellationToken ct = default)
|
|
{
|
|
var state = await GetStateAsync(ct);
|
|
if (state != DriveState.Fault)
|
|
return false;
|
|
|
|
await FaultResetAsync(ct);
|
|
return true;
|
|
}
|
|
|
|
public async Task<bool> IsEnabledAsync(CancellationToken ct = default)
|
|
{
|
|
var state = await GetStateAsync(ct);
|
|
return state == DriveState.OperationEnabled;
|
|
}
|
|
|
|
public async Task<bool> IsReadyAsync(CancellationToken ct = default)
|
|
{
|
|
var state = await GetStateAsync(ct);
|
|
return state == DriveState.ReadyToSwitchOn || state == DriveState.SwitchedOn;
|
|
}
|
|
|
|
#endregion
|
|
|
|
#region Feedback polling fallback
|
|
|
|
/// <summary>
|
|
/// Vòng poll nền làm tươi Position/Velocity qua SDO khi TPDO không về.
|
|
/// Cần thiết vì OdometryService và UI đọc CachedPosition/CachedVelocity một cách đồng bộ,
|
|
/// mà cache đó chỉ do TPDO ghi — TPDO hỏng thì odometry đứng yên mà không báo lỗi gì.
|
|
/// Nếu TPDO hoạt động, TTL còn tươi nên vòng lặp này không phát sinh SDO nào.
|
|
/// </summary>
|
|
private void StartFeedbackPolling()
|
|
{
|
|
StopFeedbackPolling();
|
|
|
|
_feedbackPollCts = new CancellationTokenSource();
|
|
var ct = _feedbackPollCts.Token;
|
|
|
|
_feedbackPollTask = Task.Run(async () =>
|
|
{
|
|
// Xoay vòng statusword -> position -> velocity, mỗi tick đọc TỐI ĐA MỘT object.
|
|
// Kênh SDO là một hàng đợi tuần tự dùng chung cho cả node (2 trục): nếu mỗi tick
|
|
// đọc cả 3 object x 2 instance thì lệnh điều khiển (controlword, target velocity
|
|
// fallback SDO) phải xếp hàng sau feedback -> điều khiển bị trễ rõ rệt.
|
|
// Với tick 50ms, mỗi giá trị vẫn được làm tươi tối đa mỗi 150ms khi TPDO mất hẳn.
|
|
int pollPhase = 0;
|
|
|
|
while (!ct.IsCancellationRequested)
|
|
{
|
|
try
|
|
{
|
|
await Task.Delay(FeedbackPollIntervalMs, ct);
|
|
|
|
bool statuswordStale, positionStale, velocityStale;
|
|
lock (_lock)
|
|
{
|
|
var now = DateTime.UtcNow;
|
|
statuswordStale = now - _statuswordUpdatedAt >= TpdoCacheTtl;
|
|
positionStale = now - _positionUpdatedAt >= TpdoCacheTtl;
|
|
velocityStale = now - _velocityUpdatedAt >= TpdoCacheTtl;
|
|
}
|
|
|
|
int selected = -1;
|
|
for (int i = 0; i < 3; i++)
|
|
{
|
|
int idx = (pollPhase + i) % 3;
|
|
bool isStale = idx switch { 0 => statuswordStale, 1 => positionStale, _ => velocityStale };
|
|
if (isStale)
|
|
{
|
|
selected = idx;
|
|
pollPhase = idx + 1;
|
|
break;
|
|
}
|
|
}
|
|
|
|
switch (selected)
|
|
{
|
|
// Statusword cũng phải được làm tươi, nếu không _cachedStatusword đứng
|
|
// mãi ở giá trị lần cuối (thường là OperationEnabled sau khi enable xong)
|
|
// và UI báo OperationEnabled vĩnh viễn kể cả khi drive đã fault.
|
|
case 0:
|
|
var before = CachedStatusword.Value;
|
|
var after = await ReadStatuswordViaSdoAsync(RequireDevice(), ct);
|
|
if (after.Value != before)
|
|
UpdateProperties();
|
|
break;
|
|
|
|
case 1:
|
|
await GetActualPositionAsync(ct);
|
|
break;
|
|
|
|
case 2:
|
|
await GetActualVelocityAsync(ct);
|
|
break;
|
|
}
|
|
|
|
// Báo cáo trung thực trạng thái TPDO: chỉ cảnh báo khi TPDO vắng LIÊN TỤC
|
|
// đủ lâu (không tính giai đoạn khởi động, lúc trục kia còn đang reset/cấu hình
|
|
// lại node), và báo lại khi TPDO hoạt động trở lại.
|
|
bool tpdoAlive = !positionStale;
|
|
|
|
if (tpdoAlive)
|
|
{
|
|
_tpdoAbsentSinceUtc = null;
|
|
|
|
if (_feedbackPollWarned)
|
|
{
|
|
_feedbackPollWarned = false;
|
|
_logger.LogInformation(
|
|
"ZLAC8015D Node {NodeId} axis {Axis}: TPDO feedback đã hoạt động trở lại, ngừng fallback SDO.",
|
|
_nodeId, _axis);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
_tpdoAbsentSinceUtc ??= DateTime.UtcNow;
|
|
|
|
if (!_feedbackPollWarned &&
|
|
DateTime.UtcNow - _tpdoAbsentSinceUtc.Value >= TpdoAbsentGracePeriod)
|
|
{
|
|
_feedbackPollWarned = true;
|
|
_logger.LogWarning(
|
|
"ZLAC8015D Node {NodeId} axis {Axis}: không nhận được TPDO suốt {Seconds}s, đang fallback poll Statusword/Position/Velocity qua SDO mỗi {IntervalMs}ms. " +
|
|
"Kiểm tra node đã ở trạng thái Operational chưa.",
|
|
_nodeId, _axis, TpdoAbsentGracePeriod.TotalSeconds, FeedbackPollIntervalMs);
|
|
}
|
|
}
|
|
}
|
|
catch (OperationCanceledException)
|
|
{
|
|
return;
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogDebug(ex, "Feedback poll failed for Node {NodeId} axis {Axis}", _nodeId, _axis);
|
|
}
|
|
}
|
|
}, ct);
|
|
}
|
|
|
|
private void StopFeedbackPolling()
|
|
{
|
|
if (_feedbackPollCts == null)
|
|
return;
|
|
|
|
try
|
|
{
|
|
_feedbackPollCts.Cancel();
|
|
_feedbackPollCts.Dispose();
|
|
}
|
|
catch
|
|
{
|
|
// Ignore
|
|
}
|
|
|
|
_feedbackPollCts = null;
|
|
_feedbackPollTask = null;
|
|
}
|
|
|
|
#endregion
|
|
|
|
protected override void Dispose(bool disposing)
|
|
{
|
|
if (disposing)
|
|
{
|
|
StopFeedbackPolling();
|
|
|
|
if (_canOpenDevice != null)
|
|
{
|
|
_canOpenDevice.PdoReceived -= OnPdoReceived;
|
|
}
|
|
}
|
|
base.Dispose(disposing);
|
|
}
|
|
}
|