private static void OnSensorDetached(SensorPort port)
{
lock (sensorArrayLock)
{
sensorArray [(int)port] = null;
}
}
public AnalogSensor(SensorPort port)
{
this.port = port;
SensorManager.Instance.ResetI2C(this.port);
SensorManager.Instance.ResetUart(this.port);
analogMemory = SensorManager.Instance.AnalogMemory;
}
public static void FinalizePRogram()
{
BIOS_FINALE_BEEP();
Writeln("Finalization called!");
if (IsRobot)
{
for (MotorPort i = MotorPort.OutA; i <= MotorPort.OutD; i++)
{
// Turn off all motors
Motor FinaleMotor = new Motor(i);
FinaleMotor.Brake();
FinaleMotor.ResetTacho();
FinaleMotor.Off();
}
for (SensorPort i = SensorPort.In1; i <= SensorPort.In4; i++)
{
// Cannot finalize all sensors
}
}
else
{
// Windows
}
Writeln("Finalization finished.");
LogFileWriter.Close();
LogFileWriter = null;
}
public ISensorModel this[SensorPort port]
{
get
{
return(sensors[(int)port]);
}
}
public void GetSensorState(SensorPort port, out SensorType type, out SensorMode mode, out int raw, out int normalized, out int scaled)
{
mre.Reset();
GetInputState(port);
if (!mre.WaitOne(1000, false))
{
throw new NxtTimeout();
}
if (nError > 0)
{
throw new InvalidResult();
}
if (result[4] != 1)
{
throw new InvalidResult();
}
type = (SensorType)result[6];
mode = (SensorMode)result[7];
raw = Buffer2Word(result, 8);
normalized = Buffer2Word(result, 10);
scaled = Buffer2Word(result, 12);
}
public ISensorModel this[SensorPort port]
{
get
{
return sensors[(int) port];
}
}
public AnalogSensor (SensorPort port)
{
this.port = port;
SensorManager.Instance.ResetI2C(this.port);
SensorManager.Instance.ResetUart(this.port);
analogMemory = SensorManager.Instance.AnalogMemory;
}
/// <summary>
/// Gets the specified sensor type from the specified port.
/// </summary>
/// <typeparam name="T">The type of sensor to retrieve</typeparam>
/// <param name="port">The port to retrieve the sensor from</param>
/// <returns></returns>
public T GetSensor <T>(SensorPort port) where T : Sensor
{
T sensor = null;
try
{
switch (port)
{
case SensorPort.In1:
sensor = (T)Sensor1;
break;
case SensorPort.In2:
sensor = (T)Sensor2;
break;
case SensorPort.In3:
sensor = (T)Sensor3;
break;
case SensorPort.In4:
sensor = (T)Sensor4;
break;
}
}
catch (InvalidCastException e)
{
//todo: tell person to initialize the sensor.
throw new InvalidCastException("Could not return the specified sensor as the specified type. This usually happens because the sensor was not initialized.", e);
}
return(sensor);
}
public NXTColorSensor(SensorPort sensorPort, SensorType sensorType) : base(sensorPort)
{
if (sensorType < SensorType.COLOR_FULL && sensorType > SensorType.COLOR_NONE)
{
throw new ArgumentOutOfRangeException();
}
SensorType = sensorType;
}
internal async Task Attach(NxtBrick brick, SensorPort port)
{
await Task.Run(() =>
{
this.SensorPort = port;
this.brick = brick;
});
}
public I2CSensor(SensorPort port, byte address, I2CMode mode)
{
this.port = port;
this.I2CAddress = address;
I2CDevice = SensorManager.Instance.I2CDevice;
this.mode = mode;
SensorManager.Instance.SetAnalogMode((AnalogMode)mode, port);
}
public I2CSensor(SensorPort port, byte address, I2CMode mode)
{
this.port = port;
this.I2CAddress = address;
I2CDevice = SensorManager.Instance.I2CDevice;
this.mode = mode;
SensorManager.Instance.SetAnalogMode((AnalogMode)mode, port);
}
public EV3TouchSensor(SensorPort sensorPort, SensorType sensorType) : base(sensorPort)
{
if (sensorType != SensorType.EV3_TOUCH_0 && sensorType != SensorType.EV3_TOUCH_DEBOUNCE)
{
throw new ArgumentOutOfRangeException();
}
SensorType = sensorType;
}
public async Task Remove(SensorPort port, bool deferInitialization = false)
{
this[port] = new RawSensor(port);
if (!deferInitialization)
{
await brick.InitializeSensors();
}
}
/// <summary>
/// Initialize an EV3 Touch Sensor
/// </summary>
/// <param name="port">Sensor port</param>
/// <param name="timeout">Period in millisecond to check sensor value changes</param>
public EV3TouchSensor(Brick brick, SensorPort port, int timeout)
{
_brick = brick;
Port = port;
brick.SetSensorType((byte)Port, SensorType.EV3Touch);
_periodRefresh = timeout;
_timer = new Timer(UpdateSensor, this, TimeSpan.FromMilliseconds(timeout), TimeSpan.FromMilliseconds(timeout));
}
AttachSensorAsync(SensorPort port, SensorBase sensor)
{
return AttachSensorAsyncInternal(port, sensor)
#if WINRT
.AsAsyncAction()
#endif
;
}
/// <summary>
/// Initialize a NXT Sound Sensor
/// </summary>
/// <param name="brick"></param>
/// <param name="port">Sensor port</param>
/// <param name="timeout">Period in millisecond to check sensor value changes</param>
public NXTSoundSensor(Brick brick, SensorPort port, int timeout)
{
_brick = brick;
Port = port;
brick.SetSensorType((byte)Port, SensorType.Custom, new int[] { (int)SensorCustom.Pin1_9V });
_periodRefresh = timeout;
_timer = new Timer(UpdateSensor, this, TimeSpan.FromMilliseconds(timeout), TimeSpan.FromMilliseconds(timeout));
}
/// <summary>
/// Initilaize an EV3 Color Sensor
/// </summary>
/// <param name="brick"></param>
/// <param name="port">Sensor port</param>
/// <param name="mode">Color mode</param>
/// <param name="timeout">Period in millisecond to check sensor value changes</param>
public EV3ColorSensor(Brick brick, SensorPort port, ColorSensorMode mode, int timeout)
{
_brick = brick;
Port = port;
_colorMode = mode;
brick.SetSensorType((byte)Port, GetEV3Mode(mode));
_periodRefresh = timeout;
_timer = new Timer(UpdateSensor, this, TimeSpan.FromMilliseconds(timeout), TimeSpan.FromMilliseconds(timeout));
}
/// <summary>
/// Initialize an EV3 Gyro Sensor
/// </summary>
/// <param name="brick">Interface to main Brick component</param>
/// <param name="port">Sensor port</param>
/// <param name="mode">Gyro mode</param>
/// <param name="timeout">Period in millisecond to check sensor value changes</param>
public EV3GyroSensor(Brick brick, SensorPort port, GyroMode mode, int timeout)
{
_brick = brick;
Port = port;
_gmode = mode;
brick.SetSensorType((byte)Port, (SensorType)mode);
_periodRefresh = timeout;
_timer = new Timer(UpdateSensor, this, TimeSpan.FromMilliseconds(timeout), TimeSpan.FromMilliseconds(timeout));
}
public I2CHelper(SensorPort port) : base(port, 0x02, I2CMode.LowSpeed9V)
{
base.Initialise();
sensorDictionary = new Dictionary <byte[], ISensor>();
sensorDictionary.Add(SonarName, new NXTUltraSonicSensor(port));
sensorDictionary.Add(HTColorName, new HiTecColorSensor(port));
sensorDictionary.Add(HTCompassName, new HiTecCompassSensor(port));
sensorDictionary.Add(HTAccelName, new HiTecTiltSensor(port));
}
private byte[] SetupCommand(SensorPort sensorPort, ConnectionType conn, SensorType type, byte mode)
{
lock (setupLock) {
sensorData [(int)sensorPort] = (byte)conn;
sensorData [(int)sensorPort + NumberOfSensorPorts] = (byte)type;
sensorData [(int)sensorPort + 2 * NumberOfSensorPorts] = mode;
return(sensorData);
}
}
public void LowSpeedRead(SensorPort nPort)
{
byte[] data = new byte[3];
data[0] = 0x00;
data[1] = 0x10;
data[2] = (byte)nPort;
Tx(data, 20);
}
public MSSensorMUXBase (SensorPort Port, MSSensorMUXPort Address, MSSensorMUXMode SensorMode) : base (Port, (byte)Address, I2CMode.LowSpeed)
{
base.Initialise ();
Mode = (byte)SensorMode;
if (SensorMode != MSSensorMUXMode.TouchMode)
NumberOfM = 3;
else
NumberOfM = 1;
SetSensorMode ();
}
public void SetAnalogMode(AnalogMode mode, SensorPort port)
{
byte [] modes = new byte[SensorManager.NumberOfSensorPorts];
for (int i = 0; i < modes.Length; i++)
{
modes[i] = (byte)AnalogMode.None;
}
modes[(int)port] = (byte)mode;
DeviceManager.Write(modes);
}
/// <summary>
/// Initialize an EV3 IR Sensor
/// </summary>
/// <param name="brick">Interface to main Brick component</param>
/// <param name="port">Sensor port</param>
/// <param name="mode">IR mode</param>
/// <param name="timeout">Period in millisecond to check sensor value changes</param>
public EV3InfraredSensor(Brick brick, SensorPort port, IRMode mode, int timeout)
{
_brick = brick;
Mode = mode;
Port = port;
Channel = IRChannel.One;
brick.SetSensorType((byte)Port, (SensorType)mode);
_periodRefresh = timeout;
_timer = new Timer(UpdateSensor, this, TimeSpan.FromMilliseconds(timeout), TimeSpan.FromMilliseconds(timeout));
}
/// <summary>
/// Initialize a NXT Light Sensor
/// </summary>
/// <param name="brick"></param>
/// <param name="port">Sensor port</param>
/// <param name="mode">Light mode</param>
/// <param name="timeout">Period in millisecond to check sensor value changes</param>
public NXTLightSensor(Brick brick, SensorPort port, LightMode mode, int timeout)
{
_brick = brick;
Port = port;
_lightMode = mode;
CutOff = 512;
brick.SetSensorType((byte)Port, (SensorType)mode);
_periodRefresh = timeout;
_timer = new Timer(UpdateSensor, this, TimeSpan.FromMilliseconds(timeout), TimeSpan.FromMilliseconds(timeout));
}
public static ISensor[] GetSensorArray()
{
SensorPort[] ports = new SensorPort[]{ SensorPort.In1, SensorPort.In2, SensorPort.In3, SensorPort.In4 };
ISensor[] sensors = new ISensor[ports.Length];
for(int i = 0; i < ports.Length; i++)
{
sensors[i] = GetSensor(ports[i]);
}
return sensors;
}
public static ISensor[] GetSensorArray()
{
SensorPort[] ports = new SensorPort[] { SensorPort.In1, SensorPort.In2, SensorPort.In3, SensorPort.In4 };
ISensor[] sensors = new ISensor[ports.Length];
for (int i = 0; i < ports.Length; i++)
{
sensors[i] = GetSensor(ports[i]);
}
return(sensors);
}
public void GetInputState(SensorPort nPort)
{
byte[] data = new byte[3];
data[0] = 0x00;
data[1] = 0x07;
data[2] = (byte)nPort;
Tx(data, 16);
}
public void ResetInput(SensorPort nPort)
{
byte[] data = new byte[3];
data[0] = 0x80;
data[1] = 0x08;
data[2] = (byte)nPort;
Tx(data);
}
public void GetLowSpeedStatus(SensorPort nPort)
{
byte[] data = new byte[3];
data[0] = 0x00;
data[1] = 0x0e;
data[2] = (byte)nPort;
Tx(data, 4);
}
public MSSensorMUXBase (SensorPort Port, MSSensorMUXPort Address, GyroMode SensorMode) : base (Port, (byte)Address, I2CMode.LowSpeed)
{
base.Initialise ();
if (SensorMode == GyroMode.Angle)
Mode = 0;
else if (SensorMode == GyroMode.AngularVelocity)
Mode = 1;
NumberOfM = 1;
SetSensorMode ();
}
public UARTHelper(SensorPort port) : base(port)
{
base.Initialise(base.uartMode);
base.SetMode(UARTMode.Mode0);
sensorDictionary = new Dictionary <byte[], ISensor>();
sensorDictionary.Add(IRName, new EV3IRSensor(port));
sensorDictionary.Add(GyroName, new EV3GyroSensor(port));
sensorDictionary.Add(ColorName, new EV3ColorSensor(port));
sensorDictionary.Add(UltrasonicName, new EV3UltrasonicSensor(port));
System.Threading.Thread.Sleep(100);
}
public void SetInputMode(SensorPort nPort, SensorType nType, SensorMode nMode)
{
byte[] data = new byte[5];
data[0] = 0x80;
data[1] = 0x05;
data[2] = (byte)nPort;
data[3] = (byte)nType;
data[4] = (byte)nMode;
Tx(data);
}
/// <summary>
/// Initialize an EV3 Ultrasonic Sensor
/// </summary>
/// <param name="port">Sensor port</param>
/// <param name="usmode">Ultrasonic mode</param>
/// <param name="timeout">Period in millisecond to check sensor value changes</param>
public EV3UltraSonicSensor(Brick brick, SensorPort port, UltraSonicMode usmode, int timeout)
{
_brick = brick;
Port = port;
if (UltraSonicMode.Listen == _mode)
{
_mode = UltraSonicMode.Centimeter;
}
_mode = usmode;
brick.SetSensorType((byte)Port, (SensorType)usmode);
_periodRefresh = timeout;
_timer = new Timer(UpdateSensor, this, TimeSpan.FromMilliseconds(timeout), TimeSpan.FromMilliseconds(timeout));
}
public I2CHelper(SensorPort port) : base(port, 0x02, I2CMode.LowSpeed9V)
{
base.Initialise();
sensorDictionary = new Dictionary <Tuple <string, string>, ISensor>();
sensorDictionary.Add(new Tuple <string, string>("LEGO", "Sonar"), new NXTUltraSonicSensor(port));
sensorDictionary.Add(new Tuple <string, string>("HiTechnc", "Color"), new HiTecColorSensor(port));
sensorDictionary.Add(new Tuple <string, string>("HiTechnc", "Compass"), new HiTecCompassSensor(port));
sensorDictionary.Add(new Tuple <string, string>("HITECHNC", "Accel"), new HiTecTiltSensor(port));
sensorDictionary.Add(new Tuple <string, string>("mndsnsrs", "AngSens"), new MSAngleSensor(port));
sensorDictionary.Add(new Tuple <string, string>("mndsnsrs", "DIST-S"), new MSDistanceSensor(port));
sensorDictionary.Add(new Tuple <string, string>("mndsnsrs", "DIST-M"), new MSDistanceSensor(port));
sensorDictionary.Add(new Tuple <string, string>("mndsnsrs", "DIST-L"), new MSDistanceSensor(port));
}
public MSSensorMUXBase (SensorPort Port, MSSensorMUXPort Address, ColorMode SensorMode) : base (Port, (byte)Address, I2CMode.LowSpeed)
{
base.Initialise ();
if (SensorMode == ColorMode.Reflection)
Mode = 0;
else if (SensorMode == ColorMode.Ambient)
Mode = 1;
else if (SensorMode == ColorMode.Color)
Mode = 2;
NumberOfM = 2;
SetSensorMode ();
}
public MSSensorMUXBase (SensorPort Port, MSSensorMUXPort Address, UltraSonicMode SensorMode) : base (Port, (byte)Address, I2CMode.LowSpeed)
{
base.Initialise ();
if (SensorMode == UltraSonicMode.Centimeter)
Mode = 0;
else if (SensorMode == UltraSonicMode.Inch)
Mode = 1;
else if (SensorMode == UltraSonicMode.Listen)
Mode = 2;
SetSensorMode ();
NumberOfM = 2;
}
public override TimeSpan?InitializeSensorRequest(ProtocolArray requestData)
{
TimeSpan?result;
result = base.InitializeSensorRequest(requestData);
if (SensorType == SensorType.ULTRASONIC_CONT)
{
//override sensortype to fake an I2C
requestData[Const.SensorIndex + (int)SensorPort.ArduinoPort()] = (byte)SensorType.I2C;
}
return(result);
}
public void LowSpeedWrite(SensorPort nPort, byte b1, byte b2, byte nRx)
{
byte[] data = new byte[7];
data[0] = 0x80;
data[1] = 0x0f;
data[2] = (byte)nPort;
data[3] = 2;
data[4] = nRx;
data[5] = b1;
data[6] = b2;
Tx(data);
}
/// <summary>
/// Initializes a new instance of the <see cref="MonoBrick.EV3.TouchSensor"/> class in boolean mode
/// </summary>
public NXTTouchSensor(SensorPort port)
: base(port)
{
}
/// <summary>
/// [Native] Get a sensor's current state.
/// </summary>
/// <param name="port">The port of the attached sensor.</param>
/// <returns>Returns a SensorInput package.</returns>
public SensorInput GetSensorValues(SensorPort port)
{
byte[] request = new byte[] {
0x00,
(byte) DirectCommand.GetInputValues,
(byte) port
};
byte[] reply = CompleteRequest(request);
if (reply[3] != (byte)port)
{
throw new NXTReplyIncorrect();
}
SensorInput result = new SensorInput();
result.valid = (reply[4] != 0x00);
result.calibrated = (reply[5] != 0x00);
result.type = (SensorType)reply[6];
result.mode = (SensorMode)reply[7];
result.value_raw = GetUInt16(reply, 8);
result.value_normalized = GetUInt16(reply, 10);
result.value_scaled = GetInt16(reply, 12);
result.value_calibrated = GetInt16(reply, 14);
return result;
}
/// <summary>
/// [Native] Set a sensor's state. This is REQUIRED
/// to read realistic values through GetSensorValues().
/// </summary>
/// <param name="port">The port of the attached sensor.</param>
/// <param name="type">The type of the attached sensor.</param>
/// <param name="mode">The mode of the attached sensor.</param>
public void SetSensorMode(SensorPort port,
SensorType type, SensorMode mode)
{
byte[] request = new byte[] {
(byte) (0x00),
(byte) (DirectCommand.SetInputMode),
(byte) port,
(byte) type,
(byte) mode
};
CompleteRequest(request);
return;
}
public void ResetInput(SensorPort nPort)
{
byte[] data = new byte[3];
data[0] = 0x80;
data[1] = 0x08;
data[2] = (byte)nPort;
Tx(data);
}
public void SetInputMode(SensorPort nPort, SensorType nType, SensorMode nMode)
{
byte[] data = new byte[5];
data[0] = 0x80;
data[1] = 0x05;
data[2] = (byte)nPort;
data[3] = (byte)nType;
data[4] = (byte)nMode;
Tx(data);
}
/// <summary>
/// Append a sensor port
/// </summary>
/// <param name="port">Sensor port to append</param>
public void Append (SensorPort port)
{
Append((sbyte) port, ParameterFormat.Short);
}
/// <summary>
/// [Native] Reads the number of bytes readable in a lowpeed sensor port.
/// </summary>
/// <param name="port">The port of the attached sensor.</param>
/// <returns>The data from the lowspeed sensor, as a byte array.</returns>
public byte[] LowspeedRead(SensorPort port)
{
byte[] request = new byte[] {
0x00,
(byte) DirectCommand.LSRead,
(byte) port
};
byte[] reply = CompleteRequest(request);
byte bytesRead = reply[3];
byte[] rxData = new byte[bytesRead];
Array.Copy(reply, 4, rxData, 0, bytesRead);
return rxData;
}
public void GetSensorState(SensorPort port, out SensorType type, out SensorMode mode, out int raw, out int normalized, out int scaled)
{
mre.Reset();
GetInputState(port);
if (!mre.WaitOne(1000, false))
throw new NxtTimeout();
if (nError > 0)
throw new InvalidResult();
if (result[4] != 1)
throw new InvalidResult();
type = (SensorType)result[6];
mode = (SensorMode)result[7];
raw = Buffer2Word(result, 8);
normalized = Buffer2Word(result, 10);
scaled = Buffer2Word(result, 12);
}
public void LowSpeedRead(SensorPort nPort)
{
byte[] data = new byte[3];
data[0] = 0x00;
data[1] = 0x10;
data[2] = (byte)nPort;
Tx(data, 20);
}
public void GetLowSpeedStatus(SensorPort nPort)
{
byte[] data = new byte[3];
data[0] = 0x00;
data[1] = 0x0e;
data[2] = (byte)nPort;
Tx(data, 4);
}
public int GetSensorRaw(SensorPort port)
{
mre.Reset();
GetInputState(port);
if (!mre.WaitOne(1000, false))
throw new NxtTimeout();
if (nError > 0)
throw new InvalidResult();
if (result[4] != 1)
throw new InvalidResult();
return Buffer2Word(result, 8);
}
public void GetInputState(SensorPort nPort)
{
byte[] data = new byte[3];
data[0] = 0x00;
data[1] = 0x07;
data[2] = (byte)nPort;
Tx(data, 16);
}
/// <summary>
/// Initializes a new instance of the <see cref="MonoBrick.NXT.HiTecCompass"/> class.
/// </summary>
public HiTecTiltSensor(SensorPort port)
: base(port, CompassAddress, I2CMode.LowSpeed9V)
{
base.Initialise();
}
/// <summary>
/// [Native] Reads the number of bytes readable in a lowpeed sensor port.
/// </summary>
/// <param name="port">The port of the attached sensor.</param>
/// <returns>Count of available bytes to read.</returns>
public byte LowspeedGetStatus(SensorPort port)
{
byte[] request = new byte[] {
(byte) (0x00),
(byte) (DirectCommand.LSGetStatus),
(byte) port
};
byte[] reply = CompleteRequest(request);
return reply[3];
}
/// <summary>
/// [Native] Reset's the selected sensor's scaled value.
/// </summary>
/// <param name="port">The port of the attached sensor.</param>
public void ResetSensorScale(SensorPort port)
{
byte[] request = new byte[] {
(byte) (0x00),
(byte) (DirectCommand.ResetInputScaledValue),
(byte) port
};
CompleteRequest(request);
return;
}
public void LowSpeedWrite(SensorPort nPort, byte b1, byte b2, byte b3, byte nRx)
{
byte[] data = new byte[8];
data[0] = 0x80;
data[1] = 0x0f;
data[2] = (byte)nPort;
data[3] = 3;
data[4] = nRx;
data[5] = b1;
data[6] = b2;
data[7] = b3;
Tx(data);
}
public MSDistanceSensor (SensorPort port) : base(port, (byte)0x02, I2CMode.LowSpeed9V)
{
base.Initialise();
}
public byte ReadUltrasonicSensor(SensorPort nPort)
{
LowSpeedWrite(nPort, 0x02, 0x42, 1);
Thread.Sleep(100);
mre.Reset();
LowSpeedRead(nPort);
if (!mre.WaitOne(1000, false))
throw new NxtTimeout();
if (nError > 0)
throw new InvalidResult();
if (result[3] != 1)
throw new InvalidResult();
return result[4];
}
private void HandleSensorDetached (SensorPort port)
{
Sensors[port].DetachSensor ();
}
/// <summary>
/// [Native] Writes an array of bytes to a lowspeed sensor port.
/// </summary>
/// <param name="port">The port of the attached sensor.</param>
/// <param name="txData">The tx Data to be written.</param>
/// <param name="rxDataLength">The rx Data Length. </param>
public void LowspeedWrite(SensorPort port, byte[] txData, byte rxDataLength)
{
byte txDataLength = (byte)txData.Length;
if (txDataLength == 0)
throw new NXTException("No data to send.");
if (txDataLength > 16)
throw new NXTException("Tx data may not exceed 16 bytes.");
if (rxDataLength < 0 || 16 < rxDataLength)
throw new NXTException("Rx data length should be in the interval 0-16.");
byte[] request = new byte[5 + txDataLength];
request[0] = (byte)(0x00);
request[1] = (byte)DirectCommand.LSWrite;
request[2] = (byte)port;
request[3] = txDataLength;
request[4] = rxDataLength;
txData.CopyTo(request, 5);
CompleteRequest(request);
return;
}
