diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index c547e7d..526b454 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -33,6 +33,7 @@ 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.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.numbers.N1; @@ -309,5 +310,7 @@ public static class ClimbK { .withReverseSoftLimitEnable(true) .withReverseSoftLimitThreshold(minAngle); } - + public static class QuestK { + 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 new file mode 100644 index 0000000..f625aa5 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Quest.java @@ -0,0 +1,110 @@ +package frc.robot.subsystems; + +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; +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; + +@Logged +public class Quest extends SubsystemBase { + QuestNav questNav = new QuestNav(); + + //private Swerve swerve = new Swerve(null, null); + + private SwerveDrive swerveDrive; + + //private final SwerveDrivePoseEstimator poseEstimator = new SwerveDrivePoseEstimator(null, null, null, null); + + public void GetPose() { + // First, Declare our geometrical transform from the robot center to the Quest + + + // Get the latest pose data frames from the Quest + PoseFrame[] poseFrames = questNav.getAllUnreadPoseFrames(); + + if (poseFrames.length > 0) { + // Get the most recent Quest pose + Pose3d questPose = poseFrames[poseFrames.length - 1].questPose3d(); + + // Transform by the mount pose to get your robot pose + 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!*/ ); + + // 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(QuestK.ROBOT_TO_QUEST); + + // Send the reset operation + questNav.setPose(questPose); + } + @NotLogged + 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(); + + // 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.ROBOT_TO_QUEST.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.toPose2d(), timestamp, 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<>(); 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(questFrames); + } + @Logged(name = "Quest Results") + 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 d4efd2b..c89d93f 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -13,8 +13,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; @@ -49,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; @@ -60,14 +58,14 @@ 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; 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; //! may not be needed with questnav @SuppressWarnings("unused") private Pose2d pathPlannerTarget = Pose2d.kZero; // For logging // PID Alignment @@ -80,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; @@ -129,15 +127,18 @@ 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()) { - EstimatedRobotPose pose = result.getFirst(); - if (!initializedOdometryFromVision) { - resetOdometry(pose.estimatedPose.toPose2d()); - initializedOdometryFromVision = true; - continue; - } - swerveDrive.addVisionMeasurement(pose.estimatedPose.toPose2d(), pose.timestampSeconds, result.getSecond()); - } + + //! 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()); + // } } private void setupPathPlanner() { @@ -340,6 +341,7 @@ public String getCurrentCommandName() { return cmd.getName(); } + //! may not be needed with questnav public boolean initializedOdometryFromVision() { return initializedOdometryFromVision; } diff --git a/vendordeps/questnavlib.json b/vendordeps/questnavlib.json new file mode 100644 index 0000000..5a15703 --- /dev/null +++ b/vendordeps/questnavlib.json @@ -0,0 +1,21 @@ +{ + "fileName": "questnavlib.json", + "name": "questnavlib", + "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-2.1.0-beta/questnavlib-json-2025-2.1.0-beta.json", + "javaDependencies": [ + { + "groupId": "gg.questnav", + "artifactId": "questnavlib-java", + "version": "2025-2.1.0-beta" + } + ], + "cppDependencies": [], + "jniDependencies": [] +} \ No newline at end of file