96 lines
3.7 KiB
C#
96 lines
3.7 KiB
C#
using RobotNet.VDA5050.Factsheet;
|
|
using RobotNet.VDA5050.Type;
|
|
using RobotNet10.RobotApp.Interfaces;
|
|
using RobotNet10.RobotApp.Modules;
|
|
using RobotNet10.RobotApp.Services.Exceptions;
|
|
|
|
namespace RobotNet10.RobotApp.Services.Robot.Actions;
|
|
|
|
[RobotAction(ActionType.LIFT_ROTATE,
|
|
[ActionScope.INSTANT, ActionScope.NODE, ActionScope.EDGE],
|
|
[BlockingType.HARD],
|
|
"Xoay bàn nâng của robot.",
|
|
"Robot đã xoay bàn nâng.")]
|
|
public class RobotLiftRotateAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider)
|
|
{
|
|
private static readonly IReadOnlyList<ActionParameter> ActionParameters = new List<ActionParameter>
|
|
{
|
|
new()
|
|
{
|
|
Key = "angle",
|
|
Description = "Góc xoay của bàn nâng. (rad)",
|
|
ValueDataType = ValueDataType.FLOAT,
|
|
IsOptional = false,
|
|
}
|
|
}.AsReadOnly();
|
|
private double Angle = 0; // Degree
|
|
private IRotationModule? RotationModule;
|
|
private IPlcController? PlcController;
|
|
|
|
private const int MAX_TIMEOUT = 60000 * 2;
|
|
private int CountTimeout = 0;
|
|
|
|
protected override Task StartAction()
|
|
{
|
|
RotationModule = ServiceProvider.GetRequiredService<IRotationModule>();
|
|
PlcController = ServiceProvider.GetRequiredService<IPlcController>();
|
|
PlcController.SetOperationState(OperationState.LiftRotating);
|
|
RotationModule.RotateToAngleAsync(Angle);
|
|
return base.StartAction();
|
|
}
|
|
|
|
protected override Task CleanupAction()
|
|
{
|
|
PlcController?.SetOperationState(OperationState.None);
|
|
return base.CleanupAction();
|
|
}
|
|
|
|
protected override async Task ExecuteAction()
|
|
{
|
|
if (RotationModule is null)
|
|
{
|
|
SetStatus(ActionEvent.FAILED);
|
|
ResultDescription = "Không tìm thấy mô-đun xoay.";
|
|
}
|
|
else if (RotationModule.State == RotationModuleState.Error)
|
|
{
|
|
SetStatus(ActionEvent.FAILED);
|
|
ResultDescription = "Mô-đun xoay gặp lỗi.";
|
|
|
|
}
|
|
else if (RotationModule.State == RotationModuleState.Ready && Math.Abs(await RotationModule.GetCurrentAngleAsync() - Angle) < 1)
|
|
{
|
|
SetStatus(ActionEvent.FINISHED);
|
|
ResultDescription = string.IsNullOrEmpty(ActionAttribute.ResultDescription) ? ResultDescription : ActionAttribute.ResultDescription;
|
|
}
|
|
else if (CountTimeout++ >= MAX_TIMEOUT / ActionInterval)
|
|
{
|
|
SetStatus(ActionEvent.FAILED);
|
|
ResultDescription = "Timeout action";
|
|
}
|
|
await base.ExecuteAction();
|
|
}
|
|
|
|
protected override void Initialize()
|
|
{
|
|
base.Initialize();
|
|
|
|
if (ActionParameters.Count > 0)
|
|
{
|
|
foreach (var parameterStore in ActionParameters)
|
|
{
|
|
if (!parameterStore.IsOptional)
|
|
{
|
|
if (Action is null || Action.ActionParameters is null || !Action.ActionParameters.Any(a => a.Key == parameterStore.Key)) throw new ActionException($"Thiếu tham số bắt buộc '{parameterStore.Key}' cho action {Type}.");
|
|
}
|
|
}
|
|
Parameters = Action is null || Action.ActionParameters is null ? [] : Action.ActionParameters;
|
|
}
|
|
|
|
var anglePara = Parameters.FirstOrDefault(p => p.Key == "angle") ?? throw new ActionException($"Action {Type} không tìm thấy parameter key 'angle'");
|
|
var angleParse = double.TryParse(anglePara.Value, out double angleData);
|
|
if (!angleParse) throw new ActionException($"Action {Type} có parameter 'angle' không đúng kiểu dữ liệu");
|
|
Angle = angleData * 180 / Math.PI;
|
|
}
|
|
}
|