private void RoundButton_Click(object sender, RoutedEventArgs e)
{
if (ConnectToBrick != null)
{
ConnectToBrick.Invoke(sender, e);
}
}
static void Main(string[] args)
{
brick = new ConnectToBrick();
brick.Connect();
brick.brick.BrickChanged += EventCalled;
Console.ReadLine();
}
public MainWindow()
{
InitializeComponent();
brick = new ConnectToBrick(); // connecting to brick method.
brick.Connect();
this.DataContext = brick;
}
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;
_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)
{
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);
}
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;
}
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> 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;
}
