From c02cec69070359a2db261b4202656e68ae713eab Mon Sep 17 00:00:00 2001
From: Jade Turner <spacey-sooty@proton.me>
Date: Wed, 4 Dec 2024 14:14:33 +0800
Subject: [PATCH] fix crash

Signed-off-by: Jade Turner <spacey-sooty@proton.me>
---
 src/main/java/frc/robot/Robot.java            | 23 +++++++++++++--
 .../frc/robot/generated/BuildConstants.java   | 10 +++----
 .../subsystems/vision/VisionIOLimelight.java  | 28 ++++++++++---------
 .../robot/subsystems/vision/VisionSource.java |  4 +++
 4 files changed, 45 insertions(+), 20 deletions(-)

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index aa041a3..f6343b9 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -185,8 +185,8 @@ public Robot() {
         drive.applyRequest(
             () ->
                 driveReq
-                    .withVelocityX(-driver.getLeftY() * MaxSpeed)
-                    .withVelocityY(-driver.getLeftX() * MaxSpeed)
+                    .withVelocityX(-driver.getLeftY() * MaxSpeed) // controller x is wpilib y
+                    .withVelocityY(-driver.getLeftX() * MaxSpeed) // controller y is wpilib x
                     .withRotationalRate(
                         driver.getRightX()
                             * MaxAngularRate) // Drive counterclockwise with negative X (left)
@@ -280,6 +280,25 @@ public Robot() {
           return routine.cmd();
         });
 
+    autoChooser.addOption(
+        "Test Path",
+        () -> {
+          final AutoLoop routine = autoFactory.newLoop("test");
+          final AutoTrajectory path = autoFactory.trajectory("TestPath", routine);
+
+          routine
+              .enabled()
+              .onTrue(
+                  drive
+                      .runOnce(
+                          () ->
+                              path.getInitialPose()
+                                  .ifPresentOrElse(pose -> drive.resetPose(pose), routine::kill))
+                      .andThen(path.cmd()));
+
+          return routine.cmd();
+        });
+
     autoChooser.addDefaultOption(
         "Set Pose to Vision Pose",
         () -> {
diff --git a/src/main/java/frc/robot/generated/BuildConstants.java b/src/main/java/frc/robot/generated/BuildConstants.java
index c9c4554..8e1ef8c 100644
--- a/src/main/java/frc/robot/generated/BuildConstants.java
+++ b/src/main/java/frc/robot/generated/BuildConstants.java
@@ -7,12 +7,12 @@ public final class BuildConstants {
   public static final String MAVEN_GROUP = "";
   public static final String MAVEN_NAME = "2025-Reefscape";
   public static final String VERSION = "unspecified";
-  public static final int GIT_REVISION = 10;
-  public static final String GIT_SHA = "484b2f32db42fc568fa4e2def7ecb32c61ab00f6";
-  public static final String GIT_DATE = "2024-12-01 20:13:22 AWST";
+  public static final int GIT_REVISION = 11;
+  public static final String GIT_SHA = "438d14fa33e145a166cad585d0deaf523ef636ab";
+  public static final String GIT_DATE = "2024-12-04 13:29:11 AWST";
   public static final String GIT_BRANCH = "master";
-  public static final String BUILD_DATE = "2024-12-04 13:26:00 AWST";
-  public static final long BUILD_UNIX_TIME = 1733289960008L;
+  public static final String BUILD_DATE = "2024-12-04 14:12:23 AWST";
+  public static final long BUILD_UNIX_TIME = 1733292743200L;
   public static final int DIRTY = 1;
 
   private BuildConstants(){}
diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java b/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java
index 024efb7..b7d080e 100644
--- a/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java
+++ b/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java
@@ -31,19 +31,21 @@ public VisionIOLimelight(String camera) {
   @Override
   public void updateInputs(VisionIOInputs inputs) {
     var pose = botpose.get();
-    var rotation =
-        new Rotation3d(
-            degreesToRadians(pose[3]), degreesToRadians(pose[4]), degreesToRadians(pose[5]));
-    inputs.estimatedPose = new Pose3d(pose[0], pose[1], pose[2], rotation);
-    inputs.timestamp = Util.now().in(Milliseconds);
-    inputs.latency = pose[6];
-    inputs.tagCount = pose[7];
-    inputs.inField = Util.inField(inputs.estimatedPose);
-    inputs.camera = camera;
-    inputs.validIds = validIds;
-    inputs.averageTagDistance = pose[9];
-    inputs.averageTagArea = pose[10];
-    inputs.cameraOffset = cameraOffset;
+    if (pose != null && pose.length != 0) {
+      var rotation =
+          new Rotation3d(
+              degreesToRadians(pose[3]), degreesToRadians(pose[4]), degreesToRadians(pose[5]));
+      inputs.estimatedPose = new Pose3d(pose[0], pose[1], pose[2], rotation);
+      inputs.timestamp = Util.now().in(Milliseconds);
+      inputs.latency = pose[6];
+      inputs.tagCount = pose[7];
+      inputs.inField = Util.inField(inputs.estimatedPose);
+      inputs.camera = camera;
+      inputs.validIds = validIds;
+      inputs.averageTagDistance = pose[9];
+      inputs.averageTagArea = pose[10];
+      inputs.cameraOffset = cameraOffset;
+    }
   }
 
   @Override
diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSource.java b/src/main/java/frc/robot/subsystems/vision/VisionSource.java
index e5749db..a6668b9 100644
--- a/src/main/java/frc/robot/subsystems/vision/VisionSource.java
+++ b/src/main/java/frc/robot/subsystems/vision/VisionSource.java
@@ -32,6 +32,10 @@ public void periodic() {
 
   /** Pose with camera offset accounted for */
   public Pose3d getPose() {
+    if (inputs.estimatedPose == null) {
+      return new Pose3d();
+    }
+
     if (inputs.cameraOffset != null) {
       return inputs.estimatedPose.plus(inputs.cameraOffset);
     } else {