Package frc.robot.localization

Class RobotState

java.lang.Object
frc.robot.localization.RobotState
public class RobotState extends Object
Total state of the robot

  • Constructor Details

    • RobotState

      public RobotState(SwerveModulePosition[] wheelPositions, Rotation2d gyroYaw)
      Creates a new swerve state estimator.
      Parameters:
      wheelPositions - the initial swerve module positions used to seed odometry
      gyroYaw - the initial reported gyro yaw

  • Method Details

    • resetPose

      public void resetPose(Pose2d pose)
      Resets the internal pose estimate to a known field pose.

      This method forces the underlying swerve odometry to the specified pose, effectively redefining the robot's position on the field. It should be used when the robot pose is known with high confidence, such as:

      • At the start of autonomous
      • After a field-aligned reset
      • Following a trusted vision-based localization event

      This method updates only the pose estimator / odometry state owned by RobotState. It does not update any associated simulation state or drivetrain model.

      Most code should prefer Swerve.overridePose(java.util.function.Supplier<edu.wpi.first.math.geometry.Pose2d>) when resetting the robot pose, as that method ensures both the estimator and any simulated drivetrain pose remain consistent.

      Future odometry and vision updates will be applied relative to this new pose.

      Parameters:
      pose - the desired robot pose in field coordinates

    • resetInit

      public void resetInit()

    • addOdometryObservation

      public void addOdometryObservation(SwerveModulePosition[] wheelPositions, Rotation2d gyroYaw, double timestamp)
      Updates odometry and pose estimates using swerve module encoders and an optional gyro measurement.
      Parameters:
      wheelPositions - current swerve module positions
      gyroYaw - current robot yaw, if available
      timestamp - measurement timestamp in seconds

    • updateMeasuredSpeeds

      public void updateMeasuredSpeeds(ChassisSpeeds speeds)
      Updates the robot's current chassis speeds.
      Parameters:
      speeds - the current robot-relative chassis speeds

    • overrideInit

      public void overrideInit(Pose2d pose)
      Forcibly initializes the pose estimator using a known robot pose.
      Parameters:
      pose - the known robot pose in field coordinates to initialize the estimator with

    • getTurretRobotToCamera

      public static Transform3d getTurretRobotToCamera(Transform3d turretToCamera, Rotation2d turretRotation)
      Get robot to camera for camera mounted on the turret

    • getTurretRobotToCamera

      public Optional<Transform3d> getTurretRobotToCamera(Transform3d turretToCamera, double timestamp)
      Get robot to camera for camera mounted on the turret

    • setTurretRawAngle

      public void setTurretRawAngle(double timestamp, Angle angle)
      Set the current turret angle

    • addVisionObservation

      public void addVisionObservation(VisionObservation observations)
      Add potentially asequent observation from camera

    • addVisionObservation

      public boolean addVisionObservation(CameraConstants camera, org.photonvision.targeting.PhotonPipelineResult pipelineResult)
      Add potentially asequent observation from camera

    • getGlobalPoseEstimate

      public Pose2d getGlobalPoseEstimate()
      Returns the current best estimate of the robot's global field pose.
      Returns:
      estimated robot pose in field coordinates

    • getTurretCenterFieldFrame

      public Pose2d getTurretCenterFieldFrame()

    • getFieldRelativeSpeeds

      public ChassisSpeeds getFieldRelativeSpeeds()

    • setTrims

      public void setTrims(double trimUp, double trimLeft)
      Set trim values for autoshooting

    • incTrims

      public void incTrims(double incUp, double incLeft)
      Increment trim values for autoshooting

    • getTrimUp

      public double getTrimUp()

    • getTrimLeft

      public double getTrimLeft()

    • updateTargeting

      public void updateTargeting()
      Update autoshoot target

    • isInitted

      public boolean isInitted()

    • getDesiredFlywheelSpeed

      public double getDesiredFlywheelSpeed()

    • getDesiredHoodAngleDeg

      public double getDesiredHoodAngleDeg()

    • isOkayToShoot

      public boolean isOkayToShoot()

    • getDesiredTurretHeadingFieldRelative

      public Rotation2d getDesiredTurretHeadingFieldRelative()

    • setFlywheelSpeed

      public void setFlywheelSpeed(double flywheelSpeed)

    • limitPosition

      public void limitPosition(Pose2d pose, Consumer<Pose2d> resetPose)
      limits position of a given pose
      Parameters:
      pose - new pose of robot reactangle
      resetPose - reset pose