public MoveCommand(Vector3i location, UInt16 time_milliseconds)
{
this.x = (Int32)location.x;
this.y = (Int32)location.y;
this.z = (Int32)location.z;
this.time_milliseconds = time_milliseconds;
data_valid = false;
}
public override void ProcessResponse(byte[] data)
{
data_valid = (data[0] == commandCode);
if (data.Length <= 14)
{
data_valid = false;
return;
}
Int32 x = 0;
Int32 y = 0;
Int32 z = 0;
for (int i = 4; i >= 1; i--)
{
x = (x << 8) | data[i];
y = (y << 8) | data[i + 4];
z = (z << 8) | data[i + 8];
}
byte status_bits = data[13];
bool x_moving = (status_bits & 0x01) > 0;
bool y_moving = (status_bits & 0x02) > 0;
bool z_moving = (status_bits & 0x04) > 0;
is_moving = x_moving | y_moving | z_moving;
locations = data[14];
currentPosition = new Vector3i(x, y, z);
//Console.WriteLine("Location = ({0}, {1}, {2}), Moving = ({3}, {4}, {5})", x, y, z, x_moving, y_moving, z_moving);
}
/// <summary>
/// Run the router from the current position to the given position
/// </summary>
/// <param name="p">Destination location in inches</param>
/// <param name="inches_per_minute">Tool speed in inches per second</param>
public override void GoTo(Vector3 p, float inches_per_minute)
{
lock (thisLock)
{
Vector3 delta = lastPosition - p;
// Z axis has a slower maximum speed.
// TODO: implement clamping to a maximum speed for each axis.
delta.Z = delta.Z * 10;
float inches = delta.Length;
Vector3i pointInt = new Vector3i(Vector3.Multiply(p, ScaleFactors));
UInt16 time_milliseconds = (UInt16)(1000 * 60 * inches / inches_per_minute);
if (time_milliseconds > 0)
{
Console.WriteLine("Moving ({0}, {1}, {2}) to ({3}, {4}, {5}) in {6} milliseconds",
lastPosition.X,
lastPosition.Y,
lastPosition.Z,
pointInt.x,
pointInt.y,
pointInt.z,
time_milliseconds);
lastPosition = p;
currentCommand = new MoveCommand(pointInt, time_milliseconds);
}
else
{
Console.WriteLine("Ignoring command with time of 0");
}
}
}