예제 #1
0
 public IEnumerator <ITask> SubscribeHandler(encoder.Subscribe subscribe)
 {
     yield return(Arbiter.Choice(
                      SubscribeHelper(_subMgrPort, subscribe.Body, subscribe.ResponsePort),
                      delegate(SuccessResult success)
     {
         SendNotification <encoder.Replace>(_subMgrPort, subscribe.Body.Subscriber, new encoder.Replace(_state));
     },
                      delegate(Exception fault)
     {
         LogError(fault);
     }
                      ));
 }
        public IEnumerator <ITask> SubscribeHandler(pxencoder.Subscribe subscribe)
        {
            LogInfo("TrackRoamerEncoder " + _state.HardwareIdentifier + " received Subscription request from Subscriber=" + subscribe.Body.Subscriber);

            yield return(Arbiter.Choice(
                             SubscribeHelper(_subMgrPort, subscribe.Body, subscribe.ResponsePort),
                             delegate(SuccessResult success)
            {
                LogInfo("TrackRoamerEncoder : Subscription granted (succeeded) subscriber: " + subscribe.Body.Subscriber);
                //_subMgrPort.Post(new submgr.Submit(subscribe.Body.Subscriber, DsspActions.ReplaceRequest, _state, null));
            },
                             delegate(Exception fault)
            {
                LogError(fault);
            }
                             ));
        }
예제 #3
0
        private IEnumerator <ITask> ConfigureDrive()
        {
            LogInfo("TrackRoamerDriveService:: ConfigureDrive()");

            bool noError = true;

            // Configure motor connections
            motor.Replace configureLeftMotor = new motor.Replace();
            configureLeftMotor.Body = _state.LeftWheel.MotorState;
            _leftMotorPort.Post(configureLeftMotor);

            motor.Replace configureRightMotor = new motor.Replace();
            configureRightMotor.Body = _state.RightWheel.MotorState;
            _rightMotorPort.Post(configureRightMotor);

            yield return(Arbiter.Choice(configureLeftMotor.ResponsePort,
                                        delegate(DefaultReplaceResponseType success) { LogInfo("    Left Motor Port set"); },
                                        delegate(Fault fault) { LogError(fault); noError = false; }));

            yield return(Arbiter.Choice(configureRightMotor.ResponsePort,
                                        delegate(DefaultReplaceResponseType success) { LogInfo("    Right Motor Port set"); },
                                        delegate(Fault fault) { LogError(fault); noError = false; }));

            // Configure encoder connections
            if (_leftEncoderCmdPort != null)
            {
                encoder.Replace configureLeftEncoder = new encoder.Replace();
                configureLeftEncoder.Body = _state.LeftWheel.EncoderState;
                _leftEncoderCmdPort.Post(configureLeftEncoder);

                yield return(Arbiter.Choice(configureLeftEncoder.ResponsePort,
                                            delegate(DefaultReplaceResponseType success) { LogInfo("    Left Encoder Port set"); },
                                            delegate(Fault fault) { LogError(fault); noError = false; }));

                encoder.Subscribe op = new encoder.Subscribe();
                op.Body             = new SubscribeRequestType();
                op.NotificationPort = _leftEncoderTickPort;
                _leftEncoderCmdPort.Post(op);

                yield return(Arbiter.Choice(op.ResponsePort,
                                            delegate(SubscribeResponseType response)
                {
                    //subscription was successful, start listening for encoder replace messages
                    Activate(Arbiter.Receive <encoder.UpdateTickCount>(true, _leftEncoderTickPort,
                                                                       delegate(encoder.UpdateTickCount update)
                    {
                        StopMotorWithEncoderHandler(_leftEncoderTickPort, "left", update, _leftMotorPort);
                    }));
                },
                                            delegate(Fault fault) { LogError(fault); }
                                            ));
            }

            if (_rightEncoderCmdPort != null)
            {
                encoder.Replace configureRightEncoder = new encoder.Replace();
                configureRightEncoder.Body = _state.RightWheel.EncoderState;
                _rightEncoderCmdPort.Post(configureRightEncoder);

                yield return(Arbiter.Choice(configureRightEncoder.ResponsePort,
                                            delegate(DefaultReplaceResponseType success) { LogInfo("    Right Encoder Port set"); },
                                            delegate(Fault fault) { LogError(fault); noError = false; }));

                encoder.Subscribe op2 = new encoder.Subscribe();
                op2.Body             = new SubscribeRequestType();
                op2.NotificationPort = _rightEncoderTickPort;
                _leftEncoderCmdPort.Post(op2);

                yield return(Arbiter.Choice(op2.ResponsePort,
                                            delegate(SubscribeResponseType response)
                {
                    //subscription was successful, start listening for encoder replace messages
                    Activate(Arbiter.Receive <encoder.UpdateTickCount>(true, _rightEncoderTickPort,
                                                                       delegate(encoder.UpdateTickCount update)
                    {
                        StopMotorWithEncoderHandler(_rightEncoderTickPort, "right", update, _rightMotorPort);
                    }
                                                                       ));
                },
                                            delegate(Fault fault) { LogError(fault); }
                                            ));
            }

            if (noError)
            {
                LogInfo("TrackRoamerDriveService:: ConfigureDrive() - success");
                _state.IsEnabled = true;
            }

            yield break;
        }
