Package trclib.pathdrive

Class TrcPurePursuitDrive

java.lang.Object
trclib.pathdrive.TrcPurePursuitDrive
public class TrcPurePursuitDrive extends Object
This class implements a platform independent Pure Pursuit drive for holonomic or non-holonomic robots. Essentially, a pure pursuit drive navigates the robot to chase a point along the path. The point to chase is chosen by intersecting a proximity circle centered on the robot with a specific radius with the path, and chasing the "furthest" intersection. The smaller the radius is, the more "tightly" the robot will follow a path, but it will be more prone to oscillation and sharp turns. A larger radius will tend to smooth out turns and corners. Note that the error tolerance must be less than the proximity radius, so choose them accordingly.

A path consists of an array of waypoints, specifying position, velocity, and heading. All other properties of the TrcWaypoint object may be ignored.The path may be low resolution, as this automatically interpolates between waypoints. If you want the robot to maintain heading to a fixed target, call enableFixedHeading with the target heading offset and it will ignore all the heading values. Otherwise, call disableFixedHeading, ensure that the heading tolerance and pid coefficients are set, and it will follow the heading values specified by the path. Note that FixedHeading is only supported for holonomic robots.

A somewhat similar idea is here: ... or ...

Note that this paper is for non-holonomic robots. This means that all the turning radius stuff isn't very relevant. Technically, we could impose limits on the turning radius as a function of robot velocity and max rot vel, but that's unnecessarily complicated, in my view. Additionally, it does point injection instead of interpolation, and path smoothing, which we don't do, since a nonzero proximity radius will naturally smooth it anyway.

  • Field Details

    • INVERTED_TARGET

      private static final boolean INVERTED_TARGET
      See Also:

    • tracer

      public TrcDbgTrace tracer

    • instanceName

      private String instanceName

    • driveBase

      private TrcDriveBase driveBase

    • proximityRadius

      private volatile double proximityRadius

    • posTolerance

      private volatile double posTolerance

    • turnTolerance

      private volatile double turnTolerance

    • xPosPidCtrl

      private TrcPidController xPosPidCtrl

    • yPosPidCtrl

      private TrcPidController yPosPidCtrl

    • turnPidCtrl

      private TrcPidController turnPidCtrl

    • velPidCtrl

      private TrcPidController velPidCtrl

    • warpSpace

      private TrcWarpSpace warpSpace

    • driveTaskObj

      private TrcTaskMgr.TaskObject driveTaskObj

    • logRobotPoseEvents

      private boolean logRobotPoseEvents

    • tracePidInfo

      private boolean tracePidInfo

    • verbosePidInfo

      private boolean verbosePidInfo

    • battery

      private TrcRobotBattery battery

    • DEF_BEEP_FREQUENCY

      private static final double DEF_BEEP_FREQUENCY
      See Also:

    • DEF_BEEP_DURATION

      private static final double DEF_BEEP_DURATION
      See Also:

    • beepDevice

      private TrcTone beepDevice

    • beepFrequency

      private double beepFrequency

    • beepDuration

      private double beepDuration

    • moveOutputLimit

      private double moveOutputLimit

    • rotOutputLimit

      private double rotOutputLimit

    • waypointEventHandler

      private TrcPurePursuitDrive.WaypointEventHandler waypointEventHandler

    • interpolationType

      private TrcPurePursuitDrive.InterpolationType interpolationType

    • incrementalTurn

      private volatile boolean incrementalTurn

    • targetHeadingOffset

      private TrcPurePursuitDrive.TargetHeadingOffset targetHeadingOffset

    • stalled

      private volatile boolean stalled

    • owner

      private String owner

    • path

      private TrcPath path

    • onFinishedEvent

      private TrcEvent onFinishedEvent

    • timedOutTime

      private double timedOutTime

    • pathIndex

      private int pathIndex

    • referencePose

      private TrcPose2D referencePose

    • relativeTargetPose

      private TrcPose2D relativeTargetPose

    • fastModeEnabled

      private boolean fastModeEnabled

    • resetError

      private boolean resetError

  • Constructor Details

    • TrcPurePursuitDrive

      public TrcPurePursuitDrive(String instanceName, TrcDriveBase driveBase, double proximityRadius, double posTolerance, double turnTolerance, TrcPidController xPidCtrl, TrcPidController yPidCtrl, TrcPidController turnPidCtrl, TrcPidController velPidCtrl)
      Constructor: Create an instance of the object.
      Parameters:
      instanceName - specifies the instance name.
      driveBase - specifies the reference to the drive base.
      proximityRadius - specifies the distance between the robot and next following point.
      posTolerance - specifies the position tolerance.
      turnTolerance - specifies the turn tolerance.
      xPidCtrl - specifies the position PID controller for X.
      yPidCtrl - specifies the position PID controller for Y.
      turnPidCtrl - specifies the turn PID controller.
      velPidCtrl - specifies the velocity PID controller.

    • TrcPurePursuitDrive

      public TrcPurePursuitDrive(String instanceName, TrcDriveBase driveBase, double proximityRadius, double posTolerance, double turnTolerance, TrcPidController.PidCoefficients xPosPidCoeff, TrcPidController.PidCoefficients yPosPidCoeff, TrcPidController.PidCoefficients turnPidCoeff, TrcPidController.PidCoefficients velPidCoeff)
      Constructor: Create an instance of the object.
      Parameters:
      instanceName - specifies the instance name.
      driveBase - specifies the reference to the drive base.
      proximityRadius - specifies the distance between the robot and next following point.
      posTolerance - specifies the position tolerance.
      turnTolerance - specifies the turn tolerance.
      xPosPidCoeff - specifies the position PID coefficients for X.
      yPosPidCoeff - specifies the position PID coefficients for Y.
      turnPidCoeff - specifies the turn PID coefficients.
      velPidCoeff - specifies the velocity PID coefficients.

  • Method Details

    • commonInit

      public void commonInit(String instanceName, TrcDriveBase driveBase, double proximityRadius, double posTolerance, double turnTolerance, TrcPidController xPidCtrl, TrcPidController yPidCtrl, TrcPidController turnPidCtrl, TrcPidController velPidCtrl)
      Constructor: Create an instance of the object.
      Parameters:
      instanceName - specifies the instance name.
      driveBase - specifies the reference to the drive base.
      proximityRadius - specifies the distance between the robot and next following point.
      posTolerance - specifies the position tolerance.
      turnTolerance - specifies the turn tolerance.
      xPidCtrl - specifies the position PID controller for X.
      yPidCtrl - specifies the position PID controller for Y.
      turnPidCtrl - specifies the turn PID controller.
      velPidCtrl - specifies the velocity PID controller.

    • toString

      public String toString()
      This method returns the instance name.
      Overrides:
      toString in class Object
      Returns:
      instance name.

    • getXPosPidCtrl

      public TrcPidController getXPosPidCtrl()
      This method returns the X position PID controller created for Pure Pursuit Drive.
      Returns:
      X position PID controller.

    • getYPosPidCtrl

      public TrcPidController getYPosPidCtrl()
      This method returns the Y position PID controller created for the Pure Pursuit Drive.
      Returns:
      Y position PID controller.

    • getTurnPidCtrl

      public TrcPidController getTurnPidCtrl()
      This method returns the turn PID controller created for the Pure Pursuit Drive.
      Returns:
      turn PID controller.

    • setTraceLevel

      public void setTraceLevel(TrcDbgTrace.MsgLevel msgLevel, boolean logRobotPoseEvents, boolean tracePidInfo, boolean verbosePidInfo, TrcRobotBattery battery)
      This method sets the message tracer for logging trace messages.
      Parameters:
      msgLevel - specifies the message level.
      logRobotPoseEvents - specifies true to log robot pose events, false otherwise.
      tracePidInfo - specifies true to enable tracing of PID info, false otherwise.
      verbosePidInfo - specifies true to trace verbose PID info, false otherwise.
      battery - specifies the battery object to get battery info for the message.

    • setTraceLevel

      public void setTraceLevel(TrcDbgTrace.MsgLevel msgLevel, boolean logRobotPoseEvents, boolean tracePidInfo, boolean verbosePidInfo)
      This method sets the message tracer for logging trace messages.
      Parameters:
      msgLevel - specifies the message level.
      logRobotPoseEvents - specifies true to log robot pose events, false otherwise.
      tracePidInfo - specifies true to enable tracing of PID info, false otherwise.
      verbosePidInfo - specifies true to trace verbose PID info, false otherwise.

    • setBeep

      public void setBeep(TrcTone beepDevice, double beepFrequency, double beepDuration)
      This method sets the beep device and the beep tones so that it can play beeps when motor stalled or if the limit switches are activated/deactivated.
      Parameters:
      beepDevice - specifies the beep device object.
      beepFrequency - specifies the beep frequency.
      beepDuration - specifies the beep duration.

    • setBeep

      public void setBeep(TrcTone beepDevice)
      This method sets the beep device so that it can play beeps at default frequency and duration when motor stalled or if the limit switches are activated/deactivated.
      Parameters:
      beepDevice - specifies the beep device object.

    • setStallDetectionEnabled

      public void setStallDetectionEnabled(double stallDetectionDelay, double stallDetectionTimeout, double stallErrorRateThreshold)
      This method enables/disables stall detection.
      Parameters:
      stallDetectionDelay - specifies stall detection start delay in seconds, zero to disable stall detection.
      stallDetectionTimeout - specifies stall timeout in seconds which is the minimum elapsed time for the motor to be motionless to be considered stalled.
      stallErrorRateThreshold - specifies the error rate threshold below which it will consider stalling.

    • setStallDetectionEnabled

      public void setStallDetectionEnabled(boolean enabled)
      This method enables/disables stall detection.
      Parameters:
      enabled - specifies true to enable stall detection, false to disable.

    • setFastModeEnabled

      public void setFastModeEnabled(boolean enabled)

    • setIncrementalTurnEnabled

      public void setIncrementalTurnEnabled(boolean enabled)
      This method enables/disables incremental turn when running a path segment. Incremental turn is only applicable for holonomic drive base.
      Parameters:
      enabled - specifies true to enable incremental turn, false to disable.

    • enableFixedHeading

      public void enableFixedHeading(TrcPurePursuitDrive.TargetHeadingOffset headingOffset)
      This method enables fixed heading mode. This is especially useful for the robot to track a vision target while following the path. To do this, the Turn PID controller must be vision-based and the heading offset to the vision target is typically zero.
      Parameters:
      headingOffset - specifies the heading offset supplier.

    • disableFixedHeading

      public void disableFixedHeading()
      This method disables fixed heading mode.

    • setPositionToleranceAndProximityRadius

      public void setPositionToleranceAndProximityRadius(Double posTolerance, Double proximityRadius)
      Set both the position tolerance and proximity radius.
      Parameters:
      posTolerance - specifies the distance at which the controller will stop itself.
      proximityRadius - specifies the distance between the robot and next following point.

    • setPositionTolerance

      public void setPositionTolerance(double posTolerance)
      Set the position tolerance to end the path. Units need to be consistent.
      Parameters:
      posTolerance - specifies the distance at which the controller will stop itself.

    • setProximityRadius

      public void setProximityRadius(double proximityRadius)
      Set the following distance for the pure pursuit controller.
      Parameters:
      proximityRadius - specifies the distance between the robot and next following point.

    • setTurnTolerance

      public void setTurnTolerance(double turnTolerance)
      Set the turn tolerance for the closed loop control on turning.
      Parameters:
      turnTolerance - specifies the turn tolerance, in degrees. Should be positive.

    • setXPositionPidCoefficients

      public void setXPositionPidCoefficients(TrcPidController.PidCoefficients xPidCoefficients)
      Sets the pid coefficients for the X position controller. This will work in the middle of an operation as well.
      Parameters:
      xPidCoefficients - specifies the new PID coefficients for the X position controller.

    • setYPositionPidCoefficients

      public void setYPositionPidCoefficients(TrcPidController.PidCoefficients yPidCoefficients)
      Sets the pid coefficients for the Y position controller. This will work in the middle of an operation as well.
      Parameters:
      yPidCoefficients - specifies the new PID coefficients for the Y position controller.

    • setPositionPidCoefficients

      public void setPositionPidCoefficients(TrcPidController.PidCoefficients pidCoefficients)
      Sets the pid coefficients for both X and Y position controllers. This will work in the middle of an operation as well.
      Parameters:
      pidCoefficients - specifies the new PID coefficients for both X and Y position controllers.

    • setTurnPidCoefficients

      public void setTurnPidCoefficients(TrcPidController.PidCoefficients pidCoefficients)
      Sets the pid coefficients for the turn controller. This will work in the middle of an operation as well.
      Parameters:
      pidCoefficients - specifies the new PID coefficients for the heading controller.

    • setVelocityPidCoefficients

      public void setVelocityPidCoefficients(TrcPidController.PidCoefficients pidCoefficients)
      Sets the pid coefficients for the position controller. This will work in the middle of an operation as well. Note that velocity controllers should have an F term as well.
      Parameters:
      pidCoefficients - specifies the new PIDF coefficients for the velocity controller.

    • setMoveOutputLimit

      public void setMoveOutputLimit(double limit)
      Sets the movement output power limit.
      Parameters:
      limit - specifies the output power limit for movement (X and Y).

    • setRotOutputLimit

      public void setRotOutputLimit(double limit)
      Sets the rotation output power limit.
      Parameters:
      limit - specifies the output power limit for rotation.

    • setInterpolationType

      public void setInterpolationType(TrcPurePursuitDrive.InterpolationType interpolationType)
      Configure the method of interpolating between waypoints. Methods ending with INV will favor the ending point.
      Parameters:
      interpolationType - The type of interpolation to use.

    • getTargetFieldPosition

      public TrcPose2D getTargetFieldPosition()
      This method returns the field position of the target waypoint of the path (i.e. the last waypoint in the path).
      Returns:
      field position of the last waypoint in the path.

    • setWaypointEventHandler

      public void setWaypointEventHandler(TrcPurePursuitDrive.WaypointEventHandler handler)
      This method sets the waypoint event handler that gets called when the robot crosses each waypoint. This allows the caller to perform actions when each waypoint is reached. Waypoint handler is cleared when the start method is called. In other words, this method should only be called after the start method is called and the Waypoint event handler is only valid for the path started by the start method.
      Parameters:
      handler - specifies the waypoint event handler, can be null to clear the event handler.

    • start

      public void start(String owner, TrcPath path, TrcEvent event, double timeout, Double maxVel, Double maxAccel, Double maxDecel)
      Start following the supplied path using a pure pursuit controller. The velocity must always be positive, and the path must start at (0,0). Heading is absolute and position is relative in the starting robot reference frame.
      Parameters:
      owner - specifies the ID string of the caller requesting exclusive access.
      path - The path to follow. Must start at (0,0).
      event - When finished, signal this event.
      timeout - Number of seconds after which to cancel this operation. 0.0 for no timeout.
      maxVel - specifies the maximum velocity if applying trapezoid velocity profile, null if not.
      maxAccel - specifies the maximum acceleration if applying trapezoid velocity profile, null if not.
      maxDecel - specifies the maximum deceleration if applying trapezoid velocity profile, null if not.

    • start

      public void start(TrcPath path, TrcEvent event, double timeout, Double maxVel, Double maxAccel, Double maxDecel)
      Start following the supplied path using a pure pursuit controller. The velocity must always be positive, and the path must start at (0,0). Heading is absolute and position is relative in the starting robot reference frame.
      Parameters:
      path - The path to follow. Must start at (0,0).
      event - When finished, signal this event.
      timeout - Number of seconds after which to cancel this operation. 0.0 for no timeout.
      maxVel - specifies the maximum velocity if applying trapezoid velocity profile, null if not.
      maxAccel - specifies the maximum acceleration if applying trapezoid velocity profile, null if not.
      maxDecel - specifies the maximum deceleration if applying trapezoid velocity profile, null if not.

    • start

      public void start(TrcPath path, TrcEvent event, Double maxVel, Double maxAccel, Double maxDecel)
      Start following the supplied path using a pure pursuit controller. The velocity must always be positive, and the path must start at (0,0). Heading is absolute and position is relative in the starting robot reference frame.
      Parameters:
      path - The path to follow. Must start at (0,0).
      event - When finished, signal this event.
      maxVel - specifies the maximum velocity if applying trapezoid velocity profile, null if not.
      maxAccel - specifies the maximum acceleration if applying trapezoid velocity profile, null if not.
      maxDecel - specifies the maximum deceleration if applying trapezoid velocity profile, null if not.

    • start

      public void start(TrcPath path, Double maxVel, Double maxAccel, Double maxDecel)
      Start following the supplied path using a pure pursuit controller. The velocity must always be positive, and the path must start at (0,0). Heading is absolute and position is relative in the starting robot reference frame.
      Parameters:
      path - The path to follow. Must start at (0,0).
      maxVel - specifies the maximum velocity if applying trapezoid velocity profile, null if not.
      maxAccel - specifies the maximum acceleration if applying trapezoid velocity profile, null if not.
      maxDecel - specifies the maximum deceleration if applying trapezoid velocity profile, null if not.

    • start

      public void start(String owner, TrcPath path, TrcEvent event, double timeout)
      Start following the supplied path using a pure pursuit controller. The velocity must always be positive, and the path must start at (0,0). Heading is absolute and position is relative in the starting robot reference frame.
      Parameters:
      owner - specifies the ID string of the caller requesting exclusive access.
      path - The path to follow. Must start at (0,0).
      event - When finished, signal this event.
      timeout - Number of seconds after which to cancel this operation. 0.0 for no timeout.

    • start

      public void start(TrcPath path, TrcEvent event, double timeout)
      Start following the supplied path using a pure pursuit controller. The velocity must always be positive, and the path must start at (0,0). Heading is absolute and position is relative in the starting robot reference frame.
      Parameters:
      path - The path to follow. Must start at (0,0).
      event - When finished, signal this event.
      timeout - Number of seconds after which to cancel this operation. 0.0 for no timeout.

    • start

      public void start(TrcPath path, TrcEvent event)
      Start following the supplied path using a pure pursuit controller. The velocity must always be positive, and the path must start at (0,0). Heading is absolute and position is relative in the starting robot reference frame.
      Parameters:
      path - The path to follow. Must start at (0,0).
      event - When finished, signal this event.

    • start

      public void start(TrcPath path)
      Start following the supplied path using a pure pursuit controller. The velocity must always be positive, and the path must start at (0,0). Heading is absolute and position is relative in the starting robot reference frame.
      Parameters:
      path - The path to follow. Must start at (0,0).

    • start

      public void start(String owner, TrcEvent event, double timeout, boolean incrementalPath, Double maxVel, Double maxAccel, Double maxDecel, TrcPose2D... poses)
      This method starts the Pure Pursuit drive with the specified poses in the drive path.
      Parameters:
      owner - specifies the ID string of the caller requesting exclusive access.
      event - When finished, signal this event.
      timeout - specifies the maximum time allowed for this operation, 0.0 for no timeout.
      incrementalPath - specifies true if appending point is relative to the previous point in the path, false if appending point is in the same reference frame as startingPose.
      maxVel - specifies the maximum velocity if applying trapezoid velocity profile, null if not.
      maxAccel - specifies the maximum acceleration if applying trapezoid velocity profile, null if not.
      maxDecel - specifies the maximum deceleration if applying trapezoid velocity profile, null if not.
      poses - specifies an array of waypoint poses in the drive path.

    • start

      public void start(TrcEvent event, double timeout, boolean incrementalPath, Double maxVel, Double maxAccel, Double maxDecel, TrcPose2D... poses)
      This method starts the Pure Pursuit drive with the specified poses in the drive path.
      Parameters:
      event - When finished, signal this event.
      timeout - specifies the maximum time allowed for this operation, 0.0 for no timeout.
      incrementalPath - specifies true if appending point is relative to the previous point in the path, false if appending point is in the same reference frame as startingPose.
      maxVel - specifies the maximum velocity if applying trapezoid velocity profile, null if not.
      maxDecel - specifies the maximum deceleration if applying trapezoid velocity profile, null if not.
      maxAccel - specifies the maximum acceleration if applying trapezoid velocity profile, null if not.
      poses - specifies an array of waypoint poses in the drive path.

    • start

      public void start(TrcEvent event, double timeout, boolean incrementalPath, Double maxVel, Double maxAccel, Double maxDecel, String path, boolean loadFromResources)
      This method starts the Pure Pursuit drive with the specified poses read either from the built-in resources or from a file.
      Parameters:
      event - When finished, signal this event.
      timeout - specifies the maximum time allowed for this operation, 0.0 for no timeout.
      incrementalPath - specifies true if appending point is relative to the previous point in the path, false if appending point is in the same reference frame as startingPose.
      maxVel - specifies the maximum velocity if applying trapezoid velocity profile, null if not.
      maxAccel - specifies the maximum acceleration if applying trapezoid velocity profile, null if not.
      maxDecel - specifies the maximum deceleration if applying trapezoid velocity profile, null if not.
      path - specifies the file system path or resource name.
      loadFromResources - specifies true if the data is from attached resources, false if from file system.

    • start

      public void start(TrcEvent event, boolean incrementalPath, Double maxVel, Double maxAccel, Double maxDecel, TrcPose2D... poses)
      This method starts the Pure Pursuit drive with the specified poses in the drive path.
      Parameters:
      event - When finished, signal this event.
      incrementalPath - specifies true if appending point is relative to the previous point in the path, false if appending point is in the same reference frame as startingPose.
      maxVel - specifies the maximum velocity if applying trapezoid velocity profile, null if not.
      maxAccel - specifies the maximum acceleration if applying trapezoid velocity profile, null if not.
      maxDecel - specifies the maximum deceleration if applying trapezoid velocity profile, null if not.
      poses - specifies an array of waypoint poses in the drive path.

    • start

      public void start(boolean incrementalPath, Double maxVel, Double maxAccel, Double maxDecel, TrcPose2D... poses)
      This method starts the Pure Pursuit drive with the specified poses in the drive path.
      Parameters:
      incrementalPath - specifies true if appending point is relative to the previous point in the path, false if appending point is in the same reference frame as startingPose.
      maxVel - specifies the maximum velocity if applying trapezoid velocity profile, null if not.
      maxAccel - specifies the maximum acceleration if applying trapezoid velocity profile, null if not.
      maxDecel - specifies the maximum deceleration if applying trapezoid velocity profile, null if not.
      poses - specifies an array of waypoint poses in the drive path.

    • start

      public void start(String owner, TrcEvent event, double timeout, boolean incrementalPath, TrcPose2D... poses)
      This method starts the Pure Pursuit drive with the specified poses in the drive path.
      Parameters:
      owner - specifies the ID string of the caller requesting exclusive access.
      event - When finished, signal this event.
      timeout - specifies the maximum time allowed for this operation, 0.0 for no timeout.
      incrementalPath - specifies true if appending point is relative to the previous point in the path, false if appending point is in the same reference frame as startingPose.
      poses - specifies an array of waypoint poses in the drive path.

    • start

      public void start(TrcEvent event, double timeout, boolean incrementalPath, TrcPose2D... poses)
      This method starts the Pure Pursuit drive with the specified poses in the drive path.
      Parameters:
      event - When finished, signal this event.
      timeout - specifies the maximum time allowed for this operation, 0.0 for no timeout.
      incrementalPath - specifies true if appending point is relative to the previous point in the path, false if appending point is in the same reference frame as startingPose.
      poses - specifies an array of waypoint poses in the drive path.

    • start

      public void start(TrcEvent event, boolean incrementalPath, TrcPose2D... poses)
      This method starts the Pure Pursuit drive with the specified poses in the drive path.
      Parameters:
      event - When finished, signal this event.
      incrementalPath - specifies true if appending point is relative to the previous point in the path, false if appending point is in the same reference frame as startingPose.
      poses - specifies an array of waypoint poses in the drive path.

    • start

      public void start(boolean incrementalPath, TrcPose2D... poses)
      This method starts the Pure Pursuit drive with the specified poses in the drive path.
      Parameters:
      incrementalPath - specifies true if appending point is relative to the previous point in the path, false if appending point is in the same reference frame as startingPose.
      poses - specifies an array of waypoint poses in the drive path.

    • isActive

      public boolean isActive()
      Checks if the robot is currently following a path.
      Returns:
      True if the pure pursuit controller is active, false otherwise.

    • cancel

      public void cancel(String owner)
      If the controller is currently active, cancel the path following operation. Otherwise, do nothing. If there is an event to signal, mark it as cancelled.
      Parameters:
      owner - specifies the ID string of the caller requesting exclusive access.

    • cancel

      public void cancel()
      If the controller is currently active, cancel the path following operation. Otherwise, do nothing. If there is an event to signal, mark it as cancelled.

    • stop

      private void stop()
      Stops PurePursuit drive.

    • getPath

      public TrcPath getPath()
      This method returns the pure pursuit path.
      Returns:
      path.

    • getXPosition

      private double getXPosition()
      This method is called by xPosPidCtrl only in INVERTED_TARGET mode for getting the X distance to target.
      Returns:
      x distance to target.

    • getYPosition

      private double getYPosition()
      This method is called by yPosPidCtrl only in INVERTED_TARGET mode for getting the Y distance to target.
      Returns:
      y distance to target.

    • getVelocityInput

      private double getVelocityInput()
      This method is called by the Velocity PID controller to get the polar magnitude of the robot's velocity.
      Returns:
      robot's velocity magnitude.

    • driveTask

      private void driveTask(TrcTaskMgr.TaskType taskType, TrcRobot.RunMode runMode, boolean slowPeriodicLoop)
      This task is called periodically to calculate the next target point on the path. The next target point on the path has a distance of followDistance from the current robot position intersecting with the path segment towards the end of the endpoint of the path segment.
      Parameters:
      taskType - specifies the type of task being run.
      runMode - specifies the competition mode that is about to end (e.g. Autonomous, TeleOp, Test).
      slowPeriodicLoop - specifies true if it is running the slow periodic loop on the main robot thread, false otherwise.

    • interpolate

      private double interpolate(double startValue, double endValue, double weight)
      Returns a weighted value between given values.
      Parameters:
      startValue - specifies the start value.
      endValue - specifies the end value.
      weight - specifies the weight between the values.
      Returns:
      weighted value between the given values.

    • interpolate

      private TrcWaypoint interpolate(TrcWaypoint point1, TrcWaypoint point2, double weight, TrcPose2D robotPose)
      Interpolates a waypoint that's weighted between two given waypoints.
      Parameters:
      point1 - specifies the start point of the path segment.
      point2 - specifies the end point of the path segment.
      weight - specifies the weight between the two provided points.
      robotPose - specifies the robot's position, set to null for holonomic drivebase.
      Returns:
      weighted interpolated waypoint.

    • getFollowingPointOnSegment

      private TrcWaypoint getFollowingPointOnSegment(TrcWaypoint startWaypoint, TrcWaypoint endWaypoint, TrcPose2D robotPose)
      This method calculates the waypoint on the path segment that intersects the robot's proximity circle that is closest to the end point of the path segment. The algorithm is based on this article: ...
      Parameters:
      startWaypoint - specifies the start point of the path segment.
      endWaypoint - specifies the end point of the path segment.
      robotPose - specifies the robot's position.
      Returns:
      calculated waypoint.

    • getFollowingPoint

      private TrcWaypoint getFollowingPoint(TrcPose2D robotPose)
      Determines the next target point for Pure Pursuit Drive to follow.
      Parameters:
      robotPose - specifies the robot's location.
      Returns:
      next target point for the robot to follow.