Пример #1
0
        public virtual IEnumerator <ITask> ConnectToBrickHandler(ConnectToBrick update)
        {
            // Validate the sensor port.
            if ((update.Body.SensorPort & NxtSensorPort.AnySensorPort)
                != update.Body.SensorPort)
            {
                update.ResponsePort.Post(
                    Fault.FromException(
                        new ArgumentException(
                            string.Format("Invalid Sensor Port: {0}",
                                          ((LegoNxtPort)update.Body.SensorPort)))));
                yield break;
            }

            _state.SensorPort = update.Body.SensorPort;
            _state.Connected  = false;

            if (!string.IsNullOrEmpty(update.Body.Name))
            {
                _state.Name = update.Body.Name;
            }
            _state.PollingFrequencyMs = update.Body.PollingFrequencyMs;

            Fault fault = null;

            pxbrick.Registration registration = new pxbrick.Registration(
                new LegoNxtConnection((LegoNxtPort)_state.SensorPort),
                LegoDeviceType.DigitalSensor,
                Contract.DeviceModel,
                Contract.Identifier,
                ServiceInfo.Service,
                _state.Name);

            // Reserve the port
            yield return(Arbiter.Choice(_legoBrickPort.ReserveDevicePort(registration),
                                        delegate(pxbrick.AttachResponse reserveResponse)
            {
                if (reserveResponse.DeviceModel == registration.DeviceModel)
                {
                    registration.Connection = reserveResponse.Connection;
                }
            },
                                        delegate(Fault f)
            {
                fault = f;
                LogError(fault);
                registration.Connection.Port = LegoNxtPort.NotConnected;
            }));


            if (registration.Connection.Port == LegoNxtPort.NotConnected)
            {
                if (fault == null)
                {
                    fault = Fault.FromException(new Exception("Failure Configuring HiTechnic Compass on Port " + update.Body.ToString()));
                }
                update.ResponsePort.Post(fault);
                yield break;
            }

            pxbrick.AttachRequest attachRequest = new pxbrick.AttachRequest(registration);

            attachRequest.InitializationCommands = new nxtcmd.NxtCommandSequence(
                new nxtcmd.LegoSetInputMode((NxtSensorPort)registration.Connection.Port, LegoSensorType.I2C_9V, LegoSensorMode.RawMode));

            attachRequest.PollingCommands = new nxtcmd.NxtCommandSequence(_state.PollingFrequencyMs,
                                                                          new I2CReadHiTechnicCompassSensor(_state.SensorPort));

            pxbrick.AttachResponse response = null;

            yield return(Arbiter.Choice(_legoBrickPort.AttachAndSubscribe(attachRequest, _legoBrickNotificationPort),
                                        delegate(pxbrick.AttachResponse rsp) { response = rsp; },
                                        delegate(Fault f) { fault = f; }));

            if (response == null)
            {
                if (fault == null)
                {
                    fault = Fault.FromException(new Exception("Failure Configuring HiTechnic Compass"));
                }
                update.ResponsePort.Post(fault);
                yield break;
            }

            _state.Connected = (response.Connection.Port != LegoNxtPort.NotConnected);
            if (_state.Connected)
            {
                _state.SensorPort = (NxtSensorPort)response.Connection.Port;
            }
            else if (update.Body.SensorPort != NxtSensorPort.NotConnected)
            {
                update.ResponsePort.Post(Fault.FromException(new Exception(string.Format("Failure Configuring HiTechnic Compass on port: {0}", update.Body.SensorPort))));
                yield break;
            }

            // Set the compass name
            if (string.IsNullOrEmpty(_state.Name) ||
                _state.Name.StartsWith("Compass Sensor on "))
            {
                _state.Name = "Compass Sensor on " + response.Connection.Port.ToString();
            }

            // Send a notification of the connected port
            update.Body.Name = _state.Name;
            update.Body.PollingFrequencyMs = _state.PollingFrequencyMs;
            update.Body.SensorPort         = _state.SensorPort;
            SendNotification <ConnectToBrick>(_subMgrPort, update);

            // Send the message response
            update.ResponsePort.Post(DefaultUpdateResponseType.Instance);
            yield break;
        }
        public virtual IEnumerator<ITask> ConnectToBrickHandler(ConnectToBrick update)
        {
            // Validate the sensor port.
            if ((update.Body.SensorPort & NxtSensorPort.AnySensorPort)
                != update.Body.SensorPort)
            {
                update.ResponsePort.Post(
                    Fault.FromException(
                        new ArgumentException(
                            string.Format("Invalid Sensor Port: {0}",
                                ((LegoNxtPort)update.Body.SensorPort)))));
                yield break;
            }

            _state.SensorPort = update.Body.SensorPort;
            _state.Connected = false;

            if (!string.IsNullOrEmpty(update.Body.Name))
                _state.Name = update.Body.Name;
            _state.PollingFrequencyMs = update.Body.PollingFrequencyMs;

            Fault fault = null;

            pxbrick.Registration registration = new pxbrick.Registration(new LegoNxtConnection((LegoNxtPort)_state.SensorPort),
                    LegoDeviceType.DigitalSensor,
                    Contract.DeviceModel,
                    Contract.Identifier,
                    ServiceInfo.Service,
                    _state.Name);

            // Reserve the port
            yield return Arbiter.Choice(_legoBrickPort.ReserveDevicePort(registration),
                delegate(pxbrick.AttachResponse reserveResponse)
                {
                    if (reserveResponse.DeviceModel == registration.DeviceModel)
                    {
                        registration.Connection = reserveResponse.Connection;
                    }
                },
                delegate(Fault f)
                {
                    fault = f;
                    LogError(fault);
                    registration.Connection.Port = LegoNxtPort.NotConnected;
                });

            if (registration.Connection.Port == LegoNxtPort.NotConnected)
            {
                if (fault == null)
                    fault = Fault.FromException(new Exception("Failure Configuring HiTechnic Compass on Port " + update.Body.ToString()));
                update.ResponsePort.Post(fault);
                yield break;
            }

            pxbrick.AttachRequest attachRequest = new pxbrick.AttachRequest(registration);

            attachRequest.InitializationCommands = new nxtcmd.NxtCommandSequence(
                new nxtcmd.LegoSetInputMode((NxtSensorPort)registration.Connection.Port, LegoSensorType.I2C_9V, LegoSensorMode.RawMode));

            attachRequest.PollingCommands = new nxtcmd.NxtCommandSequence(_state.PollingFrequencyMs,
                new I2CReadHiTechnicCompassSensor(_state.SensorPort));

            pxbrick.AttachResponse response = null;

            yield return Arbiter.Choice(_legoBrickPort.AttachAndSubscribe(attachRequest, _legoBrickNotificationPort),
                delegate(pxbrick.AttachResponse rsp) { response = rsp; },
                delegate(Fault f) { fault = f; });

            if (response == null)
            {
                if (fault == null)
                    fault = Fault.FromException(new Exception("Failure Configuring HiTechnic Compass"));
                update.ResponsePort.Post(fault);
                yield break;
            }

            _state.Connected = (response.Connection.Port != LegoNxtPort.NotConnected);
            if (_state.Connected)
            {
                _state.SensorPort = (NxtSensorPort)response.Connection.Port;
            }
            else if (update.Body.SensorPort != NxtSensorPort.NotConnected)
            {
                update.ResponsePort.Post(Fault.FromException(new Exception(string.Format("Failure Configuring HiTechnic Compass on port: {0}", update.Body.SensorPort))));
                yield break;
            }

            // Set the compass name
            if (string.IsNullOrEmpty(_state.Name)
                || _state.Name.StartsWith("Compass Sensor on "))
                _state.Name = "Compass Sensor on " + response.Connection.Port.ToString();

            // Send a notification of the connected port
            update.Body.Name = _state.Name;
            update.Body.PollingFrequencyMs = _state.PollingFrequencyMs;
            update.Body.SensorPort = _state.SensorPort;
            SendNotification<ConnectToBrick>(_subMgrPort, update);

            // Send the message response
            update.ResponsePort.Post(DefaultUpdateResponseType.Instance);
            yield break;
        }
        public virtual IEnumerator<ITask> ConnectToBrickHandler(ConnectToBrick update)
        {
            if ((update.Body.SensorPort & NxtSensorPort.AnySensorPort) != update.Body.SensorPort)
            {
                update.ResponsePort.Post(Fault.FromException(new ArgumentException(string.Format("Invalid Sensor Port: {0}", ((LegoNxtPort) update.Body.SensorPort)))));
                yield break;
            }

            _state.SensorPort = update.Body.SensorPort;
            _state.Connected = false;

            if (!string.IsNullOrEmpty(update.Body.Name))
                _state.Name = update.Body.Name;
            _state.PollingFrequencyMs = update.Body.PollingFrequencyMs;

            Fault fault = null;

            var registration = new pxbrick.Registration(
                new LegoNxtConnection((LegoNxtPort) _state.SensorPort),
                LegoDeviceType.DigitalSensor,
                Contract.DeviceModel,
                Contract.Identifier,
                ServiceInfo.Service,
                _state.Name) {I2CBusAddress = 0x10};

            yield return Arbiter.Choice(_legoBrickPort.ReserveDevicePort(registration),
                                        reserveResponse =>
                                            {
                                                if (reserveResponse.DeviceModel == registration.DeviceModel)
                                                    registration.Connection = reserveResponse.Connection;
                                            },
                                        f =>
                                            {
                                                fault = f;
                                                LogError(fault);
                                                registration.Connection.Port = LegoNxtPort.NotConnected;
                                            });

            if (registration.Connection.Port == LegoNxtPort.NotConnected)
            {
                if (fault == null)
                    fault = Fault.FromException(new Exception("Failure Configuring HiTechnic Accelerometer on Port " + update.Body));
                update.ResponsePort.Post(fault);
                yield break;
            }

            var attachRequest = new pxbrick.AttachRequest(registration)
                {
                    InitializationCommands = new NxtCommandSequence(
                        (LegoCommand) new LegoSetInputMode((NxtSensorPort) registration.Connection.Port, LegoSensorType.I2C_9V, LegoSensorMode.RawMode))
                };

            //attachRequest.PollingCommands = new NxtCommandSequence(500, new I2CReadHiTechnicPrototypeBoard(_state.SensorPort));

            pxbrick.AttachResponse response = null;

            yield return Arbiter.Choice(_legoBrickPort.AttachAndSubscribe(attachRequest, _legoBrickNotificationPort),
                                        rsp => response = rsp,
                                        f => fault = f);

            if (response == null)
            {
                LogError("* Failed to attach prototype board.");
                if (fault == null)
                    fault = Fault.FromException(new Exception("Failure Configuring Prototype Board"));
                update.ResponsePort.Post(fault);
                yield break;
            }
            LogInfo(string.Format("LOGINFO: Attach respoonse: {0} on {1}", response.DeviceModel, response.Connection.Port.ToString()));

            _state.Connected = (response.Connection.Port != LegoNxtPort.NotConnected);
            if (_state.Connected)
                _state.SensorPort = (NxtSensorPort) response.Connection.Port;
            else if (update.Body.SensorPort != NxtSensorPort.NotConnected)
            {
                update.ResponsePort.Post(Fault.FromException(new Exception(string.Format("Failure Configuring HiTechnic Prototype Board on port: {0}", update.Body.SensorPort))));
                yield break;
            }

            _state.Name = "Prototype Board on " + response.Connection.Port.ToString();

            // Send a notification of the connected port
            update.Body.Name = _state.Name;
            update.Body.PollingFrequencyMs = _state.PollingFrequencyMs;
            update.Body.SensorPort = _state.SensorPort;
            SendNotification(_subMgrPort, update);

            // Send the message response
            update.ResponsePort.Post(DefaultUpdateResponseType.Instance);
        }