private IRobotCommand GetNextCommand(bool canAcceptMoveCommand)
{
currentCommand = null;
if (commandGenerator == null)
{
return(null);
}
if (sendZeroCommand)
{
currentCommand = commandGenerator.GenerateZeroCommand();
sendZeroCommand = false;
}
if (sendCancelCommand)
{
currentCommand = commandGenerator.GenerateCancelCommand();
sendCancelCommand = false;
}
else if (sendResumeCommand)
{
currentCommand = commandGenerator.GenerateResumeCommand();
sendResumeCommand = false;
}
else if (sendPauseCommand)
{
currentCommand = commandGenerator.GeneratePauseCommand();
sendPauseCommand = false;
}
else if (sendEnableStepperCommand)
{
currentCommand = commandGenerator.GenerateStepperEnableCommand();
sendEnableStepperCommand = false;
}
else if (sendDisableStepperCommand)
{
currentCommand = commandGenerator.GenerateStepperDisableCommand();
sendDisableStepperCommand = false;
}
else if (canAcceptMoveCommand && lastPositionKnown)
{
while (currentCommand == null && commands.Count > 0)
{
ICommand command = commands.Dequeue();
if (command is MoveTool)
{
currentCommand = CreateRobotCommand(command as MoveTool, commandGenerator);
}
}
}
if (currentCommand == null)
{
currentCommand = commandGenerator.GenerateStatusCommand();
}
return(currentCommand);
}
private void NewDataAvailable(SerialPortWrapper.SimpleSerialPacket packet)
{
lock (thisLock)
{
if (currentCommand == null)
{
Console.WriteLine("Error: Received data, but no command was sent!");
foreach (byte b in packet.Data)
{
Console.Write(b.ToString("x") + ", ");
}
Console.WriteLine();
}
else
{
currentCommand.ProcessResponse(packet.Data);
if (currentCommand.IsDataValid())
{
// See if there's any state information in the command used to
// update location or other fields...
byte locations = MaxLocations;
if (currentCommand is StatusCommand)
{
StatusCommand c = currentCommand as StatusCommand;
currentPosition = Vector3.Divide (c.CurrentPosition.ToVector3(), ScaleFactors);
locations = c.Locations;
if (onPositionUpdate != null)
{
onPositionUpdate(this, EventArgs.Empty);
}
}
if (currentCommand is MoveCommand)
{
MoveCommand m = currentCommand as MoveCommand;
locations = m.Locations;
}
currentCommand = GetNextCommand(locations);
elapsedCounter = 0;
serial.Transmit(currentCommand.GenerateCommand(), 0x21);
}
else
{
Console.WriteLine("Error: Did not process data correctly!");
}
}
}
}
internal override bool ProcessResponse(byte data)
{
if (data == 0xCA) // Only the binary robot will send this character (it's the packet start character)
{
// Drop anything earlier from the accumulator
accumulator.Clear();
Console.WriteLine("Found 0xCA start byte, looks like the packetized binary protocol");
commandGenerator = new PacketizedCommandGenerator();
binaryStatusCommand = commandGenerator.GenerateStatusCommand();
}
accumulator.Add(data);
if (binaryStatusCommand != null)
{
// The response will be a binary status command, just forward the data and wait until it's good.
return(binaryStatusCommand.ProcessResponse(data));
}
else
{
// Look for an ASCII communicating robot (like GRBL)
var s = System.Text.Encoding.ASCII.GetString(accumulator.ToArray());
if (s.EndsWith("\r\n", StringComparison.OrdinalIgnoreCase))
{
if (s.StartsWith("Grbl ") && s.Length >= 9)
{
var version = s.Substring(5, 3);
float version_float = 0.0f;
if (float.TryParse(version, out version_float) && version_float >= 0.8f)
{
Console.WriteLine("Compatible Grbl type robot found: " + s.Substring(0, 9));
commandGenerator = new GrblCommandGenerator();
return(true);
}
}
else
{
// Seems like a GRBL type robot, but the start of the string wasn't right. Maybe some garbage
// or an extra \r\n, clear it out and wait for more.
accumulator.Clear();
}
}
}
return(false);
}
private void NewDataAvailable(byte data)
{
lock (thisLock)
{
elapsedCounter = 0;
if (currentCommand == null)
{
Console.WriteLine("Error: Received data, but no command was sent!");
Console.Write(data.ToString("x") + ", ");
Console.WriteLine();
}
else
{
if (currentCommand.ProcessResponse(data))
{
bool canAcceptMoveCommand = false;
if (currentCommand is IRobotCommandWithStatus)
{
IRobotCommandWithStatus status = currentCommand as IRobotCommandWithStatus;
currentPosition = status.CurrentPosition;
canAcceptMoveCommand = status.CanAcceptMoveCommand;
if (this.lastPositionKnown == false)
{
lastPosition = currentPosition;
lastPositionKnown = true;
}
if (onRobotStatusChange != null)
{
onRobotStatusChange(status, EventArgs.Empty);
}
}
else if (currentCommand is RobotDetectionCommand)
{
RobotDetectionCommand r = currentCommand as RobotDetectionCommand;
commandGenerator = r.GetCommandGenerator();
}
currentCommand = GetNextCommand(canAcceptMoveCommand);
serial.Transmit(currentCommand.GenerateCommand());
}
}
}
}
private IRobotCommand GetNextCommand(byte locations)
{
currentCommand = null;
if (locations < MaxLocations)
{
// Ok to pass in another movement command
// TODO: rework this to use a local buffer...
if (onRobotReady != null)
{
onRobotReady(this, EventArgs.Empty);
}
}
if (currentCommand == null)
{
currentCommand = new StatusCommand();
}
return currentCommand;
}
void t_Elapsed(object sender, ElapsedEventArgs e)
{
t.Stop();
lock (thisLock)
{
if (serial != null && serial.IsOpen)
{
elapsedCounter++;
if ((elapsedCounter * 50) > (1000)) // More than 1 second to reply
{
Console.WriteLine("Device Timeout!");
// Send a status command
currentCommand = new StatusCommand();
serial.Transmit(currentCommand.GenerateCommand(), 0x21);
elapsedCounter = 0;
}
}
}
t.Start();
}
/// <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");
}
}
}
void t_Elapsed(object sender, ElapsedEventArgs e)
{
t.Stop();
lock (thisLock)
{
if (elapsedCounter > timeout_ms)
{
if (currentCommand != null && currentCommand is RobotDetectionCommand)
{
Console.WriteLine("Unexpected Response from robot detection: " + (currentCommand as RobotDetectionCommand).DumpData());
}
// Expected reply not received within 1 second, assume command was lost.
Console.WriteLine("Device Timeout!");
}
if (serial == null || !serial.IsOpen || elapsedCounter > timeout_ms)
{
lastPositionKnown = false;
commandGenerator = null;
currentCommand = null;
elapsedCounter = 0;
}
else
{
if (currentCommand == null)
{
currentCommand = new RobotDetectionCommand();
serial.Transmit(currentCommand.GenerateCommand());
elapsedCounter = 0;
}
else
{
elapsedCounter += 50;
}
}
}
t.Start();
}
internal override bool ProcessResponse(byte data)
{
if (data == 0xCA) // Only the binary robot will send this character (it's the packet start character)
{
// Drop anything earlier from the accumulator
accumulator.Clear();
Console.WriteLine("Found 0xCA start byte, looks like the packetized binary protocol");
commandGenerator = new PacketizedCommandGenerator();
binaryStatusCommand = commandGenerator.GenerateStatusCommand();
}
accumulator.Add(data);
if (binaryStatusCommand != null)
{
// The response will be a binary status command, just forward the data and wait until it's good.
return binaryStatusCommand.ProcessResponse(data);
}
else
{
// Look for an ASCII communicating robot (like GRBL)
var s = System.Text.Encoding.ASCII.GetString(accumulator.ToArray());
if (s.EndsWith("\r\n", StringComparison.OrdinalIgnoreCase))
{
if (s.StartsWith("Grbl ") && s.Length >= 9)
{
var version = s.Substring(5, 3);
float version_float = 0.0f;
if (float.TryParse(version, out version_float) && version_float >= 0.8f)
{
Console.WriteLine("Compatible Grbl type robot found: " + s.Substring(0, 9));
commandGenerator = new GrblCommandGenerator();
return true;
}
}
else
{
// Seems like a GRBL type robot, but the start of the string wasn't right. Maybe some garbage
// or an extra \r\n, clear it out and wait for more.
accumulator.Clear();
}
}
}
return false;
}
void t_Elapsed(object sender, ElapsedEventArgs e)
{
t.Stop();
lock (thisLock)
{
if (elapsedCounter > timeout_ms)
{
if (currentCommand != null && currentCommand is RobotDetectionCommand)
{
Console.WriteLine("Unexpected Response from robot detection: " + (currentCommand as RobotDetectionCommand).DumpData());
}
// Expected reply not received within 1 second, assume command was lost.
Console.WriteLine("Device Timeout!");
}
if (serial == null || !serial.IsOpen || elapsedCounter > timeout_ms)
{
lastPositionKnown = false;
commandGenerator = null;
currentCommand = null;
elapsedCounter = 0;
}
else
{
if (currentCommand == null)
{
currentCommand = new RobotDetectionCommand();
serial.Transmit(currentCommand.GenerateCommand());
elapsedCounter = 0;
}
else
{
elapsedCounter += 50;
}
}
}
t.Start();
}
private void NewDataAvailable(byte data)
{
lock (thisLock)
{
elapsedCounter = 0;
if (currentCommand == null)
{
Console.WriteLine("Error: Received data, but no command was sent!");
Console.Write(data.ToString("x") + ", ");
Console.WriteLine();
}
else
{
if (currentCommand.ProcessResponse(data))
{
bool canAcceptMoveCommand = false;
if (currentCommand is IRobotCommandWithStatus)
{
IRobotCommandWithStatus status = currentCommand as IRobotCommandWithStatus;
currentPosition = status.CurrentPosition;
canAcceptMoveCommand = status.CanAcceptMoveCommand;
if (this.lastPositionKnown == false)
{
lastPosition = currentPosition;
lastPositionKnown = true;
}
if (onRobotStatusChange != null)
{
onRobotStatusChange(status, EventArgs.Empty);
}
}
else if (currentCommand is RobotDetectionCommand)
{
RobotDetectionCommand r = currentCommand as RobotDetectionCommand;
commandGenerator = r.GetCommandGenerator();
}
currentCommand = GetNextCommand(canAcceptMoveCommand);
serial.Transmit(currentCommand.GenerateCommand());
}
}
}
}
private IRobotCommand GetNextCommand(bool canAcceptMoveCommand)
{
currentCommand = null;
if (commandGenerator == null)
{
return null;
}
if (sendZeroCommand)
{
currentCommand = commandGenerator.GenerateZeroCommand();
sendZeroCommand = false;
}
if (sendCancelCommand)
{
currentCommand = commandGenerator.GenerateCancelCommand();
sendCancelCommand = false;
}
else if (sendResumeCommand)
{
currentCommand = commandGenerator.GenerateResumeCommand();
sendResumeCommand = false;
}
else if (sendPauseCommand)
{
currentCommand = commandGenerator.GeneratePauseCommand();
sendPauseCommand = false;
}
else if (sendEnableStepperCommand)
{
currentCommand = commandGenerator.GenerateStepperEnableCommand();
sendEnableStepperCommand = false;
}
else if (sendDisableStepperCommand)
{
currentCommand = commandGenerator.GenerateStepperDisableCommand();
sendDisableStepperCommand = false;
}
else if (canAcceptMoveCommand && lastPositionKnown)
{
while (currentCommand == null && commands.Count > 0)
{
ICommand command = commands.Dequeue();
if (command is MoveTool)
{
currentCommand = CreateRobotCommand(command as MoveTool, commandGenerator);
}
}
}
if (currentCommand == null)
{
currentCommand = commandGenerator.GenerateStatusCommand();
}
return currentCommand;
}
/// <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");
}
}
}
void t_Elapsed(object sender, ElapsedEventArgs e)
{
t.Stop();
lock (thisLock)
{
if (serial != null && serial.IsOpen)
{
elapsedCounter++;
if ((elapsedCounter * 50) > (1000)) // More than 1 second to reply
{
Console.WriteLine("Device Timeout!");
// Send a status command
currentCommand = new StatusCommand();
serial.Transmit(currentCommand.GenerateCommand(), 0x21);
elapsedCounter = 0;
}
}
}
t.Start();
}
private void NewDataAvailable(SerialPortWrapper.SimpleSerialPacket packet)
{
lock (thisLock)
{
if (currentCommand == null)
{
Console.WriteLine("Error: Received data, but no command was sent!");
foreach (byte b in packet.Data)
{
Console.Write(b.ToString("x") + ", ");
}
Console.WriteLine();
}
else
{
currentCommand.ProcessResponse(packet.Data);
if (currentCommand.IsDataValid())
{
// See if there's any state information in the command used to
// update location or other fields...
byte locations = MaxLocations;
if (currentCommand is StatusCommand)
{
StatusCommand c = currentCommand as StatusCommand;
currentPosition = Vector3.Divide (c.CurrentPosition.ToVector3(), ScaleFactors);
locations = c.Locations;
if (onPositionUpdate != null)
{
onPositionUpdate(this, EventArgs.Empty);
}
}
if (currentCommand is MoveCommand)
{
MoveCommand m = currentCommand as MoveCommand;
locations = m.Locations;
}
currentCommand = GetNextCommand(locations);
elapsedCounter = 0;
serial.Transmit(currentCommand.GenerateCommand(), 0x21);
}
else
{
Console.WriteLine("Error: Did not process data correctly!");
}
}
}
}
private IRobotCommand GetNextCommand(byte locations)
{
currentCommand = null;
if (locations < MaxLocations)
{
// Ok to pass in another movement command
// TODO: rework this to use a local buffer...
if (onRobotReady != null)
{
onRobotReady(this, EventArgs.Empty);
}
}
if (currentCommand == null)
{
currentCommand = new StatusCommand();
}
return currentCommand;
}
