Пример #1
0
        public IEnumerator <ITask> ResetEncodersHandler(ResetEncoders resetEncoders)
        {
            encoder.Reset rightResetRequest = null;
            encoder.Reset leftResetRequest  = null;

            if (this._rightEncoderPort != null)
            {
                rightResetRequest = new encoder.Reset();

                this._rightEncoderPort.Post(rightResetRequest);
            }

            if (this._leftEncoderPort != null)
            {
                leftResetRequest = new encoder.Reset();

                this._leftEncoderPort.Post(leftResetRequest);
            }

            if (rightResetRequest != null)
            {
                yield return(rightResetRequest.ResponsePort.Choice(EmptyHandler, EmptyHandler));
            }

            if (leftResetRequest != null)
            {
                yield return(leftResetRequest.ResponsePort.Choice(EmptyHandler, EmptyHandler));
            }

            this.SendNotification <drive.ResetEncoders>(this._subMgrPort, resetEncoders.Body);

            resetEncoders.ResponsePort.Post(new DefaultUpdateResponseType());

            yield break;
        }
Пример #2
0
        /// <summary>
        /// drives a specified number of meters
        /// </summary>
        IEnumerator <ITask> DriveUntilDistance(double distance, double power)
        {
            //reset encoders
            encoder.Reset Lreset = new encoder.Reset();
            _leftEncoderPort.Post(Lreset);
            yield return(Arbiter.Choice(Lreset.ResponsePort,
                                        delegate(DefaultUpdateResponseType response) { },
                                        delegate(Fault fault) { LogError(fault); }
                                        ));

            encoder.Reset Rreset = new encoder.Reset();
            _rightEncoderPort.Post(Rreset);
            yield return(Arbiter.Choice(Rreset.ResponsePort,
                                        delegate(DefaultUpdateResponseType response) { },
                                        delegate(Fault fault) { LogError(fault); }
                                        ));

            //compute tick to stop at
            int stopLeftWheelAt  = (int)Math.Round(Math.Abs(distance) / (2.0 * 3.14159 * _state.LeftWheel.Radius / _state.LeftWheel.EncoderState.TicksPerRevolution));
            int stopRightWheelAt = (int)Math.Round(Math.Abs(distance) / (2.0 * 3.14159 * _state.RightWheel.Radius / _state.RightWheel.EncoderState.TicksPerRevolution));

            // Subscribe to the encoders on internal ports
            encoder.EncoderOperations leftNotificationPort  = new encoder.EncoderOperations();
            encoder.EncoderOperations rightNotificationPort = new encoder.EncoderOperations();

            encoder.Subscribe op = new encoder.Subscribe();
            op.Body             = new SubscribeRequestType();
            op.NotificationPort = leftNotificationPort;
            _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>(false, leftNotificationPort,
                                                                   delegate(encoder.UpdateTickCount update)
                {
                    StopMotorWithEncoderHandler(leftNotificationPort, update, _leftMotorPort, stopLeftWheelAt);
                }));
            },
                                        delegate(Fault fault) { LogError(fault); }
                                        ));


            encoder.Subscribe op2 = new encoder.Subscribe();
            op2.Body             = new SubscribeRequestType();
            op2.NotificationPort = rightNotificationPort;
            _leftEncoderPort.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>(false, rightNotificationPort,
                                                                   delegate(encoder.UpdateTickCount update)
                {
                    StopMotorWithEncoderHandler(rightNotificationPort, update, _rightMotorPort, stopRightWheelAt);
                }
                                                                   ));
            },
                                        delegate(Fault fault) { LogError(fault); }
                                        ));


            //start moving

            double Pow;

            if (distance > 0)
            {
                Pow = power;
            }
            else
            {
                Pow = -power;
            }

            PortSet <DefaultUpdateResponseType, Fault> responsePort = new PortSet <DefaultUpdateResponseType, Fault>();


            // send notification of driveDistance start to subscription manager
            _state.DriveDistanceStage = DriveStage.Started;

            DriveDistance driveUpdate = new DriveDistance();

            driveUpdate.Body.DriveDistanceStage = DriveStage.Started;
            _internalDriveOperationsPort.Post(driveUpdate);

            Motor.SetMotorPower leftPower = new Motor.SetMotorPower(Pow);
            leftPower.ResponsePort = responsePort;
            _leftMotorPort.Post(leftPower);

            Motor.SetMotorPower rightPower = new Motor.SetMotorPower(Pow);
            rightPower.ResponsePort = responsePort;
            _rightMotorPort.Post(rightPower);

            Activate(Arbiter.MultipleItemReceive <DefaultUpdateResponseType, Fault>(responsePort, 2,
                                                                                    delegate(ICollection <DefaultUpdateResponseType> successList, ICollection <Fault> failureList)
            {
                foreach (Fault fault in failureList)
                {
                    LogError(fault);
                }
            }
                                                                                    ));

            // send notification of driveDistance complete to subscription manager
            driveUpdate.Body.DriveDistanceStage = DriveStage.Completed;
            _internalDriveOperationsPort.Post(driveUpdate);

            _state.DriveDistanceStage = DriveStage.Completed;
        }
