Initial commit
This commit is contained in:
@@ -0,0 +1,250 @@
|
||||
// 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.");
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user