public virtual IEnumerator <ITask> ConnectToBrickHandler(ConnectToBrick update)
{
// Validate the sensor port.
if ((update.Body.SensorPort & NxtSensorPort.AnySensorPort)
!= update.Body.SensorPort)
{
update.ResponsePort.Post(
Fault.FromException(
new ArgumentException(
string.Format("Invalid Sensor Port: {0}",
((LegoNxtPort)update.Body.SensorPort)))));
yield break;
}
_state.SensorPort = update.Body.SensorPort;
_state.Connected = false;
_state.PollingFrequencyMs = update.Body.PollingFrequencyMs;
if (!string.IsNullOrEmpty(update.Body.Name))
{
_state.Name = update.Body.Name;
}
Fault fault = null;
brick.AttachRequest attachRequest = new brick.AttachRequest(
new brick.Registration(
new LegoNxtConnection((LegoNxtPort)_state.SensorPort),
LegoDeviceType.AnalogSensor,
Contract.DeviceModel,
Contract.Identifier,
ServiceInfo.Service,
_state.Name));
attachRequest.InitializationCommands = new nxtcmd.NxtCommandSequence(
new nxtcmd.LegoSetInputMode(_state.SensorPort, LegoSensorType.Switch, LegoSensorMode.BooleanMode));
attachRequest.PollingCommands = new nxtcmd.NxtCommandSequence(_state.PollingFrequencyMs,
new nxtcmd.LegoGetInputValues(_state.SensorPort));
brick.AttachResponse response = null;
yield return(Arbiter.Choice(_legoBrickPort.AttachAndSubscribe(attachRequest, _legoBrickNotificationPort),
delegate(brick.AttachResponse rsp) { response = rsp; },
delegate(Fault f) { fault = f; }));
_state.Connected = (response != null && (response.Connection.Port != LegoNxtPort.NotConnected));
if (response == null ||
(!_state.Connected && update.Body.SensorPort != NxtSensorPort.NotConnected))
{
if (fault == null)
{
fault = Fault.FromException(new Exception("Failure Configuring NXT Touch Sensor on port: " + update.Body.SensorPort.ToString()));
}
update.ResponsePort.Post(fault);
yield break;
}
if (_state.Connected)
{
_state.SensorPort = (NxtSensorPort)response.Connection.Port;
// Set the sensor name
if (string.IsNullOrEmpty(_state.Name) ||
_state.Name.StartsWith("Touch Sensor on "))
{
_state.Name = "Touch Sensor on " + response.Connection.ToString();
}
// Send a connection notification
update.Body.Name = _state.Name;
update.Body.PollingFrequencyMs = _state.PollingFrequencyMs;
update.Body.SensorPort = _state.SensorPort;
SendNotification <ConnectToBrick>(_subMgrPort, update);
}
// Send the message response
update.ResponsePort.Post(DefaultUpdateResponseType.Instance);
yield break;
}
public virtual IEnumerator <ITask> ConnectToBrickHandler(ConnectToBrick update)
{
// Validate the sensor port.
if ((update.Body.SensorPort & NxtSensorPort.AnySensorPort)
!= update.Body.SensorPort)
{
update.ResponsePort.Post(
Fault.FromException(
new ArgumentException(
string.Format("Invalid Sensor Port: {0}",
((LegoNxtPort)update.Body.SensorPort)))));
yield break;
}
_state.SensorPort = update.Body.SensorPort;
_state.Connected = false;
if (!string.IsNullOrEmpty(update.Body.Name))
{
_state.Name = update.Body.Name;
}
_state.PollingFrequencyMs = update.Body.PollingFrequencyMs;
Fault fault = null;
brick.Registration registration = new brick.Registration(
new LegoNxtConnection((LegoNxtPort)_state.SensorPort),
LegoDeviceType.DigitalSensor,
Contract.DeviceModel,
Contract.Identifier,
ServiceInfo.Service,
_state.Name);
// Reserve the port
yield return(Arbiter.Choice(_legoBrickPort.ReserveDevicePort(registration),
delegate(brick.AttachResponse reserveResponse)
{
if (reserveResponse.DeviceModel == registration.DeviceModel)
{
registration.Connection = reserveResponse.Connection;
}
},
delegate(Fault f)
{
fault = f;
LogError(fault);
registration.Connection.Port = LegoNxtPort.NotConnected;
}));
if (registration.Connection.Port == LegoNxtPort.NotConnected && update.Body.SensorPort != NxtSensorPort.NotConnected)
{
if (fault == null)
{
fault = Fault.FromException(new Exception("Failure Configuring NXT Ultrasonic Sensor on Port " + update.Body.ToString()));
}
update.ResponsePort.Post(fault);
yield break;
}
_state.Connected = true;
brick.AttachRequest attachRequest = new brick.AttachRequest(registration);
brick.AttachResponse response = null;
byte[] requestSingleShotReading = { 0x02, 0x41, 0x01 };
byte[] startContinuousReadings = { 0x02, 0x41, 0x02 };
byte[] setContinuousReadingInterval = { 0x02, 0x40, 0x010 };
attachRequest.InitializationCommands = new nxtcmd.NxtCommandSequence(
new nxtcmd.LegoLSGetStatus((NxtSensorPort)registration.Connection.Port),
new nxtcmd.LegoLSRead((NxtSensorPort)registration.Connection.Port),
new nxtcmd.LegoSetInputMode((NxtSensorPort)registration.Connection.Port, LegoSensorType.I2C_9V, LegoSensorMode.RawMode),
new nxtcmd.LegoLSWrite((NxtSensorPort)registration.Connection.Port, startContinuousReadings, 0));
attachRequest.PollingCommands = new nxtcmd.NxtCommandSequence(_state.PollingFrequencyMs,
new nxtcmd.I2CReadSonarSensor(_state.SensorPort, nxtcmd.UltraSonicPacket.ReadMeasurement1));
yield return(Arbiter.Choice(_legoBrickPort.AttachAndSubscribe(attachRequest, _legoBrickNotificationPort),
delegate(brick.AttachResponse rsp) { response = rsp; },
delegate(Fault f) { fault = f; }));
if (response == null)
{
if (fault == null)
{
fault = Fault.FromException(new Exception("Failure Configuring NXT Ultrasonic Sensor"));
}
update.ResponsePort.Post(fault);
yield break;
}
if ((LegoNxtPort)_state.SensorPort != response.Connection.Port)
{
_state.SensorPort = (NxtSensorPort)response.Connection.Port;
}
// Set the motor name
if (string.IsNullOrEmpty(_state.Name) ||
_state.Name.StartsWith("Ultrasonic Sensor on "))
{
_state.Name = "Ultrasonic Sensor on " + response.Connection.ToString();
}
// Send a notification of the connected port
update.Body.Name = _state.Name;
update.Body.PollingFrequencyMs = _state.PollingFrequencyMs;
update.Body.SensorPort = _state.SensorPort;
SendNotification <ConnectToBrick>(_subMgrPort, update);
// Send the message response
update.ResponsePort.Post(DefaultUpdateResponseType.Instance);
yield break;
}
public virtual IEnumerator <ITask> ConnectToBrickHandler(ConnectToBrick update)
{
// Validate the sensor port.
if ((update.Body.SensorPort & NxtSensorPort.AnySensorPort)
!= update.Body.SensorPort)
{
update.ResponsePort.Post(
Fault.FromException(
new ArgumentException(
string.Format("Invalid Sensor Port: {0}",
((LegoNxtPort)update.Body.SensorPort)))));
yield break;
}
_state.SensorPort = update.Body.SensorPort;
_state.Connected = false;
if (!string.IsNullOrEmpty(update.Body.Name))
{
_state.Name = update.Body.Name;
}
_state.PollingFrequencyMs = update.Body.PollingFrequencyMs;
_state.SpotlightOn = update.Body.SpotlightOn;
// Set the hardware identifier from the connected motor port.
_genericState.HardwareIdentifier = NxtCommon.HardwareIdentifier(_state.SensorPort);
Fault fault = null;
brick.AttachRequest attachRequest = new brick.AttachRequest(
new brick.Registration(
new LegoNxtConnection((LegoNxtPort)_state.SensorPort),
LegoDeviceType.AnalogSensor,
Contract.DeviceModel,
Contract.Identifier,
ServiceInfo.Service,
_state.Name));
attachRequest.InitializationCommands = new NxtCommandSequence(
new LegoSetInputMode(_state.SensorPort, _state.SpotlightOn ? LegoSensorType.LightActive : LegoSensorType.LightInactive, LegoSensorMode.PercentFullScaleMode));
attachRequest.PollingCommands = new NxtCommandSequence(_state.PollingFrequencyMs,
new LegoGetInputValues(_state.SensorPort));
brick.AttachResponse response = null;
yield return(Arbiter.Choice(_legoBrickPort.AttachAndSubscribe(attachRequest, _legoBrickNotificationPort),
delegate(brick.AttachResponse rsp) { response = rsp; },
delegate(Fault f) { fault = f; }));
if (response == null)
{
if (fault == null)
{
fault = Fault.FromException(new Exception("Failure Configuring NXT Motor"));
}
update.ResponsePort.Post(fault);
yield break;
}
_state.Connected = (response.Connection.Port != LegoNxtPort.NotConnected);
if (_state.Connected)
{
_state.SensorPort = (NxtSensorPort)response.Connection.Port;
}
else if (update.Body.SensorPort != NxtSensorPort.NotConnected)
{
fault = Fault.FromException(new Exception("Failure Configuring NXT Light Sensor on Port " + update.Body.ToString()));
update.ResponsePort.Post(fault);
yield break;
}
// Set the motor name
if (string.IsNullOrEmpty(_state.Name) ||
_state.Name.StartsWith("Light Sensor on "))
{
_state.Name = "Light Sensor on " + response.Connection.ToString();
}
// Send a notification of the connected port
update.Body.Name = _state.Name;
update.Body.PollingFrequencyMs = _state.PollingFrequencyMs;
update.Body.SensorPort = _state.SensorPort;
update.Body.SpotlightOn = _state.SpotlightOn;
// Only send connection notifications to native subscribers
SendNotification <ConnectToBrick>(_subMgrPort, update);
update.ResponsePort.Post(DefaultUpdateResponseType.Instance);
yield break;
}
public virtual IEnumerator <ITask> ConnectToBrickHandler(ConnectToBrick update)
{
// Validate the sensor port.
if ((update.Body.SensorPort & NxtSensorPort.AnySensorPort)
!= update.Body.SensorPort)
{
update.ResponsePort.Post(
Fault.FromException(
new ArgumentException(
string.Format("Invalid Sensor Port: {0}",
((LegoNxtPort)update.Body.SensorPort)))));
yield break;
}
_state.SensorPort = update.Body.SensorPort;
_state.Connected = false;
if (!string.IsNullOrEmpty(update.Body.Name))
{
_state.Name = update.Body.Name;
}
_state.PollingFrequencyMs = update.Body.PollingFrequencyMs;
_state.SensorMode = update.Body.SensorMode;
// Set the hardware identifier from the connected sensor port.
_genericState.HardwareIdentifier = NxtCommon.HardwareIdentifier(_state.SensorPort);
Fault fault = null;
brick.AttachRequest attachRequest = new brick.AttachRequest(
new brick.Registration(
new LegoNxtConnection((LegoNxtPort)_state.SensorPort),
LegoDeviceType.AnalogSensor,
Contract.DeviceModel,
Contract.Identifier,
ServiceInfo.Service,
_state.Name));
// Get the correct code for the Sensor Type that the Brick understands
LegoSensorType st;
switch (_state.SensorMode)
{
case ColorSensorMode.Color:
st = LegoSensorType.ColorFull;
break;
case ColorSensorMode.Red:
st = LegoSensorType.ColorRed;
break;
case ColorSensorMode.Green:
st = LegoSensorType.ColorGreen;
break;
case ColorSensorMode.Blue:
st = LegoSensorType.ColorBlue;
break;
case ColorSensorMode.None:
st = LegoSensorType.ColorNone;
break;
default:
st = LegoSensorType.ColorFull;
break;
}
// The Color Sensor is a special case of an Analog sensor so if uses the
// LegoSetInputMode request. Note that it is in Raw mode.
attachRequest.InitializationCommands = new NxtCommandSequence(
new LegoSetInputMode(_state.SensorPort, st, LegoSensorMode.RawMode));
// Polling uses LegoGetInputValues to read the analog value
attachRequest.PollingCommands = new NxtCommandSequence(_state.PollingFrequencyMs,
new LegoGetInputValues(_state.SensorPort));
brick.AttachResponse response = null;
yield return(Arbiter.Choice(_legoBrickPort.AttachAndSubscribe(attachRequest, _legoBrickNotificationPort),
delegate(brick.AttachResponse rsp) { response = rsp; },
delegate(Fault f) { fault = f; }));
if (response == null)
{
if (fault == null)
{
fault = Fault.FromException(new Exception("Failure Configuring NXT Color Sensor"));
}
update.ResponsePort.Post(fault);
yield break;
}
_state.Connected = (response.Connection.Port != LegoNxtPort.NotConnected);
if (_state.Connected)
{
_state.SensorPort = (NxtSensorPort)response.Connection.Port;
}
else if (update.Body.SensorPort != NxtSensorPort.NotConnected)
{
fault = Fault.FromException(new Exception("Failure Configuring NXT Color Sensor on Port " + update.Body.ToString()));
update.ResponsePort.Post(fault);
yield break;
}
// Set the Color Sensor name
if (string.IsNullOrEmpty(_state.Name) ||
_state.Name.StartsWith("Color Sensor on "))
{
_state.Name = "Color Sensor on " + response.Connection.ToString();
}
// Send a notification of the connected port
// Only send connection notifications to native subscribers
update.Body.Name = _state.Name;
update.Body.PollingFrequencyMs = _state.PollingFrequencyMs;
update.Body.SensorPort = _state.SensorPort;
update.Body.SensorMode = _state.SensorMode;
SendNotification <ConnectToBrick>(_subMgrPort, update);
update.ResponsePort.Post(DefaultUpdateResponseType.Instance);
yield break;
}
