Package edu.wpi.first.math.controller

Class LTVUnicycleController

java.lang.Object
edu.wpi.first.math.controller.LTVUnicycleController
public class LTVUnicycleController
extends Object
The linear time-varying unicycle controller has a similar form to the LQR, but the model used to compute the controller gain is the nonlinear model linearized around the drivetrain's current state.

See section 8.9 in Controls Engineering in FRC for a derivation of the control law we used shown in theorem 8.9.1.

  • Constructor Summary

    Constructors 
    Constructor Description
    LTVUnicycleController​(double dt)
    Constructs a linear time-varying unicycle controller with default maximum desired error tolerances of (0.0625 m, 0.125 m, 2 rad) and default maximum desired control effort of (1 m/s, 2 rad/s).
    LTVUnicycleController​(double dt, double maxVelocity)
    Constructs a linear time-varying unicycle controller with default maximum desired error tolerances of (0.0625 m, 0.125 m, 2 rad) and default maximum desired control effort of (1 m/s, 2 rad/s).
    LTVUnicycleController​(Vector<N3> qelems, Vector<N2> relems, double dt)
    Constructs a linear time-varying unicycle controller.
    LTVUnicycleController​(Vector<N3> qelems, Vector<N2> relems, double dt, double maxVelocity)
    Constructs a linear time-varying unicycle controller.

  • Method Summary

    Modifier and Type Method Description
    boolean atReference()
    Returns true if the pose error is within tolerance of the reference.
    ChassisSpeeds calculate​(Pose2d currentPose, Pose2d poseRef, double linearVelocityRef, double angularVelocityRef)
    Returns the linear and angular velocity outputs of the LTV controller.
    ChassisSpeeds calculate​(Pose2d currentPose, Trajectory.State desiredState)
    Returns the next output of the LTV controller.
    void setEnabled​(boolean enabled)
    Enables and disables the controller for troubleshooting purposes.
    void setTolerance​(Pose2d poseTolerance)
    Sets the pose error which is considered tolerable for use with atReference().

    Methods inherited from class java.lang.Object

    clone, equals, finalize, getClass, hashCode, notify, notifyAll, toString, wait, wait, wait

  • Constructor Details

    • LTVUnicycleController

      public LTVUnicycleController​(double dt)
      Constructs a linear time-varying unicycle controller with default maximum desired error tolerances of (0.0625 m, 0.125 m, 2 rad) and default maximum desired control effort of (1 m/s, 2 rad/s).
      Parameters:
      dt - Discretization timestep in seconds.

    • LTVUnicycleController

      public LTVUnicycleController​(double dt, double maxVelocity)
      Constructs a linear time-varying unicycle controller with default maximum desired error tolerances of (0.0625 m, 0.125 m, 2 rad) and default maximum desired control effort of (1 m/s, 2 rad/s).
      Parameters:
      dt - Discretization timestep in seconds.
      maxVelocity - The maximum velocity in meters per second for the controller gain lookup table. The default is 9 m/s.
      Throws:
      IllegalArgumentException - if maxVelocity <= 0.

    • LTVUnicycleController

      public LTVUnicycleController​(Vector<N3> qelems, Vector<N2> relems, double dt)
      Constructs a linear time-varying unicycle controller.
      Parameters:
      qelems - The maximum desired error tolerance for each state.
      relems - The maximum desired control effort for each input.
      dt - Discretization timestep in seconds.

    • LTVUnicycleController

      public LTVUnicycleController​(Vector<N3> qelems, Vector<N2> relems, double dt, double maxVelocity)
      Constructs a linear time-varying unicycle controller.
      Parameters:
      qelems - The maximum desired error tolerance for each state.
      relems - The maximum desired control effort for each input.
      dt - Discretization timestep in seconds.
      maxVelocity - The maximum velocity in meters per second for the controller gain lookup table. The default is 9 m/s.
      Throws:
      IllegalArgumentException - if maxVelocity <= 0.

  • Method Details

    • atReference

      public boolean atReference()
      Returns true if the pose error is within tolerance of the reference.
      Returns:
      True if the pose error is within tolerance of the reference.

    • setTolerance

      public void setTolerance​(Pose2d poseTolerance)
      Sets the pose error which is considered tolerable for use with atReference().
      Parameters:
      poseTolerance - Pose error which is tolerable.

    • calculate

      public ChassisSpeeds calculate​(Pose2d currentPose, Pose2d poseRef, double linearVelocityRef, double angularVelocityRef)
      Returns the linear and angular velocity outputs of the LTV controller.

      The reference pose, linear velocity, and angular velocity should come from a drivetrain trajectory.

      Parameters:
      currentPose - The current pose.
      poseRef - The desired pose.
      linearVelocityRef - The desired linear velocity in meters per second.
      angularVelocityRef - The desired angular velocity in radians per second.
      Returns:
      The linear and angular velocity outputs of the LTV controller.

    • calculate

      public ChassisSpeeds calculate​(Pose2d currentPose, Trajectory.State desiredState)
      Returns the next output of the LTV controller.

      The reference pose, linear velocity, and angular velocity should come from a drivetrain trajectory.

      Parameters:
      currentPose - The current pose.
      desiredState - The desired pose, linear velocity, and angular velocity from a trajectory.
      Returns:
      The linear and angular velocity outputs of the LTV controller.

    • setEnabled

      public void setEnabled​(boolean enabled)
      Enables and disables the controller for troubleshooting purposes.
      Parameters:
      enabled - If the controller is enabled or not.