public virtual IEnumerator <ITask> AnalogReplaceHandler(pxanalogsensor.Replace replace)
{
_state.SensorPort = NxtCommon.GetNxtSensorPortFromHardwareIdentifier(replace.Body.HardwareIdentifier);
SendNotification <pxanalogsensor.Replace>(_genericSubMgrPort, replace);
replace.ResponsePort.Post(DefaultReplaceResponseType.Instance);
yield break;
}
/// <summary>
/// Identifies a Sensor Range on the specified Sensor Port.
/// </summary>
/// <param name="hardwareIdentifier"></param>
/// <param name="model"></param>
/// <param name="sensorName"></param>
/// <param name="rangeName"></param>
public SensorRange(int hardwareIdentifier, string model, string sensorName, string rangeName)
{
_sensorPort = NxtCommon.GetNxtSensorPortFromHardwareIdentifier(hardwareIdentifier);
Model = model;
SensorName = sensorName;
RangeName = rangeName;
}
/// <summary>
/// Initialize the Light Sensor state
/// </summary>
private void InitializeState()
{
if (_state == null)
{
_state = new LightSensorState();
}
if (_state.PollingFrequencyMs == 0)
{
_state.PollingFrequencyMs = Contract.DefaultPollingFrequencyMs;
}
_state.TimeStamp = DateTime.MinValue;
_state.Intensity = 0;
_state.Connected = false;
if (_genericState == null)
{
_genericState = new pxanalogsensor.AnalogSensorState();
}
_genericState.RawMeasurementRange = 255.0;
_genericState.RawMeasurement = 0.0;
_genericState.NormalizedMeasurement = 0.0;
_genericState.TimeStamp = DateTime.MinValue;
// Set the hardware identifier from the connected motor port.
_genericState.HardwareIdentifier = NxtCommon.HardwareIdentifier(_state.SensorPort);
}
/// <summary>
/// Initialize the Color Sensor state
/// </summary>
private void InitializeState()
{
if (_state == null)
{
_state = new ColorSensorState();
// Default to Color mode if no config file
_state.SensorMode = ColorSensorMode.Color;
}
if (_state.PollingFrequencyMs == 0)
{
_state.PollingFrequencyMs = Contract.DefaultPollingFrequencyMs;
}
_state.TimeStamp = DateTime.MinValue;
_state.Reading = 0;
_state.Connected = false;
if (_genericState == null)
{
_genericState = new pxanalogsensor.AnalogSensorState();
}
// Analog range is 0-1023 (10-bit A/D)
_genericState.RawMeasurementRange = 1023.0;
_genericState.RawMeasurement = 0.0;
_genericState.NormalizedMeasurement = 0.0;
_genericState.TimeStamp = DateTime.MinValue;
// Set the hardware identifier from the connected motor port.
_genericState.HardwareIdentifier = NxtCommon.HardwareIdentifier(_state.SensorPort);
}
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;
}
/// <summary>
/// Process Brick State
/// </summary>
/// <param name="brickState"></param>
private IEnumerator <ITask> ProcessBrickState(brick.NxtBrickState brickState)
{
foreach (string key in brickState.Runtime.Devices.Keys)
{
brick.AttachRequest device = brickState.Runtime.Devices[key];
if (device.Registration.DeviceType != LegoDeviceType.AnalogSensor &&
device.Registration.DeviceType != LegoDeviceType.DigitalSensor)
{
continue;
}
PortSet <DsspDefaultLookup, DsspDefaultGet> lookupPort = ServiceForwarder <PortSet <DsspDefaultLookup, DsspDefaultGet> >(device.Registration.ServiceUri);
DsspDefaultLookup lu = new DsspDefaultLookup();
lookupPort.Post(lu);
yield return(Arbiter.Choice(lu.ResponsePort,
delegate(LookupResponse luResponse)
{
foreach (PartnerType pt in luResponse.PartnerList)
{
// See if this service supports the analog sensor contract
if (pt.Contract == analog.Contract.Identifier)
{
// Check if we have already processed this one.
if (_sensorList.ContainsKey(pt.Service))
{
break;
}
string name = device.Registration.Name;
string model = device.Registration.DeviceModel;
int hardwareIdentifier = NxtCommon.HardwareIdentifier(device.Registration.Connection.Port);
LogVerbose(LogGroups.Console, string.Format("Configuring {0}:{1} on {2} with analog service at {3}", model, name, hardwareIdentifier, pt.Service));
analog.AnalogSensorOperations sensorPort = ServiceForwarder <analog.AnalogSensorOperations>(pt.Service);
Activate(Arbiter.Choice(sensorPort.Subscribe(_analogSensorNotificationsPort),
delegate(SubscribeResponseType response)
{
// Keep track of the subscription manager response
// so that we can unsubscribe later.
_sensorList.Add(pt.Service, response);
},
delegate(Fault fault)
{
LogError(LogGroups.Console, string.Format("Failure subscribing to {0} on port {1}.", model, hardwareIdentifier));
}));
foreach (SensorConfiguration cfg in _state.SensorConfiguration)
{
if (cfg.DeviceModel != model)
{
continue;
}
SensorRange range = new SensorRange(hardwareIdentifier, model, name, cfg.RangeName);
PortConfiguration portConfig = new PortConfiguration(hardwareIdentifier, range.ContactSensorName, cfg.SuccessRangeMin, cfg.SuccessRangeMax);
portConfig.AnalogSensorServiceUri = pt.Service;
if (portConfig != null)
{
_state.RuntimeConfiguration.Add(range, portConfig);
}
}
break;
}
}
},
delegate(Fault f) { }));
}
}
