Class HolonomicDriveController


  • public class HolonomicDriveController
    extends Object
    This holonomic drive controller can be used to follow trajectories using a holonomic drivetrain (i.e. swerve or mecanum). Holonomic trajectory following is a much simpler problem to solve compared to skid-steer style drivetrains because it is possible to individually control forward, sideways, and angular velocity.

    The holonomic drive controller takes in one PID controller for each direction, forward and sideways, and one profiled PID controller for the angular direction. Because the heading dynamics are decoupled from translations, users can specify a custom heading that the drivetrain should point toward. This heading reference is profiled for smoothness.

    • Constructor Detail

      • HolonomicDriveController

        public HolonomicDriveController​(PIDController xController,
                                        PIDController yController,
                                        ProfiledPIDController thetaController)
        Constructs a holonomic drive controller.
        Parameters:
        xController - A PID Controller to respond to error in the field-relative x direction.
        yController - A PID Controller to respond to error in the field-relative y direction.
        thetaController - A profiled PID controller to respond to error in angle.
    • Method Detail

      • 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 tolerance)
        Sets the pose error which is considered tolerance for use with atReference().
        Parameters:
        tolerance - The pose error which is tolerable.
      • calculate

        public ChassisSpeeds calculate​(Pose2d currentPose,
                                       Pose2d poseRef,
                                       double linearVelocityRefMeters,
                                       Rotation2d angleRef)
        Returns the next output of the holonomic drive controller.
        Parameters:
        currentPose - The current pose.
        poseRef - The desired pose.
        linearVelocityRefMeters - The linear velocity reference.
        angleRef - The angular reference.
        Returns:
        The next output of the holonomic drive controller.
      • calculate

        public ChassisSpeeds calculate​(Pose2d currentPose,
                                       Trajectory.State desiredState,
                                       Rotation2d angleRef)
        Returns the next output of the holonomic drive controller.
        Parameters:
        currentPose - The current pose.
        desiredState - The desired trajectory state.
        angleRef - The desired end-angle.
        Returns:
        The next output of the holonomic drive controller.
      • setEnabled

        public void setEnabled​(boolean enabled)
        Enables and disables the controller for troubleshooting problems. When calculate() is called on a disabled controller, only feedforward values are returned.
        Parameters:
        enabled - If the controller is enabled or not.