/// <summary>
/// Send a byte array to the bricks mailbox system
/// </summary>
/// <param name='data'>
/// Data array to write to the mailbox
/// </param>
/// <param name='inbox'>
/// The mailbox to send to
/// </param>
/// <param name='reply'>
/// If set to <c>true</c> the brick will send a reply
/// </param>
public void Send(byte[] data, Box inbox, bool reply){
var command = new Command(CommandType.DirecCommand, CommandByte.MessageWrite, reply);
if(data.Length > 57){
Array.Resize(ref data,57);
}
command.Append((byte)inbox);
command.Append(System.Convert.ToByte(data.Length+1));
command.Append(data);
command.Append((byte)0);
command.Print();
connection.Send(command);
if(reply){
var brickReply = connection.Receive();
Error.CheckForError(brickReply,3);
}
}
/// <summary>
/// Read a specefic number of bytes from a file
/// </summary>
/// <param name='file'>
/// The file to read from
/// </param>
/// <param name='bytesToRead'>
/// Bytes to read
/// </param>
public byte[] Read(BrickFile file, UInt16 bytesToRead){
var command = new Command(CommandType.SystemCommand, CommandByte.Read, true);
command.Append(file.Handle);
command.Append(bytesToRead);
connection.Send(command);
var reply = connection.Receive();
if(reply.HasError){
CloseFile(file);
Error.ThrowException(reply);
}
//UInt16 bytesRead = reply.GetUInt16(4); //use this for some error checking
byte[] result = reply.GetData(6);
if(OnBytesRead != null)
OnBytesRead(result.Length);
return result;
}
private int Write(BrickFile file, byte[] data, int offset, int length){
var command = new Command(CommandType.SystemCommand, CommandByte.Write,true);
command.Append(file.Handle);
command.Append(data, offset, length);
connection.Send(command);
//Console.WriteLine(file.Size);
//command.Print();
var reply = connection.Receive();
Error.CheckForError(reply,6, delegate(){CloseFile(file);});
int result = (int) reply.GetUInt16(4);//The number of bytes written
if(OnBytesWritten != null)
OnBytesWritten(result);
return result;
}
private BrickFile FindNext(BrickFile file){
var command = new Command(CommandType.SystemCommand,CommandByte.FindNext,true);
command.Append(file.Handle);
connection.Send(command);
var reply = connection.Receive();
if(reply.HasError && reply.ErrorCode != (byte)BrickError.FileNotFound){
Error.ThrowException(reply);
}
if(reply.ErrorCode == (byte)BrickError.FileNotFound){
return new BrickFile();//empty file
}
if(reply.Length != 28){
throw new BrickException(BrickError.WrongNumberOfBytes);
}
BrickFile newFile = new BrickFile(reply.GetString(4),reply[3],reply.GetUInt32(24));
try{
CloseFile(newFile);
}
catch(MonoBrickException e){
if(e.ErrorCode != (byte)BrickError.HandleAlreadyClosed)
Error.ThrowException(e.ErrorCode);
}
return newFile;
}
/// <summary>
/// Send a string to brick's mailbox system
/// </summary>
/// <param name='s'>
/// string to write
/// </param>
/// <param name='inbox'>
/// The mailbox to send to
/// </param>
/// <param name='reply'>
/// If set to <c>true</c> the brick will send a reply
/// </param>
public void Send(string s, Box inbox, bool reply){
var command = new Command(CommandType.DirecCommand, CommandByte.MessageWrite, reply);
if(s.Length > 57){
s.Remove(57);
}
command.Append((byte)inbox);
command.Append(System.Convert.ToByte(s.Length+1));
command.Append(s);
connection.Send(command);
if(reply){
var brickReply = connection.Receive();
Error.CheckForError(brickReply,3);
}
}
/// <summary>
/// Gets the output state of the motor
/// </summary>
/// <returns>
/// The output state
/// </returns>
public OutputState GetOutputState() {
OutputState motorOutput = new OutputState();
var command = new Command(CommandType.DirecCommand, CommandByte.GetOutputState,true);
command.Append((byte)port);
connection.Send(command);
var reply = connection.Receive();
Error.CheckForError(reply,25);
motorOutput.Speed = reply.GetSbyte(4);
motorOutput.Mode = (MotorMode)reply[5];
motorOutput.Regulation = (MotorRegulation)reply[6];
motorOutput.TurnRatio = reply.GetSbyte(7);
motorOutput.RunState = (MotorRunState)reply[8];
motorOutput.TachoLimit = reply.GetUInt32(9);
motorOutput.TachoCount = reply.GetInt32(13);
motorOutput.BlockTachoCount = reply.GetInt32(17);
motorOutput.RotationCount = reply.GetInt32(21);
return motorOutput;
}
/// <summary>
/// Deletes flash memory
/// </summary>
public void DeleteFlash(){
var command = new Command(CommandType.SystemCommand,CommandByte.DeleteUserFlash, true);
connection.Send(command);
var reply = connection.Receive();
Error.CheckForError(reply,3);
if(OnFlashDeleted != null)
OnFlashDeleted();
}
/// <summary>
/// Closes the file.
/// </summary>
/// <param name='file'>
/// File to close
/// </param>
public void CloseFile(BrickFile file){
var command = new Command(CommandType.SystemCommand, CommandByte.Close, true);
command.Append(file.Handle);
Connection.Send(command);
var reply = connection.Receive();
if(reply.HasError && reply.ErrorCode != (byte)BrickError.HandleAlreadyClosed){
Error.ThrowException(reply);
}
//compare the handle numbers
}
private byte[] I2CRead()
{
var command = new Command(CommandType.DirecCommand, CommandByte.LsRead, true);
command.Append((byte)Port);
var reply = connection.SendAndReceive(command);
Error.CheckForError(reply, 20);
byte size = reply[3];
byte[] data = reply.GetData(4);
Array.Resize(ref data,size);
return data;
}
/// <summary>
/// Write byte array to sensor
/// </summary>
/// <param name='txData'>
/// The byte array to write
/// </param>
/// <param name='rxLength'>
/// The length of the expected reply
/// </param>
/// <param name='reply'>
/// If set to <c>true</c> brick will send a reply
/// </param>
protected void I2CWrite(byte[] txData, byte rxLength, bool reply)
{
var command = new Command(CommandType.DirecCommand, CommandByte.LsWrite, reply);
command.Append((byte)Port);
command.Append((byte)txData.Length);
command.Append(rxLength);
command.Append(txData);
connection.Send(command);
if(reply){
var brickReply = connection.Receive();
Error.CheckForError(brickReply,5);
}
}
internal SensorReadings GetSensorReadings()
{
if (!hasInit)
{
Initialize();
}
SensorReadings sensorReadings = new SensorReadings();
var command = new Command(CommandType.DirecCommand,CommandByte.GetInputValues, true);
command.Append((byte) port);
var reply = connection.SendAndReceive(command);
Error.CheckForError(reply, 16);
sensorReadings.Raw = reply.GetUInt16(8);
sensorReadings.Normalized = reply.GetUInt16(10);
sensorReadings.Scaled = reply.GetInt16(12);
return sensorReadings;
}
/// <summary>
/// Updates the sensor type and mode.
/// </summary>
/// <param name='sensorType'>
/// Sensor type.
/// </param>
/// <param name='sensorMode'>
/// Sensor mode.
/// </param>
protected void UpdateTypeAndMode(SensorType sensorType, SensorMode sensorMode){
Type = sensorType;
Mode = sensorMode;
var command = new Command(CommandType.DirecCommand, CommandByte.SetInputMode, true);
command.Append((byte)port);
command.Append((byte)Type);
command.Append((byte)Mode);
connection.Send(command);
var reply = connection.Receive();
Error.CheckForError(reply, 3);
}
/// <summary>
/// Resets the scaled value.
/// </summary>
/// <param name='reply'>
/// If set to <c>true</c> brick will send a reply
/// </param>
protected void ResetScaledValue(bool reply){
var command = new Command(CommandType.DirecCommand, CommandByte.ResetInputScaledValue, reply);
command.Append((byte)port);
connection.Send(command);
if (reply) {
var brickReply = connection.Receive();
Error.CheckForError(brickReply, 3);
}
}
private void ResetMotorPosition(bool relative, bool reply) {
var command = new Command(CommandType.DirecCommand, CommandByte.ResetMotorPosition, reply);
command.Append((byte)Port);
command.Append(relative);
connection.Send(command);
if (reply)
{
var brickReply = connection.Receive();
Error.CheckForError(brickReply, 3);
}
}
/// <summary>
/// Opens file for write. If file exist it is overwritten
/// </summary>
/// <returns>
/// The file.
/// </returns>
/// <param name='fileName'>
/// The name of the file
/// </param>
/// <param name='fileSize'>
/// File size in bytes
/// </param>
/// <param name='fileType'>
/// File mode when opening
/// </param>
public BrickFile OpenWrite(string fileName, UInt32 fileSize, FileMode fileType){
var command = new Command(CommandType.SystemCommand, (CommandByte) fileType, true);
command.Append(fileName,MaxFileNameLength,true);
command.Append(fileSize);
connection.Send(command);
var reply = connection.Receive();
Error.CheckForError(reply,4);
if(fileName.Length >MaxFileNameLength)
fileName.Remove(MaxFileNameLength);
return new BrickFile(fileName, reply[3], fileSize);
}
/// <summary>
/// Opens file to append data
/// </summary>
/// <returns>
/// The file
/// </returns>
/// <param name='fileName'>
/// File name
/// </param>
public BrickFile OpenAppend(string fileName){
var command = new Command(CommandType.SystemCommand, CommandByte.OpenAppendData,true);
command.Append(fileName,MaxFileNameLength,true);
connection.Send(command);
var reply = connection.Receive();
Error.CheckForError(reply,8);
if(fileName.Length >MaxFileNameLength)
fileName.Remove(MaxFileNameLength);
return new BrickFile(fileName,reply[3],reply.GetUInt32(4));
}
private byte BytesReady()
{
var command = new Command(CommandType.DirecCommand, CommandByte.LsGetStatus, true);
command.Append((byte)port);
var reply = connection.SendAndReceive(command);
Error.CheckForError(reply, 4);
return reply[3];
}
/// <summary>
/// Deletes file.
/// </summary>
/// <param name='fileName'>
/// Name of file to delete
/// </param>
public void DeleteFile(string fileName){
var command = new Command(CommandType.SystemCommand, CommandByte.Delete, true);
command.Append(fileName,MaxFileNameLength, true);
connection.Send(command);
var reply = connection.Receive();
if(reply.HasError)//dont't know the length of the reply compare the file name
Error.ThrowException(reply.ErrorCode);
}
/// <summary>
/// Read a byte array from the brick's mailbox system
/// </summary>
/// <returns>
/// The message as a byte array
/// </returns>
/// <param name='mailbox'>
/// The mailbox to read
/// </param>
/// <param name='removeMessage'>
/// If set to <c>true</c> the message will be removed from the mailbox
/// </param>
public byte[] Read(Box mailbox, bool removeMessage){
var command = new Command(CommandType.DirecCommand, CommandByte.MessageRead, true);
command.Append((byte)((byte)mailbox + (byte)10));
command.Append((byte)((byte)mailbox + (byte)0));
command.Append(removeMessage);
connection.Send(command);
var reply = connection.Receive();
Error.CheckForError(reply,64);
byte size = reply[4];
byte[] returnValue = new byte[size];
for(int i = 0; i < size; i++){
returnValue[i] = reply[i+5];
}
return returnValue;
}
private void ClientThread(object stateInfo)
{
byte[] message = null;
byte[] lengthBytes = new byte[2];
int bytesToRead;
int bytesRead;
//Stopwatch stopWatch = new Stopwatch();
//Stopwatch stopWatch2 = new Stopwatch();
NXT.Command nxtCommand = null;
NXT.Reply nxtReply = null;
EV3.Command ev3Command = null;
EV3.Reply ev3Reply = null;
logQueue.AddToQueue(IdName + "Connect from " + address);
IsConnected = true;
bool run = true;
while (run)
{
bytesToRead = 0;
bytesRead = 0;
message = null;
try
{
bytesRead = networkStream.ReadAll(lengthBytes);
if (bytesRead > 0)
{
bytesToRead = (ushort)(0x0000 | lengthBytes[0] | (lengthBytes[1] << 2));
message = new byte[bytesToRead];
bytesRead = 0;
bytesRead = networkStream.ReadAll(message);
bool CommandValid = true;
if (bytesRead == bytesToRead)
{
try{
if (type == BrickType.NXT)
{
nxtCommand = new NXT.Command(message);
}
else
{
ev3Command = new EV3.Command(message);
}
}
catch (Exception) {
CommandValid = false;
logQueue.AddToQueue(IdName + "Invalid command is ignored");
}
}
else
{
CommandValid = false;
run = false;
logQueue.AddToQueue(IdName + "Not enough bytes read");
}
if (CommandValid)
{
nxtReply = null;
mutex.WaitOne();
try{
if (type == BrickType.NXT)
{
if ((nxtCommand).CommandType == NXT.CommandType.TunnelCommand)
{
if (TunnelCommandReceived == null)
{
nxtReply = new NXT.Reply(NXT.CommandType.ReplyCommand, nxtCommand.CommandByte, (byte)TunnelError.UnsupportedCommand);
}
else
{
var brickReply = TunnelCommandReceived(nxtCommand);
nxtReply = new NXT.Reply(brickReply.Data);
}
SendNetworkReply(nxtReply);
}
else
{
if (nxtCommand.ReplyRequired)
{
if (LogActivity)
{
logQueue.AddToQueue(IdName + "Forward ".PadRight(12) + nxtCommand.CommandByte.ToString() + " with reply to NXT");
}
}
else
{
if (LogActivity)
{
logQueue.AddToQueue(IdName + "Forward ".PadRight(12) + nxtCommand.CommandByte.ToString() + " without reply to NXT");
}
}
Brick.Send(nxtCommand);
if (nxtCommand.ReplyRequired)
{
nxtReply = (NXT.Reply)Brick.Receive();
SendNetworkReply(nxtReply);
if (LogActivity)
{
logQueue.AddToQueue(IdName + "Received ".PadRight(12) + nxtReply.CommandByte.ToString() + " from NXT");
}
}
}
}
else
{
throw new NotImplementedException("EV3 support is not implemented");
}
}
catch (Exception e) {
if (e is MonoBrickException)
{
if (e is ConnectionException)
{
logQueue.AddToQueue(IdName + e.Message);
logQueue.AddToQueue("Closing connection to client " + ID);
run = false;
}
else
{
logQueue.AddToQueue(IdName + e.Message);
if (nxtReply == null) //try to send the message with error
{
nxtReply = new NXT.Reply(NXT.CommandType.ReplyCommand, nxtCommand.CommandByte, ((MonoBrickException)e).ErrorCode);
SendNetworkReply(nxtReply);
}
if (ev3Reply == null)
{
throw new NotImplementedException("EV3 support is not implemented");
}
}
}
else
{
if (!wasThrownOff)
{
logQueue.AddToQueue(IdName + e.Message);
logQueue.AddToQueue(IdName + "--------- Stack Trace --------\n" + e.StackTrace);
}
run = false;
}
}
mutex.ReleaseMutex();
}
}
else
{
run = false;
}
}
catch (Exception e)
{
if (!wasThrownOff)
{
logQueue.AddToQueue(IdName + e.Message);
logQueue.AddToQueue(IdName + "--------- Stack Trace --------\n" + e.StackTrace);
}
break;
}
}
try{
networkStream.Close();
}
catch {}
logQueue.AddToQueue(IdName + "Disconnected");
if (onDisconnected != null)
{
onDisconnected(this);
}
IsConnected = false;
}
/// <summary>
/// Sets the output state of the motor
/// </summary>
/// <param name='state'>
/// Outputstate
/// </param>
/// <param name='reply'>
/// If set to <c>true</c> the brick will send a reply
/// </param>
public void SetOutputState(OutputState state, bool reply){
if(state.Speed > 100){
state.Speed = 100;
}
if(state.Speed < -100){
state.Speed = -100;
}
if(state.TurnRatio > 100){
state.TurnRatio = 100;
}
if(state.TurnRatio < -100){
state.TurnRatio = 100;
}
if (Reverse)
state.Speed = (sbyte)-state.Speed;
var command = new Command(CommandType.DirecCommand,CommandByte.SetOutputState, reply);
command.Append((byte)port);
command.Append(state.Speed);
command.Append((byte)state.Mode);
command.Append((byte)state.Regulation);
command.Append(state.TurnRatio);
command.Append((byte)state.RunState);
command.Append(state.TachoLimit);
command.Append((byte) 0x00);//why a 5th byte?
connection.Send(command);
if(reply){
var brickReply = connection.Receive();
Error.CheckForError(brickReply,3);
}
}
