Package edu.wpi.first.math.controller

Class LTVDifferentialDriveController

java.lang.Object
edu.wpi.first.math.controller.LTVDifferentialDriveController
public class LTVDifferentialDriveController
extends Object
The linear time-varying differential drive 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. We precomputed gains for important places in our state-space, then interpolated between them with a LUT to save computational resources.

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

  • Constructor Details

    • LTVDifferentialDriveController

      public LTVDifferentialDriveController​(LinearSystem<N2,​N2,​N2> plant, double trackwidth, Vector<N5> qelems, Vector<N2> relems, double dt)
      Constructs a linear time-varying differential drive controller.
      Parameters:
      plant - The differential drive velocity plant.
      trackwidth - The distance between the differential drive's left and right wheels in meters.
      qelems - The maximum desired error tolerance for each state.
      relems - The maximum desired control effort for each input.
      dt - Discretization timestep in seconds.
      Throws:
      IllegalArgumentException - if max velocity of plant with 12 V input <= 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, double leftVelocityTolerance, double rightVelocityTolerance)
      Sets the pose error which is considered tolerable for use with atReference().
      Parameters:
      poseTolerance - Pose error which is tolerable.
      leftVelocityTolerance - Left velocity error which is tolerable in meters per second.
      rightVelocityTolerance - Right velocity error which is tolerable in meters per second.

    • calculate

      public DifferentialDriveWheelVoltages calculate​(Pose2d currentPose, double leftVelocity, double rightVelocity, Pose2d poseRef, double leftVelocityRef, double rightVelocityRef)
      Returns the left and right output voltages of the LTV controller.

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

      Parameters:
      currentPose - The current pose.
      leftVelocity - The current left velocity in meters per second.
      rightVelocity - The current right velocity in meters per second.
      poseRef - The desired pose.
      leftVelocityRef - The desired left velocity in meters per second.
      rightVelocityRef - The desired right velocity in meters per second.
      Returns:
      Left and right output voltages of the LTV controller.

    • calculate

      public DifferentialDriveWheelVoltages calculate​(Pose2d currentPose, double leftVelocity, double rightVelocity, Trajectory.State desiredState)
      Returns the left and right output voltages of the LTV controller.

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

      Parameters:
      currentPose - The current pose.
      leftVelocity - The left velocity in meters per second.
      rightVelocity - The right velocity in meters per second.
      desiredState - The desired pose, linear velocity, and angular velocity from a trajectory.
      Returns:
      The left and right output voltages of the LTV controller.