diff --git a/src/main/java/frc/trigon/lib b/src/main/java/frc/trigon/lib index 6b6c7adb..9403200d 160000 --- a/src/main/java/frc/trigon/lib +++ b/src/main/java/frc/trigon/lib @@ -1 +1 @@ -Subproject commit 6b6c7adbf9d1111fa67fdc0b801e0520044bc7f8 +Subproject commit 9403200d5d1161a2c75be560251618af01230b5a diff --git a/src/main/java/frc/trigon/robot/commands/CommandConstants.java b/src/main/java/frc/trigon/robot/commands/CommandConstants.java index d9d41127..96258ac8 100644 --- a/src/main/java/frc/trigon/robot/commands/CommandConstants.java +++ b/src/main/java/frc/trigon/robot/commands/CommandConstants.java @@ -52,7 +52,7 @@ public class CommandConstants { RobotContainer.SWERVE ), CALCULATE_CAMERA_POSITION_COMMAND = new CameraPositionCalculationCommand( - RobotContainer.ROBOT_POSE_ESTIMATOR::getEstimatedRobotPose, + RobotContainer.ROBOT_POSE_ESTIMATOR::getEstimated2DRobotPose, Rotation2d.fromDegrees(0), (omegaRadiansPerSecond) -> RobotContainer.SWERVE.selfRelativeDrive(new ChassisSpeeds(0, 0, omegaRadiansPerSecond)), RobotContainer.SWERVE diff --git a/src/main/java/frc/trigon/robot/commands/commandclasses/GamePieceAutoDriveCommand.java b/src/main/java/frc/trigon/robot/commands/commandclasses/GamePieceAutoDriveCommand.java index 51976806..0a12f5c4 100644 --- a/src/main/java/frc/trigon/robot/commands/commandclasses/GamePieceAutoDriveCommand.java +++ b/src/main/java/frc/trigon/robot/commands/commandclasses/GamePieceAutoDriveCommand.java @@ -36,7 +36,7 @@ private Command getTrackGamePieceCommand() { } public static Translation2d calculateDistanceFromTrackedGamePiece() { - final Pose2d robotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose(); + final Pose2d robotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimated2DRobotPose(); final Translation2d trackedObjectPositionOnField = OBJECT_POSE_ESTIMATOR.getClosestObjectToRobot(); if (trackedObjectPositionOnField == null) return null; @@ -65,7 +65,7 @@ public static boolean shouldMoveTowardsGamePiece(Translation2d distanceFromTrack } public static FlippableRotation2d calculateTargetAngle() { - final Pose2d robotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose(); + final Pose2d robotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimated2DRobotPose(); final Translation2d trackedObjectFieldRelativePosition = OBJECT_POSE_ESTIMATOR.getClosestObjectToRobot(); if (trackedObjectFieldRelativePosition == null) return null; diff --git a/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java b/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java index fce03ab1..c1e87850 100644 --- a/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java +++ b/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java @@ -112,7 +112,7 @@ private void trackGamePiece() { } private Translation2d calculateSelfRelativeDistanceFromBestGamePiece() { - final Pose2d robotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose(); + final Pose2d robotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimated2DRobotPose(); final Translation2d bestGamePieceFieldRelativePosition = getBestGamePieceFieldRelativePosition(robotPose); if (bestGamePieceFieldRelativePosition == null) diff --git a/src/main/java/frc/trigon/robot/commands/commandclasses/LEDAutoSetupCommand.java b/src/main/java/frc/trigon/robot/commands/commandclasses/LEDAutoSetupCommand.java index aa76f81f..11a78bb1 100644 --- a/src/main/java/frc/trigon/robot/commands/commandclasses/LEDAutoSetupCommand.java +++ b/src/main/java/frc/trigon/robot/commands/commandclasses/LEDAutoSetupCommand.java @@ -50,7 +50,7 @@ private Command getUpdateAutoStartPoseCommand() { } private Translation2d calculateRobotRelativeDifference() { - final Pose2d robotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose(); + final Pose2d robotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimated2DRobotPose(); final Translation2d robotRelativeRobotTranslation = robotPose.getTranslation().minus(this.autoStartPose.getTranslation()); return robotRelativeRobotTranslation.rotateBy(robotPose.getRotation()); } diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java index cbf6158a..db6df46e 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java @@ -2,6 +2,7 @@ import com.pathplanner.lib.commands.PathPlannerAuto; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.*; import frc.trigon.lib.utilities.flippable.FlippablePose2d; @@ -124,7 +125,7 @@ public static Command getResetPoseToAutoPoseCommand(Supplier autoName) { () -> { if (DriverStation.isEnabled()) return; - RobotContainer.ROBOT_POSE_ESTIMATOR.resetPose(getAutoStartPose(autoName.get())); + RobotContainer.ROBOT_POSE_ESTIMATOR.resetPose(new Pose3d(getAutoStartPose(autoName.get()))); } ).ignoringDisable(true); } diff --git a/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java b/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java index 7842e1da..b6fdb5bc 100644 --- a/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java +++ b/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java @@ -9,6 +9,7 @@ import com.pathplanner.lib.pathfinding.Pathfinding; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.CommandScheduler; @@ -63,8 +64,8 @@ public static void init() { private static void configureAutoBuilder() { AutoBuilder.configure( - RobotContainer.ROBOT_POSE_ESTIMATOR::getEstimatedRobotPose, - RobotContainer.ROBOT_POSE_ESTIMATOR::resetPose, + RobotContainer.ROBOT_POSE_ESTIMATOR::getEstimated2DRobotPose, + (resetPose2d) -> RobotContainer.ROBOT_POSE_ESTIMATOR.resetPose(new Pose3d(resetPose2d)), RobotContainer.SWERVE::getSelfRelativeChassisSpeeds, RobotContainer.SWERVE::drivePathPlanner, AUTO_PATH_FOLLOWING_CONTROLLER, diff --git a/src/main/java/frc/trigon/robot/misc/objectdetection/ObjectPoseEstimator.java b/src/main/java/frc/trigon/robot/misc/objectdetection/ObjectPoseEstimator.java index 43141fc0..3e393e7e 100644 --- a/src/main/java/frc/trigon/robot/misc/objectdetection/ObjectPoseEstimator.java +++ b/src/main/java/frc/trigon/robot/misc/objectdetection/ObjectPoseEstimator.java @@ -76,7 +76,7 @@ public void removeClosestObjectToRobot() { * @param intakeTransform the transform of the intake relative to the robot */ public void removeClosestObjectToIntake(Transform2d intakeTransform) { - final Pose2d robotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose(); + final Pose2d robotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimated2DRobotPose(); final Translation2d closestObjectToIntake = getClosestTrackedObjectToPosition(robotPose.transformBy(intakeTransform).getTranslation()); if (closestObjectToIntake == null) return; @@ -129,7 +129,7 @@ public boolean hasObjects() { * @return the closest object to the robot */ public Translation2d getClosestObjectToRobot() { - return getClosestTrackedObjectToPosition(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getTranslation()); + return getClosestTrackedObjectToPosition(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimated2DRobotPose().getTranslation()); } private void updateTrackedObjectsPositions() { diff --git a/src/main/java/frc/trigon/robot/misc/objectdetection/objectdetectioncamera/ObjectDetectionCamera.java b/src/main/java/frc/trigon/robot/misc/objectdetection/objectdetectioncamera/ObjectDetectionCamera.java index 1100fcc6..f9021fc9 100644 --- a/src/main/java/frc/trigon/robot/misc/objectdetection/objectdetectioncamera/ObjectDetectionCamera.java +++ b/src/main/java/frc/trigon/robot/misc/objectdetection/objectdetectioncamera/ObjectDetectionCamera.java @@ -40,7 +40,7 @@ public void periodic() { */ public Translation2d calculateClosestObjectPositionOnField(SimulatedGamePieceConstants.GamePieceType targetGamePiece) { final Translation2d[] targetObjectsTranslation = getObjectPositionsOnField(targetGamePiece); - final Translation2d currentRobotTranslation = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getTranslation(); + final Translation2d currentRobotTranslation = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimated2DRobotPose().getTranslation(); if (targetObjectsTranslation.length == 0) return null; Translation2d closestObjectTranslation = targetObjectsTranslation[0]; @@ -85,7 +85,7 @@ public Rotation3d[] getObjectsRotations(SimulatedGamePieceConstants.GamePieceTyp * @return the object's 2D position on the field (z is assumed to be 0) */ private Translation2d calculateObjectPositionFromRotation(Rotation3d objectRotation) { - final Pose2d robotPoseAtResultTimestamp = RobotContainer.ROBOT_POSE_ESTIMATOR.samplePoseAtTimestamp(objectDetectionCameraInputs.latestResultTimestamp); + final Pose2d robotPoseAtResultTimestamp = RobotContainer.ROBOT_POSE_ESTIMATOR.sample2DPoseAtTimestamp(objectDetectionCameraInputs.latestResultTimestamp); if (robotPoseAtResultTimestamp == null) return new Translation2d(); final Pose3d cameraPose = new Pose3d(robotPoseAtResultTimestamp).plus(robotCenterToCamera); diff --git a/src/main/java/frc/trigon/robot/misc/objectdetection/objectdetectioncamera/io/SimulationObjectDetectionCameraIO.java b/src/main/java/frc/trigon/robot/misc/objectdetection/objectdetectioncamera/io/SimulationObjectDetectionCameraIO.java index 0b285c54..930c5584 100644 --- a/src/main/java/frc/trigon/robot/misc/objectdetection/objectdetectioncamera/io/SimulationObjectDetectionCameraIO.java +++ b/src/main/java/frc/trigon/robot/misc/objectdetection/objectdetectioncamera/io/SimulationObjectDetectionCameraIO.java @@ -28,8 +28,8 @@ public SimulationObjectDetectionCameraIO(String hostname, Transform3d robotCente @Override protected void updateInputs(ObjectDetectionCameraInputsAutoLogged inputs) { - final Pose2d robotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose(); - final Pose3d cameraPose = new Pose3d(robotPose).plus(robotCenterToCamera); + final Pose3d robotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose(); + final Pose3d cameraPose = robotPose.plus(robotCenterToCamera); final ArrayList>[] visibleGamePieces = calculateAllVisibleGamePieces(cameraPose); boolean hasAnyObject = false; diff --git a/src/main/java/frc/trigon/robot/misc/shootingphysics/ShootingCalculations.java b/src/main/java/frc/trigon/robot/misc/shootingphysics/ShootingCalculations.java index 4be30b5c..35c1323e 100644 --- a/src/main/java/frc/trigon/robot/misc/shootingphysics/ShootingCalculations.java +++ b/src/main/java/frc/trigon/robot/misc/shootingphysics/ShootingCalculations.java @@ -32,22 +32,22 @@ public ShootingState getTargetShootingState() { @AutoLogOutput(key = "Shooting/CurrentFuelExitPosition") public Translation3d calculateCurrentFuelExitPose() { - final Pose2d robotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose(); + final Pose3d robotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose(); final Rotation2d hoodPitch = RobotContainer.HOOD.getCurrentAngle(); final Rotation2d turretSelfRelativeYaw = RobotContainer.TURRET.getCurrentSelfRelativeAngle(); return calculateFieldRelativeFuelExitPose(robotPose, hoodPitch, turretSelfRelativeYaw); } public Translation3d calculateTargetFuelExitPosition(double posePredictionTimeSeconds) { - final Pose2d predictedRobotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getPredictedRobotPose(posePredictionTimeSeconds); + final Pose3d predictedRobotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getPredictedRobotPose(posePredictionTimeSeconds); final Rotation2d hoodPitch = RobotContainer.HOOD.getTargetAngle(); final Rotation2d turretSelfRelativeYaw = RobotContainer.TURRET.getTargetSelfRelativeAngle(); return calculateFieldRelativeFuelExitPose(predictedRobotPose, hoodPitch, turretSelfRelativeYaw); } - public Translation3d calculateFieldRelativeFuelExitPose(Pose2d robotPose, Rotation2d hoodPitch, Rotation2d turretSelfRelativeYaw) { + public Translation3d calculateFieldRelativeFuelExitPose(Pose3d robotPose, Rotation2d hoodPitch, Rotation2d turretSelfRelativeYaw) { final Transform3d robotToFuelExitPosition = calculateRobotToFuelExitTransform(hoodPitch, turretSelfRelativeYaw); - return new Pose3d(robotPose).transformBy(robotToFuelExitPosition).getTranslation(); + return robotPose.transformBy(robotToFuelExitPosition).getTranslation(); } private Transform3d calculateRobotToFuelExitTransform(Rotation2d hoodPitch, Rotation2d turretSelfRelativeYaw) { diff --git a/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulatedGamePiece.java b/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulatedGamePiece.java index 3735b342..2c3190fe 100644 --- a/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulatedGamePiece.java +++ b/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulatedGamePiece.java @@ -93,14 +93,14 @@ private boolean isHeld() { } private Rotation2d calculateClosestSpindexerRotationFromCurrentPose() { - final Pose2d robotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose(); + final Pose3d robotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose(); final Pose3d spindexerRobotRelativePose = RobotContainer.SPINDEXER.calculateComponentPose(); final Transform3d spindexerRobotRelativeTransform = new Transform3d( new Pose3d(), spindexerRobotRelativePose ); - final double yOffset = new Pose3d(fieldRelativePosition, new Rotation3d()).relativeTo(new Pose3d(robotPose).plus(spindexerRobotRelativeTransform)).getY(); + final double yOffset = new Pose3d(fieldRelativePosition, new Rotation3d()).relativeTo(robotPose.plus(spindexerRobotRelativeTransform)).getY(); final double fuelTargetOffsetFromSpindexer = SimulatedGamePieceConstants.ROBOT_RELATIVE_HELD_FUEL_OFFSET_FROM_SPINDEXER_METERS.toTranslation2d().getNorm(); final double closestSpindexerRotationRadians = Math.asin(MathUtil.clamp(yOffset / fuelTargetOffsetFromSpindexer, -1, 1)); diff --git a/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationFieldHandler.java b/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationFieldHandler.java index 3a82cb79..b6168972 100644 --- a/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationFieldHandler.java +++ b/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationFieldHandler.java @@ -140,7 +140,7 @@ private static Translation3d calculateHeldFuelFieldRelativePosition(Rotation2d h * @return the field relative pose */ private static Translation3d robotRelativeToFieldRelative(Translation3d robotRelativePose) { - final Pose3d robotPose = new Pose3d(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose()); + final Pose3d robotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose(); return robotPose.plus(new Transform3d(robotRelativePose, new Rotation3d())).getTranslation(); } } \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java index c7689a5c..3a73be9f 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java @@ -15,10 +15,10 @@ public class AprilTagCamera { protected final String name; private final AprilTagCameraInputsAutoLogged inputs = new AprilTagCameraInputsAutoLogged(); - private final Transform2d cameraToRobotCenter; + private final Transform3d cameraToRobotCenter; private final StandardDeviations standardDeviations; private final AprilTagCameraIO aprilTagCameraIO; - private Pose2d estimatedRobotPose = new Pose2d(); + private Pose3d estimatedRobotPose = new Pose3d(); /** * Constructs a new AprilTagCamera. @@ -26,9 +26,7 @@ public class AprilTagCamera { * @param aprilTagCameraType the type of camera * @param name the camera's name * @param robotCenterToCamera the transform of the robot's origin point to the camera. - * only the x, y and yaw values will be used for transforming the camera pose to the robot's center, - * to avoid more inaccuracies like pitch and roll. - * The reset will be used for creating a camera in simulation + * The values will be used for creating a camera in simulation * @param standardDeviations the initial calibrated standard deviations for the camera's estimated pose, * will be changed as the distance from the tag(s) changes and the number of tags changes */ @@ -37,7 +35,7 @@ public AprilTagCamera(AprilTagCameraConstants.AprilTagCameraType aprilTagCameraT StandardDeviations standardDeviations) { this.name = name; this.standardDeviations = standardDeviations; - this.cameraToRobotCenter = toTransform2d(robotCenterToCamera).inverse(); + this.cameraToRobotCenter = robotCenterToCamera.inverse(); aprilTagCameraIO = AprilTagCameraIO.generateIO(aprilTagCameraType, name, robotCenterToCamera); } @@ -50,7 +48,7 @@ public void update() { logCameraInfo(); } - public Pose2d getEstimatedRobotPose() { + public Pose3d getEstimatedRobotPose() { return estimatedRobotPose; } @@ -95,38 +93,38 @@ public StandardDeviations calculateStandardDeviations() { return new StandardDeviations(translationStandardDeviation, thetaStandardDeviation); } - private Pose2d calculateRobotPose() { + private Pose3d calculateRobotPose() { if (!hasValidResult()) return null; return chooseBestRobotPose(); } - private Pose2d chooseBestRobotPose() { + private Pose3d chooseBestRobotPose() { if (!inputs.hasConstrainedResult || isWithinBestTagRangeForAccurateSolvePNPResult()) return chooseBestNormalSolvePNPPose(); - return cameraPoseToRobotPose(inputs.constrainedSolvePNPPose.toPose2d()); + return cameraPoseToRobotPose(inputs.constrainedSolvePNPPose); } - private Pose2d chooseBestNormalSolvePNPPose() { - final Pose2d bestPose = cameraPoseToRobotPose(inputs.bestCameraSolvePNPPose.toPose2d()); + private Pose3d chooseBestNormalSolvePNPPose() { + final Pose3d bestPose = cameraPoseToRobotPose(inputs.bestCameraSolvePNPPose); if (inputs.bestCameraSolvePNPPose.equals(inputs.alternateCameraSolvePNPPose)) return bestPose; if (inputs.alternateCameraSolvePNPPose.getTranslation().toTranslation2d().getDistance(FieldConstants.TAG_ID_TO_POSE.get(inputs.visibleTagIDs[0]).getTranslation().toTranslation2d()) < 0.1 || DriverStation.isDisabled()) return bestPose; - final Pose2d alternatePose = cameraPoseToRobotPose(inputs.alternateCameraSolvePNPPose.toPose2d()); - final Rotation2d robotAngleAtResultTime = RobotContainer.ROBOT_POSE_ESTIMATOR.samplePoseAtTimestamp(inputs.latestResultTimestampSeconds).getRotation(); + final Pose3d alternatePose = cameraPoseToRobotPose(inputs.alternateCameraSolvePNPPose); + final Rotation2d robotAngleAtResultTime = RobotContainer.ROBOT_POSE_ESTIMATOR.sample2DPoseAtTimestamp(inputs.latestResultTimestampSeconds).getRotation(); - final double bestAngleDifference = Math.abs(bestPose.getRotation().minus(robotAngleAtResultTime).getRadians()); - final double alternateAngleDifference = Math.abs(alternatePose.getRotation().minus(robotAngleAtResultTime).getRadians()); + final double bestAngleDifference = Math.abs(bestPose.toPose2d().getRotation().minus(robotAngleAtResultTime).getRadians()); + final double alternateAngleDifference = Math.abs(alternatePose.toPose2d().getRotation().minus(robotAngleAtResultTime).getRadians()); return bestAngleDifference > alternateAngleDifference ? alternatePose : bestPose; } - private Pose2d cameraPoseToRobotPose(Pose2d cameraPose) { + private Pose3d cameraPoseToRobotPose(Pose3d cameraPose) { return cameraPose.transformBy(cameraToRobotCenter); } @@ -148,7 +146,7 @@ private void logCameraInfo() { logUsedTags(); if (hasValidResult()) { - Logger.recordOutput("Poses/Robot/Cameras/" + name + "Pose", new Pose2d[]{estimatedRobotPose}); + Logger.recordOutput("Poses/Robot/Cameras/" + name + "Pose", new Pose3d[]{estimatedRobotPose}); return; } diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java index bd693b84..5b91cada 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java @@ -1,8 +1,9 @@ package frc.trigon.robot.poseestimation.apriltagcamera; -import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform3d; +import frc.trigon.lib.hardware.RobotHardwareStats; import frc.trigon.robot.constants.FieldConstants; import frc.trigon.robot.poseestimation.apriltagcamera.io.AprilTagLimelightIO; import frc.trigon.robot.poseestimation.apriltagcamera.io.AprilTagPhotonCameraIO; @@ -10,7 +11,6 @@ import org.photonvision.PhotonPoseEstimator; import org.photonvision.simulation.SimCameraProperties; import org.photonvision.simulation.VisionSystemSim; -import frc.trigon.lib.hardware.RobotHardwareStats; import java.util.function.BiFunction; @@ -18,7 +18,7 @@ public class AprilTagCameraConstants { static final double MAXIMUM_DISTANCE_FROM_TAG_FOR_ACCURATE_RESULT_METERS = 5, MAXIMUM_DISTANCE_FROM_TAG_FOR_ACCURATE_SOLVE_PNP_RESULT_METERS = 2; - static final Pose2d[] EMPTY_POSE_ARRAY = new Pose2d[0]; + static final Pose3d[] EMPTY_POSE_ARRAY = new Pose3d[0]; static final double MAXIMUM_AMBIGUITY = 0.4; public static final PhotonPoseEstimator.ConstrainedSolvepnpParams CONSTRAINED_SOLVE_PNP_PARAMS = new PhotonPoseEstimator.ConstrainedSolvepnpParams(false, 0.1); diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java index 138968e4..6888160f 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java @@ -139,12 +139,13 @@ private Pose3d calculateConstrainedSolvePNPPose(PhotonPipelineResult result, Pos return null; Pose3d fieldToRobotSeed = bestCameraSolvePNPPose.transformBy(this.robotToCamera.inverse()); - final Rotation2d robotYawAtTimestamp = RobotContainer.ROBOT_POSE_ESTIMATOR.samplePoseAtTimestamp(result.getTimestampSeconds()).getRotation(); + final Rotation2d robotYawAtTimestamp = RobotContainer.ROBOT_POSE_ESTIMATOR.sample2DPoseAtTimestamp(result.getTimestampSeconds()).getRotation(); + final Rotation3d robotYawAtTimestampRotation = new Rotation3d(robotYawAtTimestamp); if (!AprilTagCameraConstants.CONSTRAINED_SOLVE_PNP_PARAMS.headingFree()) { fieldToRobotSeed = new Pose3d( fieldToRobotSeed.getTranslation(), - new Rotation3d(robotYawAtTimestamp) + robotYawAtTimestampRotation ); } diff --git a/src/main/java/frc/trigon/robot/poseestimation/relativerobotposesource/RelativeRobotPoseSource.java b/src/main/java/frc/trigon/robot/poseestimation/relativerobotposesource/RelativeRobotPoseSource.java index 0d1e1b9f..d4d6a3ce 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/relativerobotposesource/RelativeRobotPoseSource.java +++ b/src/main/java/frc/trigon/robot/poseestimation/relativerobotposesource/RelativeRobotPoseSource.java @@ -1,7 +1,7 @@ package frc.trigon.robot.poseestimation.relativerobotposesource; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Transform3d; import org.littletonrobotics.junction.AutoLogOutput; /** @@ -10,12 +10,13 @@ */ public class RelativeRobotPoseSource { private final RelativeRobotPoseSourceInputsAutoLogged inputs = new RelativeRobotPoseSourceInputsAutoLogged(); - private final Transform2d cameraToRobotCenter; + private final Transform3d cameraToRobotCenter; private final RelativeRobotPoseSourceIO relativeRobotPoseSourceIO; private double lastResultTimestampSeconds = 0; - private Pose2d robotPoseAtSyncTime = new Pose2d(); - private Pose2d relativePoseSourceEstimatedPoseAtSyncTime = new Pose2d(); + private Pose3d + robotPoseAtSyncTime = new Pose3d(), + relativePoseSourceEstimatedPoseAtSyncTime = new Pose3d(); /** * Constructs a new RelativeRobotPoseSource. @@ -23,7 +24,7 @@ public class RelativeRobotPoseSource { * @param cameraToRobotCenter the transform of the camera to the robot's origin point * @param hostname the name of the camera in the NetworkTables */ - public RelativeRobotPoseSource(Transform2d cameraToRobotCenter, String hostname) { + public RelativeRobotPoseSource(Transform3d cameraToRobotCenter, String hostname) { this.cameraToRobotCenter = cameraToRobotCenter; this.relativeRobotPoseSourceIO = RelativeRobotPoseSourceIO.generateIO(hostname); } @@ -34,19 +35,19 @@ public void update() { /** * Resets the offset from the relative robot pose source to the robot pose. - * The offset is stored as two {@link Pose2d}s, one for the robot's pose and the other for the relative pose source's pose both at the same timestamp. + * The offset is stored as two {@link Pose3d}s, one for the robot's pose and the other for the relative pose source's pose both at the same timestamp. * With these two poses, we can calculate the total distance moved from the relative pose source's perspective and transform the robot's pose by the same amount. * * @param robotPose the current pose of the robot */ - public void resetOffset(Pose2d robotPose) { + public void resetOffset(Pose3d robotPose) { this.robotPoseAtSyncTime = robotPose; this.relativePoseSourceEstimatedPoseAtSyncTime = getRobotPoseFromCameraPose(inputs.pose); } @AutoLogOutput(key = "RelativeRobotPoseSource/EstimatedRobotPose") - public Pose2d getEstimatedRobotPose() { - final Transform2d movementFromSyncedPose = new Transform2d(relativePoseSourceEstimatedPoseAtSyncTime, getRobotPoseFromCameraPose(inputs.pose)); + public Pose3d getEstimatedRobotPose() { + final Transform3d movementFromSyncedPose = new Transform3d(relativePoseSourceEstimatedPoseAtSyncTime, getRobotPoseFromCameraPose(inputs.pose)); return robotPoseAtSyncTime.transformBy(movementFromSyncedPose); } @@ -66,7 +67,7 @@ private boolean isNewTimestamp() { return true; } - private Pose2d getRobotPoseFromCameraPose(Pose2d cameraPose) { + private Pose3d getRobotPoseFromCameraPose(Pose3d cameraPose) { return cameraPose.transformBy(cameraToRobotCenter); } } diff --git a/src/main/java/frc/trigon/robot/poseestimation/relativerobotposesource/RelativeRobotPoseSourceIO.java b/src/main/java/frc/trigon/robot/poseestimation/relativerobotposesource/RelativeRobotPoseSourceIO.java index 9c30de39..ceadaa6e 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/relativerobotposesource/RelativeRobotPoseSourceIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/relativerobotposesource/RelativeRobotPoseSourceIO.java @@ -1,10 +1,10 @@ package frc.trigon.robot.poseestimation.relativerobotposesource; -import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import frc.trigon.lib.hardware.RobotHardwareStats; import frc.trigon.robot.poseestimation.relativerobotposesource.io.RelativeRobotPoseSourceSimulationIO; import frc.trigon.robot.poseestimation.relativerobotposesource.io.RelativeRobotPoseSourceT265IO; import org.littletonrobotics.junction.AutoLog; -import frc.trigon.lib.hardware.RobotHardwareStats; public class RelativeRobotPoseSourceIO { static RelativeRobotPoseSourceIO generateIO(String hostname) { @@ -22,7 +22,7 @@ protected void updateInputs(RelativeRobotPoseSourceInputsAutoLogged inputs) { public static class RelativeRobotPoseSourceInputs { public int framesPerSecond = 0; public double batteryPercentage = 0; - public Pose2d pose = new Pose2d(); + public Pose3d pose = new Pose3d(); public double resultTimestampSeconds = 0; public boolean hasResult = false; } diff --git a/src/main/java/frc/trigon/robot/poseestimation/relativerobotposesource/io/RelativeRobotPoseSourceT265IO.java b/src/main/java/frc/trigon/robot/poseestimation/relativerobotposesource/io/RelativeRobotPoseSourceT265IO.java index f2c5d224..e4f8ff6f 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/relativerobotposesource/io/RelativeRobotPoseSourceT265IO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/relativerobotposesource/io/RelativeRobotPoseSourceT265IO.java @@ -1,13 +1,13 @@ package frc.trigon.robot.poseestimation.relativerobotposesource.io; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.networktables.NetworkTableInstance; +import frc.trigon.lib.utilities.JsonHandler; import frc.trigon.robot.poseestimation.relativerobotposesource.RelativeRobotPoseSourceIO; import frc.trigon.robot.poseestimation.relativerobotposesource.RelativeRobotPoseSourceInputsAutoLogged; -import frc.trigon.lib.utilities.JsonHandler; public class RelativeRobotPoseSourceT265IO extends RelativeRobotPoseSourceIO { private final NetworkTableEntry jsonDumpEntry; @@ -43,18 +43,18 @@ private T265JsonDump readJsonDump() { return JsonHandler.parseJsonStringToObject(jsonDumpEntry.getString(""), T265JsonDump.class); } - private Pose2d extractPose(T265JsonDump jsonDump) { - final Translation2d translation = extractTranslation(jsonDump); - final Rotation2d heading = extractHeading(jsonDump); - return new Pose2d(translation, heading); + private Pose3d extractPose(T265JsonDump jsonDump) { + final Translation3d translation = extractTranslation(jsonDump); + final Rotation3d rotation = extractHeading(jsonDump); + return new Pose3d(translation, rotation); } - private Translation2d extractTranslation(T265JsonDump jsonDump) { - return new Translation2d(jsonDump.xPositionMeters, jsonDump.yPositionMeters); + private Translation3d extractTranslation(T265JsonDump jsonDump) { + return new Translation3d(jsonDump.xPositionMeters, jsonDump.yPositionMeters, jsonDump.zPositionMeters); } - private Rotation2d extractHeading(T265JsonDump jsonDump) { - return Rotation2d.fromRadians(jsonDump.rotationRadians); + private Rotation3d extractHeading(T265JsonDump jsonDump) { + return new Rotation3d(0, 0, jsonDump.yawRadians); } private static class T265JsonDump { @@ -62,6 +62,7 @@ private static class T265JsonDump { private double batteryPercentage = 0; private double xPositionMeters = 0; private double yPositionMeters = 0; - private double rotationRadians = 0; + private double zPositionMeters = 0; + private double yawRadians = 0; } } \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/poseestimation/robotposeestimator/RobotPoseEstimator.java b/src/main/java/frc/trigon/robot/poseestimation/robotposeestimator/RobotPoseEstimator.java index 84c63c08..06b8d306 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/robotposeestimator/RobotPoseEstimator.java +++ b/src/main/java/frc/trigon/robot/poseestimation/robotposeestimator/RobotPoseEstimator.java @@ -3,12 +3,11 @@ import com.pathplanner.lib.util.PathPlannerLogging; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator3d; +import edu.wpi.first.math.geometry.*; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveOdometry; +import edu.wpi.first.math.kinematics.SwerveDriveOdometry3d; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -30,8 +29,8 @@ * A class that estimates the robot's pose using WPILib's {@link SwerveDrivePoseEstimator} and {@link SwerveDriveOdometry}. */ public class RobotPoseEstimator implements AutoCloseable { - private final SwerveDrivePoseEstimator swerveDrivePoseEstimator = createSwerveDrivePoseEstimator(); - private final SwerveDriveOdometry swerveDriveOdometry = createSwerveDriveOdometry(); + private final SwerveDrivePoseEstimator3d swerveDrivePoseEstimator = createSwerveDrivePoseEstimator(); + private final SwerveDriveOdometry3d swerveDriveOdometry = createSwerveDriveOdometry(); private final Field2d field = new Field2d(); private final AprilTagCamera[] aprilTagCameras; private final RelativeRobotPoseSource relativeRobotPoseSource; @@ -77,11 +76,11 @@ public void periodic() { else updateFromAprilTagCameras(); - field.setRobotPose(getEstimatedRobotPose()); + field.setRobotPose(getEstimated2DRobotPose()); } public void resetHeading() { - final Rotation2d resetRotation = Flippable.isRedAlliance() ? Rotation2d.k180deg : Rotation2d.kZero; + final Rotation3d resetRotation = new Rotation3d(Flippable.isRedAlliance() ? Rotation2d.k180deg : Rotation2d.kZero); swerveDrivePoseEstimator.resetRotation(resetRotation); swerveDriveOdometry.resetRotation(resetRotation); } @@ -91,8 +90,8 @@ public void resetHeading() { * * @param newPose the pose to reset to, relative to the blue alliance's driver station right corner */ - public void resetPose(Pose2d newPose) { - RobotContainer.SWERVE.setHeading(newPose.getRotation()); + public void resetPose(Pose3d newPose) { + RobotContainer.SWERVE.setHeading(newPose.toPose2d().getRotation()); swerveDrivePoseEstimator.resetPose(newPose); swerveDriveOdometry.resetPose(newPose); @@ -108,15 +107,19 @@ public void resetOdometry() { * @return the estimated pose of the robot, relative to the blue alliance's driver station right corner */ @AutoLogOutput(key = "Poses/Robot/PoseEstimator/EstimatedRobotPose") - public Pose2d getEstimatedRobotPose() { + public Pose3d getEstimatedRobotPose() { return swerveDrivePoseEstimator.getEstimatedPosition(); } + public Pose2d getEstimated2DRobotPose() { + return getEstimatedRobotPose().toPose2d(); + } + /** * @return the odometry's estimated pose of the robot, relative to the blue alliance's driver station right corner */ @AutoLogOutput(key = "Poses/Robot/PoseEstimator/EstimatedOdometryPose") - public Pose2d getEstimatedOdometryPose() { + public Pose3d getEstimatedOdometryPose() { return swerveDriveOdometry.getPoseMeters(); } @@ -128,7 +131,7 @@ public Pose2d getEstimatedOdometryPose() { * @param swerveWheelPositions the swerve wheel positions accumulated since the last update * @param gyroRotations the gyro rotations accumulated since the last update */ - public void updatePoseEstimatorOdometry(SwerveModulePosition[][] swerveWheelPositions, Rotation2d[] gyroRotations, double[] timestamps) { + public void updatePoseEstimatorOdometry(SwerveModulePosition[][] swerveWheelPositions, Rotation3d[] gyroRotations, double[] timestamps) { for (int i = 0; i < swerveWheelPositions.length; i++) { swerveDrivePoseEstimator.updateWithTime(timestamps[i], gyroRotations[i], swerveWheelPositions[i]); swerveDriveOdometry.update(gyroRotations[i], swerveWheelPositions[i]); @@ -137,15 +140,26 @@ public void updatePoseEstimatorOdometry(SwerveModulePosition[][] swerveWheelPosi /** * Gets the estimated pose of the robot at the target timestamp. - * Unlike {@link #getPredictedRobotPose} which predicts a future pose, this gets a stored pose from the estimator's buffer. + * Unlike {@link #getPredictedRobotPose(double)} which predicts a future pose, this gets a stored pose from the estimator's buffer. * * @param timestamp the Rio's FPGA timestamp * @return the robot's estimated pose at the timestamp */ - public Pose2d samplePoseAtTimestamp(double timestamp) { + public Pose3d samplePoseAtTimestamp(double timestamp) { return swerveDrivePoseEstimator.sampleAt(timestamp).orElse(null); } + /** + * Gets the estimated two-dimensional pose of the robot at the target timestamp. + * Unlike {@link #getPredictedRobotPose(double)} which predicts a future pose, this gets a stored pose from the estimator's buffer. + * + * @param timestamp the Rio's FPGA timestamp + * @return the robot's estimated pose at the timestamp + */ + public Pose2d sample2DPoseAtTimestamp(double timestamp) { + return swerveDrivePoseEstimator.sampleAt(timestamp).map((Pose3d::toPose2d)).orElse(null); + } + /** * Predicts the robot's pose after the specified time. * Unlike {@link #samplePoseAtTimestamp(double)} which gets a previous pose from the buffer, this predicts the future pose of the robot. @@ -153,12 +167,27 @@ public Pose2d samplePoseAtTimestamp(double timestamp) { * @param seconds the number of seconds into the future to predict the robot's pose for * @return the predicted pose */ - public Pose2d getPredictedRobotPose(double seconds) { + public Pose3d getPredictedRobotPose(double seconds) { final ChassisSpeeds robotVelocity = RobotContainer.SWERVE.getSelfRelativeChassisSpeeds(); final double predictedX = robotVelocity.vxMetersPerSecond * seconds; final double predictedY = robotVelocity.vyMetersPerSecond * seconds; final Rotation2d predictedRotation = Rotation2d.fromRadians(robotVelocity.omegaRadiansPerSecond * seconds); - return getEstimatedRobotPose().transformBy(new Transform2d(predictedX, predictedY, predictedRotation)); + return getEstimatedRobotPose().transformBy(new Transform3d(predictedX, predictedY, 0, new Rotation3d(predictedRotation))); + } + + /** + * Predicts the robot's two-dimensional pose after the specified time. + * Unlike {@link #sample2DPoseAtTimestamp(double)} which gets a previous pose from the buffer, this predicts the future pose of the robot. + * + * @param seconds the number of seconds into the future to predict the robot's pose for + * @return the predicted pose + */ + public Pose2d getPredicted2DRobotPose(double seconds) { + final ChassisSpeeds robotVelocity = RobotContainer.SWERVE.getSelfRelativeChassisSpeeds(); + final double predictedX = robotVelocity.vxMetersPerSecond * seconds; + final double predictedY = robotVelocity.vyMetersPerSecond * seconds; + final Rotation2d predictedRotation = Rotation2d.fromRadians(robotVelocity.omegaRadiansPerSecond * seconds); + return getEstimated2DRobotPose().transformBy(new Transform2d(predictedX, predictedY, predictedRotation)); } private void initialize() { @@ -252,7 +281,7 @@ private void sortCamerasByLatestResultTimestamp(AprilTagCamera[] aprilTagCameras QuickSortHandler.sort(aprilTagCameras, AprilTagCamera::getLatestResultTimestampSeconds); } - private SwerveDriveOdometry createSwerveDriveOdometry() { + private SwerveDriveOdometry3d createSwerveDriveOdometry() { final SwerveModulePosition[] swerveModulePositions = { new SwerveModulePosition(), new SwerveModulePosition(), @@ -260,14 +289,15 @@ private SwerveDriveOdometry createSwerveDriveOdometry() { new SwerveModulePosition() }; - return new SwerveDriveOdometry( + return new SwerveDriveOdometry3d( SwerveConstants.KINEMATICS, - new Rotation2d(), - swerveModulePositions + new Rotation3d(), + swerveModulePositions, + new Pose3d() ); } - private SwerveDrivePoseEstimator createSwerveDrivePoseEstimator() { + private SwerveDrivePoseEstimator3d createSwerveDrivePoseEstimator() { final SwerveModulePosition[] swerveModulePositions = { new SwerveModulePosition(), new SwerveModulePosition(), @@ -275,13 +305,13 @@ private SwerveDrivePoseEstimator createSwerveDrivePoseEstimator() { new SwerveModulePosition() }; - return new SwerveDrivePoseEstimator( + return new SwerveDrivePoseEstimator3d( SwerveConstants.KINEMATICS, - new Rotation2d(), + new Rotation3d(), swerveModulePositions, - new Pose2d(), + new Pose3d(), RobotPoseEstimatorConstants.ODOMETRY_STANDARD_DEVIATIONS.toMatrix(), - VecBuilder.fill(0, 0, 0) + VecBuilder.fill(0, 0, 0, 0) ); } } \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/poseestimation/robotposeestimator/StandardDeviations.java b/src/main/java/frc/trigon/robot/poseestimation/robotposeestimator/StandardDeviations.java index 689e45c0..d55408f8 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/robotposeestimator/StandardDeviations.java +++ b/src/main/java/frc/trigon/robot/poseestimation/robotposeestimator/StandardDeviations.java @@ -3,7 +3,7 @@ import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.math.numbers.N4; /** * Standard Deviations represent how ambiguous an estimated pose is using calibrated gains. @@ -21,8 +21,9 @@ public record StandardDeviations(double translationStandardDeviation, double the public StandardDeviations { } - public Matrix toMatrix() { + public Matrix toMatrix() { return VecBuilder.fill( + translationStandardDeviation, translationStandardDeviation, translationStandardDeviation, thetaStandardDeviation diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java b/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java index 68b6b02b..32b7da27 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java @@ -2,6 +2,7 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveModulePosition; @@ -92,7 +93,7 @@ public Translation2d getFieldRelativeVelocity() { public ChassisSpeeds getFieldRelativeChassisSpeeds() { final ChassisSpeeds selfRelativeSpeeds = getSelfRelativeChassisSpeeds(); - return ChassisSpeeds.fromRobotRelativeSpeeds(selfRelativeSpeeds, RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getRotation()); + return ChassisSpeeds.fromRobotRelativeSpeeds(selfRelativeSpeeds, getPoseEstimatorHeading()); } public Translation2d getSelfRelativeVelocity() { @@ -121,16 +122,16 @@ public boolean atPose(FlippablePose2d flippablePose2d) { public boolean atXAxisPosition(double xAxisPosition) { final double currentXAxisVelocity = getFieldRelativeChassisSpeeds().vxMetersPerSecond; - return atTranslationPosition(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getX(), xAxisPosition, currentXAxisVelocity); + return atTranslationPosition(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimated2DRobotPose().getX(), xAxisPosition, currentXAxisVelocity); } public boolean atYAxisPosition(double yAxisPosition) { final double currentYAxisVelocity = getFieldRelativeChassisSpeeds().vyMetersPerSecond; - return atTranslationPosition(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getY(), yAxisPosition, currentYAxisVelocity); + return atTranslationPosition(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimated2DRobotPose().getY(), yAxisPosition, currentYAxisVelocity); } public boolean atAngle(FlippableRotation2d angle) { - final boolean atTargetAngle = Math.abs(angle.get().minus(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getRotation()).getDegrees()) < SwerveConstants.ROTATION_TOLERANCE_DEGREES; + final boolean atTargetAngle = Math.abs(angle.get().minus(getPoseEstimatorHeading()).getDegrees()) < SwerveConstants.ROTATION_TOLERANCE_DEGREES; final boolean isAngleStill = Math.abs(getSelfRelativeChassisSpeeds().omegaRadiansPerSecond) < SwerveConstants.ROTATION_VELOCITY_TOLERANCE; Logger.recordOutput("Swerve/AtTargetAngle/atTargetAngle", atTargetAngle); Logger.recordOutput("Swerve/AtTargetAngle/isStill", isAngleStill); @@ -177,7 +178,7 @@ public void selfRelativeDrive(ChassisSpeeds targetSpeeds) { } public Rotation2d getDriveRelativeAngle() { - final Rotation2d currentAngle = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getRotation(); + final Rotation2d currentAngle = getPoseEstimatorHeading(); return Flippable.isRedAlliance() ? currentAngle.rotateBy(Rotation2d.k180deg) : currentAngle; } @@ -187,7 +188,7 @@ public void initializeDrive(boolean shouldUseClosedLoop) { } void resetRotationController() { - SwerveConstants.PROFILED_ROTATION_PID_CONTROLLER_DEGREES.reset(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getRotation().getDegrees()); + SwerveConstants.PROFILED_ROTATION_PID_CONTROLLER_DEGREES.reset(getPoseEstimatorHeading().getDegrees()); } /** @@ -250,6 +251,10 @@ void selfRelativeDrive(double xPower, double yPower, FlippableRotation2d targetA selfRelativeDrive(speeds); } + private Rotation2d getPoseEstimatorHeading() { + return RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimated2DRobotPose().getRotation(); + } + private void setTargetModuleStates(SwerveModuleState[] swerveModuleStates) { for (int i = 0; i < swerveModules.length; i++) swerveModules[i].setTargetState(swerveModuleStates[i]); @@ -281,7 +286,7 @@ private ChassisSpeeds powersToSpeeds(double xPower, double yPower, double thetaP } private ChassisSpeeds calculateSelfRelativePIDSpeedsToPose(FlippablePose2d targetPose) { - final Pose2d currentPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getPredictedRobotPose(SwerveConstants.PID_TO_POSE_PREDICTION_TIME_SECONDS); + final Pose2d currentPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getPredicted2DRobotPose(SwerveConstants.PID_TO_POSE_PREDICTION_TIME_SECONDS); final Pose2d flippedTargetPose = targetPose.get(); final double xSpeed = SwerveConstants.X_TRANSLATION_PID_CONTROLLER.atSetpoint() ? @@ -302,14 +307,23 @@ private ChassisSpeeds calculateSelfRelativePIDSpeedsToPose(FlippablePose2d targe } private void updatePoseEstimatorStates() { - final double[] odometryUpdatesYawDegrees = gyro.getThreadedSignal(Pigeon2Signal.YAW); - final int totalOdometryUpdates = odometryUpdatesYawDegrees.length; + final double[][] odometryUpdatesDegrees = new double[][]{ + gyro.getThreadedSignal(Pigeon2Signal.ROLL), + gyro.getThreadedSignal(Pigeon2Signal.PITCH), + gyro.getThreadedSignal(Pigeon2Signal.YAW) + }; + final int totalOdometryUpdates = odometryUpdatesDegrees[0].length; + final SwerveModulePosition[][] swerveWheelPositions = new SwerveModulePosition[totalOdometryUpdates][]; - final Rotation2d[] gyroRotations = new Rotation2d[totalOdometryUpdates]; + final Rotation3d[] gyroRotations = new Rotation3d[totalOdometryUpdates]; for (int i = 0; i < totalOdometryUpdates; i++) { swerveWheelPositions[i] = getSwerveWheelPositions(i); - gyroRotations[i] = Rotation2d.fromDegrees(odometryUpdatesYawDegrees[i]); + gyroRotations[i] = new Rotation3d( + Units.degreesToRadians(odometryUpdatesDegrees[0][i]), + Units.degreesToRadians(odometryUpdatesDegrees[1][i]), + Units.degreesToRadians(odometryUpdatesDegrees[2][i]) + ); } RobotContainer.ROBOT_POSE_ESTIMATOR.updatePoseEstimatorOdometry(swerveWheelPositions, gyroRotations, phoenix6SignalThread.getLatestTimestamps()); @@ -332,7 +346,7 @@ private double calculateProfiledAngularVelocityRadiansPerSecond(FlippableRotatio return 0; SwerveConstants.PROFILED_ROTATION_PID_CONTROLLER_DEGREES.setGoal(targetAngle.get().getDegrees()); - final Rotation2d currentAngle = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getRotation(); + final Rotation2d currentAngle = getPoseEstimatorHeading(); final double outputSpeedRadians = Units.degreesToRadians(SwerveConstants.PROFILED_ROTATION_PID_CONTROLLER_DEGREES.calculate(currentAngle.getDegrees())); final boolean atGoal = SwerveConstants.PROFILED_ROTATION_PID_CONTROLLER_DEGREES.atGoal(); Logger.recordOutput("Swerve/ProfiledRotationPIDController/Setpoint", SwerveConstants.PROFILED_ROTATION_PID_CONTROLLER_DEGREES.getSetpoint().position); diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java index 39093736..ffc07e9c 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java @@ -109,6 +109,8 @@ private static void configureGyro() { GYRO.applyConfiguration(config); GYRO.setSimulationYawVelocitySupplier(() -> RobotContainer.SWERVE.getRotationalVelocityRadiansPerSecond());//IMPORTANT: Leave as lambda expression, method reference will crash code + GYRO.registerThreadedSignal(Pigeon2Signal.ROLL, RobotPoseEstimatorConstants.ODOMETRY_FREQUENCY_HERTZ); + GYRO.registerThreadedSignal(Pigeon2Signal.PITCH, RobotPoseEstimatorConstants.ODOMETRY_FREQUENCY_HERTZ); GYRO.registerThreadedSignal(Pigeon2Signal.YAW, RobotPoseEstimatorConstants.ODOMETRY_FREQUENCY_HERTZ); } diff --git a/src/main/java/frc/trigon/robot/subsystems/turret/Turret.java b/src/main/java/frc/trigon/robot/subsystems/turret/Turret.java index 2a52ef04..65b19209 100644 --- a/src/main/java/frc/trigon/robot/subsystems/turret/Turret.java +++ b/src/main/java/frc/trigon/robot/subsystems/turret/Turret.java @@ -82,6 +82,7 @@ public void updateMechanism() { @Override public void stop() { masterMotor.stopMotor(); + targetSelfRelativeAngle = new Rotation2d(); } public Pose3d calculateVisualizationPose() { @@ -93,7 +94,7 @@ public Pose3d calculateVisualizationPose() { } public Rotation2d getTargetFieldRelativeAngle() { - return targetSelfRelativeAngle.plus(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getRotation()); + return targetSelfRelativeAngle.plus(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimated2DRobotPose().getRotation()); } public Rotation2d getTargetSelfRelativeAngle() { @@ -101,7 +102,7 @@ public Rotation2d getTargetSelfRelativeAngle() { } public Rotation2d getCurrentFieldRelativeAngle() { - return getCurrentSelfRelativeAngle().plus(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getRotation()); + return getCurrentSelfRelativeAngle().plus(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimated2DRobotPose().getRotation()); } public Rotation2d getCurrentSelfRelativeAngle() { @@ -123,7 +124,7 @@ void alignForDelivery() { } void setTargetFieldRelativeAngle(Rotation2d targetAngle) { - final Rotation2d targetRobotRelativeAngle = Rotation2d.fromDegrees(targetAngle.getDegrees() - RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getRotation().getDegrees()); + final Rotation2d targetRobotRelativeAngle = targetAngle.minus(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimated2DRobotPose().getRotation()); setTargetSelfRelativeAngle(targetRobotRelativeAngle); } @@ -143,7 +144,7 @@ private double calculateResistSwerveRotationFeedforward() { } private Rotation2d calculateTargetAngleForDelivery() { - final Pose2d currentPosition = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose(); + final Pose2d currentPosition = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimated2DRobotPose(); if (currentPosition.getTranslation().getDistance(FieldConstants.LEFT_DELIVERY_POSITION.get()) < currentPosition.getTranslation().getDistance(FieldConstants.RIGHT_DELIVERY_POSITION.get())) return calculateTargetAngleToPose(FieldConstants.LEFT_DELIVERY_POSITION.get(), currentPosition); return calculateTargetAngleToPose(FieldConstants.RIGHT_DELIVERY_POSITION.get(), currentPosition);