From 2e8fe61be4772fe737ed9c6a0064443fdd920648 Mon Sep 17 00:00:00 2001 From: yoyoconnor <111534505+yoyoconnor@users.noreply.github.com> Date: Fri, 10 Feb 2023 16:39:43 -0600 Subject: [PATCH] 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 Co-authored-by: Geoff Schmit --- .../java/frc/lib/team3061/vision/Vision.java | 14 ++++- src/main/java/frc/robot/RobotContainer.java | 24 ++++--- .../operator_interface/DualJoysticksOI.java | 5 ++ .../operator_interface/OperatorInterface.java | 4 ++ .../operator_interface/SingleHandheldOI.java | 5 ++ .../subsystems/drivetrain/Drivetrain.java | 10 +++ vendordeps/AdvantageKit.json | 10 +-- vendordeps/PathplannerLib.json | 6 +- vendordeps/Phoenix.json | 62 +++++++++---------- vendordeps/REVLib.json | 10 +-- vendordeps/photonlib.json | 8 +-- 11 files changed, 102 insertions(+), 56 deletions(-) diff --git a/src/main/java/frc/lib/team3061/vision/Vision.java b/src/main/java/frc/lib/team3061/vision/Vision.java index 5e49cd53..0122241b 100644 --- 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,11 +97,14 @@ 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); } } } @@ -108,6 +112,10 @@ public void periodic() { } } + 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 index 2d9fc94d..28f27f16 100644 --- 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 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 index 873ff3bf..8fe7c938 100644 --- 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 index ac7ff4f4..ff30aad2 100644 --- 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 index 779c1cd9..e26439c0 100644 --- 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 index a5dd0472..e9c7d92a 100644 --- 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 index 0a6bc50e..cae0375b 100644 --- a/vendordeps/AdvantageKit.json +++ b/vendordeps/AdvantageKit.json @@ -1,7 +1,7 @@ { "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", @@ -9,24 +9,24 @@ { "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 index 65655e26..53c27617 100644 --- 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,7 +11,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2023.3.3" + "version": "2023.4.1" } ], "jniDependencies": [], @@ -19,7 +19,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2023.3.3", + "version": "2023.4.1", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/Phoenix.json b/vendordeps/Phoenix.json index 72371c03..55bc0990 100644 --- a/vendordeps/Phoenix.json +++ b/vendordeps/Phoenix.json @@ -1,7 +1,7 @@ { "fileName": "Phoenix.json", "name": "CTRE-Phoenix (v5)", - "version": "5.30.3", + "version": "5.30.4", "frcYear": 2023, "uuid": "ab676553-b602-441f-a38d-f1296eff6537", "mavenUrls": [ @@ -12,19 +12,19 @@ { "groupId": "com.ctre.phoenix", "artifactId": "api-java", - "version": "5.30.3" + "version": "5.30.4" }, { "groupId": "com.ctre.phoenix", "artifactId": "wpiapi-java", - "version": "5.30.3" + "version": "5.30.4" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix", "artifactId": "cci", - "version": "5.30.3", + "version": "5.30.4", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -37,7 +37,7 @@ { "groupId": "com.ctre.phoenix.sim", "artifactId": "cci-sim", - "version": "5.30.3", + "version": "5.30.4", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -50,7 +50,7 @@ { "groupId": "com.ctre.phoenixpro", "artifactId": "tools", - "version": "23.0.2", + "version": "23.0.5", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -63,7 +63,7 @@ { "groupId": "com.ctre.phoenixpro.sim", "artifactId": "tools-sim", - "version": "23.0.2", + "version": "23.0.5", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -76,7 +76,7 @@ { "groupId": "com.ctre.phoenixpro.sim", "artifactId": "simTalonSRX", - "version": "23.0.2", + "version": "23.0.5", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -89,7 +89,7 @@ { "groupId": "com.ctre.phoenixpro.sim", "artifactId": "simTalonFX", - "version": "23.0.2", + "version": "23.0.5", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -102,7 +102,7 @@ { "groupId": "com.ctre.phoenixpro.sim", "artifactId": "simVictorSPX", - "version": "23.0.2", + "version": "23.0.5", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -115,7 +115,7 @@ { "groupId": "com.ctre.phoenixpro.sim", "artifactId": "simPigeonIMU", - "version": "23.0.2", + "version": "23.0.5", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -128,7 +128,7 @@ { "groupId": "com.ctre.phoenixpro.sim", "artifactId": "simCANCoder", - "version": "23.0.2", + "version": "23.0.5", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -141,7 +141,7 @@ { "groupId": "com.ctre.phoenixpro.sim", "artifactId": "simProTalonFX", - "version": "23.0.2", + "version": "23.0.5", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -154,7 +154,7 @@ { "groupId": "com.ctre.phoenixpro.sim", "artifactId": "simProCANcoder", - "version": "23.0.2", + "version": "23.0.5", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -167,7 +167,7 @@ { "groupId": "com.ctre.phoenixpro.sim", "artifactId": "simProPigeon2", - "version": "23.0.2", + "version": "23.0.5", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -182,7 +182,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "wpiapi-cpp", - "version": "5.30.3", + "version": "5.30.4", "libName": "CTRE_Phoenix_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -197,7 +197,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "api-cpp", - "version": "5.30.3", + "version": "5.30.4", "libName": "CTRE_Phoenix", "headerClassifier": "headers", "sharedLibrary": true, @@ -212,7 +212,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "cci", - "version": "5.30.3", + "version": "5.30.4", "libName": "CTRE_PhoenixCCI", "headerClassifier": "headers", "sharedLibrary": true, @@ -227,7 +227,7 @@ { "groupId": "com.ctre.phoenixpro", "artifactId": "tools", - "version": "23.0.2", + "version": "23.0.5", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -242,7 +242,7 @@ { "groupId": "com.ctre.phoenix.sim", "artifactId": "wpiapi-cpp-sim", - "version": "5.30.3", + "version": "5.30.4", "libName": "CTRE_Phoenix_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -257,7 +257,7 @@ { "groupId": "com.ctre.phoenix.sim", "artifactId": "api-cpp-sim", - "version": "5.30.3", + "version": "5.30.4", "libName": "CTRE_PhoenixSim", "headerClassifier": "headers", "sharedLibrary": true, @@ -272,7 +272,7 @@ { "groupId": "com.ctre.phoenix.sim", "artifactId": "cci-sim", - "version": "5.30.3", + "version": "5.30.4", "libName": "CTRE_PhoenixCCISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -287,7 +287,7 @@ { "groupId": "com.ctre.phoenixpro.sim", "artifactId": "tools-sim", - "version": "23.0.2", + "version": "23.0.5", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -302,7 +302,7 @@ { "groupId": "com.ctre.phoenixpro.sim", "artifactId": "simTalonSRX", - "version": "23.0.2", + "version": "23.0.5", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -317,7 +317,7 @@ { "groupId": "com.ctre.phoenixpro.sim", "artifactId": "simTalonFX", - "version": "23.0.2", + "version": "23.0.5", "libName": "CTRE_SimTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -332,7 +332,7 @@ { "groupId": "com.ctre.phoenixpro.sim", "artifactId": "simVictorSPX", - "version": "23.0.2", + "version": "23.0.5", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -347,7 +347,7 @@ { "groupId": "com.ctre.phoenixpro.sim", "artifactId": "simPigeonIMU", - "version": "23.0.2", + "version": "23.0.5", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -362,7 +362,7 @@ { "groupId": "com.ctre.phoenixpro.sim", "artifactId": "simCANCoder", - "version": "23.0.2", + "version": "23.0.5", "libName": "CTRE_SimCANCoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -377,7 +377,7 @@ { "groupId": "com.ctre.phoenixpro.sim", "artifactId": "simProTalonFX", - "version": "23.0.2", + "version": "23.0.5", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -392,7 +392,7 @@ { "groupId": "com.ctre.phoenixpro.sim", "artifactId": "simProCANcoder", - "version": "23.0.2", + "version": "23.0.5", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -407,7 +407,7 @@ { "groupId": "com.ctre.phoenixpro.sim", "artifactId": "simProPigeon2", - "version": "23.0.2", + "version": "23.0.5", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json index cdbbe6ed..f2d0b7df 100644 --- a/vendordeps/REVLib.json +++ b/vendordeps/REVLib.json @@ -1,7 +1,7 @@ { "fileName": "REVLib.json", "name": "REVLib", - "version": "2023.1.2", + "version": "2023.1.3", "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", "mavenUrls": [ "https://maven.revrobotics.com/" @@ -11,14 +11,14 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-java", - "version": "2023.1.2" + "version": "2023.1.3" } ], "jniDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2023.1.2", + "version": "2023.1.3", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -36,7 +36,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-cpp", - "version": "2023.1.2", + "version": "2023.1.3", "libName": "REVLib", "headerClassifier": "headers", "sharedLibrary": false, @@ -54,7 +54,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2023.1.2", + "version": "2023.1.3", "libName": "REVLibDriver", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json index 8fdf301a..4f378c49 100644 --- a/vendordeps/photonlib.json +++ b/vendordeps/photonlib.json @@ -1,7 +1,7 @@ { "fileName": "photonlib.json", "name": "photonlib", - "version": "v2023.1.2", + "version": "v2023.3.0", "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004 ", "mavenUrls": [ "https://maven.photonvision.org/repository/internal", @@ -13,7 +13,7 @@ { "groupId": "org.photonvision", "artifactId": "PhotonLib-cpp", - "version": "v2023.1.2", + "version": "v2023.3.0", "libName": "Photon", "headerClassifier": "headers", "sharedLibrary": true, @@ -30,12 +30,12 @@ { "groupId": "org.photonvision", "artifactId": "PhotonLib-java", - "version": "v2023.1.2" + "version": "v2023.3.0" }, { "groupId": "org.photonvision", "artifactId": "PhotonTargeting-java", - "version": "v2023.1.2" + "version": "v2023.3.0" } ] } \ No newline at end of file