private static Coord FindNewCoord(CoordCmd cmd, Coord lk)
{
if (cmd.Type == MoveType.Absolute)
return new Coord(cmd.X ?? lk.X, cmd.Y ?? lk.Y, cmd.Z ?? lk.Z, cmd.A ?? lk.A);
else {
return new Coord(
lk.X + cmd.X ?? 0,
lk.Y + cmd.Y ?? 0,
lk.Z + cmd.Z ?? 0,
lk.A + cmd.A ?? 0
);
}
}
public void MoveTo(CoordCmd cmd, bool sync = true)
{
ThrowIfError();
if (!_homed) {
_host.WriteLogMessage(ComponentName, "MoveTo cancelled: machine not homed.");
throw new Exception();
}
if (!IsIdle) {
RequestStatus();
WaitForIdle();
}
var dstCoord = FindNewCoord(cmd, _lkpos);
if (!_coordSystem.BoundsCheck(dstCoord)) {
_host.WriteLogMessage(ComponentName, "MoveTo cancelled: failed bounds check.");
throw new Exception();
}
var cmdStr = "G0";
var machineDst = _coordSystem.ToInner(dstCoord);
if (!machineDst.IsFinite()) throw new Exception();
if (cmd.X.HasValue) cmdStr += " X" + machineDst.X;
if (cmd.Y.HasValue) cmdStr += " Y" + machineDst.Y;
if (cmd.Z.HasValue) cmdStr += " Z" + machineDst.Z;
if (cmd.A.HasValue) cmdStr += " A" + machineDst.A;
var j = new JObject();
j["gc"] = cmdStr;
ResetWaiters();
RawSend(j);
WaitForMovement();
if (sync) {
WaitForIdle();
}
}