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;
pxbrick.Registration registration = new pxbrick.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(pxbrick.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)
{
if (fault == null)
{
fault = Fault.FromException(new Exception("Failure Configuring HiTechnic Compass on Port " + update.Body.ToString()));
}
update.ResponsePort.Post(fault);
yield break;
}
pxbrick.AttachRequest attachRequest = new pxbrick.AttachRequest(registration);
attachRequest.InitializationCommands = new nxtcmd.NxtCommandSequence(
new nxtcmd.LegoSetInputMode((NxtSensorPort)registration.Connection.Port, LegoSensorType.I2C_9V, LegoSensorMode.RawMode));
attachRequest.PollingCommands = new nxtcmd.NxtCommandSequence(_state.PollingFrequencyMs,
new I2CReadHiTechnicCompassSensor(_state.SensorPort));
pxbrick.AttachResponse response = null;
yield return(Arbiter.Choice(_legoBrickPort.AttachAndSubscribe(attachRequest, _legoBrickNotificationPort),
delegate(pxbrick.AttachResponse rsp) { response = rsp; },
delegate(Fault f) { fault = f; }));
if (response == null)
{
if (fault == null)
{
fault = Fault.FromException(new Exception("Failure Configuring HiTechnic Compass"));
}
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)
{
update.ResponsePort.Post(Fault.FromException(new Exception(string.Format("Failure Configuring HiTechnic Compass on port: {0}", update.Body.SensorPort))));
yield break;
}
// Set the compass name
if (string.IsNullOrEmpty(_state.Name) ||
_state.Name.StartsWith("Compass Sensor on "))
{
_state.Name = "Compass Sensor on " + response.Connection.Port.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;
Fault fault = null;
pxbrick.Registration registration = new pxbrick.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(pxbrick.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)
{
if (fault == null)
fault = Fault.FromException(new Exception("Failure Configuring HiTechnic Compass on Port " + update.Body.ToString()));
update.ResponsePort.Post(fault);
yield break;
}
pxbrick.AttachRequest attachRequest = new pxbrick.AttachRequest(registration);
attachRequest.InitializationCommands = new nxtcmd.NxtCommandSequence(
new nxtcmd.LegoSetInputMode((NxtSensorPort)registration.Connection.Port, LegoSensorType.I2C_9V, LegoSensorMode.RawMode));
attachRequest.PollingCommands = new nxtcmd.NxtCommandSequence(_state.PollingFrequencyMs,
new I2CReadHiTechnicCompassSensor(_state.SensorPort));
pxbrick.AttachResponse response = null;
yield return Arbiter.Choice(_legoBrickPort.AttachAndSubscribe(attachRequest, _legoBrickNotificationPort),
delegate(pxbrick.AttachResponse rsp) { response = rsp; },
delegate(Fault f) { fault = f; });
if (response == null)
{
if (fault == null)
fault = Fault.FromException(new Exception("Failure Configuring HiTechnic Compass"));
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)
{
update.ResponsePort.Post(Fault.FromException(new Exception(string.Format("Failure Configuring HiTechnic Compass on port: {0}", update.Body.SensorPort))));
yield break;
}
// Set the compass name
if (string.IsNullOrEmpty(_state.Name)
|| _state.Name.StartsWith("Compass Sensor on "))
_state.Name = "Compass Sensor on " + response.Connection.Port.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)
{
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;
var registration = new pxbrick.Registration(
new LegoNxtConnection((LegoNxtPort) _state.SensorPort),
LegoDeviceType.DigitalSensor,
Contract.DeviceModel,
Contract.Identifier,
ServiceInfo.Service,
_state.Name) {I2CBusAddress = 0x10};
yield return Arbiter.Choice(_legoBrickPort.ReserveDevicePort(registration),
reserveResponse =>
{
if (reserveResponse.DeviceModel == registration.DeviceModel)
registration.Connection = reserveResponse.Connection;
},
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 HiTechnic Accelerometer on Port " + update.Body));
update.ResponsePort.Post(fault);
yield break;
}
var attachRequest = new pxbrick.AttachRequest(registration)
{
InitializationCommands = new NxtCommandSequence(
(LegoCommand) new LegoSetInputMode((NxtSensorPort) registration.Connection.Port, LegoSensorType.I2C_9V, LegoSensorMode.RawMode))
};
//attachRequest.PollingCommands = new NxtCommandSequence(500, new I2CReadHiTechnicPrototypeBoard(_state.SensorPort));
pxbrick.AttachResponse response = null;
yield return Arbiter.Choice(_legoBrickPort.AttachAndSubscribe(attachRequest, _legoBrickNotificationPort),
rsp => response = rsp,
f => fault = f);
if (response == null)
{
LogError("* Failed to attach prototype board.");
if (fault == null)
fault = Fault.FromException(new Exception("Failure Configuring Prototype Board"));
update.ResponsePort.Post(fault);
yield break;
}
LogInfo(string.Format("LOGINFO: Attach respoonse: {0} on {1}", response.DeviceModel, response.Connection.Port.ToString()));
_state.Connected = (response.Connection.Port != LegoNxtPort.NotConnected);
if (_state.Connected)
_state.SensorPort = (NxtSensorPort) response.Connection.Port;
else if (update.Body.SensorPort != NxtSensorPort.NotConnected)
{
update.ResponsePort.Post(Fault.FromException(new Exception(string.Format("Failure Configuring HiTechnic Prototype Board on port: {0}", update.Body.SensorPort))));
yield break;
}
_state.Name = "Prototype Board on " + response.Connection.Port.ToString();
// Send a notification of the connected port
update.Body.Name = _state.Name;
update.Body.PollingFrequencyMs = _state.PollingFrequencyMs;
update.Body.SensorPort = _state.SensorPort;
SendNotification(_subMgrPort, update);
// Send the message response
update.ResponsePort.Post(DefaultUpdateResponseType.Instance);
}