예제 #4
0
        private IEnumerator<ITask> ConfigureDrive()
        {
            LogInfo("TrackRoamerDriveService:: ConfigureDrive()");

            bool noError = true;

            // Configure motor connections
            motor.Replace configureLeftMotor = new motor.Replace();
            configureLeftMotor.Body = _state.LeftWheel.MotorState;
            _leftMotorPort.Post(configureLeftMotor);

            motor.Replace configureRightMotor = new motor.Replace();
            configureRightMotor.Body = _state.RightWheel.MotorState;
            _rightMotorPort.Post(configureRightMotor);

            yield return Arbiter.Choice(configureLeftMotor.ResponsePort,
                delegate(DefaultReplaceResponseType success) { LogInfo("    Left Motor Configured"); },
                delegate(Fault fault) { LogError(fault); noError = false; });

            yield return Arbiter.Choice(configureRightMotor.ResponsePort,
                delegate(DefaultReplaceResponseType success) { LogInfo("    Right Motor Configured"); },
                delegate(Fault fault) { LogError(fault); noError = false; });

            // Configure encoder connections, and permanently subscribe to the encoders on internal ports.
            if (_leftEncoderPort != null)
            {
                encoder.Replace configureLeftEncoder = new encoder.Replace();
                configureLeftEncoder.Body = _state.LeftWheel.EncoderState;
                _leftEncoderPort.Post(configureLeftEncoder);

                yield return Arbiter.Choice(configureLeftEncoder.ResponsePort,
                    delegate(DefaultReplaceResponseType success) { LogInfo("    Left Encoder Configured"); },
                    delegate(Fault fault) { LogError(fault); noError = false; });

                encoder.Subscribe op = new encoder.Subscribe();
                op.Body = new SubscribeRequestType();
                op.NotificationPort = _leftEncoderTickPort;
                _leftEncoderPort.Post(op);

                yield return (Arbiter.Choice(op.ResponsePort,
                    delegate(SubscribeResponseType response)
                    {
                        //subscription was successful, start listening for encoder replace messages
                        Activate(Arbiter.Receive<encoder.UpdateTickCount>(true, _leftEncoderTickPort,       // "true" here makes listener to subscription permanent
                            delegate(encoder.UpdateTickCount update)
                            {
            #if TRACEDEBUGTICKS
                                LogInfo("Drive: left encoder tick: " + update.Body.Count);
            #endif // TRACEDEBUGTICKS
                                StopMotorWithEncoderHandler(_leftEncoderTickPort, "left", update, _leftMotorPort);
                            }));
                    },
                    delegate(Fault fault) { LogError(fault); noError = false; }
                ));
            }

            if (_rightEncoderPort != null)
            {
                encoder.Replace configureRightEncoder = new encoder.Replace();
                configureRightEncoder.Body = _state.RightWheel.EncoderState;
                _rightEncoderPort.Post(configureRightEncoder);

                yield return Arbiter.Choice(configureRightEncoder.ResponsePort,
                    delegate(DefaultReplaceResponseType success) { LogInfo("    Right Encoder Configured"); },
                    delegate(Fault fault) { LogError(fault); noError = false; });

                encoder.Subscribe op2 = new encoder.Subscribe();
                op2.Body = new SubscribeRequestType();
                op2.NotificationPort = _rightEncoderTickPort;
                _rightEncoderPort.Post(op2);

                yield return (Arbiter.Choice(op2.ResponsePort,
                    delegate(SubscribeResponseType response)
                    {
                        //subscription was successful, start listening for encoder replace messages
                        Activate(Arbiter.Receive<encoder.UpdateTickCount>(true, _rightEncoderTickPort,       // "true" here makes listener to subscription permanent
                            delegate(encoder.UpdateTickCount update)
                            {
            #if TRACEDEBUGTICKS
                                LogInfo("Drive: right encoder tick: " + update.Body.Count);
            #endif // TRACEDEBUGTICKS
                                StopMotorWithEncoderHandler(_rightEncoderTickPort, "right", update, _rightMotorPort);
                            }
                        ));
                    },
                    delegate(Fault fault) { LogError(fault); noError = false; }
                ));
            }

            if (noError)
            {
                LogInfo("TrackRoamerDriveService:: ConfigureDrive() - success");
                _state.IsEnabled = true;

                // Start the encoder polling interval
                this.encodersPollingPort.Post(DateTime.Now);
            }
            else
            {
                LogError("TrackRoamerDriveService:: ConfigureDrive() - failure");
                _state.IsEnabled = false;
            }

            yield break;
        }
