Logic fixes + cleaning for sim cams

Author: Strflightmight09

src/main/java/frc/trigon/robot/commands/LEDAutoSetupCommand.java

@@ -23,7 +23,7 @@ public class LEDAutoSetupCommand extends SequentialCommandGroup {
             TOLERANCE_METERS = 0.1,
             TOLERANCE_DEGREES = 2;
     private static final int AMOUNT_OF_SECTIONS = 3;
-    private final Supplier<Color>[] LEDColors = new Supplier[] {
+    private final Supplier<Color>[] LEDColors = new Supplier[]{
             () -> getDesiredLEDColorFromRobotPose(this.autoStartPose.getRotation().getDegrees() - getCurrentRobotPose().getRotation().getDegrees(), TOLERANCE_DEGREES),
             () -> getDesiredLEDColorFromRobotPose(this.autoStartPose.getX() - getCurrentRobotPose().getX(), TOLERANCE_METERS),
             () -> getDesiredLEDColorFromRobotPose(this.autoStartPose.getY() - getCurrentRobotPose().getY(), TOLERANCE_METERS)

src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java

@@ -57,14 +57,11 @@ public void update() {
         aprilTagCameraIO.updateInputs(inputs);
 
         robotPose = calculateBestRobotPose();
-
         logCameraInfo();
-        if (RobotHardwareStats.isSimulation())
-            AprilTagCameraConstants.VISION_SIMULATION.update(RobotContainer.POSE_ESTIMATOR.getCurrentOdometryPose());
     }
 
     public boolean hasNewResult() {
-        return (inputs.hasResult && inputs.distanceFromBestTag != 0) && isNewTimestamp();
+        return (inputs.hasResult && inputs.distanceFromBestTag != Double.POSITIVE_INFINITY) && isNewTimestamp();
     }
 
     public Pose2d getEstimatedRobotPose() {
@@ -109,7 +106,7 @@ public double getDistanceToBestTagMeters() {
      * @return the robot's pose
      */
     private Pose2d calculateBestRobotPose() {
-        final Rotation2d gyroHeadingAtTimestamp =  RobotHardwareStats.isSimulation() ? RobotContainer.SWERVE.getHeading() : PoseEstimator6328.getInstance().samplePose(inputs.latestResultTimestampSeconds).getRotation();
+        final Rotation2d gyroHeadingAtTimestamp = RobotHardwareStats.isSimulation() ? RobotContainer.POSE_ESTIMATOR.getCurrentOdometryPose().getRotation() : PoseEstimator6328.getInstance().samplePose(inputs.latestResultTimestampSeconds).getRotation();
         return calculateAssumedRobotHeadingPose(gyroHeadingAtTimestamp);
     }
 
@@ -134,7 +131,7 @@ private Translation2d getFieldRelativeRobotTranslation(Rotation2d currentHeading
         if (bestTagPose == null)
             return null;
 
-        setProperCameraRotation();
+        correctTargetRelativeRotation();
 
         final Translation2d tagRelativeCameraTranslation = calculateTagRelativeCameraTranslation(currentHeading, bestTagPose);
         final Translation2d fieldRelativeRobotPose = getFieldRelativeRobotPose(tagRelativeCameraTranslation, bestTagPose);
@@ -146,7 +143,7 @@ private Translation2d getFieldRelativeRobotTranslation(Rotation2d currentHeading
      * When the roll of the camera changes, the target pitch and yaw are also affected.
      * This method corrects the yaw and pitch based on the camera's mount position roll.
      */
-    private void setProperCameraRotation() {
+    private void correctTargetRelativeRotation() {
         final Translation2d targetRotation = new Translation2d(inputs.bestTargetRelativePitchRadians, inputs.bestTargetRelativeYawRadians);
         targetRotation.rotateBy(Rotation2d.fromRadians(robotCenterToCamera.getRotation().getX()));
         inputs.bestTargetRelativePitchRadians = targetRotation.getX();
@@ -214,7 +211,7 @@ private void logCameraInfo() {
         Logger.processInputs("Cameras/" + name, inputs);
         if (!FieldConstants.TAG_ID_TO_POSE.isEmpty())
             logUsedTags();
-        if (!inputs.hasResult || inputs.distanceFromBestTag == 0 || robotPose == null) {
+        if (!inputs.hasResult || inputs.distanceFromBestTag == Double.POSITIVE_INFINITY || robotPose == null) {
             logEstimatedRobotPose();
             logSolvePNPPose();
         } else {

src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java

@@ -17,7 +17,7 @@ public class AprilTagCameraConstants {
     static final int CALCULATE_YAW_ITERATIONS = 3;
     static final Pose2d[] EMPTY_POSE_LIST = new Pose2d[0];
 
-    public static final double MAXIMUM_AMBIGUITY = 0.5;
+    public static final double MAXIMUM_AMBIGUITY = 0.4;
 
     public static final VisionSystemSim VISION_SIMULATION = new VisionSystemSim("VisionSimulation");
     private static final int

src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java

@@ -10,7 +10,9 @@
 import frc.trigon.robot.RobotContainer;
 import frc.trigon.robot.constants.FieldConstants;
 import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCamera;
+import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraConstants;
 import org.littletonrobotics.junction.Logger;
+import org.trigon.hardware.RobotHardwareStats;
 
 import java.util.ArrayList;
 import java.util.Comparator;
@@ -108,6 +110,8 @@ private void updateFromVision() {
         getViableVisionObservations().stream()
                 .sorted(Comparator.comparingDouble(PoseEstimator6328.VisionObservation::timestamp))
                 .forEach(poseEstimator6328::addVisionObservation);
+        if (RobotHardwareStats.isSimulation())
+            AprilTagCameraConstants.VISION_SIMULATION.update(RobotContainer.POSE_ESTIMATOR.getCurrentOdometryPose());
     }
 
     private List<PoseEstimator6328.VisionObservation> getViableVisionObservations() {

src/main/java/frc/trigon/robot/subsystems/swerve/SwerveModule.java

@@ -178,13 +178,14 @@ private void configureSignals() {
         driveMotor.registerSignal(TalonFXSignal.TORQUE_CURRENT, 100);
         driveMotor.registerSignal(TalonFXSignal.MOTOR_VOLTAGE, 100);
 
+        driveMotor.registerThreadedSignal(TalonFXSignal.POSITION, PoseEstimatorConstants.ODOMETRY_FREQUENCY_HERTZ);
+
         steerMotor.registerSignal(TalonFXSignal.VELOCITY, 100);
         steerMotor.registerSignal(TalonFXSignal.MOTOR_VOLTAGE, 100);
 
+        steerMotor.registerThreadedSignal(TalonFXSignal.POSITION, PoseEstimatorConstants.ODOMETRY_FREQUENCY_HERTZ);
+
         steerEncoder.registerSignal(CANcoderSignal.POSITION, 100);
         steerEncoder.registerSignal(CANcoderSignal.VELOCITY, 100);
-        
-        driveMotor.registerThreadedSignal(TalonFXSignal.POSITION, PoseEstimatorConstants.ODOMETRY_FREQUENCY_HERTZ);
-        steerMotor.registerThreadedSignal(TalonFXSignal.POSITION, PoseEstimatorConstants.ODOMETRY_FREQUENCY_HERTZ);
     }
 }