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; }