From 8633f36942e6585eebaf237fabb9daa30448446b Mon Sep 17 00:00:00 2001 From: FarmerOfEggplants Date: Thu, 18 Sep 2025 16:11:18 -0500 Subject: [PATCH 01/21] QuestNav maybe ready, needs to be looked over by chris --- src/main/java/frc/robot/Constants.java | 7 +- src/main/java/frc/robot/subsystems/Quest.java | 85 +++++++++++++++++++ vendordeps/questnavlib.json | 21 +++++ 3 files changed, 112 insertions(+), 1 deletion(-) create mode 100644 src/main/java/frc/robot/subsystems/Quest.java create mode 100644 vendordeps/questnavlib.json diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index c547e7d..78c213c 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -33,7 +33,9 @@ import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Pair; import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; @@ -49,8 +51,8 @@ import edu.wpi.first.units.measure.LinearVelocity; import edu.wpi.first.units.measure.Time; import edu.wpi.first.units.measure.Voltage; +import edu.wpi.first.wpilibj.Encoder; import edu.wpi.first.wpilibj.Filesystem; -import frc.robot.lib.util.Encoder; public class Constants { @@ -309,5 +311,8 @@ public static class ClimbK { .withReverseSoftLimitEnable(true) .withReverseSoftLimitThreshold(minAngle); } + public static class QuestK { + public static final Transform2d questOffset = new Transform2d(0, 0, Rotation2d.k180deg); + } } diff --git a/src/main/java/frc/robot/subsystems/Quest.java b/src/main/java/frc/robot/subsystems/Quest.java new file mode 100644 index 0000000..c6ac225 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Quest.java @@ -0,0 +1,85 @@ +package frc.robot.subsystems; + +import com.ctre.phoenix6.Utils; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants.QuestK; +import gg.questnav.questnav.PoseFrame; +import gg.questnav.questnav.QuestNav; +import swervelib.SwerveDrive; + +public class Quest extends SubsystemBase { + QuestNav questNav = new QuestNav(); + + private void GetPose() { + // First, Declare our geometrical transform from the robot center to the Quest + Transform2d ROBOT_TO_QUEST = new Transform2d( /*TODO: Put your x, y, rotational offsets here!*/ ); + + // Get the latest pose data frames from the Quest + PoseFrame[] poseFrames = questNav.getAllUnreadPoseFrames(); + + if (poseFrames.length > 0) { + // Get the most recent Quest pose + Pose2d questPose = poseFrames[poseFrames.length - 1].questPose(); + + // Transform by the mount pose to get your robot pose + Pose2d robotPose = questPose.transformBy(ROBOT_TO_QUEST.inverse()); + } + } + + private void SetPose() { + // First, Declare our geometrical transform from the robot center to the quest + Transform2d ROBOT_TO_QUEST = new Transform2d( /*TODO: Put your x, y, rotational offsets here!*/ ); + + // Assume this is the requested reset pose + Pose2d robotPose = new Pose2d( /* Some pose data */ ); + + // Transform by the offset to get the Quest pose + Pose2d questPose = robotPose.transformBy(ROBOT_TO_QUEST); + + // Send the reset operation + questNav.setPose(questPose); + } + + + private void QuestSwerve() { + SwerveDrive swerveDrive = new SwerveDrive(null, null, 0, null); + Matrix QUESTNAV_STD_DEVS = + VecBuilder.fill( + 0.02, // Trust down to 2cm in X direction + 0.02, // Trust down to 2cm in Y direction + 0.035 // Trust down to 2 degrees rotational + ); + + if (questNav.isTracking()) { + // Get the latest pose data frames from the Quest + PoseFrame[] questFrames = questNav.getAllUnreadPoseFrames(); + + // Loop over the pose data frames and send them to the pose estimator + for (PoseFrame questFrame : questFrames) { + // Get the pose of the Quest + Pose2d questPose = questFrame.questPose(); + // Get timestamp for when the data was sent + double timestamp = questFrame.dataTimestamp(); + + // Transform by the mount pose to get your robot pose + Pose2d robotPose = questPose.transformBy(QuestK.questOffset.inverse()); + + // Convert FPGA timestamp to CTRE's time domain using Phoenix 6 utility + double ctreTimestamp = Utils.fpgaToCurrentTime(timestamp); + + // // You can put some sort of filtering here if you would like! + + // // Add the measurement to our estimator + swerveDrive.addVisionMeasurement(robotPose, ctreTimestamp, QUESTNAV_STD_DEVS); + }; + } +}}; + + diff --git a/vendordeps/questnavlib.json b/vendordeps/questnavlib.json new file mode 100644 index 0000000..c7c60d8 --- /dev/null +++ b/vendordeps/questnavlib.json @@ -0,0 +1,21 @@ +{ + "fileName": "questnavlib.json", + "name": "questnavlib", + "version": "2025-1.1.1-beta", + "uuid": "a706fe68-86e5-4aed-92c5-ce05aca007f0", + "frcYear": "2025", + "mavenUrls": [ + "https://maven.questnav.gg/releases", + "https://maven.questnav.gg/snapshots" + ], + "jsonUrl": "https://maven.questnav.gg/releases/gg/questnav/questnavlib-json/2025-1.1.1-beta/questnavlib-json-2025-1.1.1-beta.json", + "javaDependencies": [ + { + "groupId": "gg.questnav", + "artifactId": "questnavlib-java", + "version": "2025-1.1.1-beta" + } + ], + "cppDependencies": [], + "jniDependencies": [] +} \ No newline at end of file From 9591cf1d8cbaeeec9980af9fd79e092d7ee0fafd Mon Sep 17 00:00:00 2001 From: chris Date: Sun, 21 Sep 2025 22:05:45 -0500 Subject: [PATCH 02/21] fix import issue in constants --- src/main/java/frc/robot/Constants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 78c213c..e1af846 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -51,8 +51,8 @@ import edu.wpi.first.units.measure.LinearVelocity; import edu.wpi.first.units.measure.Time; import edu.wpi.first.units.measure.Voltage; -import edu.wpi.first.wpilibj.Encoder; import edu.wpi.first.wpilibj.Filesystem; +import frc.robot.lib.util.Encoder; public class Constants { From 1bc9ea13eaaa96be77e9a71eca00a9eeaca37fc3 Mon Sep 17 00:00:00 2001 From: ADavis012 Date: Thu, 25 Sep 2025 15:58:40 -0500 Subject: [PATCH 03/21] commented out vision code in swerve to mark where it is, still VERY wip --- .../java/frc/robot/subsystems/Swerve.java | 21 ++++++------------- 1 file changed, 6 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index d4efd2b..d9f7e6d 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -5,17 +5,12 @@ import static edu.wpi.first.units.Units.MetersPerSecond; import static edu.wpi.first.units.Units.RadiansPerSecond; import static edu.wpi.first.units.Units.Seconds; -import static edu.wpi.first.units.Units.Volts; -import java.io.IOException; import java.util.Set; import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; import java.util.function.Supplier; -import org.photonvision.EstimatedRobotPose; - -import com.ctre.phoenix6.SignalLogger; import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.Pigeon2; import com.ctre.phoenix6.hardware.TalonFX; @@ -28,7 +23,6 @@ import edu.wpi.first.epilogue.Logged; import edu.wpi.first.math.controller.ProfiledPIDController; -import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Translation2d; @@ -52,9 +46,6 @@ import frc.robot.subsystems.Vision.VisionResults; import swervelib.SwerveDrive; import swervelib.motors.TalonFXSwerve; -import swervelib.parser.SwerveParser; -import swervelib.telemetry.SwerveDriveTelemetry; -import swervelib.telemetry.SwerveDriveTelemetry.TelemetryVerbosity; @Logged public class Swerve extends SubsystemBase { // physicalproperties/conversionFactors/angle/factor = 360.0 deg/4096.0 units per rotation @@ -67,7 +58,7 @@ public class Swerve extends SubsystemBase { // physicalproperties/conversionFact private final TalonFX backRight; private final SysIdRoutine sysIdRoutine; private final PPHolonomicDriveController pathPlannerController = new PPHolonomicDriveController(SwerveK.ppTranslationConstants, SwerveK.ppRotationConstants); - private boolean initializedOdometryFromVision = false; + //private boolean initializedOdometryFromVision = false; //! replace with Quest @SuppressWarnings("unused") private Pose2d pathPlannerTarget = Pose2d.kZero; // For logging // PID Alignment @@ -80,8 +71,8 @@ public class Swerve extends SubsystemBase { // physicalproperties/conversionFact private final ProfiledPIDController yController = new ProfiledPIDController(SwerveK.translationConstants.kP, SwerveK.translationConstants.kI, SwerveK.translationConstants.kD, SwerveK.defaultTranslationConstraints); private final ProfiledPIDController thetaController = new ProfiledPIDController(SwerveK.rotationConstants.kP, SwerveK.rotationConstants.kI, SwerveK.rotationConstants.kD, SwerveK.defaultRotationConstraints); - public Swerve(Supplier visionSource, BooleanSupplier overridePathFollowing) { - this.visionSource = visionSource; + public Swerve(Supplier visionSource, BooleanSupplier overridePathFollowing) { + //!this.visionSource = visionSource; this.overridePathFollowing = overridePathFollowing; SwerveDriveTelemetry.verbosity = TelemetryVerbosity.HIGH; SwerveParser parser = null; @@ -129,14 +120,14 @@ public Swerve(Supplier visionSource, BooleanSupplier overridePath public void periodic() { SmartDashboard.putNumber("X setpoint", xController.getSetpoint().position); SmartDashboard.putNumber("Y setpoint", yController.getSetpoint().position); - for (var result : visionSource.get().results()) { + for (var result : //!visionSource.get().results()) { EstimatedRobotPose pose = result.getFirst(); if (!initializedOdometryFromVision) { resetOdometry(pose.estimatedPose.toPose2d()); initializedOdometryFromVision = true; continue; } - swerveDrive.addVisionMeasurement(pose.estimatedPose.toPose2d(), pose.timestampSeconds, result.getSecond()); + swerveDrive.//!addVisionMeasurement(pose.estimatedPose.toPose2d(), pose.timestampSeconds, result.getSecond()); } } @@ -340,7 +331,7 @@ public String getCurrentCommandName() { return cmd.getName(); } - public boolean initializedOdometryFromVision() { + public boolean //!initializedOdometryFromVision() { return initializedOdometryFromVision; } From 6d1fbb97d3bcde371bf87e2057a39d7ae2357b09 Mon Sep 17 00:00:00 2001 From: ADavis012 Date: Thu, 25 Sep 2025 17:09:48 -0500 Subject: [PATCH 04/21] more reading and finding things in Quest (SwerveDrivePoseEstimator), removed comments --- .../java/frc/robot/subsystems/Swerve.java | 20 +++++++++++++------ 1 file changed, 14 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index d9f7e6d..52e1c17 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -6,11 +6,15 @@ import static edu.wpi.first.units.Units.RadiansPerSecond; import static edu.wpi.first.units.Units.Seconds; +import java.io.IOException; import java.util.Set; import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; import java.util.function.Supplier; +import org.photonvision.EstimatedRobotPose; + +import com.ctre.phoenix6.SignalLogger; import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.Pigeon2; import com.ctre.phoenix6.hardware.TalonFX; @@ -23,6 +27,7 @@ import edu.wpi.first.epilogue.Logged; import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Translation2d; @@ -46,6 +51,9 @@ import frc.robot.subsystems.Vision.VisionResults; import swervelib.SwerveDrive; import swervelib.motors.TalonFXSwerve; +import swervelib.parser.SwerveParser; +import swervelib.telemetry.SwerveDriveTelemetry; +import swervelib.telemetry.SwerveDriveTelemetry.TelemetryVerbosity; @Logged public class Swerve extends SubsystemBase { // physicalproperties/conversionFactors/angle/factor = 360.0 deg/4096.0 units per rotation @@ -58,7 +66,7 @@ public class Swerve extends SubsystemBase { // physicalproperties/conversionFact private final TalonFX backRight; private final SysIdRoutine sysIdRoutine; private final PPHolonomicDriveController pathPlannerController = new PPHolonomicDriveController(SwerveK.ppTranslationConstants, SwerveK.ppRotationConstants); - //private boolean initializedOdometryFromVision = false; //! replace with Quest + private boolean initializedOdometryFromVision = false; //! replace with Quest @SuppressWarnings("unused") private Pose2d pathPlannerTarget = Pose2d.kZero; // For logging // PID Alignment @@ -71,8 +79,8 @@ public class Swerve extends SubsystemBase { // physicalproperties/conversionFact private final ProfiledPIDController yController = new ProfiledPIDController(SwerveK.translationConstants.kP, SwerveK.translationConstants.kI, SwerveK.translationConstants.kD, SwerveK.defaultTranslationConstraints); private final ProfiledPIDController thetaController = new ProfiledPIDController(SwerveK.rotationConstants.kP, SwerveK.rotationConstants.kI, SwerveK.rotationConstants.kD, SwerveK.defaultRotationConstraints); - public Swerve(Supplier visionSource, BooleanSupplier overridePathFollowing) { - //!this.visionSource = visionSource; + public Swerve(Supplier visionSource, BooleanSupplier overridePathFollowing) { + this.visionSource = visionSource; this.overridePathFollowing = overridePathFollowing; SwerveDriveTelemetry.verbosity = TelemetryVerbosity.HIGH; SwerveParser parser = null; @@ -120,14 +128,14 @@ public Swerve(Supplier visionSource, BooleanSupplier overrideP public void periodic() { SmartDashboard.putNumber("X setpoint", xController.getSetpoint().position); SmartDashboard.putNumber("Y setpoint", yController.getSetpoint().position); - for (var result : //!visionSource.get().results()) { + for (var result : visionSource.get().results()) { EstimatedRobotPose pose = result.getFirst(); if (!initializedOdometryFromVision) { resetOdometry(pose.estimatedPose.toPose2d()); initializedOdometryFromVision = true; continue; } - swerveDrive.//!addVisionMeasurement(pose.estimatedPose.toPose2d(), pose.timestampSeconds, result.getSecond()); + swerveDrive.addVisionMeasurement(pose.estimatedPose.toPose2d(), pose.timestampSeconds, result.getSecond()); } } @@ -331,7 +339,7 @@ public String getCurrentCommandName() { return cmd.getName(); } - public boolean //!initializedOdometryFromVision() { + public boolean initializedOdometryFromVision() { return initializedOdometryFromVision; } From 3f564cab44fe773f0ce3648d03e66b8306100849 Mon Sep 17 00:00:00 2001 From: ADavis012 Date: Tue, 30 Sep 2025 17:31:27 -0500 Subject: [PATCH 05/21] Added a test method in Quest to take the place of VisionResults in Swerve --- src/main/java/frc/robot/subsystems/Quest.java | 18 +++++++++++++++++- 1 file changed, 17 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/Quest.java b/src/main/java/frc/robot/subsystems/Quest.java index c6ac225..1d65df3 100644 --- a/src/main/java/frc/robot/subsystems/Quest.java +++ b/src/main/java/frc/robot/subsystems/Quest.java @@ -1,8 +1,12 @@ package frc.robot.subsystems; +import java.util.ArrayList; +import java.util.List; + import com.ctre.phoenix6.Utils; import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.Pair; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Transform2d; @@ -80,6 +84,18 @@ private void QuestSwerve() { swerveDrive.addVisionMeasurement(robotPose, ctreTimestamp, QUESTNAV_STD_DEVS); }; } -}}; +} + /* + Attempt to make a QuestNav equivalent of VisionResults in Vision code + Goal is to return a wrapper object containing estimated robot poses and their standard deviations + */ + public QuestResults getQuestResults() { + List>> questResults = new ArrayList<>(); + if (questNav.isTracking()) { + questResults.addAll(/*need something to process results */(), SwerveDrivePoseEstimator); //use either SwerveDrivePoseEstimator or PoseEstimator + } + return new QuestResults(questResults); + } +}; From 8e0990ba368f17e4ef3ba262edca408116099411 Mon Sep 17 00:00:00 2001 From: ADavis012 Date: Tue, 7 Oct 2025 17:11:36 -0500 Subject: [PATCH 06/21] Changed questNav.isTracking to .isConnected, added .getAllUnreadPoseFrames and questNav.getName() --- src/main/java/frc/robot/subsystems/Quest.java | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Quest.java b/src/main/java/frc/robot/subsystems/Quest.java index 1d65df3..6e1d073 100644 --- a/src/main/java/frc/robot/subsystems/Quest.java +++ b/src/main/java/frc/robot/subsystems/Quest.java @@ -8,6 +8,7 @@ import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Pair; import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.numbers.N1; @@ -21,6 +22,8 @@ public class Quest extends SubsystemBase { QuestNav questNav = new QuestNav(); + private final SwerveDrivePoseEstimator poseEstimator = new SwerveDrivePoseEstimator(null, null, null, null); + private void GetPose() { // First, Declare our geometrical transform from the robot center to the Quest Transform2d ROBOT_TO_QUEST = new Transform2d( /*TODO: Put your x, y, rotational offsets here!*/ ); @@ -90,12 +93,13 @@ private void QuestSwerve() { Goal is to return a wrapper object containing estimated robot poses and their standard deviations */ public QuestResults getQuestResults() { - List>> questResults = new ArrayList<>(); - if (questNav.isTracking()) { - questResults.addAll(/*need something to process results */(), SwerveDrivePoseEstimator); //use either SwerveDrivePoseEstimator or PoseEstimator + List>> questResults = new ArrayList<>(); + if (questNav.isConnected()) { + questResults.addAll(/*need something to process results*/(questNav.getAllUnreadPoseFrames(), poseEstimator, questNav.getName())); //use either SwerveDrivePoseEstimator or PoseEstimator } return new QuestResults(questResults); } }; + From 6d705894a9970ded8d62fa069795193d435d665f Mon Sep 17 00:00:00 2001 From: ADavis012 Date: Tue, 21 Oct 2025 17:00:19 -0500 Subject: [PATCH 07/21] Switched out QuestResults code from Vision with code involving PoseFrames, started changing Swerve code from Vision to Quest (work to be done on initalizedOdometryFromVision and lines 133-141) --- src/main/java/frc/robot/subsystems/Quest.java | 16 +++++++-------- .../java/frc/robot/subsystems/Swerve.java | 20 +++++++++---------- 2 files changed, 18 insertions(+), 18 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Quest.java b/src/main/java/frc/robot/subsystems/Quest.java index 6e1d073..161ff11 100644 --- a/src/main/java/frc/robot/subsystems/Quest.java +++ b/src/main/java/frc/robot/subsystems/Quest.java @@ -1,12 +1,8 @@ package frc.robot.subsystems; -import java.util.ArrayList; -import java.util.List; - import com.ctre.phoenix6.Utils; import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.Pair; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; @@ -93,12 +89,16 @@ private void QuestSwerve() { Goal is to return a wrapper object containing estimated robot poses and their standard deviations */ public QuestResults getQuestResults() { - List>> questResults = new ArrayList<>(); - if (questNav.isConnected()) { - questResults.addAll(/*need something to process results*/(questNav.getAllUnreadPoseFrames(), poseEstimator, questNav.getName())); //use either SwerveDrivePoseEstimator or PoseEstimator + //List questResults = new ArrayList<>(); Remove later if code works + PoseFrame[] questFrames = null; + if (questNav.isConnected() && questNav.isTracking()) { + questFrames = questNav.getAllUnreadPoseFrames(); + //questResults.addAll(questNav.getAllUnreadPoseFrames(), poseEstimator, questNav); Remove later if code works } - return new QuestResults(questResults); + return new QuestResults(questFrames); } + + public record QuestResults(PoseFrame[] results){} }; diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 52e1c17..ba278c7 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -12,8 +12,6 @@ import java.util.function.DoubleSupplier; import java.util.function.Supplier; -import org.photonvision.EstimatedRobotPose; - import com.ctre.phoenix6.SignalLogger; import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.Pigeon2; @@ -30,6 +28,7 @@ import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveModuleState; @@ -48,7 +47,7 @@ import frc.robot.Constants.SwerveK; import frc.robot.Robot; import frc.robot.commands.DriveWheelCharacterization; -import frc.robot.subsystems.Vision.VisionResults; +import frc.robot.subsystems.Quest.QuestResults; import swervelib.SwerveDrive; import swervelib.motors.TalonFXSwerve; import swervelib.parser.SwerveParser; @@ -59,7 +58,7 @@ public class Swerve extends SubsystemBase { // physicalproperties/conversionFactors/angle/factor = 360.0 deg/4096.0 units per rotation private final SwerveDrive swerveDrive; - private final Supplier visionSource; + private final Supplier questSource; private final TalonFX frontLeft; private final TalonFX frontRight; private final TalonFX backLeft; @@ -79,8 +78,8 @@ public class Swerve extends SubsystemBase { // physicalproperties/conversionFact private final ProfiledPIDController yController = new ProfiledPIDController(SwerveK.translationConstants.kP, SwerveK.translationConstants.kI, SwerveK.translationConstants.kD, SwerveK.defaultTranslationConstraints); private final ProfiledPIDController thetaController = new ProfiledPIDController(SwerveK.rotationConstants.kP, SwerveK.rotationConstants.kI, SwerveK.rotationConstants.kD, SwerveK.defaultRotationConstraints); - public Swerve(Supplier visionSource, BooleanSupplier overridePathFollowing) { - this.visionSource = visionSource; + public Swerve(Supplier questSource, BooleanSupplier overridePathFollowing) { + this.questSource = questSource; this.overridePathFollowing = overridePathFollowing; SwerveDriveTelemetry.verbosity = TelemetryVerbosity.HIGH; SwerveParser parser = null; @@ -124,14 +123,15 @@ public Swerve(Supplier visionSource, BooleanSupplier overridePath backRight.getConfigurator().apply(SwerveK.currentLimitsConfig); } - @Override + @Override //! this space needs more work connecting QuestNav in place of Vision public void periodic() { SmartDashboard.putNumber("X setpoint", xController.getSetpoint().position); SmartDashboard.putNumber("Y setpoint", yController.getSetpoint().position); - for (var result : visionSource.get().results()) { - EstimatedRobotPose pose = result.getFirst(); + for (var pose : questSource.get().results()) { + Pose3d questPose = pose.questPose(); + Pose3d robotPose = questPose.transformBy(QuestNavConstants.ROBOT_TO_QUEST.inverse()); if (!initializedOdometryFromVision) { - resetOdometry(pose.estimatedPose.toPose2d()); + resetOdometry(robotPose.toPose2d()); initializedOdometryFromVision = true; continue; } From e5e57d88efce50b0719b4fb6a6a2bd8a97e0c113 Mon Sep 17 00:00:00 2001 From: chris Date: Thu, 30 Oct 2025 15:40:52 -0500 Subject: [PATCH 08/21] fix quest constants error --- src/main/java/frc/robot/Constants.java | 5 ++--- src/main/java/frc/robot/subsystems/Swerve.java | 6 ++++-- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index e1af846..2715cee 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -33,9 +33,8 @@ import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Pair; import edu.wpi.first.math.VecBuilder; -import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; @@ -312,7 +311,7 @@ public static class ClimbK { .withReverseSoftLimitThreshold(minAngle); } public static class QuestK { - public static final Transform2d questOffset = new Transform2d(0, 0, Rotation2d.k180deg); + public static final Transform3d ROBOT_TO_QUEST = new Transform3d(new Pose3d(), new Pose3d()); //! FIND } } diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index ba278c7..6715e2b 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -44,6 +44,7 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.Constants.ControllerK; +import frc.robot.Constants.QuestK; import frc.robot.Constants.SwerveK; import frc.robot.Robot; import frc.robot.commands.DriveWheelCharacterization; @@ -127,9 +128,10 @@ public Swerve(Supplier questSource, BooleanSupplier overridePathFo public void periodic() { SmartDashboard.putNumber("X setpoint", xController.getSetpoint().position); SmartDashboard.putNumber("Y setpoint", yController.getSetpoint().position); + for (var pose : questSource.get().results()) { - Pose3d questPose = pose.questPose(); - Pose3d robotPose = questPose.transformBy(QuestNavConstants.ROBOT_TO_QUEST.inverse()); + Pose3d questPose = pose.questPose(); // Fix this error by today hopefully + Pose3d robotPose = questPose.transformBy(QuestK.ROBOT_TO_QUEST.inverse()); if (!initializedOdometryFromVision) { resetOdometry(robotPose.toPose2d()); initializedOdometryFromVision = true; From 9bed4c4ef4666ae684de283495db91d1c11fdad2 Mon Sep 17 00:00:00 2001 From: ADavis012 Date: Thu, 6 Nov 2025 17:29:46 -0600 Subject: [PATCH 09/21] Switched pose3d to pose2d for quest in swerve code, working on quest code and swerve (134-142) --- src/main/java/frc/robot/Constants.java | 5 +++-- src/main/java/frc/robot/subsystems/Quest.java | 20 +++++++++---------- .../java/frc/robot/subsystems/Swerve.java | 10 +++++----- 3 files changed, 18 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 2715cee..fcafdf3 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -33,8 +33,9 @@ import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Pair; import edu.wpi.first.math.VecBuilder; -import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; @@ -311,7 +312,7 @@ public static class ClimbK { .withReverseSoftLimitThreshold(minAngle); } public static class QuestK { - public static final Transform3d ROBOT_TO_QUEST = new Transform3d(new Pose3d(), new Pose3d()); //! FIND + public static final Transform2d ROBOT_TO_QUEST = new Transform2d(new Pose2d(), new Pose2d()); //! FIND } } diff --git a/src/main/java/frc/robot/subsystems/Quest.java b/src/main/java/frc/robot/subsystems/Quest.java index 161ff11..f4c50cf 100644 --- a/src/main/java/frc/robot/subsystems/Quest.java +++ b/src/main/java/frc/robot/subsystems/Quest.java @@ -13,11 +13,12 @@ import frc.robot.Constants.QuestK; import gg.questnav.questnav.PoseFrame; import gg.questnav.questnav.QuestNav; -import swervelib.SwerveDrive; public class Quest extends SubsystemBase { QuestNav questNav = new QuestNav(); + private Swerve swerve = new Swerve(null, null); + private final SwerveDrivePoseEstimator poseEstimator = new SwerveDrivePoseEstimator(null, null, null, null); private void GetPose() { @@ -50,16 +51,15 @@ private void SetPose() { questNav.setPose(questPose); } + Matrix QUESTNAV_STD_DEVS = + VecBuilder.fill( + 0.02, // Trust down to 2cm in X direction + 0.02, // Trust down to 2cm in Y direction + 0.035 // Trust down to 2 degrees rotational +); - private void QuestSwerve() { - SwerveDrive swerveDrive = new SwerveDrive(null, null, 0, null); - Matrix QUESTNAV_STD_DEVS = - VecBuilder.fill( - 0.02, // Trust down to 2cm in X direction - 0.02, // Trust down to 2cm in Y direction - 0.035 // Trust down to 2 degrees rotational - ); - + @Override + public void periodic() { if (questNav.isTracking()) { // Get the latest pose data frames from the Quest PoseFrame[] questFrames = questNav.getAllUnreadPoseFrames(); diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 6715e2b..4bdbeca 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -5,6 +5,7 @@ import static edu.wpi.first.units.Units.MetersPerSecond; import static edu.wpi.first.units.Units.RadiansPerSecond; import static edu.wpi.first.units.Units.Seconds; +import static edu.wpi.first.units.Units.Volts; import java.io.IOException; import java.util.Set; @@ -28,7 +29,6 @@ import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveModuleState; @@ -130,14 +130,14 @@ public void periodic() { SmartDashboard.putNumber("Y setpoint", yController.getSetpoint().position); for (var pose : questSource.get().results()) { - Pose3d questPose = pose.questPose(); // Fix this error by today hopefully - Pose3d robotPose = questPose.transformBy(QuestK.ROBOT_TO_QUEST.inverse()); + Pose2d questPose = pose.questPose(); + Pose2d robotPose = questPose.transformBy(QuestK.ROBOT_TO_QUEST.inverse()); if (!initializedOdometryFromVision) { - resetOdometry(robotPose.toPose2d()); initializedOdometryFromVision = true; continue; } - swerveDrive.addVisionMeasurement(pose.estimatedPose.toPose2d(), pose.timestampSeconds, result.getSecond()); + + swerveDrive.addVisionMeasurement(robotPose, ctreTimestamp, QUESTNAV_STD_DEVS, result.getSecond()); } } From 62e407a253b6a0a88841a5bbd506eda8a021219d Mon Sep 17 00:00:00 2001 From: FarmerOfEggplants Date: Tue, 11 Nov 2025 16:01:51 -0600 Subject: [PATCH 10/21] Save conflict fix (pls dont cause issues pls pls pls) --- src/main/java/frc/robot/subsystems/Quest.java | 16 ++++++++-------- src/main/java/frc/robot/subsystems/Swerve.java | 2 +- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Quest.java b/src/main/java/frc/robot/subsystems/Quest.java index f4c50cf..75e1922 100644 --- a/src/main/java/frc/robot/subsystems/Quest.java +++ b/src/main/java/frc/robot/subsystems/Quest.java @@ -20,7 +20,7 @@ public class Quest extends SubsystemBase { private Swerve swerve = new Swerve(null, null); private final SwerveDrivePoseEstimator poseEstimator = new SwerveDrivePoseEstimator(null, null, null, null); - + private void GetPose() { // First, Declare our geometrical transform from the robot center to the Quest Transform2d ROBOT_TO_QUEST = new Transform2d( /*TODO: Put your x, y, rotational offsets here!*/ ); @@ -51,12 +51,12 @@ private void SetPose() { questNav.setPose(questPose); } - Matrix QUESTNAV_STD_DEVS = - VecBuilder.fill( - 0.02, // Trust down to 2cm in X direction - 0.02, // Trust down to 2cm in Y direction - 0.035 // Trust down to 2 degrees rotational -); + Matrix QUESTNAV_STD_DEVS = + VecBuilder.fill( + 0.02, // Trust down to 2cm in X direction + 0.02, // Trust down to 2cm in Y direction + 0.035 // Trust down to 2 degrees rotational + ); @Override public void periodic() { @@ -80,7 +80,7 @@ public void periodic() { // // You can put some sort of filtering here if you would like! // // Add the measurement to our estimator - swerveDrive.addVisionMeasurement(robotPose, ctreTimestamp, QUESTNAV_STD_DEVS); + swerve.addVisionMeasurement(robotPose, timestamp, QUESTNAV_STD_DEVS); }; } } diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 4bdbeca..9e97a34 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -137,7 +137,7 @@ public void periodic() { continue; } - swerveDrive.addVisionMeasurement(robotPose, ctreTimestamp, QUESTNAV_STD_DEVS, result.getSecond()); + swerveDrive.addVisionMeasurement(robotPose, pose.ctreTimestamp, QUESTNAV_STD_DEVS, result.getSecond()); } } From 5b2e1af885615a51a9e2cec50d06b0e02976d0b3 Mon Sep 17 00:00:00 2001 From: ADavis012 Date: Thu, 13 Nov 2025 16:46:14 -0600 Subject: [PATCH 11/21] Got the addVisionMeasurement to stop having a bunch of errors by changing swerve. to swerveDrive. --- src/main/java/frc/robot/subsystems/Quest.java | 5 ++++- src/main/java/frc/robot/subsystems/Swerve.java | 2 +- 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Quest.java b/src/main/java/frc/robot/subsystems/Quest.java index 75e1922..4c83ba1 100644 --- a/src/main/java/frc/robot/subsystems/Quest.java +++ b/src/main/java/frc/robot/subsystems/Quest.java @@ -13,12 +13,15 @@ import frc.robot.Constants.QuestK; import gg.questnav.questnav.PoseFrame; import gg.questnav.questnav.QuestNav; +import swervelib.SwerveDrive; public class Quest extends SubsystemBase { QuestNav questNav = new QuestNav(); private Swerve swerve = new Swerve(null, null); + private final SwerveDrive swerveDrive; + private final SwerveDrivePoseEstimator poseEstimator = new SwerveDrivePoseEstimator(null, null, null, null); private void GetPose() { @@ -80,7 +83,7 @@ public void periodic() { // // You can put some sort of filtering here if you would like! // // Add the measurement to our estimator - swerve.addVisionMeasurement(robotPose, timestamp, QUESTNAV_STD_DEVS); + swerveDrive.addVisionMeasurement(robotPose, timestamp, QUESTNAV_STD_DEVS); }; } } diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 9e97a34..aa0ee69 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -137,7 +137,7 @@ public void periodic() { continue; } - swerveDrive.addVisionMeasurement(robotPose, pose.ctreTimestamp, QUESTNAV_STD_DEVS, result.getSecond()); + swerveDrive.addVisionMeasurement(robotPose, timestamp, QUESTNAV_STD_DEVS, result.getSecond()); } } From 307584015911b23dc391e280d055087d2570474f Mon Sep 17 00:00:00 2001 From: FarmerOfEggplants Date: Tue, 25 Nov 2025 16:19:35 -0600 Subject: [PATCH 12/21] okay sir :3 --- src/main/java/frc/robot/subsystems/Quest.java | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/Quest.java b/src/main/java/frc/robot/subsystems/Quest.java index 4c83ba1..c9e7e99 100644 --- a/src/main/java/frc/robot/subsystems/Quest.java +++ b/src/main/java/frc/robot/subsystems/Quest.java @@ -61,6 +61,15 @@ private void SetPose() { 0.035 // Trust down to 2 degrees rotational ); + + + + // chris is short + + + + + @Override public void periodic() { if (questNav.isTracking()) { From de9a75fd1cc5685c75be90ab684c135ff7248c4c Mon Sep 17 00:00:00 2001 From: chris Date: Tue, 25 Nov 2025 16:24:49 -0600 Subject: [PATCH 13/21] remove unneeded comment --- src/main/java/frc/robot/subsystems/Quest.java | 8 -------- 1 file changed, 8 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Quest.java b/src/main/java/frc/robot/subsystems/Quest.java index c9e7e99..5b139a9 100644 --- a/src/main/java/frc/robot/subsystems/Quest.java +++ b/src/main/java/frc/robot/subsystems/Quest.java @@ -62,14 +62,6 @@ private void SetPose() { ); - - - // chris is short - - - - - @Override public void periodic() { if (questNav.isTracking()) { From ad1e93d2cf4678697c7aabb343faad3027ed0a13 Mon Sep 17 00:00:00 2001 From: ADavis012 Date: Tue, 2 Dec 2025 16:50:48 -0600 Subject: [PATCH 14/21] Commented out unneeded vision code in Swerve (including initializedOdometryFromVision) --- .../java/frc/robot/subsystems/Swerve.java | 31 ++++++++++--------- 1 file changed, 16 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index aa0ee69..d0e17e7 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -44,7 +44,6 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.Constants.ControllerK; -import frc.robot.Constants.QuestK; import frc.robot.Constants.SwerveK; import frc.robot.Robot; import frc.robot.commands.DriveWheelCharacterization; @@ -66,7 +65,7 @@ public class Swerve extends SubsystemBase { // physicalproperties/conversionFact private final TalonFX backRight; private final SysIdRoutine sysIdRoutine; private final PPHolonomicDriveController pathPlannerController = new PPHolonomicDriveController(SwerveK.ppTranslationConstants, SwerveK.ppRotationConstants); - private boolean initializedOdometryFromVision = false; //! replace with Quest + //private boolean initializedOdometryFromVision = false; //! may not be needed with questnav @SuppressWarnings("unused") private Pose2d pathPlannerTarget = Pose2d.kZero; // For logging // PID Alignment @@ -124,21 +123,22 @@ public Swerve(Supplier questSource, BooleanSupplier overridePathFo backRight.getConfigurator().apply(SwerveK.currentLimitsConfig); } - @Override //! this space needs more work connecting QuestNav in place of Vision + @Override public void periodic() { SmartDashboard.putNumber("X setpoint", xController.getSetpoint().position); SmartDashboard.putNumber("Y setpoint", yController.getSetpoint().position); - for (var pose : questSource.get().results()) { - Pose2d questPose = pose.questPose(); - Pose2d robotPose = questPose.transformBy(QuestK.ROBOT_TO_QUEST.inverse()); - if (!initializedOdometryFromVision) { - initializedOdometryFromVision = true; - continue; - } + //! this code was origionally made for vision, we altered it for questnav but it may not be needed here + //for (var pose : questSource.get().results()) { + //Pose2d questPose = pose.questPose(); + //Pose2d robotPose = questPose.transformBy(QuestK.ROBOT_TO_QUEST.inverse()); + //if (!initializedOdometryFromVision) { + //initializedOdometryFromVision = true; + //continue; + //} - swerveDrive.addVisionMeasurement(robotPose, timestamp, QUESTNAV_STD_DEVS, result.getSecond()); - } + //swerveDrive.addVisionMeasurement(robotPose, timestamp, QUESTNAV_STD_DEVS, result.getSecond()); + //} } private void setupPathPlanner() { @@ -341,9 +341,10 @@ public String getCurrentCommandName() { return cmd.getName(); } - public boolean initializedOdometryFromVision() { - return initializedOdometryFromVision; - } + //! may not be needed with questnav + //public boolean initializedOdometryFromVision() { + //return initializedOdometryFromVision; + //} /** * Resets the gyro and odometry to the current position but the current direction is now seen as 0. From 8f9bd10cc038b1a61a5462ee76ada3dce530a8ef Mon Sep 17 00:00:00 2001 From: FarmerOfEggplants Date: Thu, 4 Dec 2025 18:52:46 -0600 Subject: [PATCH 15/21] began to work on mounting the quest, couple pose type changes. --- src/main/java/frc/robot/Constants.java | 6 ++---- src/main/java/frc/robot/subsystems/Quest.java | 8 +++++--- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index fcafdf3..43e647b 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -33,9 +33,8 @@ import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Pair; import edu.wpi.first.math.VecBuilder; -import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; @@ -312,7 +311,6 @@ public static class ClimbK { .withReverseSoftLimitThreshold(minAngle); } public static class QuestK { - public static final Transform2d ROBOT_TO_QUEST = new Transform2d(new Pose2d(), new Pose2d()); //! FIND + public static final Transform3d ROBOT_TO_QUEST = new Transform3d(new Pose3d(), new Pose3d()); //! FIND } - } diff --git a/src/main/java/frc/robot/subsystems/Quest.java b/src/main/java/frc/robot/subsystems/Quest.java index 5b139a9..13a2cdb 100644 --- a/src/main/java/frc/robot/subsystems/Quest.java +++ b/src/main/java/frc/robot/subsystems/Quest.java @@ -6,7 +6,9 @@ import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -42,13 +44,13 @@ private void GetPose() { private void SetPose() { // First, Declare our geometrical transform from the robot center to the quest - Transform2d ROBOT_TO_QUEST = new Transform2d( /*TODO: Put your x, y, rotational offsets here!*/ ); + Transform3d ROBOT_TO_QUEST = new Transform3d( /*TODO: Put your x, y, rotational offsets here!*/ ); // Assume this is the requested reset pose - Pose2d robotPose = new Pose2d( /* Some pose data */ ); + Pose3d robotPose = new Pose3d( /* Some pose data */ ); // Transform by the offset to get the Quest pose - Pose2d questPose = robotPose.transformBy(ROBOT_TO_QUEST); + Pose3d questPose = robotPose.transformBy(ROBOT_TO_QUEST); // Send the reset operation questNav.setPose(questPose); From 016259f81592975ae162d26e164a2e2c93a73dea Mon Sep 17 00:00:00 2001 From: FarmerOfEggplants Date: Thu, 11 Dec 2025 15:34:57 -0600 Subject: [PATCH 16/21] updated questnav vendordep to 2.1.0 --- src/main/java/frc/robot/subsystems/Quest.java | 1 + vendordeps/questnavlib.json | 6 +++--- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Quest.java b/src/main/java/frc/robot/subsystems/Quest.java index 13a2cdb..2294217 100644 --- a/src/main/java/frc/robot/subsystems/Quest.java +++ b/src/main/java/frc/robot/subsystems/Quest.java @@ -75,6 +75,7 @@ public void periodic() { // Get the pose of the Quest Pose2d questPose = questFrame.questPose(); // Get timestamp for when the data was sent + double timestamp = questFrame.dataTimestamp(); // Transform by the mount pose to get your robot pose diff --git a/vendordeps/questnavlib.json b/vendordeps/questnavlib.json index c7c60d8..5a15703 100644 --- a/vendordeps/questnavlib.json +++ b/vendordeps/questnavlib.json @@ -1,19 +1,19 @@ { "fileName": "questnavlib.json", "name": "questnavlib", - "version": "2025-1.1.1-beta", + "version": "2025-2.1.0-beta", "uuid": "a706fe68-86e5-4aed-92c5-ce05aca007f0", "frcYear": "2025", "mavenUrls": [ "https://maven.questnav.gg/releases", "https://maven.questnav.gg/snapshots" ], - "jsonUrl": "https://maven.questnav.gg/releases/gg/questnav/questnavlib-json/2025-1.1.1-beta/questnavlib-json-2025-1.1.1-beta.json", + "jsonUrl": "https://maven.questnav.gg/releases/gg/questnav/questnavlib-json/2025-2.1.0-beta/questnavlib-json-2025-2.1.0-beta.json", "javaDependencies": [ { "groupId": "gg.questnav", "artifactId": "questnavlib-java", - "version": "2025-1.1.1-beta" + "version": "2025-2.1.0-beta" } ], "cppDependencies": [], From ac392d7ea477f01793ddba500872131b507f73c8 Mon Sep 17 00:00:00 2001 From: FarmerOfEggplants Date: Thu, 11 Dec 2025 16:18:32 -0600 Subject: [PATCH 17/21] =?UTF-8?q?2d=20=E2=86=92=203d?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/main/java/frc/robot/subsystems/Quest.java | 13 +++++-------- 1 file changed, 5 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Quest.java b/src/main/java/frc/robot/subsystems/Quest.java index 2294217..8b76eca 100644 --- a/src/main/java/frc/robot/subsystems/Quest.java +++ b/src/main/java/frc/robot/subsystems/Quest.java @@ -7,7 +7,6 @@ import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; @@ -28,17 +27,17 @@ public class Quest extends SubsystemBase { private void GetPose() { // First, Declare our geometrical transform from the robot center to the Quest - Transform2d ROBOT_TO_QUEST = new Transform2d( /*TODO: Put your x, y, rotational offsets here!*/ ); + Transform3d ROBOT_TO_QUEST = new Transform3d( /*TODO: Put your x, y, rotational offsets here!*/ ); // Get the latest pose data frames from the Quest PoseFrame[] poseFrames = questNav.getAllUnreadPoseFrames(); if (poseFrames.length > 0) { // Get the most recent Quest pose - Pose2d questPose = poseFrames[poseFrames.length - 1].questPose(); + Pose3d questPose = poseFrames[poseFrames.length - 1].questPose(); // Transform by the mount pose to get your robot pose - Pose2d robotPose = questPose.transformBy(ROBOT_TO_QUEST.inverse()); + Pose3d robotPose = questPose.transformBy(ROBOT_TO_QUEST.inverse()); } } @@ -73,11 +72,9 @@ public void periodic() { // Loop over the pose data frames and send them to the pose estimator for (PoseFrame questFrame : questFrames) { // Get the pose of the Quest - Pose2d questPose = questFrame.questPose(); + Pose3d questPose = questFrame.questPose(); // Get timestamp for when the data was sent - - double timestamp = questFrame.dataTimestamp(); - + double timestamp = questFrame.dataTimestamp() // Transform by the mount pose to get your robot pose Pose2d robotPose = questPose.transformBy(QuestK.questOffset.inverse()); From 17b89bfadf922464f3e18d360c20ba3ba1614068 Mon Sep 17 00:00:00 2001 From: ADavis012 Date: Thu, 11 Dec 2025 17:33:11 -0600 Subject: [PATCH 18/21] Worked on fixing errors in Quest --- src/main/java/frc/robot/subsystems/Quest.java | 24 +++++++++---------- 1 file changed, 11 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Quest.java b/src/main/java/frc/robot/subsystems/Quest.java index 8b76eca..540165d 100644 --- a/src/main/java/frc/robot/subsystems/Quest.java +++ b/src/main/java/frc/robot/subsystems/Quest.java @@ -4,8 +4,6 @@ import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; -import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; -import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.numbers.N1; @@ -19,13 +17,13 @@ public class Quest extends SubsystemBase { QuestNav questNav = new QuestNav(); - private Swerve swerve = new Swerve(null, null); + //private Swerve swerve = new Swerve(null, null); - private final SwerveDrive swerveDrive; + private SwerveDrive swerveDrive; - private final SwerveDrivePoseEstimator poseEstimator = new SwerveDrivePoseEstimator(null, null, null, null); + //private final SwerveDrivePoseEstimator poseEstimator = new SwerveDrivePoseEstimator(null, null, null, null); - private void GetPose() { + public void GetPose() { // First, Declare our geometrical transform from the robot center to the Quest Transform3d ROBOT_TO_QUEST = new Transform3d( /*TODO: Put your x, y, rotational offsets here!*/ ); @@ -34,16 +32,16 @@ private void GetPose() { if (poseFrames.length > 0) { // Get the most recent Quest pose - Pose3d questPose = poseFrames[poseFrames.length - 1].questPose(); + Pose3d questPose = poseFrames[poseFrames.length - 1].questPose3d(); // Transform by the mount pose to get your robot pose Pose3d robotPose = questPose.transformBy(ROBOT_TO_QUEST.inverse()); } } - private void SetPose() { + public void SetPose() { // First, Declare our geometrical transform from the robot center to the quest - Transform3d ROBOT_TO_QUEST = new Transform3d( /*TODO: Put your x, y, rotational offsets here!*/ ); + Transform3d ROBOT_TO_QUEST = new Transform3d( /*TODO: Put your x, y, z, yaw, pitch, and roll offsets here!*/ ); // Assume this is the requested reset pose Pose3d robotPose = new Pose3d( /* Some pose data */ ); @@ -72,11 +70,11 @@ public void periodic() { // Loop over the pose data frames and send them to the pose estimator for (PoseFrame questFrame : questFrames) { // Get the pose of the Quest - Pose3d questPose = questFrame.questPose(); + Pose3d questPose = questFrame.questPose3d(); // Get timestamp for when the data was sent - double timestamp = questFrame.dataTimestamp() + double timestamp = questFrame.dataTimestamp(); // Transform by the mount pose to get your robot pose - Pose2d robotPose = questPose.transformBy(QuestK.questOffset.inverse()); + Pose3d robotPose = questPose.transformBy(QuestK.questOffset.inverse()); // Convert FPGA timestamp to CTRE's time domain using Phoenix 6 utility double ctreTimestamp = Utils.fpgaToCurrentTime(timestamp); @@ -84,7 +82,7 @@ public void periodic() { // // You can put some sort of filtering here if you would like! // // Add the measurement to our estimator - swerveDrive.addVisionMeasurement(robotPose, timestamp, QUESTNAV_STD_DEVS); + swerveDrive.addVisionMeasurement(robotPose.toPose2d(), timestamp, QUESTNAV_STD_DEVS); }; } } From edc2bcc5a64d7adab48dfe790751a2f4e65b383a Mon Sep 17 00:00:00 2001 From: FarmerOfEggplants Date: Fri, 19 Dec 2025 09:53:06 -0600 Subject: [PATCH 19/21] fixed errors in Robot.java. Uncommented initializedOdometryFromVision for now --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/Robot.java | 10 ++++--- src/main/java/frc/robot/subsystems/Quest.java | 14 +++++----- .../java/frc/robot/subsystems/Swerve.java | 26 +++++++++---------- 4 files changed, 28 insertions(+), 24 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 43e647b..526b454 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -311,6 +311,6 @@ public static class ClimbK { .withReverseSoftLimitThreshold(minAngle); } public static class QuestK { - public static final Transform3d ROBOT_TO_QUEST = new Transform3d(new Pose3d(), new Pose3d()); //! FIND + public static final Transform3d ROBOT_TO_QUEST = new Transform3d(new Pose3d(0,0,0, new Rotation3d()), new Pose3d()); //! FIND } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 6d5f2ee..4642f31 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -52,20 +52,24 @@ import frc.robot.subsystems.Climb; import frc.robot.subsystems.Elevator; import frc.robot.subsystems.Intake; +import frc.robot.subsystems.Quest; import frc.robot.subsystems.Swerve; -import frc.robot.subsystems.Vision; @Logged public class Robot extends TimedRobot { @Logged(name = "Vision") - private final Vision vision = new Vision(); + private final Quest quest = new Quest(); private final CommandXboxController xboxController = new CommandXboxController(ControllerK.xboxPort); @Logged(name = "Swerve") - private final Swerve swerve = new Swerve(vision::getVisionResults, () -> + private final Swerve swerve = new Swerve(quest::getQuestResults, () -> Math.abs(xboxController.getLeftX()) > ControllerK.overrideThreshold || Math.abs(xboxController.getLeftY()) > ControllerK.overrideThreshold || Math.abs(xboxController.getRightX()) > ControllerK.overrideThreshold); + // private final Swerve swerve = new Swerve(vision::getVisionResults, () -> + // Math.abs(xboxController.getLeftX()) > ControllerK.overrideThreshold + // || Math.abs(xboxController.getLeftY()) > ControllerK.overrideThreshold + // || Math.abs(xboxController.getRightX()) > ControllerK.overrideThreshold); @Logged(name = "Elevator") private final Elevator elevator = new Elevator(); @Logged(name = "Intake") diff --git a/src/main/java/frc/robot/subsystems/Quest.java b/src/main/java/frc/robot/subsystems/Quest.java index 540165d..232f7a7 100644 --- a/src/main/java/frc/robot/subsystems/Quest.java +++ b/src/main/java/frc/robot/subsystems/Quest.java @@ -5,7 +5,6 @@ import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -25,7 +24,7 @@ public class Quest extends SubsystemBase { public void GetPose() { // First, Declare our geometrical transform from the robot center to the Quest - Transform3d ROBOT_TO_QUEST = new Transform3d( /*TODO: Put your x, y, rotational offsets here!*/ ); + // Get the latest pose data frames from the Quest PoseFrame[] poseFrames = questNav.getAllUnreadPoseFrames(); @@ -35,19 +34,19 @@ public void GetPose() { Pose3d questPose = poseFrames[poseFrames.length - 1].questPose3d(); // Transform by the mount pose to get your robot pose - Pose3d robotPose = questPose.transformBy(ROBOT_TO_QUEST.inverse()); + Pose3d robotPose = questPose.transformBy(QuestK.ROBOT_TO_QUEST.inverse()); } } public void SetPose() { // First, Declare our geometrical transform from the robot center to the quest - Transform3d ROBOT_TO_QUEST = new Transform3d( /*TODO: Put your x, y, z, yaw, pitch, and roll offsets here!*/ ); + // Transform3d ROBOT_TO_QUEST = new Transform3d( /*TODO: Put your x, y, z, yaw, pitch, and roll offsets here!*/ ); // Assume this is the requested reset pose Pose3d robotPose = new Pose3d( /* Some pose data */ ); // Transform by the offset to get the Quest pose - Pose3d questPose = robotPose.transformBy(ROBOT_TO_QUEST); + Pose3d questPose = robotPose.transformBy(QuestK.ROBOT_TO_QUEST); // Send the reset operation questNav.setPose(questPose); @@ -63,18 +62,19 @@ public void SetPose() { @Override public void periodic() { + if (questNav.isTracking()) { // Get the latest pose data frames from the Quest PoseFrame[] questFrames = questNav.getAllUnreadPoseFrames(); - // Loop over the pose data frames and send them to the pose estimator + // Loop over the pose data frames and send them to the pose estimator for (PoseFrame questFrame : questFrames) { // Get the pose of the Quest Pose3d questPose = questFrame.questPose3d(); // Get timestamp for when the data was sent double timestamp = questFrame.dataTimestamp(); // Transform by the mount pose to get your robot pose - Pose3d robotPose = questPose.transformBy(QuestK.questOffset.inverse()); + Pose3d robotPose = questPose.transformBy(QuestK.ROBOT_TO_QUEST.inverse()); // Convert FPGA timestamp to CTRE's time domain using Phoenix 6 utility double ctreTimestamp = Utils.fpgaToCurrentTime(timestamp); diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index d0e17e7..c89d93f 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -65,7 +65,7 @@ public class Swerve extends SubsystemBase { // physicalproperties/conversionFact private final TalonFX backRight; private final SysIdRoutine sysIdRoutine; private final PPHolonomicDriveController pathPlannerController = new PPHolonomicDriveController(SwerveK.ppTranslationConstants, SwerveK.ppRotationConstants); - //private boolean initializedOdometryFromVision = false; //! may not be needed with questnav + private boolean initializedOdometryFromVision = false; //! may not be needed with questnav @SuppressWarnings("unused") private Pose2d pathPlannerTarget = Pose2d.kZero; // For logging // PID Alignment @@ -129,16 +129,16 @@ public void periodic() { SmartDashboard.putNumber("Y setpoint", yController.getSetpoint().position); //! this code was origionally made for vision, we altered it for questnav but it may not be needed here - //for (var pose : questSource.get().results()) { - //Pose2d questPose = pose.questPose(); - //Pose2d robotPose = questPose.transformBy(QuestK.ROBOT_TO_QUEST.inverse()); - //if (!initializedOdometryFromVision) { - //initializedOdometryFromVision = true; - //continue; - //} + // for (var pose : questSource.get().results()) { + // Pose2d questPose = pose.questPose(); + // Pose2d robotPose = questPose.transformBy(QuestK.ROBOT_TO_QUEST.inverse()); + // if (!initializedOdometryFromVision) { + // initializedOdometryFromVision = true; + // continue; + // } - //swerveDrive.addVisionMeasurement(robotPose, timestamp, QUESTNAV_STD_DEVS, result.getSecond()); - //} + // swerveDrive.addVisionMeasurement(robotPose, timestamp, QUESTNAV_STD_DEVS, result.getSecond()); + // } } private void setupPathPlanner() { @@ -342,9 +342,9 @@ public String getCurrentCommandName() { } //! may not be needed with questnav - //public boolean initializedOdometryFromVision() { - //return initializedOdometryFromVision; - //} + public boolean initializedOdometryFromVision() { + return initializedOdometryFromVision; + } /** * Resets the gyro and odometry to the current position but the current direction is now seen as 0. From 52a4f85f254af95c1304e3ddc6c602665d901270 Mon Sep 17 00:00:00 2001 From: FarmerOfEggplants Date: Tue, 23 Dec 2025 16:22:27 -0600 Subject: [PATCH 20/21] added @Logged to qutestnav imports --- src/main/java/frc/robot/subsystems/Quest.java | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/Quest.java b/src/main/java/frc/robot/subsystems/Quest.java index 232f7a7..09e8bec 100644 --- a/src/main/java/frc/robot/subsystems/Quest.java +++ b/src/main/java/frc/robot/subsystems/Quest.java @@ -2,6 +2,7 @@ import com.ctre.phoenix6.Utils; +import edu.wpi.first.epilogue.Logged; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.geometry.Pose3d; @@ -13,6 +14,7 @@ import gg.questnav.questnav.QuestNav; import swervelib.SwerveDrive; +@Logged public class Quest extends SubsystemBase { QuestNav questNav = new QuestNav(); From f64b7f847bfebe1b2636113a372053193a271c12 Mon Sep 17 00:00:00 2001 From: FarmerOfEggplants Date: Wed, 7 Jan 2026 09:33:24 -0600 Subject: [PATCH 21/21] fixed logging matrix bug (thanks Brock) --- src/main/java/frc/robot/subsystems/Quest.java | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Quest.java b/src/main/java/frc/robot/subsystems/Quest.java index 09e8bec..f625aa5 100644 --- a/src/main/java/frc/robot/subsystems/Quest.java +++ b/src/main/java/frc/robot/subsystems/Quest.java @@ -3,6 +3,7 @@ import com.ctre.phoenix6.Utils; import edu.wpi.first.epilogue.Logged; +import edu.wpi.first.epilogue.NotLogged; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.geometry.Pose3d; @@ -53,7 +54,7 @@ public void SetPose() { // Send the reset operation questNav.setPose(questPose); } - + @NotLogged Matrix QUESTNAV_STD_DEVS = VecBuilder.fill( 0.02, // Trust down to 2cm in X direction @@ -101,7 +102,7 @@ public QuestResults getQuestResults() { } return new QuestResults(questFrames); } - + @Logged(name = "Quest Results") public record QuestResults(PoseFrame[] results){} };