From cb9f33dbcfa0fd1494c910825a921b1c83fc37ee Mon Sep 17 00:00:00 2001
From: Yishai Levy <96019039+levyishai@users.noreply.github.com>
Date: Thu, 9 Jan 2025 19:26:13 +0200
Subject: [PATCH] initial commit

---
 .../java/frc/trigon/robot/commands/CommandConstants.java     | 4 +++-
 .../robot/poseestimation/apriltagcamera/AprilTagCamera.java  | 5 ++++-
 .../robot/poseestimation/poseestimator/PoseEstimator.java    | 4 ++--
 3 files changed, 9 insertions(+), 4 deletions(-)

diff --git a/src/main/java/frc/trigon/robot/commands/CommandConstants.java b/src/main/java/frc/trigon/robot/commands/CommandConstants.java
index ca0cbe1..f2cc4dc 100644
--- a/src/main/java/frc/trigon/robot/commands/CommandConstants.java
+++ b/src/main/java/frc/trigon/robot/commands/CommandConstants.java
@@ -1,15 +1,18 @@
 package frc.trigon.robot.commands;
 
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.util.Color;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import frc.trigon.robot.RobotContainer;
 import frc.trigon.robot.commands.commandfactories.GeneralCommands;
+import frc.trigon.robot.constants.CameraConstants;
 import frc.trigon.robot.constants.OperatorConstants;
 import frc.trigon.robot.constants.PathPlannerConstants;
 import frc.trigon.robot.subsystems.swerve.SwerveCommands;
+import org.trigon.commands.CameraPositionCalculationCommand;
 import org.trigon.commands.WheelRadiusCharacterizationCommand;
 import org.trigon.hardware.misc.XboxController;
 import org.trigon.hardware.misc.leds.LEDCommands;
@@ -60,7 +63,6 @@ public class CommandConstants {
                     RobotContainer.SWERVE
             );
 
-
     /**
      * Calculates the target drive power from an axis value by dividing it by the shift mode value.
      *
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 959d57f..69456b2 100644
--- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java
+++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java
@@ -63,7 +63,7 @@ public double getLatestResultTimestampSeconds() {
     }
 
     public boolean hasValidResult() {
-        return inputs.hasResult && inputs.poseAmbiguity < AprilTagCameraConstants.MAXIMUM_AMBIGUITY && estimatedRobotPose != null;
+        return inputs.hasResult && inputs.poseAmbiguity < AprilTagCameraConstants.MAXIMUM_AMBIGUITY;
     }
 
     /**
@@ -107,6 +107,9 @@ private double calculateStandardDeviation(double exponent, double distance, int
     }
 
     private Pose2d calculateRobotPose() {
+        if (!hasValidResult())
+            return null;
+
         return cameraPoseToRobotPose(inputs.cameraSolvePNPPose.toPose2d());
     }
 
diff --git a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java
index 2e7ae33..6f5d67c 100644
--- a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java
+++ b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java
@@ -164,9 +164,9 @@ private void putAprilTagsOnFieldWidget() {
     private void logTargetPath() {
         PathPlannerLogging.setLogActivePathCallback((pathPoses) -> {
             field.getObject("path").setPoses(pathPoses);
-            Logger.recordOutput("Path", pathPoses.toArray(new Pose2d[0]));
+            Logger.recordOutput("PathPlanner/Path", pathPoses.toArray(new Pose2d[0]));
         });
-        PathPlannerLogging.setLogTargetPoseCallback((pose) -> Logger.recordOutput("TargetPPPose", pose));
+        PathPlannerLogging.setLogTargetPoseCallback((pose) -> Logger.recordOutput("PathPlanner/TargetPose", pose));
     }
 
     /**