fix crash

Signed-off-by: Jade Turner <spacey-sooty@proton.me>
CurtinFRC · Dec 4, 2024 · c02cec6 · c02cec6
1 parent 438d14f
commit c02cec6
Show file tree

Hide file tree

Showing 4 changed files with 45 additions and 20 deletions.
diff --git 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
diff --git 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
@@ -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 {