Пример #3
0
        /// <summary>
        /// Rotate the the drive (positive degrees turn counterclockwise)
        /// </summary>
        /// <param name="degrees">(positive degrees turn counterclockwise)</param>
        /// <param name="power">(-1.0 to 1.0)</param>
        IEnumerator<ITask> RotateUntilDegrees(double degrees, double power)
        {
            //reset encoders
            encoder.Reset Lreset = new encoder.Reset();
            _leftEncoderPort.Post(Lreset);
            yield return (Arbiter.Choice(Lreset.ResponsePort,
                delegate(DefaultUpdateResponseType response) { },
                delegate(Fault fault) { LogError(fault); }
            ));

            encoder.Reset Rreset = new encoder.Reset();
            _rightEncoderPort.Post(Rreset);
            yield return (Arbiter.Choice(Rreset.ResponsePort,
                delegate(DefaultUpdateResponseType response) { },
                delegate(Fault fault) { LogError(fault); }
            ));

            double arcDistance = Math.Abs(degrees) * _state.DistanceBetweenWheels * 3.14159 / 360;

            //compute tick to stop at
            int stopLeftWheelAt = (int)Math.Round(arcDistance / (2.0 * 3.14159 * _state.LeftWheel.Radius / _state.LeftWheel.EncoderState.TicksPerRevolution));
            int stopRightWheelAt = (int)Math.Round(arcDistance / (2.0 * 3.14159 * _state.RightWheel.Radius / _state.RightWheel.EncoderState.TicksPerRevolution));

            // Subscribe to the encoders on internal ports
            encoder.EncoderOperations leftNotificationPort = new encoder.EncoderOperations();
            encoder.EncoderOperations rightNotificationPort = new encoder.EncoderOperations();

            encoder.Subscribe op = new encoder.Subscribe();
            op.Body = new SubscribeRequestType();
            op.NotificationPort = leftNotificationPort;
            _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>(false, leftNotificationPort,
                        delegate(encoder.UpdateTickCount update)
                        {
                            StopMotorWithEncoderHandler(leftNotificationPort, update, _leftMotorPort, stopLeftWheelAt);
                        }));
                },
                delegate(Fault fault) { LogError(fault); }
            ));

            encoder.Subscribe op2 = new encoder.Subscribe();
            op2.Body = new SubscribeRequestType();
            op2.NotificationPort = rightNotificationPort;
            _leftEncoderPort.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>(false, rightNotificationPort,
                        delegate(encoder.UpdateTickCount update)
                        {
                            StopMotorWithEncoderHandler(rightNotificationPort, update, _rightMotorPort, stopRightWheelAt);
                        }
                    ));
                },
                delegate(Fault fault) { LogError(fault); }
            ));

            //start moving

            // start rotate operation
            _state.RotateDegreesStage = DriveStage.Started;

            RotateDegrees rotateUpdate = new RotateDegrees();
            rotateUpdate.Body.RotateDegreesStage = DriveStage.Started;
            _internalDriveOperationsPort.Post(rotateUpdate);

            PortSet<DefaultUpdateResponseType, Fault> responsePort = new PortSet<DefaultUpdateResponseType, Fault>();

            double rightPow;
            double leftPow;

            if (degrees > 0)
            {
                rightPow = power;
                leftPow = -power;
            }
            else
            {
                rightPow = -power;
                leftPow = power;
            }

            Motor.SetMotorPower leftPower = new Motor.SetMotorPower(leftPow);
            leftPower.ResponsePort = responsePort;
            _leftMotorPort.Post(leftPower);

            Motor.SetMotorPower rightPower = new Motor.SetMotorPower(rightPow);
            rightPower.ResponsePort = responsePort;
            _rightMotorPort.Post(rightPower);

            Activate(Arbiter.MultipleItemReceive<DefaultUpdateResponseType, Fault>(responsePort, 2,
                delegate(ICollection<DefaultUpdateResponseType> successList, ICollection<Fault> failureList)
                {
                    foreach (Fault fault in failureList)
                    {
                        LogError(fault);
                    }
                }
            ));

            _state.RotateDegreesStage = DriveStage.Completed;
            //complete
            rotateUpdate.Body.RotateDegreesStage = DriveStage.Completed;
            _internalDriveOperationsPort.Post(rotateUpdate);
        }