/// <summary>
/// Sends a command followed by command arguments to robot.
/// </summary>
/// <param name="command">A command to send.</param>
/// <param name="args">An array of bytes specifying the arguments for the command.</param>
/// <returns>An instance of SerialResponse containing the response data received from the robot.</returns>
/// <exception cref="ApplicationException">Thrown if the serial response does not match serial command.</exception>
private SerialResponse SendCommand(SerialCommands command, params byte[] args)
{
lock (this._lockCommand)
{
// Clear the port and response buffer.
this._responseBufferLength = 0;
if (Port.BytesToRead > 0)
{
Port.ReadExisting();
}
// Send command followed by any arguments.
Port.Write(new byte[] { (byte)command }, 0, 1);
if (args != null && args.Length > 0)
{
Port.Write(args, 0, args.Length);
}
// Wait to receive a complete response or until timeout.
SerialResponse serialResponse = this.ReadResponse();
if (serialResponse.SerialCommand != command)
{
throw new ApplicationException("Serial response did not match serial command.");
}
return(serialResponse);
}
}
/// <summary>
/// Captures a compressed video frame from the camera.
/// </summary>
/// <returns>A Bitmap.</returns>
public Bitmap CaptureImage()
{
SerialResponse serialResponse = this.SendCommand(SerialCommands.GrabCompressedVideoFrame);
if (!(serialResponse.Data is Bitmap))
{
throw new ApplicationException("GrabFrame request did not return a valid Image");
}
return(serialResponse.Data as Bitmap);
}
/// <summary>
/// View raw "Pixel Column Vector" data in "Wander Mode".
/// </summary>
/// <returns>An array of bytes representing 80 columns. Each column is assigned with a value
/// of 0 to 64 that indicates the pixel position of the first non-floor pixel for that
/// column (a value of 64 means there is no blockage).</returns>
public byte[] Scan()
{
SerialResponse serialResponse = this.SendCommand(SerialCommands.Scan);
if (!(serialResponse.Data is Byte[]))
{
throw new ApplicationException("Scan request did not return valid data.");
}
return(serialResponse.Data as Byte[]);
}
/// <summary>
/// Estimates the proximity on each side by emitting IR signals and counting the bounced bits.
/// </summary>
/// <returns>An instance of BounceIRResponse containing the proximity info on each side of the robot.</returns>
public BounceIRResponse GetProximity()
{
SerialResponse serialResponse = this.SendCommand(SerialCommands.BounceInfraRed);
// The response will be in form "##BounceIR - aa bb cc dd\n".
// aa bb cc dd are bounced bit counts for front, left, back, right.
if (!(serialResponse.Data is BounceIRResponse))
{
throw new ApplicationException("BounceIR request did not return a valid BounceIRResponse");
}
return(serialResponse.Data as BounceIRResponse);
}
/// <summary>
/// Waits for response data to fully load in the buffer and returns the response data.
/// </summary>
/// <remarks>
/// By convention all commands from the host are single byte ASCII characters,
/// and all commands receive an acknowledgment from the robot, which is either
/// a '#' character followed by the command, or '##' for variable length responses.
/// </remarks>
/// <param name="command">The command for which a response is expected.</param>
/// <returns>An instance of SerialResponse containing the response data received from the robot.</returns>
/// <exception cref="TimeoutException">Thrown if the response data times out before being retrieved.</exception>
/// <exception cref="ApplicationException">Thrown if the response buffer overflows.</exception>
private SerialResponse ReadResponse()
{
const int msInterval = 100;
int waitEnd = Environment.TickCount + this.ResponseTimeout;
this._responseBufferLength = 0;
// Load data into response buffer.
while (true)
{
if (Port.BytesToRead > 0)
{
int maxBytesToRead = Math.Min(this._responseBuffer.Length - this._responseBufferLength, Port.BytesToRead);
for (int lcv = 0; lcv < maxBytesToRead; lcv++)
{
// Read one byte at a time.
this._responseBuffer[this._responseBufferLength++] = (byte)Port.ReadByte();
}
// Parse the response data received so far.
SerialResponse serialResponse = this.ParseSerialResponse();
if (serialResponse != null)
{
return(serialResponse);
}
// Check the received packet won't overflow in the response buffer.
if (this._responseBufferLength == this._responseBuffer.Length)
{
throw new ApplicationException("Response buffer overflowed.");
}
}
if (Environment.TickCount > waitEnd)
{
throw new TimeoutException("Response timed out");
}
System.Threading.Thread.Sleep(msInterval);
}
}
