Package com.flarerobotics.lib.subsystem.vision.command

Class AlignToTagCommand2D

java.lang.Object
edu.wpi.first.wpilibj2.command.Command
com.flarerobotics.lib.subsystem.vision.command.AlignToTagCommand2D
All Implemented Interfaces:
edu.wpi.first.util.sendable.Sendable
public class AlignToTagCommand2D extends edu.wpi.first.wpilibj2.command.Command
Aligns the robot to the given tag from a birdseye 2D view.

Note: Uses Meters and Degrees for PID values.

Example usage: 

 
 Commands.defer(() -> new AlignToTagCommand2D( // Defer to always reconstruct the command
     m_DriveSubsystem::getPose, // Field-relative pose
     m_DriveSubsystem::driveFieldOriented, // Field-oriented drive
     AlignToTagCommand2D.getClosestTagId(m_DriveSubsystem.getPose(), // Get the nearest Reef tag
         6, 7, 8, 9, 10, 11, // Red reef
         17, 18, 19, 20, 21, 22 // Blue reef
     ),
     VisionConstants.kAlignSidewaysOffset, // Offset
     VisionConstants.kAlignReefDistance, // Distance
     0.9, // Chassis length
     new PIDConstants(3), // Translational PID
     new PIDConstants(2, 0.01, 0.007), // Angular PID
     m_DriveSubsystem), Set.of(m_DriveSubsystem) // Command requirements
  );

  • Nested Class Summary

    Nested classes/interfaces inherited from class edu.wpi.first.wpilibj2.command.Command

    edu.wpi.first.wpilibj2.command.Command.InterruptionBehavior

  • Field Summary

    Fields
    Modifier and Type
    Field
    Description
    static double
    kDefaultAngularToleranceDegrees
    The default angular tolerance in meters.
    static double
    kDefaultTranslationalToleranceMeters
    The default translational tolerance in meters.
    static boolean
    kDoDetailedDebug
    Whether details such as the PID error and setpoint should be logged.
    static int
    kErrorCountThreshold
    Error counts threshold for when to stop the command.

  • Constructor Summary

    Constructors
    Constructor
    Description
    AlignToTagCommand2D(Supplier<edu.wpi.first.math.geometry.Pose2d> poseSupplier, Consumer<edu.wpi.first.math.kinematics.ChassisSpeeds> chassisSpeedsConsumer, int tagId, double horizontalOffsetMeters, double alignDistanceMeters, double txOffsetDegrees, double chassisLengthMeters, PIDConstants translationalConstants, PIDConstants angularConstants, edu.wpi.first.wpilibj2.command.Subsystem drive)
    Constructs a new AlignToTagCommand2D.
    AlignToTagCommand2D(Supplier<edu.wpi.first.math.geometry.Pose2d> poseSupplier, Consumer<edu.wpi.first.math.kinematics.ChassisSpeeds> chassisSpeedsConsumer, int tagId, double horizontalOffsetMeters, double alignDistanceMeters, double chassisLengthMeters, PIDConstants translationalConstants, PIDConstants angularConstants, edu.wpi.first.wpilibj2.command.Subsystem drive)
    Constructs a new AlignToTagCommand2D.
    AlignToTagCommand2D(Supplier<edu.wpi.first.math.geometry.Pose2d> poseSupplier, Consumer<edu.wpi.first.math.kinematics.ChassisSpeeds> chassisSpeedsConsumer, Supplier<Integer> tagId, double horizontalOffsetMeters, double alignDistanceMeters, double txOffsetDegrees, double chassisLengthMeters, PIDConstants translationalConstants, PIDConstants angularConstants, edu.wpi.first.wpilibj2.command.Subsystem drive)
    Constructs a new AlignToTagCommand2D.
    AlignToTagCommand2D(Supplier<edu.wpi.first.math.geometry.Pose2d> poseSupplier, Consumer<edu.wpi.first.math.kinematics.ChassisSpeeds> chassisSpeedsConsumer, Supplier<Integer> tagId, double horizontalOffsetMeters, double alignDistanceMeters, double chassisLengthMeters, PIDConstants translationalConstants, PIDConstants angularConstants, edu.wpi.first.wpilibj2.command.Subsystem drive)
    Constructs a new AlignToTagCommand2D.

  • Method Summary

    Modifier and Type
    Method
    Description
    void
    end(boolean interrupted)
     
    void
    execute()
     
    static int
    getClosestTagId(edu.wpi.first.math.geometry.Pose2d robotPose, int... validTagIds)
    Finds the ID of the AprilTag whose pose is closest to the given robotPose.
    void
    initialize()
     
    boolean
    isFinished()
     
    AlignToTagCommand2D
    setAngularTolerance(double toleranceDegrees)
    Sets the tolerance of the angular PID controller.
    AlignToTagCommand2D
    setDoDebug(boolean enabled)
    Sets whether basic debugging is enabled or not.
    AlignToTagCommand2D
    setMaxTimer(double maxSeconds)
    Sets the maximum duration of the timer.
    AlignToTagCommand2D
    setTranslationalTolerance(double toleranceMeters)
    Sets the tolerance of the translational (x, y) PID controllers.

    Methods inherited from class edu.wpi.first.wpilibj2.command.Command

    addRequirements, addRequirements, alongWith, andThen, andThen, asProxy, beforeStarting, beforeStarting, cancel, deadlineFor, deadlineWith, finallyDo, finallyDo, getInterruptionBehavior, getName, getRequirements, getSubsystem, handleInterrupt, hasRequirement, ignoringDisable, initSendable, isScheduled, onlyIf, onlyWhile, raceWith, repeatedly, runsWhenDisabled, schedule, setName, setSubsystem, unless, until, withDeadline, withInterruptBehavior, withName, withTimeout, withTimeout

    Methods inherited from class java.lang.Object

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

  • Field Details

    • kDoDetailedDebug

      public static boolean kDoDetailedDebug
      Whether details such as the PID error and setpoint should be logged.

    • kDefaultTranslationalToleranceMeters

      public static double kDefaultTranslationalToleranceMeters
      The default translational tolerance in meters. Defaults to 0.5 meters.

    • kDefaultAngularToleranceDegrees

      public static double kDefaultAngularToleranceDegrees
      The default angular tolerance in meters. Defaults to 1 degree.

    • kErrorCountThreshold

      public static int kErrorCountThreshold
      Error counts threshold for when to stop the command. If the error count exceeds this value, the command will stop and send a warning via DS.

  • Constructor Details

    • AlignToTagCommand2D

      public AlignToTagCommand2D(Supplier<edu.wpi.first.math.geometry.Pose2d> poseSupplier, Consumer<edu.wpi.first.math.kinematics.ChassisSpeeds> chassisSpeedsConsumer, int tagId, double horizontalOffsetMeters, double alignDistanceMeters, double chassisLengthMeters, PIDConstants translationalConstants, PIDConstants angularConstants, edu.wpi.first.wpilibj2.command.Subsystem drive)
      Constructs a new AlignToTagCommand2D.
      Parameters:
      poseSupplier - The field-relative robot pose supplier.
      chassisSpeedsConsumer - The chassis speeds consumer.
      tagId - The tag ID to align to.
      horizontalOffsetMeters - The horizontal offset from the tag in meters.
      alignDistanceMeters - The perpendicular offset from the tag in meters.
      chassisLengthMeters - The length of the chassis when looked at from the side while aligning in meters.
      translationalConstants - The translational PID constants.
      angularConstants - The angular PID constants.
      drive - The drive subsystem for command requirements.

    • AlignToTagCommand2D

      public AlignToTagCommand2D(Supplier<edu.wpi.first.math.geometry.Pose2d> poseSupplier, Consumer<edu.wpi.first.math.kinematics.ChassisSpeeds> chassisSpeedsConsumer, int tagId, double horizontalOffsetMeters, double alignDistanceMeters, double txOffsetDegrees, double chassisLengthMeters, PIDConstants translationalConstants, PIDConstants angularConstants, edu.wpi.first.wpilibj2.command.Subsystem drive)
      Constructs a new AlignToTagCommand2D.
      Parameters:
      poseSupplier - The field-relative robot pose supplier.
      chassisSpeedsConsumer - The chassis speeds consumer.
      tagId - The tag ID to align to.
      horizontalOffsetMeters - The horizontal offset from the tag in meters.
      alignDistanceMeters - The perpendicular offset from the tag in meters.
      txOffsetDegrees - The angular offset between the tag in degrees. (Right is positive)
      chassisLengthMeters - The length of the chassis when looked at from the side while aligning in meters.
      translationalConstants - The translational PID constants.
      angularConstants - The angular PID constants.
      drive - The drive subsystem for command requirements.

    • AlignToTagCommand2D

      public AlignToTagCommand2D(Supplier<edu.wpi.first.math.geometry.Pose2d> poseSupplier, Consumer<edu.wpi.first.math.kinematics.ChassisSpeeds> chassisSpeedsConsumer, Supplier<Integer> tagId, double horizontalOffsetMeters, double alignDistanceMeters, double chassisLengthMeters, PIDConstants translationalConstants, PIDConstants angularConstants, edu.wpi.first.wpilibj2.command.Subsystem drive)
      Constructs a new AlignToTagCommand2D.
      Parameters:
      poseSupplier - The field-relative robot pose supplier.
      chassisSpeedsConsumer - The chassis speeds consumer.
      tagId - The supplier of the tag ID to align to.
      horizontalOffsetMeters - The horizontal offset from the tag in meters.
      alignDistanceMeters - The perpendicular offset from the tag in meters.
      chassisLengthMeters - The length of the chassis when looked at from the side while aligning in meters.
      translationalConstants - The translational PID constants.
      angularConstants - The angular PID constants.
      drive - The drive subsystem for command requirements.

    • AlignToTagCommand2D

      public AlignToTagCommand2D(Supplier<edu.wpi.first.math.geometry.Pose2d> poseSupplier, Consumer<edu.wpi.first.math.kinematics.ChassisSpeeds> chassisSpeedsConsumer, Supplier<Integer> tagId, double horizontalOffsetMeters, double alignDistanceMeters, double txOffsetDegrees, double chassisLengthMeters, PIDConstants translationalConstants, PIDConstants angularConstants, edu.wpi.first.wpilibj2.command.Subsystem drive)
      Constructs a new AlignToTagCommand2D.
      Parameters:
      poseSupplier - The field-relative robot pose supplier.
      chassisSpeedsConsumer - The chassis speeds consumer.
      tagId - The supplier of the tag ID to align to.
      horizontalOffsetMeters - The horizontal offset from the tag in meters.
      alignDistanceMeters - The perpendicular offset from the tag in meters.
      txOffsetDegrees - The angular offset between the tag in degrees. (Right is positive)
      chassisLengthMeters - The length of the chassis when looked at from the side while aligning in meters.
      translationalConstants - The translational PID constants.
      angularConstants - The angular PID constants.
      drive - The drive subsystem for command requirements.

  • Method Details

    • initialize

      public void initialize()
      Overrides:
      initialize in class edu.wpi.first.wpilibj2.command.Command

    • execute

      public void execute()
      Overrides:
      execute in class edu.wpi.first.wpilibj2.command.Command

    • isFinished

      public boolean isFinished()
      Overrides:
      isFinished in class edu.wpi.first.wpilibj2.command.Command

    • end

      public void end(boolean interrupted)
      Overrides:
      end in class edu.wpi.first.wpilibj2.command.Command

    • setTranslationalTolerance

      public AlignToTagCommand2D setTranslationalTolerance(double toleranceMeters)
      Sets the tolerance of the translational (x, y) PID controllers. Defaults to 0.02 meters = 2 cm.
      Parameters:
      toleranceMeters - The tolerance in meters.
      Returns:
      The instance for chaining.

    • setAngularTolerance

      public AlignToTagCommand2D setAngularTolerance(double toleranceDegrees)
      Sets the tolerance of the angular PID controller. Defaults to 1 degree.
      Parameters:
      toleranceDegrees - The tolerance in degrees.
      Returns:
      The instance for chaining.

    • setMaxTimer

      public AlignToTagCommand2D setMaxTimer(double maxSeconds)
      Sets the maximum duration of the timer. Defaults to 5 seconds.
      Parameters:
      maxSeconds - The maximum duration in seconds.
      Returns:
      The instance for chaining.

    • setDoDebug

      public AlignToTagCommand2D setDoDebug(boolean enabled)
      Sets whether basic debugging is enabled or not. Defaults to true.
      Parameters:
      enabled - True if enabled.
      Returns:
      The instance for chaining.

    • getClosestTagId

      public static int getClosestTagId(edu.wpi.first.math.geometry.Pose2d robotPose, int... validTagIds)
      Finds the ID of the AprilTag whose pose is closest to the given robotPose. If no validTagIds are passed, all tags are considered. Return -1 if no valid tags are found.
      Parameters:
      robotPose - The field-relative robot pose.
      validTagIds - (Nullable) Optional filter of allowed tag IDs.
      Returns:
      The closest tag ID.