Ejemplo n.º 1
0
        public IEnumerator <ITask> ReplaceHandler(encoder.Replace replace)
        {
            if (_subscribed)
            {
                throw new InvalidOperationException("Already subscribed");
            }
            else if (ValidState(replace.Body))
            {
                _state = replace.Body;
                SaveState(_state);
                replace.ResponsePort.Post(DefaultReplaceResponseType.Instance);
                SubscribeToNXT();
            }
            else
            {
                throw new InvalidOperationException("Invalid State for replacement");
            }

            yield break;
        }
Ejemplo n.º 2
0
        public IEnumerator <ITask> ReplaceHandler(pxencoder.Replace replace)
        {
            //LogInfo("TrackRoamerEncoder : ReplaceHandler()");

            if (_subscribed)
            {
                LogError("TrackRoamerEncoder : ReplaceHandler(): Already subscribed");
            }
            else if (ValidState(replace.Body))
            {
                //_state = (TREncoderState)replace.Body;
                _state = replace.Body;
                SaveState(_state);
                replace.ResponsePort.Post(DefaultReplaceResponseType.Instance);
                SubscribeToPowerBrick();
            }
            else
            {
                LogError("TrackRoamerEncoder : ReplaceHandler(): Invalid State for replacement");
            }

            yield break;
        }
Ejemplo n.º 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;
        }
Ejemplo n.º 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;
        }
Ejemplo n.º 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;
        }