/// <summary>
/// Create a robot command from a MoveTool command. Adjusts for robot specific
/// max speeds. May return null if there is no effective move.
/// </summary>
/// <param name="m"></param>
/// <returns></returns>
private IRobotCommand CreateRobotCommand(MoveTool m, ICommandGenerator c)
{
var p = m.Target;
p.Z += z_offset;
float inches_per_minute = m.Speed == MoveTool.SpeedType.Cutting ? MaxCutSpeed : MaxRapidSpeed;
Vector3 delta = lastPosition - p;
lastPosition = p;
if (delta.Length > 0)
{
if (Math.Abs(delta.Z) > 0.0001f)
{
inches_per_minute = Math.Min(MaxZSpeed, inches_per_minute);
}
return(c.GenerateMoveCommand(p, inches_per_minute / 60.0f));
}
else
{
Console.WriteLine("Ignoring command with length of 0");
return(null);
}
}
public Robot(SerialPortWrapper serial)
{
commandGenerator = null;
this.serial = serial;
serial.newDataAvailable += new SerialPortWrapper.newDataAvailableDelegate(NewDataAvailable);
t = new Timer();
t.Interval = 50;
t.Start();
t.Elapsed += new ElapsedEventHandler(t_Elapsed);
}
public Robot(SerialPortWrapper serial)
{
commandGenerator = null;
this.serial = serial;
serial.newDataAvailable += new SerialPortWrapper.newDataAvailableDelegate(NewDataAvailable);
t = new Timer();
t.Interval = 50;
t.Start();
t.Elapsed += new ElapsedEventHandler(t_Elapsed);
}
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());
}
}
}
}
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());
}
}
}
}
/// <summary>
/// Create a robot command from a MoveTool command. Adjusts for robot specific
/// max speeds. May return null if there is no effective move.
/// </summary>
/// <param name="m"></param>
/// <returns></returns>
private IRobotCommand CreateRobotCommand(MoveTool m, ICommandGenerator c)
{
var p = m.Target;
p.Z += z_offset;
float inches_per_minute = m.Speed == MoveTool.SpeedType.Cutting ? MaxCutSpeed : MaxRapidSpeed;
Vector3 delta = lastPosition - p;
lastPosition = p;
if (delta.Length > 0)
{
if (Math.Abs(delta.Z) > 0.0001f)
{
inches_per_minute = Math.Min(MaxZSpeed, inches_per_minute);
}
return c.GenerateMoveCommand(p, inches_per_minute / 60.0f);
}
else
{
Console.WriteLine("Ignoring command with length of 0");
return null;
}
}