251 lines
11 KiB
C#
251 lines
11 KiB
C#
// using RobotNet.VDA5050.Type;
|
|
// using RobotNet10.RobotApp.Devices;
|
|
// using RobotNet10.RobotApp.Services.Exceptions;
|
|
// using System.Globalization;
|
|
|
|
// namespace RobotNet10.RobotApp.Services.Robot.Actions;
|
|
|
|
// /// <summary>
|
|
// /// Action đưa camera (lift) tới vị trí theo chiều cao (m).
|
|
// /// Gọi trực tiếp xuống động cơ CiA402 (giống device/hub), không qua LiftModule state machine.
|
|
// /// actionParameters: HEIGHT (unit: m), ví dụ "0.825".
|
|
// /// blockingType NONE: gửi lệnh di chuyển và kết thúc ngay, không chờ hoàn thành.
|
|
// /// </summary>
|
|
// [RobotAction(ActionType.LIFT_CAMERA_BY_HEIGHT,
|
|
// [ActionScope.INSTANT, ActionScope.NODE, ActionScope.EDGE],
|
|
// [BlockingType.NONE, BlockingType.HARD, BlockingType.SOFT],
|
|
// "Lift camera with height (unit: m).",
|
|
// "Lift camera move requested.")]
|
|
// public class LiftCameraByHeightAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider)
|
|
// {
|
|
// private double _heightM;
|
|
// private double _timeoutMs = 20 * 1000;
|
|
|
|
// protected override void Initialize()
|
|
// {
|
|
// base.Initialize();
|
|
|
|
// var heightParam = Action?.ActionParameters?.FirstOrDefault(p =>
|
|
// string.Equals(p.Key, "HEIGHT", StringComparison.OrdinalIgnoreCase));
|
|
// if (heightParam is null || string.IsNullOrWhiteSpace(heightParam.Value))
|
|
// {
|
|
// throw new ActionException("LiftCameraByHeight requires actionParameter HEIGHT (unit: m).");
|
|
// }
|
|
|
|
// if (!double.TryParse(heightParam.Value, NumberStyles.Any, CultureInfo.InvariantCulture, out _heightM))
|
|
// {
|
|
// throw new ActionException($"HEIGHT value '{heightParam.Value}' is not a valid number.");
|
|
// }
|
|
// }
|
|
|
|
// protected override async Task StartAction()
|
|
// {
|
|
// try
|
|
// {
|
|
// var config = ServiceProvider.GetRequiredService<IConfiguration>();
|
|
// var deviceProvider = ServiceProvider.GetRequiredService<IDeviceProvider>();
|
|
|
|
// var deviceId = config.GetValue<string>("Modules:LiftModule:DeviceId") ?? "lift-motor";
|
|
// var enable = config.GetValue<bool>("Modules:LiftModule:Enable", true);
|
|
// if (!enable)
|
|
// {
|
|
// SetStatus(ActionEvent.FAILED);
|
|
// ResultDescription = "Lift module is disabled in config.";
|
|
// return;
|
|
// }
|
|
|
|
// var device = deviceProvider.GetDevice(deviceId);
|
|
// if (device is not ICiA402Servo servo)
|
|
// {
|
|
// SetStatus(ActionEvent.FAILED);
|
|
// ResultDescription = $"Device '{deviceId}' not found or is not ICiA402Servo.";
|
|
// return;
|
|
// }
|
|
|
|
// // Đọc config để map height (m) -> position (encoder), giống LiftModuleService.GetPositionFromHeightM
|
|
// var minHeightM = config.GetValue<double>("Modules:LiftModule:MinHeightM", 0);
|
|
// var maxHeightM = config.GetValue<double>("Modules:LiftModule:MaxHeightM", 1);
|
|
// var minPosition = config.GetValue<int>("Modules:LiftModule:MinPosition", 0);
|
|
// var maxPosition = config.GetValue<int>("Modules:LiftModule:MaxPosition", 1000000);
|
|
|
|
// int position;
|
|
// if (maxHeightM <= minHeightM)
|
|
// {
|
|
// position = minPosition;
|
|
// }
|
|
// else
|
|
// {
|
|
// var t = Math.Clamp((_heightM - minHeightM) / (maxHeightM - minHeightM), 0, 1);
|
|
// position = minPosition + (int)(t * (maxPosition - minPosition));
|
|
// }
|
|
|
|
// var velocity = config.GetValue<uint>("Modules:LiftModule:ProfileVelocity", 100000);
|
|
// var acceleration = config.GetValue<uint>("Modules:LiftModule:ProfileAcceleration", 50000);
|
|
// var deceleration = config.GetValue<uint>("Modules:LiftModule:ProfileDeceleration", 50000);
|
|
|
|
// Logger?.LogInformation("LiftCameraByHeight: calling servo directly (deviceId={DeviceId}), MoveToPositionAsync(position={Position}, height={HeightM} m).", deviceId, position, _heightM);
|
|
// await servo.MoveToPositionAsync(position, velocity, acceleration, deceleration, CancellationToken.None);
|
|
|
|
// // Timeout = (int)_timeoutMs;
|
|
// SetStatus(ActionEvent.FINISHED);
|
|
// ResultDescription = $"Lift camera move to height {_heightM} m requested (direct to drive).";
|
|
// }
|
|
// catch (Exception ex)
|
|
// {
|
|
// SetStatus(ActionEvent.FAILED);
|
|
// ResultDescription = $"Lift camera by height failed: {ex.Message}";
|
|
// }
|
|
|
|
// await base.StartAction();
|
|
// }
|
|
|
|
// // protected override async Task ExecuteAction()
|
|
// // {
|
|
// // // to do: wait for the lift camera to reach the target height
|
|
|
|
// // }
|
|
|
|
// // protected override async Task CleanupAction()
|
|
// // {
|
|
// // // to do: cleanup the lift camera
|
|
// // }
|
|
// }
|
|
|
|
using RobotNet.VDA5050.Type;
|
|
using RobotNet10.CANOpen.CiA402.Enums;
|
|
using RobotNet10.RobotApp.Devices;
|
|
using RobotNet10.RobotApp.Services.Exceptions;
|
|
using System.Globalization;
|
|
|
|
namespace RobotNet10.RobotApp.Services.Robot.Actions;
|
|
|
|
/// <summary>
|
|
/// Action đưa camera (lift) tới vị trí theo chiều cao (m).
|
|
/// Gọi trực tiếp xuống động cơ CiA402 (giống device/hub), không qua LiftModule state machine.
|
|
/// actionParameters: HEIGHT (unit: m), ví dụ "0.825".
|
|
/// blockingType NONE: gửi lệnh di chuyển và kết thúc ngay, không chờ hoàn thành.
|
|
/// </summary>
|
|
[RobotAction(ActionType.LIFT_CAMERA_BY_HEIGHT,
|
|
[ActionScope.INSTANT, ActionScope.NODE, ActionScope.EDGE],
|
|
[BlockingType.NONE, BlockingType.HARD],
|
|
"Lift camera with height (unit: m).",
|
|
"Lift camera move requested.")]
|
|
public class LiftCameraByHeightAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider)
|
|
{
|
|
private double _heightM;
|
|
|
|
protected override void Initialize()
|
|
{
|
|
base.Initialize();
|
|
|
|
var heightParam = Action?.ActionParameters?.FirstOrDefault(p =>
|
|
string.Equals(p.Key, "HEIGHT", StringComparison.OrdinalIgnoreCase));
|
|
if (heightParam is null || string.IsNullOrWhiteSpace(heightParam.Value))
|
|
{
|
|
throw new ActionException("LiftCameraByHeight requires actionParameter HEIGHT (unit: m).");
|
|
}
|
|
|
|
if (!double.TryParse(heightParam.Value, NumberStyles.Any, CultureInfo.InvariantCulture, out _heightM))
|
|
{
|
|
throw new ActionException($"HEIGHT value '{heightParam.Value}' is not a valid number.");
|
|
}
|
|
}
|
|
|
|
protected override async Task StartAction()
|
|
{
|
|
try
|
|
{
|
|
var config = ServiceProvider.GetRequiredService<IConfiguration>();
|
|
var deviceProvider = ServiceProvider.GetRequiredService<IDeviceProvider>();
|
|
|
|
var deviceId = config.GetValue<string>("Modules:LiftModule:DeviceId") ?? "lift-motor";
|
|
var enable = config.GetValue<bool>("Modules:LiftModule:Enable", true);
|
|
if (!enable)
|
|
{
|
|
SetStatus(ActionEvent.FAILED);
|
|
ResultDescription = "Lift module is disabled in config.";
|
|
return;
|
|
}
|
|
|
|
var device = deviceProvider.GetDevice(deviceId);
|
|
if (device is not ICiA402Servo servo)
|
|
{
|
|
SetStatus(ActionEvent.FAILED);
|
|
ResultDescription = $"Device '{deviceId}' not found or is not ICiA402Servo.";
|
|
return;
|
|
}
|
|
|
|
// Đọc config để map height (m) -> position (encoder), giống LiftModuleService.GetPositionFromHeightM
|
|
var minHeightM = config.GetValue<double>("Modules:LiftModule:MinHeightM", 0);
|
|
var maxHeightM = config.GetValue<double>("Modules:LiftModule:MaxHeightM", 1);
|
|
var minPosition = config.GetValue<int>("Modules:LiftModule:MinPosition", 0);
|
|
var maxPosition = config.GetValue<int>("Modules:LiftModule:MaxPosition", 1000000);
|
|
|
|
int position;
|
|
if (maxHeightM <= minHeightM)
|
|
{
|
|
position = minPosition;
|
|
}
|
|
else
|
|
{
|
|
var t = Math.Clamp((_heightM - minHeightM) / (maxHeightM - minHeightM), 0, 1);
|
|
position = minPosition + (int)(t * (maxPosition - minPosition));
|
|
}
|
|
|
|
var velocity = config.GetValue<uint>("Modules:LiftModule:ProfileVelocity", 100000);
|
|
var acceleration = config.GetValue<uint>("Modules:LiftModule:ProfileAcceleration", 50000);
|
|
var deceleration = config.GetValue<uint>("Modules:LiftModule:ProfileDeceleration", 50000);
|
|
var tolerance = config.GetValue<int>("Modules:LiftModule:ActionTargetTolerance", 900);
|
|
var checkIntervalMs = config.GetValue<int>("Modules:LiftModule:ActionStatusWordCheckIntervalMs", 100);
|
|
var timeoutMs = config.GetValue<int>("Modules:LiftModule:ActionMoveTimeoutMs", 300000);
|
|
|
|
Logger?.LogInformation("LiftCameraByHeight: calling servo directly (deviceId={DeviceId}), MoveToPositionAsync(position={Position}, height={HeightM} m).", deviceId, position, _heightM);
|
|
await servo.MoveToPositionAsync(position, velocity, acceleration, deceleration, CancellationToken.None);
|
|
await WaitForMovementCompletedAsync(servo, position, tolerance, checkIntervalMs, timeoutMs, CancellationToken.None);
|
|
|
|
SetStatus(ActionEvent.FINISHED);
|
|
ResultDescription = $"Lift camera reached height {_heightM} m (direct to drive).";
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
SetStatus(ActionEvent.FAILED);
|
|
ResultDescription = $"Lift camera by height failed: {ex.Message}";
|
|
}
|
|
|
|
await base.StartAction();
|
|
}
|
|
|
|
private static async Task WaitForMovementCompletedAsync(
|
|
ICiA402Servo servo,
|
|
int targetPosition,
|
|
int tolerance,
|
|
int checkIntervalMs,
|
|
int timeoutMs,
|
|
CancellationToken ct)
|
|
{
|
|
var startedAt = DateTime.UtcNow;
|
|
var pollInterval = Math.Max(10, checkIntervalMs);
|
|
var maxWait = TimeSpan.FromMilliseconds(Math.Max(1000, timeoutMs));
|
|
|
|
while (DateTime.UtcNow - startedAt < maxWait)
|
|
{
|
|
var statusword = await servo.GetStatuswordAsync(ct);
|
|
if (statusword.GetState() == DriveState.Fault)
|
|
{
|
|
throw new ActionException("Lift movement failed: servo entered fault state.");
|
|
}
|
|
|
|
var currentPosition = await servo.GetActualPositionAsync(ct);
|
|
if (statusword.TargetReached && Math.Abs(currentPosition - targetPosition) <= Math.Max(50, tolerance))
|
|
{
|
|
return;
|
|
}
|
|
|
|
await Task.Delay(pollInterval, ct);
|
|
}
|
|
|
|
throw new TimeoutException($"Lift movement timeout after {maxWait.TotalSeconds:F0}s.");
|
|
}
|
|
}
|
|
|