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
-
Field Summary
FieldsModifier and TypeFieldDescriptionprotected final doublestatic final doubleprotected booleanprotected doublefinal XGyrostatic final doubleprotected final org.wpilib.tunable.TunableDoubleprotected final org.wpilib.tunable.TunableDoubleprotected booleanprotected doubleprotected doubleprotected doubleprotected doubleprotected doubleprotected org.wpilib.tunable.TunableBooleanprotected doubleprotected doubleprotected doubleprotected doubledoubledoubleFields inherited from class BaseSubsystem
aKitLog, log -
Constructor Summary
ConstructorsConstructorDescriptionBasePoseSubsystem(XGyro.XGyroFactory gyroFactory, TunableFactory tunableFactory) BasePoseSubsystem(XGyro gyro, TunableFactory tunableFactory) -
Method Summary
Modifier and TypeMethodDescriptionvoidprotected doublegetCompassHeading(org.wpilib.math.geometry.Rotation2d standardHeading) org.wpilib.math.geometry.Rotation2dCan be overriden by subclasses to provide a different heading source (e.g.doubleorg.wpilib.math.geometry.Rotation2dorg.wpilib.math.geometry.Pose2dbooleanprotected abstract doublebooleanprotected abstract doubledoubleReturns the distance the robot has traveled forward.doubledoubleprotected org.wpilib.math.geometry.Rotation2dIf the RoboRIO is mounted in a position other than "flat" (e.g.org.wpilib.math.geometry.Pose2dprotected XYPairprotected doubleprotected doubledoublevoidperiodic()This method is called on eachCommandSchedulerloop.voidvoidsetCurrentHeading(double headingInDegrees) voidsetCurrentPosition(double newXPosition, double newYPosition) protected voidprotected voidprotected voidThis should be called as often as reasonably possible, to increase accuracy of the "distance traveled" calculation.Methods inherited from class BaseSubsystem
getPrefixMethods inherited from class org.wpilib.command2.SubsystemBase
getName, getTelemetryType, logTo, setNameMethods inherited from class Object
clone, equals, finalize, getClass, hashCode, notify, notifyAll, toString, wait, wait, waitMethods inherited from interface org.wpilib.command2.Subsystem
defer, getCurrentCommand, getDefaultCommand, idle, register, removeDefaultCommand, run, runEnd, runOnce, setDefaultCommand, simulationPeriodic, startEnd, startRun
-
Field Details
-
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 -
rioRotated
protected org.wpilib.tunable.TunableBoolean rioRotated -
firstUpdate
protected boolean firstUpdate -
lastSetHeadingTime
protected double lastSetHeadingTime
-
-
Constructor Details
-
BasePoseSubsystem
-
BasePoseSubsystem
-
-
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:
getCurrentHeadingin interfaceISwerveAdvisorPoseSupport- Returns:
- Current heading but if the navX is still booting up it will return 0
-
getFieldOrientedTotalDistanceTraveled
-
getTravelVector
-
getCurrentFieldPose
-
getCurrentPose2d
public org.wpilib.math.geometry.Pose2d getCurrentPose2d()- Specified by:
getCurrentPose2din interfaceISwerveAdvisorPoseSupport
-
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:
getHeadingResetRecentlyin interfaceISwerveAdvisorPoseSupport
-
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() -
periodic
public void periodic()Description copied from class:BaseSubsystemThis method is called on eachCommandSchedulerloop.- Specified by:
periodicin interfaceorg.wpilib.command2.Subsystem- Overrides:
periodicin classBaseSubsystem
-
getSimulatedFieldPose
public org.wpilib.math.geometry.Pose2d getSimulatedFieldPose()
-