made code not crash

Author: Nummun14

src/main/java/frc/trigon/robot/RobotContainer.java

@@ -73,6 +73,7 @@ private void bindDefaultCommands() {
     }
 
     private void bindControllerCommands() {
+        OperatorConstants.OPERATOR_CONTROLLER.semicolon().whileTrue(ShootingCommands.getShootAtTagCommand());
         OperatorConstants.RESET_HEADING_TRIGGER.onTrue(CommandConstants.RESET_HEADING_COMMAND);
         OperatorConstants.SET_GYRO_HEADING_TO_SOLVE_PNP_HEADING_TRIGGER.onTrue(CommandConstants.SET_GYRO_HEADING_TO_SOLVE_PNP_HEADING_COMMAND);
         OperatorConstants.DRIVE_FROM_DPAD_TRIGGER.whileTrue(CommandConstants.SELF_RELATIVE_DRIVE_FROM_DPAD_COMMAND);

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

@@ -44,7 +44,7 @@ private Command getAlignToTagCommand() {
     private Command getDriveWhileAligningToNoteCommand() {
         return SwerveCommands.getClosedLoopSelfRelativeDriveCommand(
                 () -> fieldRelativePowersToSelfRelativeXPower(OperatorConstants.DRIVER_CONTROLLER.getLeftY(), OperatorConstants.DRIVER_CONTROLLER.getLeftX()),
-                () -> -PID_CONTROLLER.calculate(RobotContainer.POSE_ESTIMATOR.getBestTagYawRelativeToRobot().getDegrees()),
+                () -> -PID_CONTROLLER.calculate(ShootingCalculations.getInstance().getAngleToTarget(new Translation2d(), RobotContainer.ROBOT_SHOWCASE.getBestTagTranslationRelativeToRobot()).get().getDegrees()),
                 this::getTargetAngle
         );
     }

src/main/java/frc/trigon/robot/commands/factories/ShootingCommands.java

@@ -20,28 +20,12 @@ public class ShootingCommands {
     private static final ShootingCalculations SHOOTING_CALCULATIONS = ShootingCalculations.getInstance();
 
     public static Command getShootAtTagCommand() {
-        return new ParallelCommandGroup(
+        return new ParallelRaceGroup(
                 getPrepareForShootingAtTargetCommand(),
                 getFeedNoteForShootingCommand()
         );
     }
 
-    private static Command getPrepareForShootingAtTargetCommand() {
-        return new ParallelCommandGroup(
-                PitcherCommands.getReachTargetPitchFromShootingCalculationsCommand(),
-                ShooterCommands.getReachTargetShootingVelocityFromShootingCalculationsCommand(),
-                SwerveCommands.getClosedLoopFieldRelativeDriveCommand(
-                        () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftY()),
-                        () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftX()),
-                        () -> SHOOTING_CALCULATIONS.getTargetShootingState().targetRobotAngle()
-                )
-        );
-    }
-
-    private static Command getUpdateShootingCalculationsCommandForShootingTarget() {
-        return new RunCommand(() -> SHOOTING_CALCULATIONS.updateCalculationsForShot(RobotContainer.ROBOT_SHOWCASE.getBestTagTranslationRelativeToRobot()));
-    }
-
     /**
      * Creates a command that adjusts the shooting mechanism to aim at a target (either delivery target or speaker target), and feeds the note once the shooting mechanism is ready to shoot (all setpoints were reached).
      *
@@ -88,6 +72,23 @@ static Command getUpdateShootingCalculationsCommand(boolean isDelivery) {
         return new RunCommand(isDelivery ? SHOOTING_CALCULATIONS::updateCalculationsForDelivery : SHOOTING_CALCULATIONS::updateCalculationsForSpeakerShot);
     }
 
+    private static Command getPrepareForShootingAtTargetCommand() {
+        return new ParallelCommandGroup(
+                getUpdateShootingCalculationsCommandForShootingTarget(),
+                PitcherCommands.getReachTargetPitchFromShootingCalculationsCommand(),
+                ShooterCommands.getReachTargetShootingVelocityFromShootingCalculationsCommand(),
+                SwerveCommands.getClosedLoopFieldRelativeDriveCommand(
+                        () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftY()),
+                        () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftX()),
+                        () -> SHOOTING_CALCULATIONS.getTargetShootingState().targetRobotAngle()
+                )
+        );
+    }
+
+    private static Command getUpdateShootingCalculationsCommandForShootingTarget() {
+        return new RunCommand(() -> SHOOTING_CALCULATIONS.updateCalculationsForShot(RobotContainer.ROBOT_SHOWCASE.getBestTagTranslationRelativeToRobot()));
+    }
+
     private static Command getPrepareForShootingCommand(boolean isDelivery) {
         return new ParallelCommandGroup(
                 getUpdateShootingCalculationsCommand(isDelivery),

src/main/java/frc/trigon/robot/constants/CameraConstants.java

@@ -1,5 +1,6 @@
 package frc.trigon.robot.constants;
 
+import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.geometry.Translation3d;
@@ -23,6 +24,15 @@ public class CameraConstants {
                     new Translation3d(-0.325 + 0.00975, 0, 0.095),
                     new Rotation3d(Math.PI - Units.degreesToRadians(0), Units.degreesToRadians(-33), Math.PI + Units.degreesToRadians(0))
             );
+    public static final Pose3d
+            FRONT_CENTER_TO_CAMERA_POSE = new Pose3d(
+            new Translation3d(0.325, 0.0465, 0.192),
+            new Rotation3d(0, Units.degreesToRadians(-31), 0)
+    ),
+            REAR_CENTER_TO_CAMERA_POSE = new Pose3d(
+                    new Translation3d(-0.325 + 0.00975, 0, 0.095),
+                    new Rotation3d(Math.PI - Units.degreesToRadians(0), Units.degreesToRadians(-33), Math.PI + Units.degreesToRadians(0))
+            );
 
     public static final AprilTagCamera
             FRONT_TAG_CAMERA = new AprilTagCamera(

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

@@ -101,20 +101,6 @@ public double getDistanceToBestTagMeters() {
         return inputs.distanceFromBestTag;
     }
 
-    public Rotation2d getBestTagYawRelativeToRobot() {
-        if (!inputs.hasResult)
-            return Rotation2d.fromDegrees(180);
-        final Rotation2d bestTagYawRelativeToCamera = Rotation2d.fromDegrees(inputs.bestTarget.getYaw());
-        final Rotation2d robotCenterToCameraYaw = Rotation2d.fromRadians(robotCenterToCamera.getRotation().getZ());
-        return bestTagYawRelativeToCamera.rotateBy(robotCenterToCameraYaw);
-    }
-
-    public Translation3d getBestTagTanslation() {
-        if (!inputs.hasResult)
-            return new Translation3d();
-        return inputs.bestTarget.getBestCameraToTarget().getTranslation();
-    }
-
     /**
      * If the robot is close enough to the tag, it calculates the pose using the solve PNP heading.
      * If it's too far, it assumes the robot's heading from the gyro and calculates its position from there.

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

@@ -3,7 +3,6 @@
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Transform3d;
 import org.littletonrobotics.junction.AutoLog;
-import org.photonvision.targeting.PhotonTrackedTarget;
 
 public class AprilTagCameraIO {
     protected void updateInputs(AprilTagCameraInputsAutoLogged inputs) {
@@ -19,7 +18,6 @@ protected void addSimulatedCameraToVisionSimulation(Transform3d robotToCamera) {
 
     @AutoLog
     public static class AprilTagCameraInputs {
-        public PhotonTrackedTarget bestTarget = new PhotonTrackedTarget();
         public boolean hasResult = false;
         public double latestResultTimestampSeconds = 0;
         public Pose3d cameraSolvePNPPose = new Pose3d();

src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java

@@ -41,7 +41,6 @@ private void updateHasResultInputs(AprilTagCameraInputsAutoLogged inputs, Photon
         final PhotonTrackedTarget bestTarget = getBestTarget(latestResult);
         final Rotation3d bestTargetRelativeRotation3d = getBestTargetRelativeRotation(bestTarget);
 
-        inputs.bestTarget = bestTarget;
         inputs.cameraSolvePNPPose = getSolvePNPPose(latestResult, bestTarget);
         inputs.latestResultTimestampSeconds = latestResult.getTimestampSeconds();
         inputs.bestTargetRelativePitchRadians = bestTargetRelativeRotation3d.getY();

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

@@ -106,15 +106,6 @@ public boolean hasResult() {
         return true;
     }
 
-    public Rotation2d getBestTagYawRelativeToRobot() {
-        Rotation2d bestTagYawRelativeToCamera = aprilTagCameras[0].getBestTagYawRelativeToRobot();
-        for (AprilTagCamera camera : aprilTagCameras) {
-            if (Math.abs(bestTagYawRelativeToCamera.getDegrees()) < Math.abs(camera.getBestTagYawRelativeToRobot().getDegrees()))
-                bestTagYawRelativeToCamera = camera.getBestTagYawRelativeToRobot();
-        }
-        return bestTagYawRelativeToCamera;
-    }
-
     private void logTargetPath() {
         PathPlannerLogging.setLogActivePathCallback((pose) -> {
             field.getObject("path").setPoses(pose);

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

@@ -1,5 +1,6 @@
 package frc.trigon.robot.poseestimation.poseestimator;
 
+import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.geometry.Translation3d;
 import frc.trigon.robot.constants.CameraConstants;
 import org.photonvision.PhotonCamera;
@@ -17,28 +18,33 @@ public class RobotShowcase {
 
     public MirrorableTranslation3d getBestTagTranslationRelativeToRobot() {
         final PhotonPipelineResult bestResult = getBestResult();
-        final Translation3d bestTagTranslation = getBestTagTranslation(bestResult);
+        if (bestResult == null)
+            return new MirrorableTranslation3d(new Translation3d(), false);
+        final Transform3d cameraToTagTransform = getBestTagTranslation(bestResult);
 
         if (isGettingResultFromFrontCamera)
-            return new MirrorableTranslation3d(bestTagTranslation.minus(CameraConstants.FRONT_CENTER_TO_CAMERA.getTranslation()), false);
-        return new MirrorableTranslation3d(bestTagTranslation.minus(CameraConstants.REAR_CENTER_TO_CAMERA.getTranslation()), false);
+            return new MirrorableTranslation3d(CameraConstants.FRONT_CENTER_TO_CAMERA_POSE.transformBy(cameraToTagTransform).getTranslation(), false);
+        return new MirrorableTranslation3d(CameraConstants.REAR_CENTER_TO_CAMERA_POSE.transformBy(cameraToTagTransform).getTranslation(), false);
     }
 
-    private Translation3d getBestTagTranslation(PhotonPipelineResult result) {
+    private Transform3d getBestTagTranslation(PhotonPipelineResult result) {
         final PhotonTrackedTarget bestTarget = result.getBestTarget();
-        final Translation3d tagTranslation = bestTarget.getBestCameraToTarget().getTranslation();
-        return tagTranslation;
+        return bestTarget.getBestCameraToTarget();
     }
 
     private PhotonPipelineResult getBestResult() {
         final PhotonPipelineResult frontResult = getLatestPipelineResult(frontTagCamera);
         final PhotonPipelineResult rearResult = getLatestPipelineResult(rearTagCamera);
 
-        if (frontResult == null && rearResult != null) {
+        if (!frontResult.hasTargets()) {
+            if (!rearResult.hasTargets())
+                return null;
             isGettingResultFromFrontCamera = false;
             return rearResult;
         }
-        if (rearResult == null) {
+        if (!rearResult.hasTargets()) {
+            if (!frontResult.hasTargets())
+                return null;
             isGettingResultFromFrontCamera = true;
             return frontResult;
         }
@@ -52,6 +58,6 @@ private PhotonPipelineResult getBestResult() {
 
     private PhotonPipelineResult getLatestPipelineResult(PhotonCamera camera) {
         final List<PhotonPipelineResult> unreadResults = camera.getAllUnreadResults();
-        return unreadResults.isEmpty() ? null : unreadResults.get(unreadResults.size() - 1);
+        return unreadResults.isEmpty() ? new PhotonPipelineResult() : unreadResults.get(unreadResults.size() - 1);
     }
 }