add toggle to enable/disable vision (#31)

* pose is updated by vision subsystem only when enabled * when vision subsystem is disabled, the rotation of the pose is updated to reflect the robot's rotation as determined by the gyro (to fix a pose that may be far off due to vision issue) * fix #14 * update to latest vendor dependencies --------- Co-authored-by: yoyoconnor Co-authored-by: jimit200 <[email protected]> Co-authored-by: Geoff Schmit <[email protected]>
HuskieRobotics · Feb 10, 2023 · 2e8fe61 · 2e8fe61
1 parent 448c250
commit 2e8fe61
Show file tree

Hide file tree

Showing 11 changed files with 102 additions and 56 deletions.
diff --git a/src/main/java/frc/lib/team3061/vision/Vision.java b/src/main/java/frc/lib/team3061/vision/Vision.java
@@ -31,6 +31,7 @@ public class Vision extends SubsystemBase {
 
   private double lastTimestamp;
   private SwerveDrivePoseEstimator poseEstimator;
+  private boolean isEnabled = true;
 
   private Alert noAprilTagLayoutAlert =
       new Alert(
@@ -96,18 +97,25 @@ public void periodic() {
                     .getTranslation()
                     .getNorm()
                 < MAX_POSE_DIFFERENCE_METERS) {
-              poseEstimator.addVisionMeasurement(robotPose.toPose2d(), getLatestTimestamp());
+              if (isEnabled) {
+                poseEstimator.addVisionMeasurement(robotPose.toPose2d(), getLatestTimestamp());
+              }
 
               Logger.getInstance().recordOutput("Vision/TagPose", tagPose);
               Logger.getInstance().recordOutput("Vision/CameraPose", cameraPose);
               Logger.getInstance().recordOutput("Vision/RobotPose", robotPose.toPose2d());
+              Logger.getInstance().recordOutput("Vision/isEnabled", isEnabled);
             }
           }
         }
       }
     }
   }
 
