557 lines
20 KiB
C#
557 lines
20 KiB
C#
using Microsoft.AspNetCore.Authorization;
|
|
using Microsoft.AspNetCore.SignalR;
|
|
using RobotNet10.CANOpen.CiA402.Enums;
|
|
using RobotNet10.CANOpen.CiA402.Models;
|
|
using RobotNet10.CANOpen.Exceptions;
|
|
using RobotNet10.RobotApp.Client.Shared.Devices;
|
|
using RobotNet10.RobotApp.Devices;
|
|
|
|
namespace RobotNet10.RobotApp.Hubs;
|
|
|
|
/// <summary>
|
|
/// SignalR Hub cho CiA402Servo device - cung cấp real-time servo data
|
|
/// </summary>
|
|
[Authorize]
|
|
public class CiA402ServoHub(IDeviceProvider deviceProvider) : Hub
|
|
{
|
|
/// <summary>
|
|
/// Lấy thông tin device (DeviceName) theo device ID
|
|
/// </summary>
|
|
public async Task<DeviceInfoDto?> GetDeviceInfo(string deviceId)
|
|
{
|
|
var device = deviceProvider.GetDevice(deviceId);
|
|
if (device is not ICiA402Servo)
|
|
{
|
|
return null;
|
|
}
|
|
|
|
return new DeviceInfoDto
|
|
{
|
|
DeviceId = device.DeviceId,
|
|
DeviceName = device.DeviceName
|
|
};
|
|
}
|
|
|
|
/// <summary>
|
|
/// Lấy dữ liệu servo theo device ID
|
|
/// </summary>
|
|
public async Task<CiA402ServoDataDto?> GetServoData(string deviceId)
|
|
{
|
|
var device = deviceProvider.GetDevice(deviceId);
|
|
if (device is not ICiA402Servo servo)
|
|
{
|
|
return null;
|
|
}
|
|
|
|
return await MapToDtoAsync(servo);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Map ICiA402Servo sang CiA402ServoDataDto. Các lệnh đọc SDO (GetOperationMode, GetLatestErrorCode, GetProfile*) có thể timeout
|
|
/// khi bus bận hoặc drive không phản hồi — bắt exception và trả về dữ liệu từ cache + giá trị mặc định.
|
|
/// </summary>
|
|
private static async Task<CiA402ServoDataDto> MapToDtoAsync(ICiA402Servo servo)
|
|
{
|
|
var statusword = servo.CachedStatusword;
|
|
var driveState = statusword.GetState();
|
|
var position = servo.CachedPosition;
|
|
var velocity = servo.CachedVelocity;
|
|
var torque = servo.CachedTorque;
|
|
|
|
OperationMode operationMode = OperationMode.ProfilePosition;
|
|
try
|
|
{
|
|
operationMode = await servo.GetOperationModeAsync();
|
|
}
|
|
catch (CanOpenTimeoutException)
|
|
{
|
|
// SDO timeout (vd. 0x6061) — dùng mặc định, tránh fail hub
|
|
}
|
|
catch (Exception)
|
|
{
|
|
// Lỗi khác khi đọc mode — giữ mặc định
|
|
}
|
|
|
|
ushort errorCode = 0;
|
|
try
|
|
{
|
|
errorCode = await servo.GetLatestErrorCodeAsync();
|
|
}
|
|
catch (CanOpenTimeoutException)
|
|
{
|
|
// SDO timeout — giữ 0
|
|
}
|
|
catch (Exception)
|
|
{
|
|
// Bỏ qua
|
|
}
|
|
|
|
uint profileSpeed = 0, profileAcceleration = 0, profileDeceleration = 0;
|
|
try
|
|
{
|
|
profileSpeed = await servo.GetProfileSpeedAsync();
|
|
profileAcceleration = await servo.GetProfileAccelerationAsync();
|
|
profileDeceleration = await servo.GetProfileDecelerationAsync();
|
|
}
|
|
catch (CanOpenTimeoutException)
|
|
{
|
|
// Timeout — giữ 0
|
|
}
|
|
catch (Exception)
|
|
{
|
|
// Nếu drive chưa sẵn sàng đọc, giữ 0
|
|
}
|
|
|
|
return new CiA402ServoDataDto
|
|
{
|
|
Statusword = statusword.Value,
|
|
DriveState = driveState.ToString(),
|
|
OperationMode = operationMode.ToString(),
|
|
Position = position,
|
|
Velocity = velocity,
|
|
Torque = torque,
|
|
ErrorCode = errorCode,
|
|
IsConnected = servo is not DeviceBase deviceBase || deviceBase.IsConnected,
|
|
ProfileSpeed = profileSpeed,
|
|
ProfileAcceleration = profileAcceleration,
|
|
ProfileDeceleration = profileDeceleration
|
|
};
|
|
}
|
|
|
|
// State Machine Control Methods
|
|
public async Task EnableOperationAsync(string deviceId)
|
|
{
|
|
var device = deviceProvider.GetDevice(deviceId);
|
|
if (device is not ICiA402Servo servo)
|
|
throw new InvalidOperationException($"Device {deviceId} is not a CiA402Servo");
|
|
|
|
await servo.EnableOperationAsync();
|
|
}
|
|
|
|
public async Task DisableOperationAsync(string deviceId)
|
|
{
|
|
var device = deviceProvider.GetDevice(deviceId);
|
|
if (device is not ICiA402Servo servo)
|
|
throw new InvalidOperationException($"Device {deviceId} is not a CiA402Servo");
|
|
|
|
await servo.DisableOperationAsync();
|
|
}
|
|
|
|
public async Task QuickStopAsync(string deviceId)
|
|
{
|
|
var device = deviceProvider.GetDevice(deviceId);
|
|
if (device is not ICiA402Servo servo)
|
|
throw new InvalidOperationException($"Device {deviceId} is not a CiA402Servo");
|
|
|
|
await servo.QuickStopAsync();
|
|
}
|
|
|
|
public async Task FaultResetAsync(string deviceId)
|
|
{
|
|
var device = deviceProvider.GetDevice(deviceId);
|
|
if (device is not ICiA402Servo servo)
|
|
throw new InvalidOperationException($"Device {deviceId} is not a CiA402Servo");
|
|
|
|
await servo.FaultResetAsync();
|
|
}
|
|
|
|
public async Task ShutdownAsync(string deviceId)
|
|
{
|
|
var device = deviceProvider.GetDevice(deviceId);
|
|
if (device is not ICiA402Servo servo)
|
|
throw new InvalidOperationException($"Device {deviceId} is not a CiA402Servo");
|
|
|
|
await servo.ShutdownAsync();
|
|
}
|
|
|
|
public async Task SwitchOnAsync(string deviceId)
|
|
{
|
|
var device = deviceProvider.GetDevice(deviceId);
|
|
if (device is not ICiA402Servo servo)
|
|
throw new InvalidOperationException($"Device {deviceId} is not a CiA402Servo");
|
|
|
|
await servo.SwitchOnAsync();
|
|
}
|
|
|
|
public async Task EnableAsync(string deviceId)
|
|
{
|
|
var device = deviceProvider.GetDevice(deviceId);
|
|
if (device is not ICiA402Servo servo)
|
|
throw new InvalidOperationException($"Device {deviceId} is not a CiA402Servo");
|
|
|
|
await servo.EnableAsync();
|
|
}
|
|
|
|
public async Task DisableAsync(string deviceId)
|
|
{
|
|
var device = deviceProvider.GetDevice(deviceId);
|
|
if (device is not ICiA402Servo servo)
|
|
throw new InvalidOperationException($"Device {deviceId} is not a CiA402Servo");
|
|
|
|
await servo.DisableAsync();
|
|
}
|
|
|
|
// Operation Mode
|
|
public async Task SetOperationModeAsync(string deviceId, string modeString)
|
|
{
|
|
var device = deviceProvider.GetDevice(deviceId);
|
|
if (device is not ICiA402Servo servo)
|
|
throw new InvalidOperationException($"Device {deviceId} is not a CiA402Servo");
|
|
|
|
if (!Enum.TryParse<OperationMode>(modeString, ignoreCase: true, out var mode))
|
|
throw new ArgumentException($"Invalid operation mode: {modeString}");
|
|
|
|
await servo.SetOperationModeAsync(mode);
|
|
}
|
|
|
|
// Position Control
|
|
public async Task SetTargetPositionAsync(string deviceId, int position)
|
|
{
|
|
var device = deviceProvider.GetDevice(deviceId);
|
|
if (device is not ICiA402Servo servo)
|
|
throw new InvalidOperationException($"Device {deviceId} is not a CiA402Servo");
|
|
|
|
await servo.SetTargetPositionAsync(position);
|
|
}
|
|
|
|
public async Task MoveToPositionAsync(string deviceId, int position, uint velocity = 1000, uint acceleration = 5000, uint deceleration = 5000)
|
|
{
|
|
var device = deviceProvider.GetDevice(deviceId);
|
|
if (device is not ICiA402Servo servo)
|
|
throw new InvalidOperationException($"Device {deviceId} is not a CiA402Servo");
|
|
|
|
await servo.MoveToPositionAsync(position, velocity, acceleration, deceleration);
|
|
}
|
|
|
|
// Velocity Control
|
|
public async Task SetTargetVelocityAsync(string deviceId, int velocity)
|
|
{
|
|
var device = deviceProvider.GetDevice(deviceId);
|
|
if (device is not ICiA402Servo servo)
|
|
throw new InvalidOperationException($"Device {deviceId} is not a CiA402Servo");
|
|
|
|
await servo.SetTargetVelocityAsync(velocity);
|
|
}
|
|
|
|
public async Task TargetVelocityAsync(string deviceId, int targetVelocity, uint acceleration = 5000, uint deceleration = 5000)
|
|
{
|
|
var device = deviceProvider.GetDevice(deviceId);
|
|
if (device is not ICiA402Servo servo)
|
|
throw new InvalidOperationException($"Device {deviceId} is not a CiA402Servo");
|
|
|
|
await servo.TargetVelocityAsync(targetVelocity, acceleration, deceleration);
|
|
}
|
|
|
|
public async Task ProfileVelocityAsync(string deviceId, int targetVelocity, uint acceleration = 5000, uint deceleration = 5000)
|
|
{
|
|
var device = deviceProvider.GetDevice(deviceId);
|
|
if (device is not ICiA402Servo servo)
|
|
throw new InvalidOperationException($"Device {deviceId} is not a CiA402Servo");
|
|
|
|
await servo.ProfileVelocityAsync(targetVelocity, acceleration, deceleration);
|
|
}
|
|
|
|
// Torque Control
|
|
public async Task SetTargetTorqueAsync(string deviceId, short torque)
|
|
{
|
|
var device = deviceProvider.GetDevice(deviceId);
|
|
if (device is not ICiA402Servo servo)
|
|
throw new InvalidOperationException($"Device {deviceId} is not a CiA402Servo");
|
|
|
|
await servo.SetTargetTorqueAsync(torque);
|
|
}
|
|
|
|
public async Task RunTorqueAsync(string deviceId, short torque)
|
|
{
|
|
var device = deviceProvider.GetDevice(deviceId);
|
|
if (device is not ICiA402Servo servo)
|
|
throw new InvalidOperationException($"Device {deviceId} is not a CiA402Servo");
|
|
|
|
await servo.RunTorqueAsync(torque);
|
|
}
|
|
|
|
// Profile Settings
|
|
public async Task SetProfileAccelerationAsync(string deviceId, uint acceleration)
|
|
{
|
|
var device = deviceProvider.GetDevice(deviceId);
|
|
if (device is not ICiA402Servo servo)
|
|
throw new InvalidOperationException($"Device {deviceId} is not a CiA402Servo");
|
|
|
|
await servo.SetProfileAccelerationAsync(acceleration);
|
|
}
|
|
|
|
public async Task SetProfileDecelerationAsync(string deviceId, uint deceleration)
|
|
{
|
|
var device = deviceProvider.GetDevice(deviceId);
|
|
if (device is not ICiA402Servo servo)
|
|
throw new InvalidOperationException($"Device {deviceId} is not a CiA402Servo");
|
|
|
|
await servo.SetProfileDecelerationAsync(deceleration);
|
|
}
|
|
|
|
public async Task SetProfileVelocityAsync(string deviceId, uint velocity)
|
|
{
|
|
var device = deviceProvider.GetDevice(deviceId);
|
|
if (device is not ICiA402Servo servo)
|
|
throw new InvalidOperationException($"Device {deviceId} is not a CiA402Servo");
|
|
|
|
await servo.SetProfileVelocityAsync(velocity);
|
|
}
|
|
|
|
public async Task SetProfileSpeedAsync(string deviceId, uint velocity)
|
|
{
|
|
var device = deviceProvider.GetDevice(deviceId);
|
|
if (device is not ICiA402Servo servo)
|
|
throw new InvalidOperationException($"Device {deviceId} is not a CiA402Servo");
|
|
|
|
await servo.SetProfileSpeedAsync(velocity);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Ghi cùng lúc cả 3 profile (speed, acceleration, deceleration) xuống drive qua SDO.
|
|
/// </summary>
|
|
public async Task SetProfileSettingsAsync(string deviceId, uint profileSpeed, uint profileAcceleration, uint profileDeceleration)
|
|
{
|
|
var device = deviceProvider.GetDevice(deviceId);
|
|
if (device is not ICiA402Servo servo)
|
|
throw new InvalidOperationException($"Device {deviceId} is not a CiA402Servo");
|
|
|
|
await servo.SetProfileSpeedAsync(profileSpeed);
|
|
await servo.SetProfileAccelerationAsync(profileAcceleration);
|
|
await servo.SetProfileDecelerationAsync(profileDeceleration);
|
|
}
|
|
|
|
// Homing
|
|
public async Task SetHomingMethodAsync(string deviceId, byte method)
|
|
{
|
|
var device = deviceProvider.GetDevice(deviceId);
|
|
if (device is not ICiA402Servo servo)
|
|
throw new InvalidOperationException($"Device {deviceId} is not a CiA402Servo");
|
|
|
|
await servo.SetHomingMethodAsync(method);
|
|
}
|
|
|
|
public async Task SetHomingSpeedAsync(string deviceId, int speed)
|
|
{
|
|
var device = deviceProvider.GetDevice(deviceId);
|
|
if (device is not ICiA402Servo servo)
|
|
throw new InvalidOperationException($"Device {deviceId} is not a CiA402Servo");
|
|
|
|
await servo.SetHomingSpeedAsync(speed);
|
|
}
|
|
|
|
public async Task SetHomingOffsetAsync(string deviceId, int offset)
|
|
{
|
|
var device = deviceProvider.GetDevice(deviceId);
|
|
if (device is not ICiA402Servo servo)
|
|
throw new InvalidOperationException($"Device {deviceId} is not a CiA402Servo");
|
|
|
|
await servo.SetHomingOffsetAsync(offset);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Đọc lại homing method từ drive để kiểm tra đã ghi xuống chưa.
|
|
/// </summary>
|
|
public async Task<byte> GetHomingMethodAsync(string deviceId)
|
|
{
|
|
var device = deviceProvider.GetDevice(deviceId);
|
|
if (device is not ICiA402Servo servo)
|
|
throw new InvalidOperationException($"Device {deviceId} is not a CiA402Servo");
|
|
return await servo.GetHomingMethodAsync();
|
|
}
|
|
|
|
/// <summary>
|
|
/// Đọc lại homing speed từ drive để kiểm tra đã ghi xuống chưa.
|
|
/// </summary>
|
|
public async Task<int> GetHomingSpeedAsync(string deviceId)
|
|
{
|
|
var device = deviceProvider.GetDevice(deviceId);
|
|
if (device is not ICiA402Servo servo)
|
|
throw new InvalidOperationException($"Device {deviceId} is not a CiA402Servo");
|
|
return await servo.GetHomingSpeedAsync();
|
|
}
|
|
|
|
/// <summary>
|
|
/// Đọc lại homing offset từ drive để kiểm tra đã ghi xuống chưa.
|
|
/// </summary>
|
|
public async Task<int> GetHomingOffsetAsync(string deviceId)
|
|
{
|
|
var device = deviceProvider.GetDevice(deviceId);
|
|
if (device is not ICiA402Servo servo)
|
|
throw new InvalidOperationException($"Device {deviceId} is not a CiA402Servo");
|
|
return await servo.GetHomingOffsetAsync();
|
|
}
|
|
|
|
public async Task StartHomingAsync(string deviceId, byte method, int speed)
|
|
{
|
|
var device = deviceProvider.GetDevice(deviceId);
|
|
if (device is not ICiA402Servo servo)
|
|
throw new InvalidOperationException($"Device {deviceId} is not a CiA402Servo");
|
|
|
|
await servo.StartHomingAsync(method, speed);
|
|
}
|
|
|
|
// Position Control - Additional Methods
|
|
public async Task StartPositionMoveAsync(string deviceId)
|
|
{
|
|
var device = deviceProvider.GetDevice(deviceId);
|
|
if (device is not ICiA402Servo servo)
|
|
throw new InvalidOperationException($"Device {deviceId} is not a CiA402Servo");
|
|
|
|
await servo.StartPositionMoveAsync();
|
|
}
|
|
|
|
public async Task WaitUntilAtTargetAsync(string deviceId, int tolerance = 100, bool useStatusword = true, int checkIntervalMs = 10)
|
|
{
|
|
var device = deviceProvider.GetDevice(deviceId);
|
|
if (device is not ICiA402Servo servo)
|
|
throw new InvalidOperationException($"Device {deviceId} is not a CiA402Servo");
|
|
|
|
await servo.WaitUntilAtTargetAsync(tolerance, useStatusword, checkIntervalMs);
|
|
}
|
|
|
|
// Error and Status Information
|
|
public async Task<bool> IsInFaultStateAsync(string deviceId)
|
|
{
|
|
var device = deviceProvider.GetDevice(deviceId);
|
|
if (device is not ICiA402Servo servo)
|
|
throw new InvalidOperationException($"Device {deviceId} is not a CiA402Servo");
|
|
|
|
return await servo.IsInFaultStateAsync();
|
|
}
|
|
|
|
public async Task<byte> GetErrorRegisterAsync(string deviceId)
|
|
{
|
|
var device = deviceProvider.GetDevice(deviceId);
|
|
if (device is not ICiA402Servo servo)
|
|
throw new InvalidOperationException($"Device {deviceId} is not a CiA402Servo");
|
|
|
|
return await servo.GetErrorRegisterAsync();
|
|
}
|
|
|
|
public async Task<ushort[]> GetErrorHistoryAsync(string deviceId)
|
|
{
|
|
var device = deviceProvider.GetDevice(deviceId);
|
|
if (device is not ICiA402Servo servo)
|
|
throw new InvalidOperationException($"Device {deviceId} is not a CiA402Servo");
|
|
|
|
return await servo.GetErrorHistoryAsync();
|
|
}
|
|
|
|
public async Task<ushort> GetLatestErrorCodeAsync(string deviceId)
|
|
{
|
|
var device = deviceProvider.GetDevice(deviceId);
|
|
if (device is not ICiA402Servo servo)
|
|
throw new InvalidOperationException($"Device {deviceId} is not a CiA402Servo");
|
|
|
|
return await servo.GetLatestErrorCodeAsync();
|
|
}
|
|
|
|
public async Task<bool> TryFaultResetAsync(string deviceId)
|
|
{
|
|
var device = deviceProvider.GetDevice(deviceId);
|
|
if (device is not ICiA402Servo servo)
|
|
throw new InvalidOperationException($"Device {deviceId} is not a CiA402Servo");
|
|
|
|
return await servo.TryFaultResetAsync();
|
|
}
|
|
|
|
public async Task<bool> IsEnabledAsync(string deviceId)
|
|
{
|
|
var device = deviceProvider.GetDevice(deviceId);
|
|
if (device is not ICiA402Servo servo)
|
|
throw new InvalidOperationException($"Device {deviceId} is not a CiA402Servo");
|
|
|
|
return await servo.IsEnabledAsync();
|
|
}
|
|
|
|
public async Task<bool> IsReadyAsync(string deviceId)
|
|
{
|
|
var device = deviceProvider.GetDevice(deviceId);
|
|
if (device is not ICiA402Servo servo)
|
|
throw new InvalidOperationException($"Device {deviceId} is not a CiA402Servo");
|
|
|
|
return await servo.IsReadyAsync();
|
|
}
|
|
|
|
// Statusword & Controlword
|
|
public async Task<ushort> GetStatuswordAsync(string deviceId)
|
|
{
|
|
var device = deviceProvider.GetDevice(deviceId);
|
|
if (device is not ICiA402Servo servo)
|
|
throw new InvalidOperationException($"Device {deviceId} is not a CiA402Servo");
|
|
|
|
var statusword = await servo.GetStatuswordAsync();
|
|
return statusword.Value;
|
|
}
|
|
|
|
public async Task SetControlwordAsync(string deviceId, ushort controlwordValue)
|
|
{
|
|
var device = deviceProvider.GetDevice(deviceId);
|
|
if (device is not ICiA402Servo servo)
|
|
throw new InvalidOperationException($"Device {deviceId} is not a CiA402Servo");
|
|
|
|
var controlword = new Controlword(controlwordValue);
|
|
await servo.SetControlwordAsync(controlword);
|
|
}
|
|
|
|
public async Task<string> GetStateAsync(string deviceId)
|
|
{
|
|
var device = deviceProvider.GetDevice(deviceId);
|
|
if (device is not ICiA402Servo servo)
|
|
throw new InvalidOperationException($"Device {deviceId} is not a CiA402Servo");
|
|
|
|
var state = await servo.GetStateAsync();
|
|
return state.ToString();
|
|
}
|
|
|
|
public async Task<string> GetOperationModeAsync(string deviceId)
|
|
{
|
|
var device = deviceProvider.GetDevice(deviceId);
|
|
if (device is not ICiA402Servo servo)
|
|
throw new InvalidOperationException($"Device {deviceId} is not a CiA402Servo");
|
|
|
|
var mode = await servo.GetOperationModeAsync();
|
|
return mode.ToString();
|
|
}
|
|
|
|
// Position, Velocity, Torque
|
|
public async Task<int> GetActualPositionAsync(string deviceId)
|
|
{
|
|
var device = deviceProvider.GetDevice(deviceId);
|
|
if (device is not ICiA402Servo servo)
|
|
throw new InvalidOperationException($"Device {deviceId} is not a CiA402Servo");
|
|
|
|
return await servo.GetActualPositionAsync();
|
|
}
|
|
|
|
public async Task<int> GetActualVelocityAsync(string deviceId)
|
|
{
|
|
var device = deviceProvider.GetDevice(deviceId);
|
|
if (device is not ICiA402Servo servo)
|
|
throw new InvalidOperationException($"Device {deviceId} is not a CiA402Servo");
|
|
|
|
return await servo.GetActualVelocityAsync();
|
|
}
|
|
|
|
public async Task<short> GetActualTorqueAsync(string deviceId)
|
|
{
|
|
var device = deviceProvider.GetDevice(deviceId);
|
|
if (device is not ICiA402Servo servo)
|
|
throw new InvalidOperationException($"Device {deviceId} is not a CiA402Servo");
|
|
|
|
return await servo.GetActualTorqueAsync();
|
|
}
|
|
|
|
// Target Position Checking
|
|
public Task<bool> IsAtTarget(string deviceId, int tolerance = 100, bool useStatusword = true)
|
|
{
|
|
var device = deviceProvider.GetDevice(deviceId);
|
|
if (device is not ICiA402Servo servo)
|
|
throw new InvalidOperationException($"Device {deviceId} is not a CiA402Servo");
|
|
|
|
return Task.FromResult(servo.IsAtTarget(tolerance, useStatusword));
|
|
}
|
|
}
|
|
|