Пример #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;

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