+  public boolean isEnabled() {
+    return isEnabled;
+  }
+
   public boolean tagVisible(int id) {
     PhotonPipelineResult result = getLatestResult();
     for (PhotonTrackedTarget target : result.getTargets()) {
@@ -154,6 +162,10 @@ public double getDistanceToTag(int id) {
     }
   }
 
+  public void enable(boolean enable) {
+    isEnabled = enable;
+  }
+
   public boolean isValidTarget(PhotonTrackedTarget target) {
     return target.getFiducialId() != -1
         && target.getPoseAmbiguity() != -1

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -57,6 +57,7 @@ public class RobotContainer {
   private OperatorInterface oi = new OperatorInterface() {};
   private RobotConfig config;
   private Drivetrain drivetrain;
+  private Vision vision;
 
   // use AdvantageKit's LoggedDashboardChooser instead of SendableChooser to ensure accurate logging
   private final LoggedDashboardChooser<Command> autoChooser =
@@ -141,7 +142,7 @@ public RobotContainer() {
 
             drivetrain = new Drivetrain(gyro, flModule, frModule, blModule, brModule);
             new Pneumatics(new PneumaticsIORev());
-            new Vision(new VisionIOPhotonVision(config.getCameraName()));
+            vision = new Vision(new VisionIOPhotonVision(config.getCameraName()));
             break;
           }
         case ROBOT_SIMBOT:
@@ -166,11 +167,12 @@ public RobotContainer() {
             } catch (IOException e) {
               layout = new AprilTagFieldLayout(new ArrayList<>(), 16.4592, 8.2296);
             }
-            new Vision(
-                new VisionIOSim(
-                    layout,
-                    drivetrain::getPose,
-                    RobotConfig.getInstance().getRobotToCameraTransform()));
+            vision =
+                new Vision(
+                    new VisionIOSim(
+                        layout,
+                        drivetrain::getPose,
+                        RobotConfig.getInstance().getRobotToCameraTransform()));
 
             break;
           }
@@ -192,7 +194,7 @@ public RobotContainer() {
           new SwerveModule(new SwerveModuleIO() {}, 3, config.getRobotMaxVelocity());
       drivetrain = new Drivetrain(new GyroIO() {}, flModule, frModule, blModule, brModule);
       new Pneumatics(new PneumaticsIO() {});
-      new Vision(new VisionIO() {});
+      vision = new Vision(new VisionIO() {});
     }
 
     // disable all telemetry in the LiveWindow to reduce the processing during each iteration
@@ -256,6 +258,14 @@ private void configureButtonBindings() {
     // x-stance
     oi.getXStanceButton().onTrue(Commands.runOnce(drivetrain::enableXstance, drivetrain));
     oi.getXStanceButton().onFalse(Commands.runOnce(drivetrain::disableXstance, drivetrain));
+    oi.getVisionIsEnabledSwitch()
+        .toggleOnTrue(
+            Commands.either(
+                Commands.parallel(
+                    Commands.runOnce(() -> vision.enable(false)),
+                    Commands.runOnce(drivetrain::resetPoseRotationToGyro)),
+                Commands.runOnce(() -> vision.enable(true), vision),
+                vision::isEnabled));
   }
 
   /** Use this method to define your commands for autonomous mode. */

diff --git a/src/main/java/frc/robot/operator_interface/DualJoysticksOI.java b/src/main/java/frc/robot/operator_interface/DualJoysticksOI.java
@@ -58,4 +58,9 @@ public Trigger getResetGyroButton() {
   public Trigger getXStanceButton() {
     return translateJoystickButtons[1];
   }
+
+  @Override
+  public Trigger getVisionIsEnabledSwitch() {
+    return rotateJoystickButtons[1];
+  }
 }
diff --git a/src/main/java/frc/robot/operator_interface/OperatorInterface.java b/src/main/java/frc/robot/operator_interface/OperatorInterface.java
@@ -36,4 +36,8 @@ public default Trigger getResetGyroButton() {
   public default Trigger getXStanceButton() {
     return new Trigger(() -> false);
   }
+
+  public default Trigger getVisionIsEnabledSwitch() {
+    return new Trigger(() -> false);
+  }
 }
diff --git a/src/main/java/frc/robot/operator_interface/SingleHandheldOI.java b/src/main/java/frc/robot/operator_interface/SingleHandheldOI.java
@@ -44,4 +44,9 @@ public Trigger getResetGyroButton() {
   public Trigger getXStanceButton() {
     return new Trigger(controller::getYButton);
   }
+
+  @Override
+  public Trigger getVisionIsEnabledSwitch() {
+    return new Trigger(controller::getXButton);
+  }
 }
diff --git a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java
@@ -238,6 +238,16 @@ public void resetOdometry(PathPlannerState state) {
         new Pose2d(state.poseMeters.getTranslation(), state.holonomicRotation));
   }
 
+  public void resetPoseRotationToGyro() {
+    for (int i = 0; i < 4; i++) {
+      swerveModulePositions[i] = swerveModules[i].getPosition();
+    }
+
+    poseEstimator.resetPosition(
+        this.getRotation(),
+        swerveModulePositions,
+        new Pose2d(this.getPose().getTranslation(), this.getRotation()));
+  }
   /**
    * Controls the drivetrain to move the robot with the desired velocities in the x, y, and
    * rotational directions. The velocities may be specified from either the robot's frame of

diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json
@@ -1,32 +1,32 @@
 {
     "fileName": "AdvantageKit.json",
     "name": "AdvantageKit",
-    "version": "2.1.1",
+    "version": "2.1.2",
     "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003",
     "mavenUrls": [],
     "jsonUrl": "https://github.com/Mechanical-Advantage/AdvantageKit/releases/latest/download/AdvantageKit.json",
     "javaDependencies": [
         {
             "groupId": "org.littletonrobotics.akit.junction",
             "artifactId": "wpilib-shim",
-            "version": "2.1.1"
+            "version": "2.1.2"
         },
         {
             "groupId": "org.littletonrobotics.akit.junction",
             "artifactId": "junction-core",
-            "version": "2.1.1"
+            "version": "2.1.2"
         },
         {
             "groupId": "org.littletonrobotics.akit.conduit",
             "artifactId": "conduit-api",
-            "version": "2.1.1"
+            "version": "2.1.2"
         }
     ],
     "jniDependencies": [
         {
             "groupId": "org.littletonrobotics.akit.conduit",
             "artifactId": "conduit-wpilibio",
-            "version": "2.1.1",
+            "version": "2.1.2",
             "skipInvalidPlatforms": false,
             "isJar": false,
             "validPlatforms": [

diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json
@@ -1,7 +1,7 @@
 {
     "fileName": "PathplannerLib.json",
     "name": "PathplannerLib",
-    "version": "2023.3.3",
+    "version": "2023.4.1",
     "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786",
     "mavenUrls": [
         "https://3015rangerrobotics.github.io/pathplannerlib/repo"
@@ -11,15 +11,15 @@
         {
             "groupId": "com.pathplanner.lib",
             "artifactId": "PathplannerLib-java",
-            "version": "2023.3.3"
+            "version": "2023.4.1"
         }
     ],
     "jniDependencies": [],
     "cppDependencies": [
         {
             "groupId": "com.pathplanner.lib",
             "artifactId": "PathplannerLib-cpp",
-            "version": "2023.3.3",
+            "version": "2023.4.1",
             "libName": "PathplannerLib",
             "headerClassifier": "headers",
             "sharedLibrary": false,