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