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