/// <summary>
/// Read Serial Port for echo
/// </summary>
/// <param name="outBuff">The outbound message to match</param>
/// <returns>ScribblerResponse</returns>
private ScribblerResponse GetEcho(byte[] outBuff, int echoSize)
{
byte[] inBuff = new byte[echoSize];
ScribblerResponse response = null;
int ixOutBuff = 0;
DateTime lastbytetime = DateTime.Now;
try
{
while (ixOutBuff < echoSize) // && Compare(DateTime.Now, lastbytetime) < ReadTimeOut)
{
byte[] temp = new byte[1];
_serialPort.Read(temp, 0, 1); //get 1 byte
if (temp[0] == outBuff[ixOutBuff])
{
inBuff[ixOutBuff] = temp[0];
ixOutBuff++;
lastbytetime = DateTime.Now;
}
else
{
Console.WriteLine("Echo missmatch");
break;
}
}
response = new ScribblerResponse();
response.Data = (byte[])inBuff.Clone();
#if DEBUG
Console.Write("Echo: ");
foreach (byte b in response.Data)
{
if (b != 0)
{
Console.Write(b + " ");
}
else
{
Console.Write("` ");
}
}
Console.Write("\n");
#endif
}
catch (Exception ex)
{
Console.WriteLine("GetCommandResponse Exception: " + ex);
//throw;
}
return(response);
}
public ScribblerResponseMessage(ScribblerResponse b)
{
base.Body = b;
}
/// <summary>
/// Read Serial Port for a number of bytes and put into ScribblerResponse
/// </summary>
/// <param name="nBytes">number of bytes to read (includes the command type byte)</param>
/// <returns>ScribblerResponse</returns>
private ScribblerResponse GetCommandResponse(int nBytes)
{
byte[] inBuff = new Byte[Math.Abs(nBytes)];
ScribblerResponse response = null;
int read = 0;
bool error = false, done = false;
try
{
while (read < Math.Abs(nBytes) && !done)
{
int canread;
int count = 0;
while (((canread = _serialPort.BytesToRead) == 0) && count++ < 5)
{
Thread.Sleep(50); //spin
}
if (count > 5)
{
break;
}
if (nBytes < 0)
{
for (int i = 0; i < canread; i++)
{
_serialPort.Read(inBuff, read++, 1);
if (inBuff[read - 1] == 0x0A)
{
done = true;
}
}
}
else
{
int needtoread = nBytes - read;
if (canread > needtoread)
{
_serialPort.Read(inBuff, read, needtoread);
read += needtoread;
}
else
{
_serialPort.Read(inBuff, read, canread);
read += canread;
}
}
}
response = new ScribblerResponse(Math.Abs(nBytes) - 1);
response.CommandType = inBuff[inBuff.Length - 1];
for (int i = 0; i < inBuff.Length - 1; i++)
{
response.Data[i] = inBuff[i];
}
#if DEBUG
Console.Write("Got: ");
foreach (byte b in response.Data)
{
if (b != 0)
{
Console.Write(b + " ");
}
else
{
Console.Write("` ");
}
}
Console.Write("\n");
#endif
}
catch (Exception ex)
{
Console.WriteLine("GetCommandResponse Exception: " + ex);
}
return(response);
}
/// <summary>
/// DO NOT USE THIS COMMAND DIRECTLY.
/// In Scribbler.cs, post a message to _scribblerComPort
/// </summary>
/// <param name="cmd"></param>
/// <returns></returns>
internal ScribblerResponse SendCommand(ScribblerCommand cmd)
{
ScribblerResponse echo = null;
ScribblerResponse response = null;
byte[] buffer = new byte[outMessageSize];
if (buffer != null)
{
int ix = 0;
//buffer = cmd.ToByteArray();
buffer[ix++] = cmd.CommandType;
if (cmd.Data != null && cmd.Data.Length > 0)
{
foreach (byte b in cmd.Data)
{
buffer[ix++] = b;
}
}
//fill to standard size
while (ix < outMessageSize)
{
buffer[ix++] = 0;
}
#if DEBUG
Console.Write("\nSent: ");
foreach (byte b in buffer)
{
if (b != 0)
{
Console.Write(b + " ");
}
else
{
Console.Write("` ");
}
}
Console.Write("\n");
#endif
try
{
// When requesting a response, clear the inbound buffer
if (_serialPort.BytesToRead > 0)
{
_serialPort.DiscardInBuffer();
}
_serialPort.Write(buffer, 0, ix);
}
catch
{
Console.WriteLine("Serial Port Timeout. Lost connection with Scribbler.");
//throw new IOException();
}
echo = GetEcho(buffer);
response = GetCommandResponse(helper.ReturnSize((ScribblerHelper.Commands)cmd.CommandType));
}
return(response);
}
/// <summary>
/// Attempt to read our data from a serial port.
/// </summary>
/// <param name="spName">Serial port name</param>
/// <returns>name of robot attached to port</returns>
private string TrySerialPort(string spName)
{
#if DEBUG
Console.WriteLine("Trying Serial Port: " + spName); //DEBUG
#endif
SerialPort p = null;
string robotname = null;
try
{
p = new SerialPort(spName, _baudRate, Parity.None, 8, StopBits.One);
p.Handshake = Handshake.None;
p.Encoding = Encoding.ASCII;
p.Open();
_serialPort = p;
#if DEBUG
Console.WriteLine("reading once"); //DEBUG
#endif
// Send the GETINFO string
ScribblerResponse srp = SendCommand(new ScribblerCommand((byte)ScribblerHelper.Commands.GET_INFO));
// GTEMP: Send twice - some problem with dongle
srp = SendCommand(new ScribblerCommand((byte)ScribblerHelper.Commands.GET_INFO));
UTF8Encoding enc = new UTF8Encoding();
string s = enc.GetString(srp.Data);
if (s.Length == 0)
{
System.Threading.Thread.Sleep((int)(noCommsTimeout * 1.5));
#if DEBUG
Console.WriteLine("reading again"); //DEBUG
#endif
s = p.ReadExisting(); //try again
}
if (s.Length == 0)
{
#if DEBUG
Console.WriteLine("length == 0"); //DEBUG
#endif
return(null);
}
// we are receiving data.
#if DEBUG
Console.WriteLine("we are receiving data."); //DEBUG
Console.WriteLine("received: \"" + s + "\"");
#endif
int index = s.IndexOf(characteristicString);
//not a Scribbler robot
if (index < 0)
{
#if DEBUG
Console.WriteLine("not a Scribbler robot."); //DEBUG
#endif
_serialPort = null;
return(null);
}
// Now get robotname
srp = SendCommand(new ScribblerCommand((byte)ScribblerHelper.Commands.GET_NAME));
enc = new UTF8Encoding();
robotname = enc.GetString(srp.Data);
if (robotname.Length == 0)
{
#if DEBUG
Console.WriteLine("Cannot get name"); //DEBUG
#endif
robotname = "Noname";
}
// Sending Echo off command
SendCommand(new ScribblerCommand((byte)ScribblerHelper.Commands.SET_ECHO_MODE, (byte)1, (byte)1));
#if DEBUG
Console.WriteLine("TrySerialPort found: " + robotname); //DEBUG
#endif
}
catch (Exception ex)
{
#if DEBUG
Console.WriteLine("TrySerialPort caught exception: " + ex);
//throw new Exception("TrySerialPort caught exception", ex);
#endif
}
finally
{
if (p != null && p.IsOpen)
{
p.Close();
}
p.Dispose();
}
return(robotname);
}
/// <summary>
/// Read Serial Port for a number of bytes and put into ScribblerResponse
/// </summary>
/// <param name="nBytes">number of bytes to read (includes the command type byte)</param>
/// <returns>ScribblerResponse</returns>
private ScribblerResponse GetCommandResponse(ScribblerCommand cmd)
{
int nBytes = cmd.ResponseLength;
bool cmdEcho = cmd.HasEcho;
//Console.WriteLine("GetCommandResponse: creating buffer");
byte[] inBuff = new Byte[Math.Abs(nBytes)];
//Console.WriteLine("Check 1");
ScribblerResponse response = null;
//Console.WriteLine("Check 2");
int read = 0;
bool error = false, done = false;
// Set the default EOM character to a linefeed
if (cmd.EndMarker1 == null)
{
cmd.EndMarker1 = 0x0A;
cmd.EndMarker2 = null;
}
while (read < Math.Abs(nBytes) && !done)
{
int canread;
int count = 0;
while (((canread = _serialPort.BytesToRead) == 0) && count++ < 100)
{
Thread.Sleep(20); //spin
}
if (canread == 0)
{
break;
}
//Console.WriteLine("Spun for " + (count*10) + " ms, got chunk of " + canread);
if (nBytes < 0)
{
//if (cmd.CommandType == (byte)ScribblerHelper.Commands.GET_JPEG_COLOR_SCAN)
//{
// Console.WriteLine("JPEG");
//}
//Console.WriteLine("Reading variable length (buffer size " + inBuff.Length + ")");
for (int i = 0; i < canread; i++)
{
_serialPort.Read(inBuff, read++, 1);
// NOTE: This is a hack to get the header length from the first
// two bytes of the return from GET_JPEG_HEADER.
if (read == 2 &&
(cmd.CommandType == (byte)ScribblerHelper.Commands.GET_JPEG_COLOR_HEADER ||
cmd.CommandType == (byte)ScribblerHelper.Commands.GET_JPEG_GRAY_HEADER))
{
nBytes = (int)inBuff[0] + ((int)inBuff[1] << 8) + 2;
//Console.WriteLine("Looking for JPEG header of " + nBytes + " bytes");
}
else if (nBytes < 0) // Have to check this again because of the JPEG hack, changed nBytes
{
//Console.WriteLine(" Got " + inBuff[read - 1] + " at " + (read - 1));
if (cmd.EndMarker2 == null)
{
if (inBuff[read - 1] == cmd.EndMarker1)
{
done = true;
}
}
else if (read >= 2)
{
if (inBuff[read - 2] == cmd.EndMarker1 && inBuff[read - 1] == cmd.EndMarker2)
{
done = true;
}
}
}
}
}
else
{
int needtoread = nBytes - read;
//Console.WriteLine("Reading fixed length of " + needtoread + ", buffer " + nBytes);
if (canread > needtoread)
{
_serialPort.Read(inBuff, read, needtoread);
read += needtoread;
}
else
{
_serialPort.Read(inBuff, read, canread);
read += canread;
}
}
}
//Console.WriteLine("GetCommandResponse: " + _serialPort.BytesToRead + " left over");
if (read < nBytes)
{
throw new ScribblerProtocolException("Command response of " + read + " was too short: " + (ScribblerHelper.Commands)cmd.CommandType);
}
int dataBytes = (cmdEcho ? read - 1 : read);
response = new ScribblerResponse(Math.Max(0, dataBytes));
response.CommandType = (cmdEcho ? inBuff[inBuff.Length - 1] : (byte)0);
Array.Copy(inBuff, response.Data, dataBytes);
#if DEBUG
Console.Write("Got: ");
foreach (byte b in response.Data)
{
if (b != 0)
{
Console.Write(b + " ");
}
else
{
Console.Write("` ");
}
}
Console.Write("\n");
#endif
return(response);
}
/// <summary>
/// Read Serial Port for echo
/// </summary>
/// <param name="outBuff">The outbound message to match</param>
/// <returns>ScribblerResponse</returns>
private ScribblerResponse GetEcho(byte[] outBuff)
{
byte[] inBuff = new byte[outBuff.Length];
ScribblerResponse response = null;
//int ixOutBuff = 0;
//DateTime lastbytetime = DateTime.Now;
//try
//{
// Read the whole echo in one chunk, with 500ms timeout
int count = 0;
while (_serialPort.BytesToRead < outBuff.Length)
{
if (++count > 50)
{
throw new TimeoutException("Timed out waiting for command echo of " + outBuff.Length + " bytes");
}
Thread.Sleep(10); //spin
}
_serialPort.Read(inBuff, 0, outBuff.Length);
for (int i = 0; i < outBuff.Length; i++)
{
if (inBuff[i] != outBuff[i])
{
Console.WriteLine("Echo mismatch");
}
}
//while (ixOutBuff < echoSize) // && Compare(DateTime.Now, lastbytetime) < ReadTimeOut)
//{
// byte[] temp = new byte[1];
// _serialPort.Read(temp, 0, 1); //get 1 byte
// if (temp[0] == outBuff[ixOutBuff])
// {
// inBuff[ixOutBuff] = temp[0];
// ixOutBuff++;
// lastbytetime = DateTime.Now;
// }
// else
// {
// Console.WriteLine("Echo missmatch");
// break;
// }
//}
response = new ScribblerResponse();
response.Data = inBuff;
#if DEBUG
Console.Write("Echo: ");
foreach (byte b in response.Data)
{
if (b != 0)
{
Console.Write(b + " ");
}
else
{
Console.Write("` ");
}
}
Console.Write("\n");
#endif
//}
//catch (Exception ex)
//{
// Console.WriteLine("GetCommandResponse Exception: " + ex);
// //throw;
//}
return(response);
}
/// <summary>
/// DO NOT USE THIS COMMAND DIRECTLY.
/// In Scribbler.cs, post a message to _scribblerComPort
/// </summary>
/// <param name="cmd"></param>
/// <returns></returns>
internal ScribblerResponse SendCommand(ScribblerCommand cmd)
{
ScribblerResponse echo = null;
ScribblerResponse response = null;
int outMessageSize = cmd.Data.Length + 1;
byte[] buffer = new byte[outMessageSize];
if (buffer != null)
{
int ix = 0;
//buffer = cmd.ToByteArray();
buffer[ix++] = cmd.CommandType;
// Changed this so it doesn't copy entire command (Fluke commands are shorter than 8 bytes)
int len = Math.Min(cmd.Data.Length, (outMessageSize - 1));
if (cmd.Data != null && cmd.Data.Length > 0)
{
Array.Copy(cmd.Data, 0, buffer, 1, len);
}
ix += len;
//foreach (byte b in cmd.Data)
// buffer[ix++] = b;
//fill to standard size
while (ix < outMessageSize)
{
buffer[ix++] = 0;
}
#if DEBUG
Console.Write("\nSent: ");
foreach (byte b in buffer)
{
if (b != 0)
{
Console.Write(b + " ");
}
else
{
Console.Write("` ");
}
}
Console.Write("\n");
#endif
//try
//{
// When requesting a response, clear the inbound buffer
//Console.WriteLine(_serialPort.BytesToRead + " bytes left over");
while (_serialPort.BytesToRead > 0)
{
_serialPort.DiscardInBuffer();
}
//Console.WriteLine(((ScribblerHelper.Commands)cmd.CommandType).ToString());
//Console.WriteLine("Command: " + cmd.Data.Length);
//Console.WriteLine("Response: " + cmd.ResponseLength);
//Console.WriteLine("Echo: " + cmd.HasEcho);
_serialPort.Write(buffer, 0, ix);
//}
//catch
//{
//Console.WriteLine("Serial Port Timeout. Lost connection with Scribbler.");
//throw new IOException();
//}
if (cmd.HasEcho)
{
echo = GetEcho(buffer);
}
response = GetCommandResponse(cmd);
}
return(response);
}
/// <summary>
/// Read Serial Port for a number of bytes and put into ScribblerResponse
/// </summary>
/// <param name="nBytes">number of bytes to read (includes the command type byte)</param>
/// <returns>ScribblerResponse</returns>
private ScribblerResponse GetCommandResponse(int nBytes, bool cmdEcho)
{
//Console.WriteLine("GetCommandResponse: creating buffer");
byte[] inBuff = new Byte[Math.Abs(nBytes)];
//Console.WriteLine("Check 1");
ScribblerResponse response = null;
//Console.WriteLine("Check 2");
int read = 0;
bool error = false, done = false;
try
{
while (read < Math.Abs(nBytes) && !done)
{
int canread;
int count = 0;
while (((canread = _serialPort.BytesToRead) == 0) && count++ < 5)
{
Thread.Sleep(50); //spin
}
if (count > 5)
{
break;
}
if (nBytes < 0)
{
//Console.WriteLine("Reading variable length (buffer size " + inBuff.Length + ")");
for (int i = 0; i < canread; i++)
{
_serialPort.Read(inBuff, read++, 1);
//Console.WriteLine(" Got " + inBuff[read - 1] + " at " + (read - 1));
if (inBuff[read - 1] == 0x0A)
{
done = true;
}
}
}
else
{
int needtoread = nBytes - read;
//Console.WriteLine("Reading fixed length of " + needtoread + ", buffer " + nBytes);
if (canread > needtoread)
{
_serialPort.Read(inBuff, read, needtoread);
read += needtoread;
}
else
{
_serialPort.Read(inBuff, read, canread);
read += canread;
}
}
}
int dataBytes = (cmdEcho ? Math.Abs(nBytes) - 1 : Math.Abs(nBytes));
response = new ScribblerResponse(Math.Max(0, dataBytes));
response.CommandType = (cmdEcho ? inBuff[inBuff.Length - 1] : (byte)0);
Array.Copy(inBuff, response.Data, dataBytes);
#if DEBUG
Console.Write("Got: ");
foreach (byte b in response.Data)
{
if (b != 0)
{
Console.Write(b + " ");
}
else
{
Console.Write("` ");
}
}
Console.Write("\n");
#endif
}
catch (Exception ex)
{
Console.WriteLine("GetCommandResponse Exception: " + ex);
//throw;
}
return(response);
}
/// <summary>
/// Read Serial Port for a number of bytes and put into ScribblerResponse
/// </summary>
/// <param name="nBytes">number of bytes to read (includes the command type byte)</param>
/// <returns>ScribblerResponse</returns>
private ScribblerResponse GetCommandResponse(ScribblerCommand cmd)
{
int nBytes = cmd.ResponseLength;
bool cmdEcho = cmd.HasEcho;
//Console.WriteLine("GetCommandResponse: creating buffer");
byte[] inBuff = new Byte[Math.Abs(nBytes)];
//Console.WriteLine("Check 1");
ScribblerResponse response = null;
//Console.WriteLine("Check 2");
int read = 0;
bool error = false, done = false;
// Set the default EOM character to a linefeed
if (cmd.EndMarker1 == null)
{
cmd.EndMarker1 = 0x0A;
cmd.EndMarker2 = null;
}
while (read < Math.Abs(nBytes) && !done)
{
int canread;
int count = 0;
while (((canread = _serialPort.BytesToRead) == 0) && count++ < 100)
Thread.Sleep(20); //spin
if (canread == 0)
break;
//Console.WriteLine("Spun for " + (count*10) + " ms, got chunk of " + canread);
if (nBytes < 0)
{
//if (cmd.CommandType == (byte)ScribblerHelper.Commands.GET_JPEG_COLOR_SCAN)
//{
// Console.WriteLine("JPEG");
//}
//Console.WriteLine("Reading variable length (buffer size " + inBuff.Length + ")");
for (int i = 0; i < canread; i++)
{
_serialPort.Read(inBuff, read++, 1);
// NOTE: This is a hack to get the header length from the first
// two bytes of the return from GET_JPEG_HEADER.
if (read == 2 &&
(cmd.CommandType == (byte)ScribblerHelper.Commands.GET_JPEG_COLOR_HEADER ||
cmd.CommandType == (byte)ScribblerHelper.Commands.GET_JPEG_GRAY_HEADER))
{
nBytes = (int)inBuff[0] + ((int)inBuff[1] << 8) + 2;
//Console.WriteLine("Looking for JPEG header of " + nBytes + " bytes");
}
else if (nBytes < 0) // Have to check this again because of the JPEG hack, changed nBytes
{
//Console.WriteLine(" Got " + inBuff[read - 1] + " at " + (read - 1));
if (cmd.EndMarker2 == null)
{
if (inBuff[read - 1] == cmd.EndMarker1)
done = true;
}
else if (read >= 2)
{
if (inBuff[read - 2] == cmd.EndMarker1 && inBuff[read - 1] == cmd.EndMarker2)
done = true;
}
}
}
}
else
{
int needtoread = nBytes - read;
//Console.WriteLine("Reading fixed length of " + needtoread + ", buffer " + nBytes);
if (canread > needtoread)
{
_serialPort.Read(inBuff, read, needtoread);
read += needtoread;
}
else
{
_serialPort.Read(inBuff, read, canread);
read += canread;
}
}
}
//Console.WriteLine("GetCommandResponse: " + _serialPort.BytesToRead + " left over");
if (read < nBytes)
throw new ScribblerProtocolException("Command response of " + read + " was too short: " + (ScribblerHelper.Commands)cmd.CommandType);
int dataBytes = (cmdEcho ? read - 1 : read);
response = new ScribblerResponse(Math.Max(0, dataBytes));
response.CommandType = (cmdEcho ? inBuff[inBuff.Length - 1] : (byte)0);
Array.Copy(inBuff, response.Data, dataBytes);
#if DEBUG
Console.Write("Got: ");
foreach (byte b in response.Data)
{
if (b != 0)
Console.Write(b + " ");
else
Console.Write("` ");
}
Console.Write("\n");
#endif
return response;
}
/// <summary>
/// Read Serial Port for echo
/// </summary>
/// <param name="outBuff">The outbound message to match</param>
/// <returns>ScribblerResponse</returns>
private ScribblerResponse GetEcho(byte[] outBuff)
{
byte[] inBuff = new byte[outBuff.Length];
ScribblerResponse response = null;
//int ixOutBuff = 0;
//DateTime lastbytetime = DateTime.Now;
//try
//{
// Read the whole echo in one chunk, with 500ms timeout
int count = 0;
while (_serialPort.BytesToRead < outBuff.Length)
{
if (++count > 50)
throw new TimeoutException("Timed out waiting for command echo of " + outBuff.Length + " bytes");
Thread.Sleep(10); //spin
}
_serialPort.Read(inBuff, 0, outBuff.Length);
for (int i = 0; i < outBuff.Length; i++)
if (inBuff[i] != outBuff[i])
Console.WriteLine("Echo mismatch");
//while (ixOutBuff < echoSize) // && Compare(DateTime.Now, lastbytetime) < ReadTimeOut)
//{
// byte[] temp = new byte[1];
// _serialPort.Read(temp, 0, 1); //get 1 byte
// if (temp[0] == outBuff[ixOutBuff])
// {
// inBuff[ixOutBuff] = temp[0];
// ixOutBuff++;
// lastbytetime = DateTime.Now;
// }
// else
// {
// Console.WriteLine("Echo missmatch");
// break;
// }
//}
response = new ScribblerResponse();
response.Data = inBuff;
#if DEBUG
Console.Write("Echo: ");
foreach (byte b in response.Data)
{
if (b != 0)
Console.Write(b + " ");
else
Console.Write("` ");
}
Console.Write("\n");
#endif
//}
//catch (Exception ex)
//{
// Console.WriteLine("GetCommandResponse Exception: " + ex);
// //throw;
//}
return response;
}
