private void ExecuteDynamic(ArcInterpolatedPositionLinkAction action, int notifyId)
{
bool isFirst = true;
foreach (var item in action.Components)
{
var component = item.Type == ArcInterpolatedPositionLinkAction.ArcComponent.ArcComponentType.X ? ArcComponent.X : ArcComponent.Y;
var data = new ArcComponentData
{
GroupId = _interpolationGroupId,
StartAngle = action.StartAngle,
Angle = action.Angle,
Radius = action.Radius,
CenterCoordinate = item.CenterCoordinate,
Component = component
};
Messenger.Send(new GetLinkMessage()
{
Id = item.LinkId,
SetLink = (link) =>
{
LinkMovementManager.Add(item.LinkId, item.TargetCoordinate, action.Duration, data, isFirst ? notifyId : 0);
isFirst = false;
}
});
}
_interpolationGroupId++;
}
public static void ExecuteAction(this ArcInterpolatedPositionLinkAction a, int actionId = 0)
{
bool isFirst = true;
foreach (var item in a.Components)
{
var component = item.Type == ArcInterpolatedPositionLinkAction.ArcComponent.ArcComponentType.X ? ArcComponent.X : ArcComponent.Y;
var data = new ArcComponentData
{
GroupId = _interpolationGroupId,
StartAngle = a.StartAngle,
Angle = a.Angle,
Radius = a.Radius,
CenterCoordinate = item.CenterCoordinate,
Component = component
};
Messenger.Default.Send(new ArcInterpolationLinkMessage(item.LinkId, item.TargetCoordinate, a.Duration, data)
{
BackNotifyId = isFirst ? actionId : 0
});
//isFirst = false;
}
_interpolationGroupId++;
}
public static BaseAction CreateBackStepAction(this ArcInterpolatedPositionLinkAction a)
{
var ba = new ArcInterpolatedPositionLinkLazyAction()
{
Direction = a.Direction == ArcInterpolatedPositionLinkAction.ArcDirection.CW ? ArcInterpolatedPositionLinkAction.ArcDirection.CCW : ArcInterpolatedPositionLinkAction.ArcDirection.CW,
Duration = a.Duration,
Radius = a.Radius,
StartAngle = a.StartAngle,
EndAngle = a.EndAngle,
Angle = -a.Angle,
Components = new List <ArcInterpolatedPositionLinkAction.ArcComponent>()
};
for (int i = 0; i < a.Components.Count(); i++)
{
ba.Components.Add(new ArcInterpolatedPositionLinkAction.ArcComponent()
{
LinkId = a.Components[i].LinkId,
CenterCoordinate = a.Components[i].CenterCoordinate,
Type = a.Components[i].Type
});
}
return(ba);
}
private void AddActionCommandImpl()
{
BaseAction action = null;
switch (ActionType)
{
case ActionType.AddPanel:
action = new AddPanelAction();
break;
case ActionType.LinearPositionLink:
action = new LinearPositionLinkAction();
break;
case ActionType.TwoPositionLink:
action = new TwoPositionLinkAction();
break;
case ActionType.LoadTool:
action = new LoadToolAction();
break;
case ActionType.UnloadTool:
action = new UnloadToolAction();
break;
case ActionType.LinearPositionLinkGantryOn:
action = new LinearPositionLinkGantryOnAction();
break;
case ActionType.LinearPositionLinkGantryOff:
action = new LinearPositionLinkGantryOffAction();
break;
case ActionType.LinearInterpolatedPositionLink:
action = new LinearInterpolatedPositionLinkAction();
break;
case ActionType.ArcInterpolatedPositionLink:
action = new ArcInterpolatedPositionLinkAction();
break;
default:
break;
}
if (action != null)
{
action.Id = _index++;
action.Name = $"Action {action.Id}";
Actions.Add(action);
MachineStep.Actions.Add(action);
}
else
{
throw new InvalidOperationException("Action type not supported!");
}
}
private void Execute(ArcInterpolatedPositionLinkAction action, int notifyId)
{
if (IsDynamic)
{
ExecuteDynamic(action, notifyId);
}
else
{
ExecuteStatic(action, notifyId);
}
}
private void ExecuteStatic(ArcInterpolatedPositionLinkAction action, int notifyId)
{
foreach (var item in action.Components)
{
Messenger.Send(new GetLinkMessage()
{
Id = item.LinkId,
SetLink = (link) =>
{
link.Value = item.TargetCoordinate;
}
});
}
NotifyExecuted(notifyId);
}
public void SetPosition(MachineStep step, double speed, double x, double y, double i, double j, bool cw)
{
var v1 = new Tuple <double, double>(X - i, Y - j);
var v2 = new Tuple <double, double>(x - i, y - j);
var a1 = Math.Atan2(v1.Item2, v1.Item1);
var a2 = Math.Atan2(v2.Item2, v2.Item1);
var a = a2 - a1;
var r = Math.Sqrt(Math.Pow(v1.Item1, 2.0) + Math.Pow(v1.Item2, 2.0));
if (cw && a > 0.0)
{
a = a - 2.0 * Math.PI;
}
else if (!cw && a < 0.0)
{
a = 2.0 * Math.PI + a;
}
var d = Math.Abs((a * r) / speed) * 60.0;
var action = new ArcInterpolatedPositionLinkAction()
{
Name = cw ? "G2" : "G3",
Direction = cw ? ArcInterpolatedPositionLinkAction.ArcDirection.CW : ArcInterpolatedPositionLinkAction.ArcDirection.CCW,
Duration = d,
Radius = r,
StartAngle = a1,
EndAngle = a2,
Angle = a,
Components = new List <ArcInterpolatedPositionLinkAction.ArcComponent>()
};
action.Components.Add(new ArcInterpolatedPositionLinkAction.ArcComponent()
{
LinkId = 1, CenterCoordinate = i, TargetCoordinate = x, Type = ArcInterpolatedPositionLinkAction.ArcComponent.ArcComponentType.X
});
action.Components.Add(new ArcInterpolatedPositionLinkAction.ArcComponent()
{
LinkId = 2, CenterCoordinate = j, TargetCoordinate = y, Type = ArcInterpolatedPositionLinkAction.ArcComponent.ArcComponentType.Y
});
step?.Actions.Add(action);
X = x;
Y = y;
}
private void SetPositionBase(MachineStep step, double speed, double x, double y, double i, double j, bool cw)
{
x += OX;
y += OY;
i += OX;
j += OY;
var xMaster = (GantryX == Gantry.First);
var yMaster = (GantryY == Gantry.First);
var actualX = xMaster ? X : U;
var actualY = yMaster ? Y : V;
var v1 = new Tuple <double, double>(actualX - i, actualY - j);
var v2 = new Tuple <double, double>(x - i, y - j);
var a1 = Math.Atan2(v1.Item2, v1.Item1);
var a2 = Math.Atan2(v2.Item2, v2.Item1);
var a = a2 - a1;
var r = Math.Sqrt(Math.Pow(v1.Item1, 2.0) + Math.Pow(v1.Item2, 2.0));
if (cw && a > 0.0)
{
a = a - 2.0 * Math.PI;
}
else if (!cw && a < 0.0)
{
a = 2.0 * Math.PI + a;
}
var d = Math.Abs((a * r) / speed) * 60.0;
var action = new ArcInterpolatedPositionLinkAction()
{
Name = cw ? "G2" : "G3",
Direction = cw ? ArcInterpolatedPositionLinkAction.ArcDirection.CW : ArcInterpolatedPositionLinkAction.ArcDirection.CCW,
Duration = d,
Radius = r,
StartAngle = a1,
EndAngle = a2,
Angle = a,
Components = new List <ArcInterpolatedPositionLinkAction.ArcComponent>()
};
action.Components.Add(new ArcInterpolatedPositionLinkAction.ArcComponent()
{
LinkId = xMaster ? 1 : 2, CenterCoordinate = i, TargetCoordinate = x, Type = ArcInterpolatedPositionLinkAction.ArcComponent.ArcComponentType.X
});
action.Components.Add(new ArcInterpolatedPositionLinkAction.ArcComponent()
{
LinkId = yMaster ? 101 : 201, CenterCoordinate = j, TargetCoordinate = y, Type = ArcInterpolatedPositionLinkAction.ArcComponent.ArcComponentType.Y
});
step?.Actions.Add(action);
if (xMaster)
{
X = x;
}
else
{
U = x;
}
if (yMaster)
{
Y = y;
}
else
{
V = y;
}
}
