Package xbot.common.subsystems.pose

Class BasePoseSubsystem

java.lang.Object
edu.wpi.first.wpilibj2.command.SubsystemBase
xbot.common.command.BaseSubsystem
xbot.common.subsystems.pose.BasePoseSubsystem
All Implemented Interfaces:
Sendable, Subsystem, DataFrameRefreshable, IPropertySupport, ISwerveAdvisorPoseSupport
Direct Known Subclasses:
MockBasePoseSubsystem
public abstract class BasePoseSubsystem extends BaseSubsystem implements DataFrameRefreshable, ISwerveAdvisorPoseSupport

  • Field Details

    • imu

      public final XGyro imu

    • leftDriveDistance

      protected double leftDriveDistance

    • rightDriveDistance

      protected double rightDriveDistance

    • totalDistanceX

      protected double totalDistanceX

    • totalDistanceY

      protected double totalDistanceY

    • totalDistanceYRobotPerspective

      protected double totalDistanceYRobotPerspective

    • velocityX

      public double velocityX

    • velocityY

      public double velocityY

    • totalVelocity

      protected double totalVelocity

    • headingOffset

      protected double headingOffset

    • FACING_AWAY_FROM_DRIVERS

      public static final double FACING_AWAY_FROM_DRIVERS
      See Also:

    • FACING_TOWARDS_DRIVERS

      public static final double FACING_TOWARDS_DRIVERS
      See Also:

    • INCHES_IN_A_METER

      public static final double INCHES_IN_A_METER
      See Also:

    • inherentRioPitch

      protected final DoubleProperty inherentRioPitch

    • inherentRioRoll

      protected final DoubleProperty inherentRioRoll

    • previousLeftDistance

      protected double previousLeftDistance

    • previousRightDistance

      protected double previousRightDistance

    • classInstantiationTime

      protected final double classInstantiationTime

    • isNavXReady

      protected boolean isNavXReady

    • rioRotated

      protected BooleanProperty rioRotated

    • firstUpdate

      protected boolean firstUpdate

    • lastSetHeadingTime

      protected double lastSetHeadingTime

    • fieldXMidpoint

      public static Distance fieldXMidpoint

    • fieldYHeight

      public static Distance fieldYHeight

  • Constructor Details

  • Method Details

    • getCompassHeading

      protected double getCompassHeading(Rotation2d standardHeading)

    • updateCurrentHeading

      protected void updateCurrentHeading()

    • updateOdometry

      protected void updateOdometry()

    • getCurrentHeadingGyroOnly

      public WrappedRotation2d getCurrentHeadingGyroOnly()
      Returns:
      Current heading but if the navX is still booting up it will return 0

    • getCurrentHeading

      public WrappedRotation2d getCurrentHeading()
      Can be overriden by subclasses to provide a different heading source (e.g. a vision system, pose estimator, etc)
      Specified by:
      getCurrentHeading in interface ISwerveAdvisorPoseSupport
      Returns:
      Current heading but if the navX is still booting up it will return 0

    • getFieldOrientedTotalDistanceTraveled

      public XYPair getFieldOrientedTotalDistanceTraveled()

    • getTravelVector

      protected XYPair getTravelVector()

    • getCurrentFieldPose

      public FieldPose getCurrentFieldPose()

    • getCurrentPose2d

      public Pose2d getCurrentPose2d()
      Specified by:
      getCurrentPose2d in interface ISwerveAdvisorPoseSupport

    • getCurrentVelocity

      public XYPair getCurrentVelocity()

    • getCurrentHeadingAngularVelocity

      public double getCurrentHeadingAngularVelocity()

    • getRobotOrientedTotalDistanceTraveled

      public double getRobotOrientedTotalDistanceTraveled()
      Returns the distance the robot has traveled forward. Rotations are ignored - if you drove forward 100 inches, then turned 180 degrees and drove another 100 inches, this would tell you that you have traveled 200 inches.
      Returns:
      Distance in inches traveled forward from the robot perspective

    • resetDistanceTraveled

      public void resetDistanceTraveled()

    • setCurrentHeading

      public void setCurrentHeading(double headingInDegrees)

    • setCurrentPosition

      public void setCurrentPosition(double newXPosition, double newYPosition)

    • getHeadingResetRecently

      public boolean getHeadingResetRecently()
      Specified by:
      getHeadingResetRecently in interface ISwerveAdvisorPoseSupport

    • updatePose

      protected void updatePose()
      This should be called as often as reasonably possible, to increase accuracy of the "distance traveled" calculation.

      The PoseSubsystem can't directly own positional sensors, so some command will need to feed in the distance values coming from the DriveSubsystem. In order to have accurate calculations, these values need to be in inches, and should never be reset - any resetting should be done here in the PoseSubsystem

    • getLeftDriveDistance

      protected abstract double getLeftDriveDistance()

    • getRightDriveDistance

      protected abstract double getRightDriveDistance()

    • getRobotPitch

      public double getRobotPitch()

    • getRobotRoll

      public double getRobotRoll()

    • getRobotYaw

      protected WrappedRotation2d getRobotYaw()
      If the RoboRIO is mounted in a position other than "flat" (e.g. with the pins facing upward) then this method will need to be overridden.

    • getUntrimmedPitch

      protected double getUntrimmedPitch()

    • getUntrimmedRoll

      protected double getUntrimmedRoll()

    • calibrateInherentRioOrientation

      public void calibrateInherentRioOrientation()

    • getYawAngularVelocity

      public double getYawAngularVelocity()

    • getNavXReady

      public boolean getNavXReady()

    • convertBluetoRed

      public static Pose2d convertBluetoRed(Pose2d blueCoordinates)
      Converts a pose from blue to red alliance, by mirroring across the field midline at an assumed X coordinate.
      Parameters:
      blueCoordinates - Blue Pose2d to convert to Red Pose2d
      Returns:
      Red Pose2d

    • convertBlueToRed

      public static Translation2d convertBlueToRed(Translation2d blueCoordinates)
      Converts a Translation2d from blue to red alliance, by mirroring across the field midline at an assumed X coordinate.
      Parameters:
      blueCoordinates - Blue Translation2d to convert to Red Translation2d
      Returns:
      Red Translation2d

    • convertBlueToRed

      public static Rotation2d convertBlueToRed(Rotation2d blueHeading)
      Converts a Rotation2d from blue to red alliance, by mirroring across the field midline at an assumed X coordinate. Note that this means that in some cases the heading won't change; a heading of 90 degrees will remain 90 degrees, as both are facing the positive Y direction.
      Parameters:
      blueHeading - Blue Rotation2d to convert to Red Rotation2d
      Returns:
      Red Rotation2d

    • convertBlueToRedIfNeeded

      public static Translation2d convertBlueToRedIfNeeded(Translation2d blueCoordinates)
      Converts a Translation2d from blue to red alliance, if and ONLY IF you are currently on the Red alliance.
      Parameters:
      blueCoordinates - Blue Translation2d to possibly convert to Red Translation2d
      Returns:
      Red Translation2d if on Red alliance, otherwise the original Blue Translation2d

    • convertBlueToRedIfNeeded

      public static Pose2d convertBlueToRedIfNeeded(Pose2d blueCoordinates)
      Converts a Pose2d from blue to red alliance, if and ONLY IF you are currently on the Red alliance.
      Parameters:
      blueCoordinates - Blue Pose2d to possibly convert to Red Pose2d
      Returns:
      Red Pose2d if on Red alliance, otherwise the original Blue Pose2d

    • convertBlueToRedIfNeeded

      public static Rotation2d convertBlueToRedIfNeeded(Rotation2d blueHeading)

    • getAlliance

      public static DriverStation.Alliance getAlliance()

    • periodic

      public void periodic()
      Description copied from class: BaseSubsystem
      This method is called on each CommandScheduler loop.
      Specified by:
      periodic in interface Subsystem
      Overrides:
      periodic in class BaseSubsystem

    • refreshDataFrame

      public void refreshDataFrame()
      Description copied from interface: DataFrameRefreshable
      Consumes and processes inputs from the device or subsystem.
      Specified by:
      refreshDataFrame in interface DataFrameRefreshable
      Overrides:
      refreshDataFrame in class BaseSubsystem

    • getSimulatedFieldPose

      public Pose2d getSimulatedFieldPose()