public void ResetGantryY(MachineStep step)
{
if (GantryY != Gantry.None)
{
if (GantryCouplingY == GantryCoupling.None)
{
throw new InvalidOperationException("Gantry coupling could not be none!");
}
var slaveUnhooked = GantryCouplingY == GantryCoupling.Single;
if (GantryY == Gantry.First)
{
step.Actions.Add(new LinearPositionLinkGantryOffAction()
{
Name = "Gantry off YV", MasterId = 101, SlaveId = 201, SlaveUnhooked = slaveUnhooked
});
}
else
{
step.Actions.Add(new LinearPositionLinkGantryOffAction()
{
Name = "Gantry off YV", MasterId = 201, SlaveId = 101, SlaveUnhooked = slaveUnhooked
});
}
GantryY = Gantry.None;
}
GantryCouplingY = GantryCoupling.None;
}
public override List <MachineStep> Convert(GIstruction istruction, State state)
{
if (istruction.Parameters.TryGetValue('L', out double ln) && istruction.Parameters.TryGetValue('T', out double t))
{
MachineStep ms = null;
if (t == 1.0)
{
ms = GetMachineStepForBlowing(ln);
}
else if (t == 2.0)
{
ms = GetMachineStepForGlue(ln);
}
else if (t == 3.0)
{
//ms = GetMachineStepForDoweling(ln);
return(GetMachineStepsForDoweling(ln));
}
else
{
throw new ArgumentOutOfRangeException("Invalid doweling operation!");
}
return((ms != null) && (ms.Actions.Count > 0) ? new List <MachineStep>()
{
ms
} : null);
}
else
{
return(null);
}
}
public override List <MachineStep> Convert(GIstruction istruction, State state)
{
var step = new MachineStep()
{
Id = GetStepId(),
Name = $"G0",
Actions = new List <BaseAction>()
};
if (istruction.Parameters != null && istruction.Parameters.Count > 0)
{
if (istruction.Parameters.TryGetValue('X', out double x))
{
state.Axes.SetRapidX(x, step);
}
if (istruction.Parameters.TryGetValue('Y', out double y))
{
state.Axes.SetRapidY(y, step);
}
if (istruction.Parameters.TryGetValue('Z', out double z))
{
state.Axes.SetRapidZ(z, step);
}
SetActionsIds(step);
SetMaxDuration(step);
}
return((step.Actions.Count() > 0) ? new List <MachineStep>()
{
step
} : null);
}
public override List <MachineStep> Convert(SetVariableIstruction istruction, T state)
{
var bytes = BitConverter.GetBytes((uint)istruction.Value);
var ar = new BitArray(bytes);
var s = GetState(state);
var r = new BitArray(ar).Xor(s);
var step = new MachineStep()
{
Id = GetStepId(),
Name = $"{istruction.Name}[{istruction.Index}]",
Actions = new List <BaseAction>()
};
for (int i = 0; i < r.Length; i++)
{
if ((_linkIds[i] > 0) && r[i])
{
AddAction(step, ar[i], _linkIds[i]);
s[i] = ar[i];
}
}
FilterMultipleActionOnSameLink(step);
SetActionsIds(step);
return((step.Actions.Count() > 0) ? new List <MachineStep>()
{
step
} : null);
}
public void ResetGantryZ2(MachineStep step)
{
if (GantryZ2 != Gantry.None)
{
if (GantryCouplingZ2 == GantryCoupling.None)
{
throw new InvalidOperationException("Gantry coupling could not be none!");
}
var slaveUnhooked = GantryCouplingZ2 == GantryCoupling.Single;
if (GantryZ2 == StateData.Axes.Gantry.First)
{
step.Actions.Add(new LinearPositionLinkGantryOffAction()
{
Name = "Gantry off AB", MasterId = 112, SlaveId = 212, SlaveUnhooked = slaveUnhooked
});
}
else
{
step.Actions.Add(new LinearPositionLinkGantryOffAction()
{
Name = "Gantry off AB", MasterId = 212, SlaveId = 112, SlaveUnhooked = slaveUnhooked
});
}
GantryZ2 = StateData.Axes.Gantry.None;
}
GantryCouplingZ2 = GantryCoupling.None;
}
public override List <MachineStep> Convert(GIstruction istruction, State state)
{
if (istruction.Parameters.TryGetValue('X', out double x))
{
var step = new MachineStep()
{
Id = GetStepId(),
Name = $"G692",
Actions = new List <BaseAction>()
};
state.Axes.SetRapidX(x, step);
SetActionsIds(step);
SetMaxDuration(step);
return(new List <MachineStep>()
{
step
});
}
else
{
return(null);
}
}
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);
}
static protected void SetActionsIds(MachineStep step)
{
for (int i = 0; i < step.Actions.Count; i++)
{
step.Actions[i].Id = i;
}
}
private void ManagePreviousSynchro(GIstruction istruction, State state, List <MachineStep> steps)
{
var step = new MachineStep()
{
Id = GetStepId(),
Name = "G140 - Reset previous",
Actions = new List <BaseAction>()
};
var resetX = istruction.Parameters.Keys.Contains('X') && istruction.Parameters.Keys.Contains('U') && istruction.Parameters.Keys.Contains('I');
var resetY = istruction.Parameters.Keys.Contains('Y') && istruction.Parameters.Keys.Contains('V') && istruction.Parameters.Keys.Contains('J');
var resetZ = istruction.Parameters.Keys.Contains('Z') && istruction.Parameters.Keys.Contains('W') && istruction.Parameters.Keys.Contains('K');
var resetZ2 = istruction.Parameters.Keys.Contains('A') && istruction.Parameters.Keys.Contains('B') && istruction.Parameters.Keys.Contains('K');
if (resetX)
{
state.Axes.ResetGantryX(step);
}
if (resetY)
{
state.Axes.ResetGantryY(step);
}
if (resetZ || resetZ2)
{
state.Axes.ResetGantryZ(step);
state.Axes.ResetGantryZ2(step);
}
if (step.Actions.Count > 0)
{
steps.Add(step);
}
}
public override List <MachineStep> Convert(GIstruction istruction, State state)
{
var step = new MachineStep()
{
Id = GetStepId(),
Name = "G3",
Actions = new List <BaseAction>()
};
istruction.Parameters.TryGetValue('X', out double x);
istruction.Parameters.TryGetValue('Y', out double y);
istruction.Parameters.TryGetValue('I', out double i);
istruction.Parameters.TryGetValue('J', out double j);
if (istruction.Parameters.TryGetValue('F', out double v))
{
state.FeedSpeed = (int)v;
}
state.Axes.SetPosition(step, state.FeedSpeed, x, y, i, j, false);
SetActionsIds(step);
return((step.Actions.Count() > 0) ? new List <MachineStep>()
{
step
} : null);
}
public override List <MachineStep> Convert(GIstruction istruction, State state)
{
double g = -1;
istruction.Parameters.TryGetValue('G', out g);
var step = new MachineStep()
{
Id = GetStepId(),
Name = $"G153 G{g}",
Actions = new List <BaseAction>()
};
switch (g)
{
case 0:
ConvertG0(istruction, state, step);
break;
case 1:
ConvertG1(istruction, state, step);
break;
default:
throw new NotImplementedException($"G153 G{g} not implemented!");
}
SetActionsIds(step);
SetMaxDuration(step);
return((step.Actions.Count() > 0) ? new List <MachineStep>()
{
step
} : null);
}
private static void ConvertG1(GIstruction istruction, State state, MachineStep step)
{
var x = new Nullable <double>();
var y = new Nullable <double>();
var z = new Nullable <double>();
double v = 0.0;
if (istruction.Parameters.TryGetValue('X', out v))
{
x = v;
}
if (istruction.Parameters.TryGetValue('Y', out v))
{
y = v;
}
if (istruction.Parameters.TryGetValue('Z', out v))
{
z = v;
}
if (istruction.Parameters.TryGetValue('F', out v))
{
state.FeedSpeed = (int)v;
}
state.Axes.SetPosition(step, state.FeedSpeed, x, y, z, false);
}
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 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 SetGantryZ(double g, MachineStep step, bool slaveUnhooked = false)
{
if ((g == 1) || (g == 2))
{
var gantry = (g == 1.0) ? Gantry.First : Gantry.Second;
int master = (g == 1.0) ? 102 : 202;
int slave = (g == 1.0) ? 202 : 102;
step.Actions.Add(new LinearPositionLinkGantryOnAction()
{
Name = "Gantry on ZW", MasterId = master, SlaveId = slave, SlaveUnhooked = slaveUnhooked
});
GantryZ = gantry;
GantryCouplingZ = slaveUnhooked ? GantryCoupling.Single : GantryCoupling.Couple;
GantryZ2 = Gantry.None;
GantryCouplingZ2 = GantryCoupling.None;
}
else if ((g == 3) || (g == 4))
{
var gantry = (g == 3.0) ? Gantry.First : Gantry.Second;
int master = (g == 3.0) ? 112 : 212;
int slave = (g == 3.0) ? 212 : 112;
step.Actions.Add(new LinearPositionLinkGantryOnAction()
{
Name = "Gantry on AB", MasterId = master, SlaveId = slave, SlaveUnhooked = slaveUnhooked
});
GantryZ2 = gantry;
GantryCouplingZ2 = slaveUnhooked ? GantryCoupling.Single : GantryCoupling.Couple;
GantryZ = Gantry.None;
GantryCouplingZ = GantryCoupling.None;
}
}
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 static void FilterMultipleActionOnSameLink(MachineStep step)
{
var itemsToRemove = new List <BaseAction>();
for (int i = 0; i < step.Actions.Count(); i++)
{
var ai = step.Actions[i] as TwoPositionLinkAction;
for (int j = i + 1; j < step.Actions.Count(); j++)
{
var aj = step.Actions[j] as TwoPositionLinkAction;
if ((ai.LinkId == aj.LinkId))
{
if (ai.RequestedState == aj.RequestedState)
{
itemsToRemove.Add(aj);
}
else
{
itemsToRemove.Add(ai);
itemsToRemove.Add(aj);
}
}
}
}
foreach (var item in itemsToRemove)
{
step.Actions.Remove(item);
}
}
protected virtual void AddAction(MachineStep step, bool rqState, int linkId)
{
step.Actions.Add(new TwoPositionLinkAction()
{
LinkId = linkId,
RequestedState = rqState ? TwoPositionLinkActionRequestedState.On : TwoPositionLinkActionRequestedState.Off
});
}
static protected void SetMaxDuration(MachineStep step)
{
if ((step.Actions != null) && (step.Actions.Count > 1))
{
var d = step.Actions.Select((a) => (a as LinearPositionLinkAction)?.Duration).Max();
step.Actions.ForEach((a) => (a as LinearPositionLinkAction).Duration = d.Value);
}
}
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);
}
}
public override List <MachineStep> Convert(GIstruction istruction, State state)
{
var step = new MachineStep()
{
Id = GetStepId(),
Name = $"G0",
Actions = new List <BaseAction>()
};
if (istruction.Parameters != null && istruction.Parameters.Count > 0)
{
if (state.Axes.M.IsIdentity() || state.Axes.M.IsIdentityFlipped())
{
if (istruction.Parameters.TryGetValue('X', out double x))
{
state.Axes.SetRapidX(x, step);
}
if (istruction.Parameters.TryGetValue('Y', out double y))
{
state.Axes.SetRapidY(y, step);
}
if (istruction.Parameters.TryGetValue('Z', out double z))
{
state.Axes.SetRapidZ(z, step);
}
}
else
{
double?nx = null;
double?ny = null;
double?nz = null;
if (istruction.Parameters.TryGetValue('X', out double x))
{
nx = x;
}
if (istruction.Parameters.TryGetValue('Y', out double y))
{
ny = y;
}
if (istruction.Parameters.TryGetValue('Z', out double z))
{
nz = z;
}
state.Axes.SetRapid(nx, ny, nz, step);
}
SetActionsIds(step);
SetMaxDuration(step);
}
return((step.Actions.Count() > 0) ? new List <MachineStep>()
{
step
} : null);
}
protected override void AddAction(MachineStep step, bool rqState, int linkId)
{
base.AddAction(step, rqState, linkId);
if (linkId == 9066)
{
base.AddAction(step, rqState, 9166);
}
}
public void SetXI(double inc, MachineStep step)
{
var t = CalcolateDuration(inc, _maxSpeedX);
X += inc;
step?.Actions.Add(new LinearPositionLinkAction()
{
Name = "Move XI", LinkId = 1, RequestedPosition = X, Duration = t
});
}
public void SetPosition(MachineStep step, double speed, double?x, double?y, double?z, bool addOffset = true)
{
var _x = new Nullable <double>();
var _y = new Nullable <double>();
var _z = new Nullable <double>();
M.Transform(x, y, z, ref _x, ref _y, ref _z);
SetPositionBase(step, speed, _x, _y, _z, addOffset);
}
public void SetYI(double inc, MachineStep step)
{
var t = CalcolateDuration(inc, _maxSpeedY);
Y += inc;
step?.Actions.Add(new LinearPositionLinkAction()
{
Name = "Move YI", LinkId = 2, RequestedPosition = Y, Duration = t
});
}
public void SetZI(double inc, MachineStep step)
{
var t = CalcolateDuration(inc, _maxSpeedZ);
Z += inc;
step?.Actions.Add(new LinearPositionLinkAction()
{
Name = "Move ZI", LinkId = 3, RequestedPosition = Z, Duration = t
});
}
private void AddSetGantrySteps(State state, List <MachineStep> steps)
{
var step = new MachineStep()
{
Id = GetStepId(), Name = "TCH - Gantry", Actions = new List <BaseAction>()
};
state.Axes.SetGantryY(1.0, step);
steps.Add(step);
}
public void SetB(double pos, MachineStep step, bool addOffset = true)
{
var offset = addOffset ? OZ + L : 0.0;
B = GetNewPositionCalcolateDuration(B, pos + offset, _maxSpeedZ, out double t);
step?.Actions.Add(new LinearPositionLinkAction()
{
Name = "Move B", LinkId = 212, RequestedPosition = B, Duration = t
});
}
public void SetV(double pos, MachineStep step, bool addOffset = true)
{
var offset = addOffset ? OY : 0.0;
V = GetNewPositionCalcolateDuration(V, pos + offset, _maxSpeedY, out double t);
step?.Actions.Add(new LinearPositionLinkAction()
{
Name = "Move V", LinkId = 201, RequestedPosition = V, Duration = t
});
}
public void SetU(double pos, MachineStep step, bool addOffset = true)
{
var offset = addOffset ? OX : 0.0;
U = GetNewPositionCalcolateDuration(U, pos + offset, _maxSpeedX, out double t);
step?.Actions.Add(new LinearPositionLinkAction()
{
Name = "Move U", LinkId = 2, RequestedPosition = U, Duration = t
});
}
