public void CommandSendReceived(Command command)
{
if (OnCommandSend != null)
{
OnCommandSend(this, new MessageEventArgs(command.ToString()));
}
}
/// <summary>
/// Sets the type of the output. I don't know what this is used for.
/// </summary>
/// <param name="port">Motor port to use.</param>
/// <param name="type">Type to use.</param>
/// <param name="reply">If set to <c>true</c> reply from brick will be send.</param>
public void SetType(MotorPort port, byte type, bool reply = false){
var command = new Command(0,0,200,reply);
command.Append(ByteCodes.OutputSetType);
command.Append(this.DaisyChainLayer);
command.Append(port);
command.Append(type, ParameterFormat.Short);
connection.Send(command);
if(reply){
var brickReply = connection.Receive();
Error.CheckForError(brickReply,200);
}
}
/// <summary>
/// Send a byte array to the mailbox
/// </summary>
/// <param name="mailboxName">Mailbox name to send to.</param>
/// <param name="data">Data to send.</param>
/// <param name="reply">If set to <c>true</c> reply from the brick will be send.</param>
public void Send(string mailboxName, byte[] data, bool reply = false)
{
var command = new Command (SystemCommand.WriteMailbox, 100, reply);
Int16 payloadLength = (Int16)data.Length;
byte nameLength = (byte)(mailboxName.Length + 1);
command.Append (nameLength);
command.Append (mailboxName);
command.Append(payloadLength);
command.Append(data);
connection.Send(command);
if(reply){
var brickReply = connection.Receive();
Error.CheckForError(brickReply,100);
}
}
/// <summary>
/// Gets the sensor type and mode.
/// </summary>
/// <param name="type">Type.</param>
/// <param name="mode">Mode.</param>
protected void GetTypeAndMode(out SensorType type, out SensorMode mode){
if(!isInitialized)
Initialize();
var command = new Command(2,0,200,true);
command.Append(ByteCodes.InputDevice);
command.Append(InputSubCodes.GetTypeMode);
command.Append(this.DaisyChainLayer);
command.Append(this.Port);
command.Append((byte)0, VariableScope.Global);
command.Append((byte)1, VariableScope.Global);
var reply = Connection.SendAndReceive(command);
Error.CheckForError(reply,200);
type = (SensorType) reply.GetByte(3);
mode = (SensorMode) reply.GetByte(4);
}
public int Open (byte number, string mailboxName)
{
var command = new Command(0, 0,100,true);
command.Append(ByteCodes.MailboxOpen);
//command.Append(number)
command.Append(number, ParameterFormat.Short);
//command.Append(0);
command.Append(0,ParameterFormat.Short);
//command.Append(0);
command.Append(0,ParameterFormat.Short);
//command.Append(0);
command.Append(0,ParameterFormat.Short);
//command.Append(30);
command.Append(10,ParameterFormat.Short);
var reply = connection.SendAndReceive(command);
Error.CheckForError(reply,100);
reply.print();
return 0;
}
/// <summary>
/// Sets the absolute position from last reset
/// </summary>
/// <param name="position">Position to use</param>
/// <param name="reply">If set to <c>true</c> reply from the brick will be send</param>
public void SetPosition(Int32 position, bool reply = false){
var command = new Command(0,0,214,reply);
command.Append(ByteCodes.OutputPosition);
command.Append(this.DaisyChainLayer);
command.Append(this.BitField);
command.Append(position, ConstantParameterType.Value);
connection.Send(command);
if(reply){
var brickReply = connection.Receive();
Error.CheckForError(brickReply,214);
}
}
/// <summary>
/// Start with the specified speed
/// </summary>
/// <param name="speed">Speed.</param>
/// <param name="reply">If set to <c>true</c> reply from brick will be send.</param>
public void Start(sbyte speed, bool reply = false){
var command = new Command(0,0,215,reply);
command.Append(ByteCodes.OutputSpeed);
command.Append(this.DaisyChainLayer);
command.Append(this.BitField);
command.Append(speed, ParameterFormat.Long);
command.Append(ByteCodes.OutputStart);
command.Append(this.DaisyChainLayer);
command.Append(this.BitField);
connection.Send (command);
if(reply){
var brickReply = connection.Receive();
Error.CheckForError(brickReply,215);
}
}
/// <summary>
/// Stop the specified brake and reply.
/// </summary>
/// <param name="brake">If set to <c>true</c> the motor will brake and not coast</param>
/// <param name="reply">If set to <c>true</c> reply from brick will be send.</param>
public void Stop(bool brake, bool reply = false){
var command = new Command(0,0,202,reply);
command.Append(ByteCodes.OutputStop);
command.Append(this.DaisyChainLayer);
command.Append(this.BitField);
byte b = 0;
if(brake){
b = 1;
}
command.Append(b, ParameterFormat.Short);
connection.Send(command);
if(reply){
var brickReply = connection.Receive();
Error.CheckForError(brickReply,202);
}
}
/// <summary>
/// Sets the power.
/// </summary>
/// <param name="power">Power.</param>
/// <param name="reply">If set to <c>true</c> reply from brick will be send.</param>
public void SetPower(byte power, bool reply = false){
var command = new Command(0,0,204,reply);
command.Append(ByteCodes.OutputPower);
command.Append(this.DaisyChainLayer);
command.Append(this.BitField);
command.Append(power, ParameterFormat.Long);
connection.Send(command);
if(reply){
var brickReply = connection.Receive();
Error.CheckForError(brickReply,204);
}
}
/// <summary>
/// Clearing tacho count when used as sensor
/// </summary>
/// <param name="reply">If set to <c>true</c> reply from brick will be send.</param>
public void ClearCount(bool reply = false){
var command = new Command(0,0,210,reply);
command.Append(ByteCodes.OutputClrCount);
command.Append(this.DaisyChainLayer);
command.Append(this.BitField);
connection.Send(command);
if(reply){
var brickReply = connection.Receive();
Error.CheckForError(brickReply,210);
}
}
/// <summary>
/// Clear changes and bumps
/// </summary>
protected void ClearChanges ()
{
if(!isInitialized)
Initialize();
var command = new Command(0,0,201,false);
command.Append(ByteCodes.InputDevice);
command.Append(InputSubCodes.ClrChanges);
command.Append(this.DaisyChainLayer);
command.Append(this.Port);
Connection.Send(command);
}
/// <summary>
/// </summary>
protected Int32 ReadyRaw ()
{
if(!isInitialized)
Initialize();
var command = new Command(4,0,201,true);
command.Append(ByteCodes.InputDevice);
command.Append(InputSubCodes.ReadyRaw);
command.Append(this.DaisyChainLayer);
command.Append(this.Port);
command.Append(0, ConstantParameterType.Value);
command.Append(mode);
command.Append(1, ConstantParameterType.Value);
command.Append((byte)0, VariableScope.Global);
var reply = Connection.SendAndReceive(command);
Error.CheckForError(reply,201);
return reply.GetInt32(3);
}
/// <summary>
/// Time sync between two motors
/// </summary>
/// <param name="speed">Speed.</param>
/// <param name="turnRatio">Turn ratio between two syncronized motors</param>
/// <param name="timeInMs">Time in ms</param>
/// <param name="brake">If set to <c>true</c> brake.</param>
/// <param name="reply">If set to <c>true</c> reply.</param>
public void SetTimeSync(sbyte speed, Int16 turnRatio, UInt32 timeInMs, bool brake, bool reply = false){
var command = new Command(0,0,210,reply);
command.Append(ByteCodes.OutputTimeSync);
command.Append(this.DaisyChainLayer);
command.Append(this.BitField);
command.Append(speed, ConstantParameterType.Value);
command.Append(turnRatio, ConstantParameterType.Value);
command.Append(timeInMs, ConstantParameterType.Value);
byte b = 0;//coast
if(brake)
b = 1;
command.Append(b, ParameterFormat.Short);
connection.Send(command);
if(reply){
var brickReply = connection.Receive();
Error.CheckForError(brickReply,210);
}
}
/// <summary>
/// Sets the sensor mode
/// </summary>
/// <param name="mode">Mode to use.</param>
protected virtual void SetMode(SensorMode mode){
isInitialized = true;
this.mode = mode;
var command = new Command(4,0, 123, true);
command.Append(ByteCodes.InputReadSI);
command.Append(this.DaisyChainLayer);
command.Append(this.Port);
command.Append(0, ConstantParameterType.Value);
command.Append(mode);
command.Append((byte)0, VariableScope.Global);
var reply = Connection.SendAndReceive(command);
Error.CheckForError(reply,123);
}
/// <summary>
/// Write data to device (only UART devices)
/// </summary>
/// <param name="data">Data array to write.</param>
protected void Write (byte[] data)
{
if(!isInitialized)
Initialize();
var command = new Command(0,0, 100, false);
command.Append(ByteCodes.InputWrite);
command.Append(this.DaisyChainLayer);
command.Append(this.Port);
command.Append((byte)data.Length, ParameterFormat.Short);
foreach(byte b in data)
command.Append(b);
Connection.Send(command);
}
/// <summary>
/// Reads the si sensor value
/// </summary>
/// <returns>The si sensor value.</returns>
protected float ReadSi()
{
if(!isInitialized)
Initialize();
var command = new Command(4,0, 123, true);
command.Append(ByteCodes.InputReadSI);
command.Append(this.DaisyChainLayer);
command.Append(this.Port);
command.Append(0, ConstantParameterType.Value);
command.Append(mode);
command.Append((byte)0, VariableScope.Global);
var reply = Connection.SendAndReceive(command);
Error.CheckForError(reply,123);
return reply.GetFloat(3);
}
/// <summary>
/// Stop all devices (e.c. motors, ...)
/// </summary>
protected void StopAll ()
{
if(!isInitialized)
Initialize();
var command = new Command(0,0,201,false);
command.Append(ByteCodes.InputDevice);
command.Append(InputSubCodes.StopAll);
command.Append(this.DaisyChainLayer);
Connection.Send(command);
}
/// <summary>
/// Generic setup/read IIC sensors
/// </summary>
/// <returns>DATA8 array (handle) to read into</returns>
/// <param name="repeat">Repeat setup/read "REPEAT" times (0 = infinite)</param>
/// <param name="reapeatTime">Time between repeats [10..1000mS] (0 = 10)</param>
/// <param name="writeData">Byte array to write</param>
protected byte SetUp (byte repeat, Int16 reapeatTime, byte[] writeData)
{
if(!isInitialized)
Initialize();
var command = new Command(4,0,201,true);
command.Append(ByteCodes.InputDevice);
command.Append(InputSubCodes.Setup);
command.Append(this.DaisyChainLayer);
command.Append(this.Port);
command.Append(repeat, ConstantParameterType.Value);
command.Append(repeat, ConstantParameterType.Value);
command.Append(writeData.Length, ConstantParameterType.Value);
command.Append(1, ConstantParameterType.Value);
command.Append((byte)0, VariableScope.Global);
var reply = Connection.SendAndReceive(command);
Error.CheckForError(reply,201);
return reply.GetByte(3);
}
/// <summary>
/// Sets the polarity.
/// </summary>
/// <param name="polarity">Polarity of the output</param>
/// <param name="reply">If set to <c>true</c> reply from brick will be send.</param>
public void SetPolarity(Polarity polarity, bool reply = false){
var command = new Command(0,0,206,reply);
command.Append(ByteCodes.OutputPolarity);
command.Append(this.DaisyChainLayer);
command.Append(this.BitField);
command.Append((sbyte) polarity, ParameterFormat.Short);
connection.Send(command);
if(reply){
var brickReply = connection.Receive();
Error.CheckForError(brickReply,206);
}
}
public byte[] Read (byte number, int bytesToRead)
{
var command = new Command(bytesToRead, 0,102,true);
command.Append(ByteCodes.MailboxClose);
//command.Append(number)
command.Append(number, ParameterFormat.Short);
//command.Append(number)
command.Append(bytesToRead, ConstantParameterType.Value);
//command.Append(number)
command.Append(bytesToRead, ConstantParameterType.Value);
command.Append((byte)0, VariableScope.Global);
var brickReply = connection.SendAndReceive(command);
Error.CheckForError(brickReply,102);
brickReply.print();
return brickReply.GetData(3);
}
/// <summary>
/// Set Ramp up, constant and rampdown steps and speed of the outputs
/// </summary>
/// <param name="speed">Speed to use</param>
/// <param name="rampUpSteps">Steps used to ramp up</param>
/// <param name="constantSpeedSteps">Steps used for constant speed</param>
/// <param name="rampDownSteps">Steps used to ramp down</param>
/// <param name="brake">If set to <c>true</c> brake when done.</param>
/// <param name="reply">If set to <c>true</c> reply from brick will be send.</param>
public void SetStepSpeed(sbyte speed, UInt32 rampUpSteps, UInt32 constantSpeedSteps, UInt32 rampDownSteps, bool brake, bool reply = false){
var command = new Command(0,0,209,reply);
command.Append(ByteCodes.OutputStepSpeed);
command.Append(this.DaisyChainLayer);
command.Append(this.BitField);
command.Append(speed, ConstantParameterType.Value);
command.Append(rampUpSteps, ConstantParameterType.Value);
command.Append(constantSpeedSteps, ConstantParameterType.Value);
command.Append(rampDownSteps, ConstantParameterType.Value);
byte b = 0;//coast
if(brake)
b = 1;
command.Append(b, ParameterFormat.Short);
connection.Send(command);
if(reply){
var brickReply = connection.Receive();
Error.CheckForError(brickReply,209);
}
}
/// <summary>
/// Gets the speed of the motor
/// </summary>
/// <returns>The speed.</returns>
/// <param name="port">Motor port to read</param>
public sbyte GetSpeed(MotorPort port){
var command = new Command(8,0,220,true);
command.Append(ByteCodes.OutputRead);
command.Append(this.DaisyChainLayer);
command.Append(port);
command.Append((byte)0, VariableScope.Global);
command.Append((byte)4, VariableScope.Global);
var brickReply = connection.SendAndReceive(command);
Error.CheckForError(brickReply,220);
return brickReply.GetSbyte(3);
//The tacho speed from outputRead does not work
// I have also tried to place the tacho reply in offset 1 (and with 5 global bytes in the reply) but get a error each time
}
/// <summary>
/// Testing if output is used
/// </summary>
/// <returns><c>true</c> if this instance is ready; otherwise, <c>false</c>.</returns>
public bool IsReady(){
var command = new Command(1,0,212,true);
command.Append(ByteCodes.OutputTest);
command.Append(this.DaisyChainLayer);
command.Append(this.BitField);
var brickReply = connection.SendAndReceive(command);
Error.CheckForError(brickReply,212);
return ! Convert.ToBoolean(brickReply.GetByte(3));
}
/// <summary>
/// Gets the min and max values that can be returned.
/// </summary>
/// <param name="min">Minimum.</param>
/// <param name="max">Maxium.</param>
protected void GetMinMax(out float min, out float max){
if(!isInitialized)
Initialize();
var command = new Command(8,0,201,true);
command.Append(ByteCodes.InputDevice);
command.Append(InputSubCodes.GetMinMax);
command.Append(this.DaisyChainLayer);
command.Append(this.Port);
command.Append((byte)0, VariableScope.Global);
command.Append((byte)4, VariableScope.Global);
var reply = Connection.SendAndReceive(command);
Error.CheckForError(reply,201);
min = reply.GetFloat(3);
max = reply.GetFloat(7);
}
/// <summary>
/// Gets the tacho count.
/// </summary>
/// <returns>The tacho count.</returns>
/// <param name="port">Motor port to use</param>
public Int32 GetCount(MotorPort port){
var command = new Command(4,0,212,true);
command.Append(ByteCodes.OutputGetCount);
command.Append(this.DaisyChainLayer);
command.Append(port);
command.Append((byte)0, VariableScope.Global);
var brickReply = connection.SendAndReceive(command);
Error.CheckForError(brickReply,212);
return brickReply.GetInt32(3);
}
/// <summary>
/// Get the bolean count since the last clear
/// </summary>
/// <returns>The bumb count.</returns>
protected float GetBumbs ()
{
if(!isInitialized)
Initialize();
var command = new Command(4,0,201,true);
command.Append(ByteCodes.InputDevice);
command.Append(InputSubCodes.GetBumps);
command.Append(this.DaisyChainLayer);
command.Append(this.Port);
command.Append((byte)0, VariableScope.Global);
var reply = Connection.SendAndReceive(command);
Error.CheckForError(reply,201);
return reply.GetFloat(3);
}
/// <summary>
/// Get device mode name
/// </summary>
/// <returns>The device mode name.</returns>
/// <param name="mode">Mode to get name of.</param>
protected string GetModeName (SensorMode mode)
{
if(!isInitialized)
Initialize();
var command = new Command(Command.ShortValueMax,0,201,true);
command.Append(ByteCodes.InputDevice);
command.Append(InputSubCodes.GetModeName);
command.Append(this.DaisyChainLayer);
command.Append(this.Port);
command.Append((byte) mode, ConstantParameterType.Value);
command.Append(Command.ShortValueMax, ConstantParameterType.Value);
command.Append((byte)0, VariableScope.Global);
var reply = Connection.SendAndReceive(command);
Error.CheckForError(reply,201);
return reply.GetString(3,(byte) Command.ShortValueMax);
}
/// <summary>
/// Gets figure layout.
/// </summary>
/// <param name="figures">Figures.</param>
/// <param name="decimals">Decimals.</param>
protected void GetFigures(out byte figures, out byte decimals){
if(!isInitialized)
Initialize();
var command = new Command(2,0,201,true);
command.Append(ByteCodes.InputDevice);
command.Append(InputSubCodes.GetFigures);
command.Append(this.DaisyChainLayer);
command.Append(this.Port);
command.Append((byte)0, VariableScope.Global);
command.Append((byte)1, VariableScope.Global);
var reply = Connection.SendAndReceive(command);
Error.CheckForError(reply,201);
figures = reply.GetByte(3);
decimals = reply.GetByte(4);
}
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;
}
public void Close (byte number)
{
var command = new Command(0, 0,101,true);
command.Append(ByteCodes.MailboxClose);
//command.Append(number)
command.Append(number, ParameterFormat.Short);
var reply = connection.SendAndReceive(command);
Error.CheckForError(reply,100);
reply.print();
}