Class BasePoseSubsystem

java.lang.Object
org.wpilib.command2.SubsystemBase
xbot.common.command.BaseSubsystem
xbot.common.subsystems.pose.BasePoseSubsystem
All Implemented Interfaces:
org.wpilib.command2.Subsystem, org.wpilib.telemetry.TelemetryLoggable, TunablePrefixProvider, ISwerveAdvisorPoseSupport
Direct Known Subclasses:
MockBasePoseSubsystem

public abstract class BasePoseSubsystem extends BaseSubsystem implements 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:
    • INCHES_IN_A_METER

      public static final double INCHES_IN_A_METER
      See Also:
    • inherentRioPitch

      protected final org.wpilib.tunable.TunableDouble inherentRioPitch
    • inherentRioRoll

      protected final org.wpilib.tunable.TunableDouble inherentRioRoll
    • previousLeftDistance

      protected double previousLeftDistance
    • previousRightDistance

      protected double previousRightDistance
    • classInstantiationTime

      protected final double classInstantiationTime
    • isNavXReady

      protected boolean isNavXReady
    • rioRotated

      protected org.wpilib.tunable.TunableBoolean rioRotated
    • firstUpdate

      protected boolean firstUpdate
    • lastSetHeadingTime

      protected double lastSetHeadingTime
  • Constructor Details

  • Method Details

    • getCompassHeading

      protected double getCompassHeading(org.wpilib.math.geometry.Rotation2d standardHeading)
    • updateCurrentHeading

      protected void updateCurrentHeading()
    • updateOdometry

      protected void updateOdometry()
    • getCurrentHeadingGyroOnly

      public org.wpilib.math.geometry.Rotation2d getCurrentHeadingGyroOnly()
      Returns:
      Current heading but if the navX is still booting up it will return 0
    • getCurrentHeading

      public org.wpilib.math.geometry.Rotation2d 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 org.wpilib.math.geometry.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 org.wpilib.math.geometry.Rotation2d 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()
    • periodic

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

      public org.wpilib.math.geometry.Pose2d getSimulatedFieldPose()