/// <summary>
/// Sends the motion command to the mill.
/// </summary>
private void SendMotionCommand(LinearMillCommand millCommand)
{
if (millCommand.Speed != linearDelay)
{
// Update the mill engraving speed, if necessary.
linearDelay = millCommand.Speed;
serialPort.Write($"D{linearDelay}\r");
}
int iterations;
int xDelay, xDelayFactor;
int yDelay, yDelayFactor;
int zDelay, zDelayFactor;
int aDelay, aDelayFactor;
DecodeMillCommand(millCommand, out iterations,
out xDelay, out xDelayFactor,
out yDelay, out yDelayFactor,
out zDelay, out zDelayFactor,
out aDelay, out aDelayFactor);
int xDir = millCommand.XSteps > 0 ? 1 : 0;
int yDir = millCommand.YSteps > 0 ? 1 : 0;
int zDir = millCommand.ZSteps > 0 ? 1 : 0;
int aDir = millCommand.ASteps > 0 ? 1 : 0;
string lineCommand = $"L{iterations}\r";
serialPort.Write(lineCommand);
char response = (char)serialPort.ReadChar();
if (response != 'X')
{
Debug.Fail("Expected confirmation that the X-axis is ready");
}
string xCommand = $"{xDir}\r{xDelay}\r{xDelayFactor}\r";
serialPort.Write(xCommand);
string yCommand = $"{yDir}\r{yDelay}\r{yDelayFactor}\r";
serialPort.Write(yCommand);
string zCommand = $"{zDir}\r{zDelay}\r{zDelayFactor}\r";
serialPort.Write(zCommand);
string aCommand = $"{aDir}\r{aDelay}\r{aDelayFactor}\r";
serialPort.Write(aCommand);
response = (char)serialPort.ReadChar();
if (response != 'D')
{
Debug.Fail("Expected comfirmation that the line command was written.");
}
}
/// <summary>
/// Decodes a mill command into the number of total iterations, delays, and delay factors to
/// get a smooth linear motion.
/// </summary>
private void DecodeMillCommand(LinearMillCommand millCommand, out int iterations,
out int xDelay, out int xDelayFactor,
out int yDelay, out int yDelayFactor,
out int zDelay, out int zDelayFactor,
out int aDelay, out int aDelayFactor)
{
int xSteps = Math.Abs(millCommand.XSteps);
int ySteps = Math.Abs(millCommand.YSteps);
int zSteps = Math.Abs(millCommand.ZSteps);
int aSteps = Math.Abs(millCommand.ASteps);
// Minimum amount to avoid aliasing issues.
iterations = 2 * Math.Max(xSteps, Math.Max(ySteps, Math.Max(zSteps, aSteps)));
DecodeSteps(iterations, xSteps, out xDelay, out xDelayFactor);
DecodeSteps(iterations, ySteps, out yDelay, out yDelayFactor);
DecodeSteps(iterations, zSteps, out zDelay, out zDelayFactor);
DecodeSteps(iterations, aSteps, out aDelay, out aDelayFactor);
}
/// <summary>
/// Adds a linear motion operation to the operations queue.
/// </summary>
public void AddOperation(LinearMillCommand millCommand)
{
millCommandQueue.Enqueue(millCommand);
}
