public void SetAB(double posA, double posB, MachineStep step, bool addOffset = true)
{
var offset = addOffset ? OZ + L : 0.0;
A = GetNewPositionCalcolateDuration(A, posA + offset, _maxSpeedZ, out double t1);
B = GetNewPositionCalcolateDuration(B, posB + offset, _maxSpeedY, out double t2);
var t = (t1 > t2) ? t1 : t2;
var action = new LinearInterpolatedPositionLinkAction()
{
Name = "Move A B",
Positions = new List <LinearInterpolatedPositionLinkAction.PositionItem>()
{
new LinearInterpolatedPositionLinkAction.PositionItem()
{
LinkId = 112, RequestPosition = A
},
new LinearInterpolatedPositionLinkAction.PositionItem()
{
LinkId = 212, RequestPosition = B
}
},
Duration = t
};
step?.Actions.Add(action);
}
public void SetYV(double posY, double posV, MachineStep step, bool addOffset = true)
{
var offset = addOffset ? OY : 0.0;
Y = GetNewPositionCalcolateDuration(Y, posY + offset, _maxSpeedY, out double t1);
V = GetNewPositionCalcolateDuration(V, posV + offset, _maxSpeedY, out double t2);
var t = (t1 > t2) ? t1 : t2;
var action = new LinearInterpolatedPositionLinkAction()
{
Name = "Move Y V",
Positions = new List <LinearInterpolatedPositionLinkAction.PositionItem>()
{
new LinearInterpolatedPositionLinkAction.PositionItem()
{
LinkId = 101, RequestPosition = Y
},
new LinearInterpolatedPositionLinkAction.PositionItem()
{
LinkId = 201, RequestPosition = V
}
},
Duration = t
};
step?.Actions.Add(action);
}
public void SetZW(double posZ, double posW, MachineStep step, bool addOffset = true)
{
var offset = addOffset ? OZ + L : 0.0;
Z = GetNewPositionCalcolateDuration(Z, posZ + offset, _maxSpeedZ, out double t1);
W = GetNewPositionCalcolateDuration(W, posW + offset, _maxSpeedY, out double t2);
var t = (t1 > t2) ? t1 : t2;
var action = new LinearInterpolatedPositionLinkAction()
{
Name = "Move Z W",
Positions = new List <LinearInterpolatedPositionLinkAction.PositionItem>()
{
new LinearInterpolatedPositionLinkAction.PositionItem()
{
LinkId = 102, RequestPosition = Z
},
new LinearInterpolatedPositionLinkAction.PositionItem()
{
LinkId = 202, RequestPosition = W
}
},
Duration = t
};
step?.Actions.Add(action);
}
public void SetXU(double posX, double posU, MachineStep step, bool addOffset = true)
{
var offset = addOffset ? OX : 0.0;
X = GetNewPositionCalcolateDuration(X, posX + offset, _maxSpeedX, out double t1);
U = GetNewPositionCalcolateDuration(U, posU + offset, _maxSpeedX, out double t2);
var t = (t1 > t2) ? t1 : t2;
var action = new LinearInterpolatedPositionLinkAction()
{
Name = "Move X U",
Positions = new List <LinearInterpolatedPositionLinkAction.PositionItem>()
{
new LinearInterpolatedPositionLinkAction.PositionItem()
{
LinkId = 1, RequestPosition = X
},
new LinearInterpolatedPositionLinkAction.PositionItem()
{
LinkId = 2, RequestPosition = U
}
},
Duration = t
};
step?.Actions.Add(action);
}
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!");
}
}
public void SetPosition(MachineStep step, double speed, double?x, double?y, double?z)
{
double dif = 0.0;
bool b = false;
var steps = new List <double>();
var action = new LinearInterpolatedPositionLinkAction()
{
Name = "G1 move", Positions = new List <LinearInterpolatedPositionLinkAction.PositionItem>()
};
if (x.HasValue)
{
X = UpdatePosition(X, x.Value, out dif, out b);
steps.Add(dif);
if (b)
{
action.Positions.Add(new LinearInterpolatedPositionLinkAction.PositionItem()
{
LinkId = 1, RequestPosition = X
});
}
}
if (y.HasValue)
{
Y = UpdatePosition(Y, y.Value, out dif, out b);
steps.Add(dif);
if (b)
{
action.Positions.Add(new LinearInterpolatedPositionLinkAction.PositionItem()
{
LinkId = 2, RequestPosition = Y
});
}
}
if (z.HasValue)
{
Z = UpdatePosition(Z, z.Value + L, out dif, out b);
steps.Add(dif);
if (b)
{
action.Positions.Add(new LinearInterpolatedPositionLinkAction.PositionItem()
{
LinkId = 3, RequestPosition = Z
});
}
}
if (action.Positions.Count() > 0)
{
var dist = Math.Sqrt(steps.Sum((d) => Math.Pow(d, 2.0)));
var t = (dist / speed) * 60.0;
action.Duration = t;
step?.Actions.Add(action);
}
}
private void Execute(LinearInterpolatedPositionLinkAction action, int notifyId)
{
if (IsDynamic)
{
ExecuteDynamic(action, notifyId);
}
else
{
ExecuteStatic(action, notifyId);
}
}
private void ExecuteDynamic(LinearInterpolatedPositionLinkAction action, int notifyId)
{
bool isFirst = true;
foreach (var p in action.Positions)
{
LinkMovementManager.Add(_interpolationGroupId, p.LinkId, p.RequestPosition, action.Duration, isFirst ? notifyId : 0);
isFirst = false;
}
_interpolationGroupId++;
}
private void ExecuteStatic(LinearInterpolatedPositionLinkAction action, int notifyId)
{
foreach (var item in action.Positions)
{
Messenger.Send(new GetLinkMessage()
{
Id = item.LinkId,
SetLink = (link) =>
{
link.Value = item.RequestPosition;
}
});
}
NotifyExecuted(notifyId);
}
public static void ExecuteAction(this LinearInterpolatedPositionLinkAction a, int actionId = 0)
{
bool isFirst = true;
foreach (var p in a.Positions)
{
//Messenger.Default.Send(new UpdateLinearLinkStateMessage(p.LinkId, p.RequestPosition));
Messenger.Default.Send(new LinearInterpolationLinkMessage(_interpolationGroupId, p.LinkId, p.RequestPosition, a.Duration)
{
BackNotifyId = isFirst ? actionId : 0
});
//isFirst = false;
}
_interpolationGroupId++;
}
public static BaseAction CreateBackStepAction(this LinearInterpolatedPositionLinkAction a)
{
var ba = new LinearInterpolatedPositionLinkLazyAction()
{
Duration = a.Duration,
Positions = new List <LinearInterpolatedPositionLinkAction.PositionItem>()
};
for (int i = 0; i < a.Positions.Count(); i++)
{
ba.Positions.Add(new LinearInterpolatedPositionLinkAction.PositionItem()
{
LinkId = a.Positions[i].LinkId
});
}
return(ba);
}
public void SetZWAB(double posZ, double posW, double posA, double posB, MachineStep step, bool addOffset = true)
{
GetToolOffset(out double toolOffsetX, out double toolOffsetY, out double toolOffsetZ);
var offset = addOffset ? OZ + toolOffsetZ : 0.0;
Z = GetNewPositionCalcolateDuration(Z, posZ + offset, _maxSpeedZ, out double t1);
W = GetNewPositionCalcolateDuration(W, posW + offset, _maxSpeedY, out double t2);
A = GetNewPositionCalcolateDuration(A, posA + offset, _maxSpeedZ, out double t3);
B = GetNewPositionCalcolateDuration(B, posB + offset, _maxSpeedY, out double t4);
var t = Math.Max(Math.Max(t1, t2), Math.Max(t3, t4));
var action = new LinearInterpolatedPositionLinkAction()
{
Name = "Move Z W A B",
Positions = new List <LinearInterpolatedPositionLinkAction.PositionItem>()
{
new LinearInterpolatedPositionLinkAction.PositionItem()
{
LinkId = 102, RequestPosition = Z
},
new LinearInterpolatedPositionLinkAction.PositionItem()
{
LinkId = 202, RequestPosition = W
},
new LinearInterpolatedPositionLinkAction.PositionItem()
{
LinkId = 112, RequestPosition = A
},
new LinearInterpolatedPositionLinkAction.PositionItem()
{
LinkId = 212, RequestPosition = B
}
},
Duration = t
};
step?.Actions.Add(action);
}
private void SetPositionBase(MachineStep step, double speed, double?x, double?y, double?z, bool addOffset = true)
{
double dif = 0.0;
bool b = false;
var steps = new List <double>();
var action = new LinearInterpolatedPositionLinkAction()
{
Name = "G1 move", Positions = new List <LinearInterpolatedPositionLinkAction.PositionItem>()
};
if (x.HasValue)
{
var offset = addOffset ? OX : 0.0;
if (GantryX == Gantry.First)
{
X = UpdatePosition(X, x.Value + offset, out dif, out b);
steps.Add(dif);
if (b)
{
action.Positions.Add(new LinearInterpolatedPositionLinkAction.PositionItem()
{
LinkId = 1, RequestPosition = X
});
}
}
else if (GantryX == Gantry.Second)
{
U = UpdatePosition(U, x.Value + offset, out dif, out b);
steps.Add(dif);
if (b)
{
action.Positions.Add(new LinearInterpolatedPositionLinkAction.PositionItem()
{
LinkId = 2, RequestPosition = U
});
}
}
else
{
throw new InvalidOperationException("SetPosition without gantry is not possible!");
}
}
if (y.HasValue)
{
var offset = addOffset ? OY : 0.0;
if (GantryY == Gantry.First)
{
Y = UpdatePosition(Y, y.Value + offset, out dif, out b);
steps.Add(dif);
if (b)
{
action.Positions.Add(new LinearInterpolatedPositionLinkAction.PositionItem()
{
LinkId = 101, RequestPosition = Y
});
}
}
else if (GantryY == Gantry.Second)
{
V = UpdatePosition(V, y.Value + offset, out dif, out b);
steps.Add(dif);
if (b)
{
action.Positions.Add(new LinearInterpolatedPositionLinkAction.PositionItem()
{
LinkId = 201, RequestPosition = V
});
}
}
else
{
throw new InvalidOperationException("SetPosition without gantry is not possible!");
}
}
if (z.HasValue)
{
var offset = addOffset ? OZ + L : 0.0;
if (GantryZ == Gantry.First)
{
Z = UpdatePosition(Z, z.Value + offset, out dif, out b);
steps.Add(dif);
if (b)
{
action.Positions.Add(new LinearInterpolatedPositionLinkAction.PositionItem()
{
LinkId = 102, RequestPosition = Z
});
}
}
else if (GantryZ == Gantry.Second)
{
W = UpdatePosition(W, z.Value + offset, out dif, out b);
steps.Add(dif);
if (b)
{
action.Positions.Add(new LinearInterpolatedPositionLinkAction.PositionItem()
{
LinkId = 202, RequestPosition = W
});
}
}
else if (GantryZ2 == Gantry.First)
{
A = UpdatePosition(A, z.Value + offset, out dif, out b);
steps.Add(dif);
if (b)
{
action.Positions.Add(new LinearInterpolatedPositionLinkAction.PositionItem()
{
LinkId = 112, RequestPosition = A
});
}
}
else if (GantryZ2 == Gantry.Second)
{
B = UpdatePosition(B, z.Value + offset, out dif, out b);
steps.Add(dif);
if (b)
{
action.Positions.Add(new LinearInterpolatedPositionLinkAction.PositionItem()
{
LinkId = 212, RequestPosition = B
});
}
}
else
{
throw new InvalidOperationException("SetPosition without gantry is not possible!");
}
}
if (action.Positions.Count() > 0)
{
var dist = Math.Sqrt(steps.Sum((d) => Math.Pow(d, 2.0)));
var t = (dist / speed) * 60.0;
action.Duration = t;
step?.Actions.Add(action);
}
}
