initial commit

Programming-TRIGON · Jan 9, 2025 · cb9f33d · cb9f33d
1 parent 6fa3c00
commit cb9f33d
Show file tree

Hide file tree

Showing 3 changed files with 9 additions and 4 deletions.
diff --git 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
@@ -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
@@ -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));
     }
 
     /**