129 lines
4.9 KiB
C#
129 lines
4.9 KiB
C#
using RobotNet.VDA5050.Factsheet;
|
|
using RobotNet.VDA5050.Type;
|
|
using RobotNet10.RobotApp.Interfaces;
|
|
using RobotNet10.RobotApp.Services.Exceptions;
|
|
using RobotNet10.RobotApp.Shared.Enums;
|
|
using System.Collections.ObjectModel;
|
|
|
|
namespace RobotNet10.RobotApp.Services.Robot.Actions;
|
|
|
|
[RobotAction(ActionType.MOVE_STRAIGHT_TO_COOR,
|
|
[ActionScope.INSTANT],
|
|
[BlockingType.HARD],
|
|
"Di chuyển thẳng đến tọa độ xác định.",
|
|
"Robot đã di chuyển thẳng đến tọa độ xác định.")]
|
|
public class RobotMoveStraightToCoorAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider)
|
|
{
|
|
private static readonly ReadOnlyCollection<ActionParameter> ActionParameters = new List<ActionParameter>
|
|
{
|
|
new()
|
|
{
|
|
Key = "x",
|
|
Description = "Tọa độ X đích đến.",
|
|
ValueDataType = ValueDataType.FLOAT,
|
|
IsOptional = false,
|
|
},
|
|
new()
|
|
{
|
|
Key = "y",
|
|
Description = "Tọa độ Y đích đến.",
|
|
ValueDataType = ValueDataType.FLOAT,
|
|
IsOptional = false,
|
|
},
|
|
new()
|
|
{
|
|
Key = "direction",
|
|
Description = "Hướng di chuyển: FORWARD, BACKWARD",
|
|
ValueDataType = ValueDataType.STRING,
|
|
IsOptional = true,
|
|
},
|
|
}.AsReadOnly();
|
|
|
|
private INavigation? Navigation;
|
|
private double TargetX;
|
|
private double TargetY;
|
|
private RobotDirection? Direction = null;
|
|
|
|
private const int MAX_TIMEOUT = 60000 * 5;
|
|
private int CountTimeout = 0;
|
|
private bool IsStartMoveStraight = false;
|
|
private bool IsHasLoad = false;
|
|
|
|
protected override Task StartAction()
|
|
{
|
|
Navigation = ServiceProvider.GetRequiredService<INavigation>();
|
|
using var scope = ServiceProvider.CreateScope();
|
|
var plcController = scope.ServiceProvider.GetRequiredService<IPlcController>();
|
|
IsHasLoad = plcController.SetHasLoadValue;
|
|
return base.StartAction();
|
|
}
|
|
|
|
protected override Task StopAction()
|
|
{
|
|
Navigation?.CancelMovement();
|
|
return base.StopAction();
|
|
}
|
|
|
|
protected override Task ExecuteAction()
|
|
{
|
|
if (Navigation is null)
|
|
{
|
|
SetStatus(ActionEvent.FAILED);
|
|
ResultDescription = "Module Navigation is not existed";
|
|
}
|
|
else
|
|
{
|
|
if (CountTimeout++ > MAX_TIMEOUT / ActionInterval)
|
|
{
|
|
Navigation?.CancelMovement();
|
|
SetStatus(ActionEvent.FAILED);
|
|
ResultDescription = "Timeout action";
|
|
}
|
|
else
|
|
{
|
|
if (!IsStartMoveStraight)
|
|
{
|
|
Navigation.MoveStraight(TargetX, TargetY, IsHasLoad, Direction);
|
|
IsStartMoveStraight = true;
|
|
}
|
|
if (Navigation.State == NavigationState.Completed)
|
|
{
|
|
SetStatus(ActionEvent.FINISHED);
|
|
ResultDescription = string.IsNullOrEmpty(ActionAttribute.ResultDescription) ? ResultDescription : ActionAttribute.ResultDescription;
|
|
}
|
|
else if (Navigation.State == NavigationState.Error)
|
|
{
|
|
SetStatus(ActionEvent.FAILED);
|
|
ResultDescription = $"Action Handle [{Type} - {Id}] is failed: Navigation Failed";
|
|
}
|
|
}
|
|
}
|
|
return 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 xPara = Parameters.FirstOrDefault(p => p.Key == "x") ?? throw new ActionException($"Action {Type} không tìm thấy parameter key 'x'");
|
|
var yPara = Parameters.FirstOrDefault(p => p.Key == "y") ?? throw new ActionException($"Action {Type} không tìm thấy parameter key 'y'");
|
|
TargetX = double.Parse(xPara.Value);
|
|
TargetY = double.Parse(yPara.Value);
|
|
var directionPara = Parameters.FirstOrDefault(p => p.Key == "direction");
|
|
Direction = directionPara is not null && Enum.TryParse<RobotDirection>(directionPara.Value, out var direction) ? direction : null;
|
|
}
|
|
}
|