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());
}
}
}
}