public IEnumerator <ITask> ResetHandler(pxencoder.Reset update)
        {
            LogInfo("TrackRoamerEncoder : ResetHandler()  m_lastResetTicks=" + m_lastResetTicks);

            // reset physical counter in the bot AX2850 controller:
            //trackroamerbot.ResetMotorEncoder resetMotorEncoder = new trackroamerbot.ResetMotorEncoder();
            //resetMotorEncoder.Body.HardwareIdentifier = _state.HardwareIdentifier;
            //_trackroamerbotPort.Post(resetMotorEncoder);

            // initialize our local logical counter:
            lock (m_lastResetTicksLock)
            {
                _state.TicksSinceReset = 0;
                _state.CurrentReading  = 0;
                _state.CurrentAngle    = 0.0d;
                m_lastResetTicks       = null;
            }

            update.ResponsePort.Post(DefaultUpdateResponseType.Instance);
            yield break;
        }
        /// <summary>
        /// drives a specified number of meters
        /// </summary>
        IEnumerator <ITask> DriveUntilDistance(double distance, double power)
        {
            LogInfo("=============== TrackRoamerDriveService:: DriveUntilDistance(distance=" + distance + " meters,  power=" + power + ")");

            _leftEncoderTickEnabled  = false;
            _rightEncoderTickEnabled = false;

            //reset encoders
            encoder.Reset Lreset = new encoder.Reset();
            _leftEncoderCmdPort.Post(Lreset);
            yield return(Arbiter.Choice(Lreset.ResponsePort,
                                        delegate(DefaultUpdateResponseType response) { },
                                        delegate(Fault fault) { LogError(fault); }
                                        ));

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

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

            _leftEncoderTickEnabled  = true;
            _rightEncoderTickEnabled = true;

            //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 = drive.DriveStage.Started;

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

            motor.SetMotorPower leftPower = new motor.SetMotorPower(new motor.SetMotorPowerRequest()
            {
                TargetPower = Pow
            });
            leftPower.ResponsePort = responsePort;
            _leftMotorPort.Post(leftPower);

            motor.SetMotorPower rightPower = new motor.SetMotorPower(new motor.SetMotorPowerRequest()
            {
                TargetPower = Pow
            });
            rightPower.ResponsePort = responsePort;
            _rightMotorPort.Post(rightPower);

            LogInfo("=============== TrackRoamerDriveService:: DriveUntilDistance() start moving: distance=" + distance + " meters");
            LogInfo("=============== TrackRoamerDriveService:: DriveUntilDistance() will stop wheel at:  Left=" + stopLeftWheelAt + "   Right=" + stopRightWheelAt);

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

            LogInfo("=============== TrackRoamerDriveService:: DriveUntilDistance() calling WaitForCompletion() - waiting for the first side to complete...");

            yield return(WaitForCompletion());

            LogInfo("=============== TrackRoamerDriveService:: DriveUntilDistance() calling WaitForCompletion() - other side should complete too...");

            yield return(WaitForCompletion());

            LogInfo("=============== TrackRoamerDriveService:: DriveUntilDistance() - both sides completed, send notification of driveDistance complete to subscription manager");

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

            _state.DriveDistanceStage = drive.DriveStage.Completed;
        }
        /// <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)
        {
            LogInfo("^^^^^^^^^^^^^^^^^^^^^ TrackRoamerDriveService:: RotateUntilDegrees(degrees=" + degrees + ",  power=" + power + ")");

            _leftEncoderTickEnabled  = false;
            _rightEncoderTickEnabled = false;

            //reset encoders
            encoder.Reset Lreset = new encoder.Reset();
            _leftEncoderCmdPort.Post(Lreset);
            yield return(Arbiter.Choice(Lreset.ResponsePort,
                                        delegate(DefaultUpdateResponseType response) { },
                                        delegate(Fault fault) { LogError(fault); }
                                        ));

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

            double arcDistance = Math.Abs(degrees) * _state.DistanceBetweenWheels * Math.PI / 360.0d;

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

            _leftEncoderTickEnabled  = true;
            _rightEncoderTickEnabled = true;

            //start moving

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

            drive.RotateDegrees rotateUpdate = new drive.RotateDegrees();
            rotateUpdate.Body.RotateDegreesStage = drive.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(new motor.SetMotorPowerRequest()
            {
                TargetPower = leftPow
            });
            leftPower.ResponsePort = responsePort;
            _leftMotorPort.Post(leftPower);

            motor.SetMotorPower rightPower = new motor.SetMotorPower(new motor.SetMotorPowerRequest()
            {
                TargetPower = rightPow
            });
            rightPower.ResponsePort = responsePort;
            _rightMotorPort.Post(rightPower);

            LogInfo("=============== TrackRoamerDriveService:: RotateUntilDegrees() start moving: degrees=" + degrees);
            LogInfo("=============== TrackRoamerDriveService:: RotateUntilDegrees() will stop wheels at:  Left=" + stopLeftWheelAt + " Right=" + stopRightWheelAt);

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

            LogInfo("=============== TrackRoamerDriveService:: RotateUntilDegrees() calling WaitForCompletion() - waiting for the first side to complete...");

            yield return(WaitForCompletion());

            LogInfo("=============== TrackRoamerDriveService:: RotateUntilDegrees() calling WaitForCompletion() - other side should complete too...");

            yield return(WaitForCompletion());

            LogInfo("=============== TrackRoamerDriveService:: RotateUntilDegrees() - both sides completed, send notification of RotateDegrees complete to subscription manager");

            // send notification of RotateDegrees complete to subscription manager
            rotateUpdate.Body.RotateDegreesStage = drive.DriveStage.Completed;
            _internalDriveOperationsPort.Post(rotateUpdate);

            _state.RotateDegreesStage = drive.DriveStage.Completed;
        }
        /// <summary>
        /// drives a specified number of meters
        /// </summary>
        IEnumerator<ITask> DriveUntilDistance(double distance, double power)
        {
            #if TRACELOG
            Tracer.Trace("=============== TrackRoamerDriveService:: DriveUntilDistance(distance=" + distance + " meters,  power=" + power + ")");
            #endif

            EncoderTicksEnabled = false;

            //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
            stopLeftWheelAt = (int)Math.Round(Math.Abs(distance) / (2.0 * 3.14159 * _state.LeftWheel.Radius / _state.LeftWheel.EncoderState.TicksPerRevolution));
            stopRightWheelAt = (int)Math.Round(Math.Abs(distance) / (2.0 * 3.14159 * _state.RightWheel.Radius / _state.RightWheel.EncoderState.TicksPerRevolution));

            EncoderTicksEnabled = true;

            pollEncoderState();     // get starting encoder state

            //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 = drive.DriveStage.Started;

            drive.DriveDistance driveUpdate = new drive.DriveDistance();
            driveUpdate.Body.DriveDistanceStage = drive.DriveStage.Started;
            #if TRACELOG
            Tracer.Trace("++++++++++++++++++ DRIVE: DriveUntilDistance() DriveStage.Started");
            #endif
            _internalDriveOperationsPort.Post(driveUpdate);

            motor.SetMotorPower leftPower = new motor.SetMotorPower(new motor.SetMotorPowerRequest() { TargetPower = Pow });
            leftPower.ResponsePort = responsePort;
            _leftMotorPort.Post(leftPower);

            motor.SetMotorPower rightPower = new motor.SetMotorPower(new motor.SetMotorPowerRequest() { TargetPower = Pow });
            rightPower.ResponsePort = responsePort;
            _rightMotorPort.Post(rightPower);

            #if TRACELOG
            Tracer.Trace("=============== TrackRoamerDriveService:: DriveUntilDistance() start moving: distance=" + distance + " meters");
            Tracer.Trace("=============== TrackRoamerDriveService:: DriveUntilDistance() will stop wheels at:  Left=" + stopLeftWheelAt + "   Right=" + stopRightWheelAt);
            #endif

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

            #if TRACELOG
            Tracer.Trace("=============== TrackRoamerDriveService:: DriveUntilDistance() calling DriveWaitForCompletionDual() - waiting for both sides to complete...");
            #endif

            yield return DriveWaitForCompletionDual();

            // send notification of driveDistance complete to subscription manager
            driveUpdate.Body.DriveDistanceStage = drive.DriveStage.Completed;
            #if TRACELOG
            Tracer.Trace("++++++++++++++++++ DRIVE: DriveUntilDistance() DriveStage.Completed");
            #endif
            _internalDriveOperationsPort.Post(driveUpdate);

            _state.DriveDistanceStage = drive.DriveStage.Completed;
        }
        public IEnumerator<ITask> ResetEncodersHandler(drive.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;
        }
        /// <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)
        {
            #if TRACELOG
            Tracer.Trace("^^^^^^^^^^^^^^^^^^^^^ TrackRoamerDriveService:: RotateUntilDegrees(degrees=" + degrees + ",  power=" + power + ")");
            #endif

            EncoderTicksEnabled = false;

            //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 * Math.PI / 360.0d;

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

            EncoderTicksEnabled = true;

            pollEncoderState();     // get starting encoder state

            //start moving

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

            drive.RotateDegrees rotateUpdate = new drive.RotateDegrees();
            rotateUpdate.Body.RotateDegreesStage = drive.DriveStage.Started;
            #if TRACELOG
            Tracer.Trace("++++++++++++++++++ DRIVE: RotateUntilDegrees() DriveStage.Started");
            #endif
            _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(new motor.SetMotorPowerRequest() { TargetPower = leftPow });
            leftPower.ResponsePort = responsePort;
            _leftMotorPort.Post(leftPower);

            motor.SetMotorPower rightPower = new motor.SetMotorPower(new motor.SetMotorPowerRequest() { TargetPower = rightPow });
            rightPower.ResponsePort = responsePort;
            _rightMotorPort.Post(rightPower);

            #if TRACELOG
            Tracer.Trace("=============== TrackRoamerDriveService:: RotateUntilDegrees() start moving: degrees=" + degrees);
            Tracer.Trace("=============== TrackRoamerDriveService:: RotateUntilDegrees() will stop wheels at:  Left=" + stopLeftWheelAt + " Right=" + stopRightWheelAt);
            #endif

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

            #if TRACELOG
            Tracer.Trace("=============== TrackRoamerDriveService:: RotateUntilDegrees() calling DriveWaitForCompletionDual() - waiting for both sides to complete...");
            #endif

            yield return DriveWaitForCompletionDual();

            #if TRACELOG
            Tracer.Trace("=============== TrackRoamerDriveService:: RotateUntilDegrees() - both sides completed, send notification of RotateDegrees complete to subscription manager");
            #endif

            // send notification of RotateDegrees complete to subscription manager
            rotateUpdate.Body.RotateDegreesStage = drive.DriveStage.Completed;
            #if TRACELOG
            Tracer.Trace("++++++++++++++++++ DRIVE: RotateUntilDegrees() DriveStage.Completed");
            #endif
            _internalDriveOperationsPort.Post(rotateUpdate);

            _state.RotateDegreesStage = drive.DriveStage.Completed;
        }
Example #7
0
 public IEnumerator <ITask> ResetHandler(encoder.Reset reset)
 {
     _state.TicksSinceReset = 0;
     reset.ResponsePort.Post(DefaultUpdateResponseType.Instance);
     yield break;
 }