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