/// <summary>
/// Find sensors on the brick which can be added to
/// our contact sensor array.
/// </summary>
/// <returns></returns>
private IEnumerator <ITask> FindSensors()
{
#region Attach to the NXT Brick
LegoNxtConnection aggregate = new LegoNxtConnection(LegoNxtPort.Aggregation);
aggregate.PortOverride = "Contact Sensor Array";
brick.AttachRequest attachRequest = new brick.AttachRequest(
new brick.Registration(
aggregate,
LegoDeviceType.Aggregation,
"ContactSensorArray",
Contract.Identifier,
ServiceInfo.Service,
"ContactSensorArray"));
yield return(Arbiter.Choice(_legoBrickPort.AttachAndSubscribe(attachRequest, _legoBrickNotificationPort),
delegate(brick.AttachResponse rsp)
{
},
delegate(Fault fault)
{
LogError("Error in LEGO NXT Contact Sensor Array while attaching to brick", fault);
}));
#endregion
PortSet <brick.NxtBrickState, Fault> brickResponse = _legoBrickPort.Get();
yield return(Arbiter.Choice(
Arbiter.ReceiveWithIterator <brick.NxtBrickState>(false, brickResponse, ProcessBrickState),
Arbiter.Receive <Fault>(false, brickResponse, EmptyHandler <Fault>)));
yield break;
}
/// <summary>
/// Connect to the LEGO NXT Brick
/// </summary>
/// <returns></returns>
private IEnumerator <ITask> ConnectToBrick()
{
#region Attach to the NXT Brick
_state.Connected = false;
brick.AttachRequest attachRequest = new brick.AttachRequest(
new brick.Registration(
new LegoNxtConnection(LegoNxtPort.Buttons),
LegoDeviceType.Internal,
Contract.DeviceModel, // "Buttons"
Contract.Identifier,
ServiceInfo.Service,
Contract.DeviceModel));
// Read Buttons
attachRequest.PollingCommands = new NxtCommandSequence(_state.PollingFrequencyMs,
new LegoGetButtonState());
yield return(Arbiter.Choice(_legoBrickPort.AttachAndSubscribe(attachRequest, _legoBrickNotificationPort),
delegate(brick.AttachResponse rsp)
{
_state.Connected = (rsp.Connection.Port != LegoNxtPort.NotConnected);
},
delegate(Fault fault)
{
LogError("LEGO NXT Buttons error attaching to brick", fault);
}));
#endregion
#region One Time Initialization to activate the main port and receive notifications
if (!_initialized)
{
_initialized = true;
base.MainPortInterleave.CombineWith(new Interleave(
new ExclusiveReceiverGroup(
Arbiter.Receive <brick.LegoSensorUpdate>(true, _legoBrickNotificationPort, NotificationHandler)
),
new ConcurrentReceiverGroup()));
}
#endregion
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;
_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;
_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;
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.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;
}
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;
_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 nxtcmd.NxtCommandSequence(
new nxtcmd.LegoSetInputMode(_state.SensorPort, LegoSensorType.SoundDb, LegoSensorMode.PercentFullScaleMode));
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; });
if (response == null)
{
if (fault == null)
fault = Fault.FromException(new Exception("Failure Configuring NXT Sound Sensor"));
update.ResponsePort.Post(fault);
yield break;
}
if (response.Connection.Port == LegoNxtPort.NotConnected && update.Body.SensorPort != NxtSensorPort.NotConnected)
{
if (fault == null)
fault = Fault.FromException(new Exception("Failure Configuring NXT Sound Sensor on Port " + update.Body.ToString()));
update.ResponsePort.Post(fault);
yield break;
}
_state.Connected = (response.Connection.Port != LegoNxtPort.NotConnected);
if (_state.Connected)
{
_state.SensorPort = (NxtSensorPort)response.Connection.Port;
_genericState.HardwareIdentifier = NxtCommon.HardwareIdentifier(_state.SensorPort);
}
// Set the motor name
if (string.IsNullOrEmpty(_state.Name)
|| _state.Name.StartsWith("Sound Sensor on "))
_state.Name = "Sound 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 motor port.
if ((update.Body.MotorPort & NxtMotorPort.AnyMotorPort)
!= update.Body.MotorPort)
{
update.ResponsePort.Post(
Fault.FromException(
new ArgumentException(
string.Format("Invalid Motor Port: {0}",
((LegoNxtPort)update.Body.MotorPort)))));
yield break;
}
Fault fault = null;
_state.Connected = false;
MotorState currentState = _state.Clone();
// Registration
brick.Registration registration = new brick.Registration(
new LegoNxtConnection((LegoNxtPort)update.Body.MotorPort),
LegoDeviceType.Actuator,
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;
else
registration.Connection.Port = LegoNxtPort.NotConnected;
},
delegate(Fault f)
{
fault = f;
LogError(fault);
registration.Connection.Port = LegoNxtPort.NotConnected;
});
if (registration.Connection.Port == LegoNxtPort.NotConnected)
{
string genericError = string.Format("Failure Configuring Motor on Port {0}", update.Body.MotorPort);
if (fault == null)
fault = Fault.FromException(new Exception(genericError));
else
{
if (fault.Reason == null) fault.Reason = new ReasonText[1];
if (fault.Reason[0] == null) fault.Reason[0] = new ReasonText();
if (string.IsNullOrEmpty(fault.Reason[0].Value)) fault.Reason[0].Value = genericError;
}
LogError(LogGroups.Console, fault.Reason[0].Value);
update.ResponsePort.Post(fault);
yield break;
}
brick.AttachRequest attachRequest = new brick.AttachRequest(registration);
attachRequest.InitializationCommands = new NxtCommandSequence(
new LegoResetMotorPosition((NxtMotorPort)registration.Connection.Port, false));
attachRequest.PollingCommands = new NxtCommandSequence(currentState.PollingFrequencyMs,
new LegoGetOutputState((NxtMotorPort)registration.Connection.Port));
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)
{
string genericError = string.Format("Failure Configuring Motor on Port {0}", update.Body.MotorPort);
if (fault == null)
fault = Fault.FromException(new Exception(genericError));
else
{
if (fault.Reason == null) fault.Reason = new ReasonText[1];
if (fault.Reason[0] == null) fault.Reason[0] = new ReasonText();
if (string.IsNullOrEmpty(fault.Reason[0].Value)) fault.Reason[0].Value = genericError;
}
LogError(LogGroups.Console, fault.Reason[0].Value);
update.ResponsePort.Post(fault);
yield break;
}
_state.ReversePolarity = update.Body.ReversePolarity;
_state.PollingFrequencyMs = (update.Body.PollingFrequencyMs == 0) ? Contract.DefaultPollingFrequencyMs : update.Body.PollingFrequencyMs;
_state.Connected = (response.Connection.Port != LegoNxtPort.NotConnected);
if (_state.Connected)
_state.MotorPort = (NxtMotorPort)response.Connection.Port;
// Set the motor name
if (!string.IsNullOrEmpty(update.Body.Name))
_state.Name = update.Body.Name;
else if (string.IsNullOrEmpty(currentState.Name)
|| (currentState.Name.StartsWith("Motor") && currentState.Name.Length == 6))
_state.Name = response.Connection.ToString();
// Send a notification of the connected port
update.Body.Name = _state.Name;
update.Body.PollingFrequencyMs = _state.PollingFrequencyMs;
update.Body.MotorPort = _state.MotorPort;
update.Body.ReversePolarity = _state.ReversePolarity;
SendNotification<ConnectToBrick>(_subMgrPort, update);
update.ResponsePort.Post(DefaultUpdateResponseType.Instance);
LogVerbose(string.Format("Motor is attached on port {0}", update.Body.MotorPort));
if (!_initialized)
{
_initialized = true;
base.ActivateDsspOperationHandlers();
}
yield break;
}
/// <summary>
/// Connect to the LEGO NXT Brick
/// </summary>
/// <returns></returns>
private IEnumerator<ITask> ConnectToBrick()
{
#region Attach to the NXT Brick
_state.Connected = false;
brick.AttachRequest attachRequest = new brick.AttachRequest(
new brick.Registration(
new LegoNxtConnection(LegoNxtPort.Battery),
LegoDeviceType.Internal,
Contract.DeviceModel, // "Battery"
Contract.Identifier,
ServiceInfo.Service,
Contract.DeviceModel));
attachRequest.PollingCommands = new nxtcmd.NxtCommandSequence(ValidatePollingFrequency(_state.BatteryPollingSeconds),
new nxtcmd.LegoGetBatteryLevel());
yield return Arbiter.Choice(_legoBrickPort.AttachAndSubscribe(attachRequest, _legoBrickNotificationPort),
delegate(brick.AttachResponse rsp)
{
_state.Connected = (rsp.Connection.Port != LegoNxtPort.NotConnected);
},
delegate(Fault fault)
{
LogError(LogGroups.Console, "LEGO NXT Battery error attaching to brick", fault);
});
#endregion
#region One Time Initialization to activate the main port and receive notifications
if (!_initialized)
{
_initialized = true;
base.MainPortInterleave.CombineWith(new Interleave(
new ExclusiveReceiverGroup(
Arbiter.Receive<brick.LegoSensorUpdate>(true, _legoBrickNotificationPort, NotificationHandler)
),
new ConcurrentReceiverGroup()));
}
#endregion
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.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) { }));
}
}
public virtual IEnumerator<ITask> MainConnectToBrickHandler(ConnectToBrick connectToBrick)
{
DriveState currentState = _state.Clone();
// Validate the motor port.
if ((connectToBrick.Body.LeftWheel.MotorPort & NxtMotorPort.AnyMotorPort)
!= connectToBrick.Body.LeftWheel.MotorPort)
if ((connectToBrick.Body.RightWheel.MotorPort & NxtMotorPort.AnyMotorPort)
!= connectToBrick.Body.RightWheel.MotorPort)
{
connectToBrick.ResponsePort.Post(
Fault.FromException(
new ArgumentException(
string.Format("Invalid Drive Ports: {0},{1}",
((LegoNxtPort)connectToBrick.Body.LeftWheel.MotorPort), ((LegoNxtPort)connectToBrick.Body.RightWheel.MotorPort)))));
yield break;
}
_state.PollingFrequencyMs = connectToBrick.Body.PollingFrequencyMs;
_state.DistanceBetweenWheels = connectToBrick.Body.DistanceBetweenWheels;
_state.LeftWheel.WheelDiameter = connectToBrick.Body.LeftWheel.WheelDiameter;
_state.LeftWheel.MotorPort = connectToBrick.Body.LeftWheel.MotorPort;
_state.LeftWheel.ReversePolarity = connectToBrick.Body.LeftWheel.ReversePolarity;
_state.RightWheel.WheelDiameter = connectToBrick.Body.RightWheel.WheelDiameter;
_state.RightWheel.MotorPort = connectToBrick.Body.RightWheel.MotorPort;
_state.RightWheel.ReversePolarity = connectToBrick.Body.RightWheel.ReversePolarity;
_state.Connected = false;
currentState = _state.Clone();
Fault fault = null;
// Registration
brick.Registration registration = new brick.Registration(
new LegoNxtConnection((LegoNxtPort)currentState.LeftWheel.MotorPort),
LegoDeviceType.Actuator,
Contract.DeviceModel,
Contract.Identifier,
ServiceInfo.Service,
Contract.DeviceModel);
#region Configure Left Motor
// Reserve the Left Motor Port
yield return Arbiter.Choice(_legoBrickPort.ReserveDevicePort(registration),
delegate(brick.AttachResponse reserveResponse)
{
if (reserveResponse.DeviceModel == registration.DeviceModel)
registration.Connection = reserveResponse.Connection;
else
registration.Connection.Port = LegoNxtPort.NotConnected;
},
delegate(Fault f)
{
fault = f;
LogError(fault);
registration.Connection.Port = LegoNxtPort.NotConnected;
});
if (registration.Connection.Port == LegoNxtPort.NotConnected)
{
_state.LeftWheel.MotorPort = NxtMotorPort.NotConnected;
if (fault == null)
fault = Fault.FromException(new Exception("Failure Configuring Drive on Left Motor Port " + connectToBrick.Body.ToString()));
connectToBrick.ResponsePort.Post(fault);
yield break;
}
brick.AttachRequest attachRequest = new brick.AttachRequest(registration);
attachRequest.PollingCommands = new NxtCommandSequence(currentState.PollingFrequencyMs,
new LegoGetOutputState((NxtMotorPort)registration.Connection.Port));
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 Left Motor on NXT Drive"));
// Update state
_state.LeftWheel.MotorPort = NxtMotorPort.NotConnected;
connectToBrick.ResponsePort.Post(fault);
yield break;
}
if ((LegoNxtPort)currentState.LeftWheel.MotorPort != response.Connection.Port)
{
_state.LeftWheel.MotorPort = currentState.LeftWheel.MotorPort = (NxtMotorPort)response.Connection.Port;
}
#endregion
#region Configure Right Motor
registration.Connection.Port = (LegoNxtPort)connectToBrick.Body.RightWheel.MotorPort;
// Reserve the Right Motor Port
yield return Arbiter.Choice(_legoBrickPort.ReserveDevicePort(registration),
delegate(brick.AttachResponse reserveResponse)
{
if (reserveResponse.DeviceModel == registration.DeviceModel)
registration.Connection = reserveResponse.Connection;
else
registration.Connection.Port = LegoNxtPort.NotConnected;
},
delegate(Fault f)
{
fault = f;
LogError(fault);
registration.Connection.Port = LegoNxtPort.NotConnected;
});
if (registration.Connection.Port == LegoNxtPort.NotConnected)
{
if (fault == null)
fault = Fault.FromException(new Exception("Failure Configuring Drive on Right Motor Port " + connectToBrick.Body.ToString()));
_state.RightWheel.MotorPort = NxtMotorPort.NotConnected;
connectToBrick.ResponsePort.Post(fault);
yield break;
}
attachRequest = new brick.AttachRequest(registration);
attachRequest.PollingCommands = new NxtCommandSequence(currentState.PollingFrequencyMs,
new LegoGetOutputState((NxtMotorPort)registration.Connection.Port));
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 Right Motor on NXT Drive"));
// Update state
_state.RightWheel.MotorPort = NxtMotorPort.NotConnected;
connectToBrick.ResponsePort.Post(fault);
yield break;
}
if ((LegoNxtPort)currentState.RightWheel.MotorPort != response.Connection.Port)
{
_state.RightWheel.MotorPort = currentState.RightWheel.MotorPort = (NxtMotorPort)response.Connection.Port;
}
#endregion
// If we made it to this point, both motors are connected.
_state.Connected = true;
// save and refresh the state.
currentState = _state.Clone();
// Send a notification of the connected port
connectToBrick.Body.LeftWheel.MotorPort = currentState.LeftWheel.MotorPort;
connectToBrick.Body.RightWheel.MotorPort = currentState.RightWheel.MotorPort;
SendNotification(_subMgrPort, connectToBrick);
connectToBrick.ResponsePort.Post(DefaultUpdateResponseType.Instance);
yield break;
}
/// <summary>
/// Find sensors on the brick which can be added to
/// our contact sensor array.
/// </summary>
/// <returns></returns>
private IEnumerator<ITask> FindSensors()
{
#region Attach to the NXT Brick
LegoNxtConnection aggregate = new LegoNxtConnection(LegoNxtPort.Aggregation);
aggregate.PortOverride = "Contact Sensor Array";
brick.AttachRequest attachRequest = new brick.AttachRequest(
new brick.Registration(
aggregate,
LegoDeviceType.Aggregation,
"ContactSensorArray",
Contract.Identifier,
ServiceInfo.Service,
"ContactSensorArray"));
yield return Arbiter.Choice(_legoBrickPort.AttachAndSubscribe(attachRequest, _legoBrickNotificationPort),
delegate(brick.AttachResponse rsp)
{
},
delegate(Fault fault)
{
LogError("Error in LEGO NXT Contact Sensor Array while attaching to brick", fault);
});
#endregion
PortSet<brick.NxtBrickState, Fault> brickResponse = _legoBrickPort.Get();
yield return Arbiter.Choice(
Arbiter.ReceiveWithIterator<brick.NxtBrickState>(false, brickResponse, ProcessBrickState),
Arbiter.Receive<Fault>(false, brickResponse, EmptyHandler<Fault>));
yield break;
}