예제 #5
0
        private IEnumerator<ITask> ConfigureDrive()
        {
            LogInfo("TrackRoamerDriveService:: ConfigureDrive()");

            bool noError = true;

            // Configure motor connections
            motor.Replace configureLeftMotor = new motor.Replace();
            configureLeftMotor.Body = _state.LeftWheel.MotorState;
            _leftMotorPort.Post(configureLeftMotor);

            motor.Replace configureRightMotor = new motor.Replace();
            configureRightMotor.Body = _state.RightWheel.MotorState;
            _rightMotorPort.Post(configureRightMotor);

            yield return Arbiter.Choice(configureLeftMotor.ResponsePort,
                delegate(DefaultReplaceResponseType success) { LogInfo("    Left Motor Port set"); },
                delegate(Fault fault) { LogError(fault); noError = false; });

            yield return Arbiter.Choice(configureRightMotor.ResponsePort,
                delegate(DefaultReplaceResponseType success) { LogInfo("    Right Motor Port set"); },
                delegate(Fault fault) { LogError(fault); noError = false; });

            // Configure encoder connections
            if (_leftEncoderCmdPort != null)
            {
                encoder.Replace configureLeftEncoder = new encoder.Replace();
                configureLeftEncoder.Body = _state.LeftWheel.EncoderState;
                _leftEncoderCmdPort.Post(configureLeftEncoder);

                yield return Arbiter.Choice(configureLeftEncoder.ResponsePort,
                    delegate(DefaultReplaceResponseType success) { LogInfo("    Left Encoder Port set"); },
                    delegate(Fault fault) { LogError(fault); noError = false; });

                encoder.Subscribe op = new encoder.Subscribe();
                op.Body = new SubscribeRequestType();
                op.NotificationPort = _leftEncoderTickPort;
                _leftEncoderCmdPort.Post(op);

                yield return (Arbiter.Choice(op.ResponsePort,
                    delegate(SubscribeResponseType response)
                    {
                        //subscription was successful, start listening for encoder replace messages
                        Activate(Arbiter.Receive<encoder.UpdateTickCount>(true, _leftEncoderTickPort,
                            delegate(encoder.UpdateTickCount update)
                            {
                                StopMotorWithEncoderHandler(_leftEncoderTickPort, "left", update, _leftMotorPort);
                            }));
                    },
                    delegate(Fault fault) { LogError(fault); }
                ));
            }

            if (_rightEncoderCmdPort != null)
            {
                encoder.Replace configureRightEncoder = new encoder.Replace();
                configureRightEncoder.Body = _state.RightWheel.EncoderState;
                _rightEncoderCmdPort.Post(configureRightEncoder);

                yield return Arbiter.Choice(configureRightEncoder.ResponsePort,
                    delegate(DefaultReplaceResponseType success) { LogInfo("    Right Encoder Port set"); },
                    delegate(Fault fault) { LogError(fault); noError = false; });

                encoder.Subscribe op2 = new encoder.Subscribe();
                op2.Body = new SubscribeRequestType();
                op2.NotificationPort = _rightEncoderTickPort;
                _leftEncoderCmdPort.Post(op2);

                yield return (Arbiter.Choice(op2.ResponsePort,
                    delegate(SubscribeResponseType response)
                    {
                        //subscription was successful, start listening for encoder replace messages
                        Activate(Arbiter.Receive<encoder.UpdateTickCount>(true, _rightEncoderTickPort,
                            delegate(encoder.UpdateTickCount update)
                            {
                                StopMotorWithEncoderHandler(_rightEncoderTickPort, "right", update, _rightMotorPort);
                            }
                        ));
                    },
                    delegate(Fault fault) { LogError(fault); }
                ));
            }

            if (noError)
            {
                LogInfo("TrackRoamerDriveService:: ConfigureDrive() - success");
                _state.IsEnabled = true;
            }

            yield break;
        }