Add QuestNav implementations of Vision

src/main/deploy/pathplanner/paths/ThreeCoral1.path

@@ -51,4 +51,4 @@
     "rotation": 180.0
   },
   "useDefaultConstraints": false
-}
+}

src/main/java/frc/robot/Robot.java

@@ -41,6 +41,7 @@
 import frc.robot.commands.misc.StateCmd;
 import frc.robot.generated.CompTunerConstants;
 import frc.robot.subsystems.drive.Drive;
+import frc.robot.subsystems.vision.MotionTrackerVision;
 import frc.simulator.engine.SimulationEngine;
 
 /**
@@ -56,7 +57,7 @@ public class Robot extends LoggedRobot {
     private RobotContainer robotContainer;
 
     private boolean hasSetupAutos = false;
-    private AutoModeBaseCmd auto_cmd_ = null ;
+    private AutoModeBaseCmd appliedAuto = null ;
 
     public Robot() throws RuntimeException {
         //
@@ -216,21 +217,28 @@ public void disabledPeriodic() {
             hasSetupAutos = true;
         }
 
+
+
         if (hasSetupAutos) {
             Command cmd = robotContainer.getAutonomousCommand();
             if (cmd != null) {
-                AutoModeBaseCmd autoCmd = (AutoModeBaseCmd) cmd;
-                if (autoCmd != null) {
-                    Drive d = RobotContainer.getInstance().drivebase() ;
-                    Pose2d autopose = autoCmd.getStartingPose() ;
-                    if (auto_cmd_ == null || auto_cmd_ != autoCmd) {
-                        Logger.recordOutput("automode/name", autoCmd.getName()) ;
+                AutoModeBaseCmd currentAuto = (AutoModeBaseCmd) cmd;
+                if (currentAuto != null) {
+                    Drive drivebase = RobotContainer.getInstance().drivebase() ;
+                    MotionTrackerVision quest = RobotContainer.getInstance().quest();
+                    Pose2d autopose = currentAuto.getStartingPose() ;
+                    if (appliedAuto == null || appliedAuto != currentAuto) { // Changing the Auto Mode
+                        Logger.recordOutput("automode/name", currentAuto.getName()) ;
                         Logger.recordOutput("automode/pose", autopose) ;
-                        d.setPose(autopose) ;
-                        auto_cmd_ = autoCmd ;
+                        Logger.recordOutput("poseinit/setting", "Robot Pose");
+                        drivebase.setPose(autopose);
+                        appliedAuto = currentAuto;
+                    } else { // Initializing the Quest Repeatedly
+                        quest.setPose(drivebase.getPose());
+                        Logger.recordOutput("poseinit/setting", "Quest Pose");
                     }
                 }
-            }
+            } 
         }
     }
 

src/main/java/frc/robot/RobotContainer.java

@@ -30,6 +30,7 @@
 import org.xerosw.util.MessageLogger;
 import org.xerosw.util.MessageType;
 
+import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.units.measure.Distance;
 import edu.wpi.first.wpilibj.RobotState;
@@ -89,8 +90,9 @@
 import frc.robot.subsystems.vision.AprilTagVision;
 import frc.robot.subsystems.vision.CameraIO;
 import frc.robot.subsystems.vision.CameraIOLimelight4;
-import frc.robot.subsystems.vision.CameraIOPhotonSim;
-import frc.robot.subsystems.vision.PoseEstimateConsumer;
+import frc.robot.subsystems.vision.MotionTrackerVision;
+import frc.robot.subsystems.vision.TrackerIO;
+import frc.robot.subsystems.vision.TrackerIOQuest;
 import frc.robot.subsystems.vision.VisionConstants;
 import frc.robot.util.Mechanism3d;
 import frc.robot.util.ReefUtil;
@@ -123,6 +125,7 @@ public static RobotContainer getInstance() {
     // Subsystems
     private Drive drivebase_;
     private AprilTagVision vision_;
+    private MotionTrackerVision questnav_;
     private OISubsystem oi_;
     private ManipulatorSubsystem manipulator_;
     private GrabberSubsystem grabber_;
@@ -160,19 +163,24 @@ private RobotContainer() {
         if (Constants.getMode() != Mode.REPLAY) {
             switch (Constants.getRobot()) {
                 case COMPETITION:
+
                     drivebase_ = new Drive(
-                            new GyroIOPigeon2(CompTunerConstants.DrivetrainConstants.Pigeon2Id, CompTunerConstants.kCANBus),
-                                ModuleIOTalonFX::new,
-                                CompTunerConstants.FrontLeft,
-                                CompTunerConstants.FrontRight,
-                                CompTunerConstants.BackLeft,
-                                CompTunerConstants.BackRight,
-                                CompTunerConstants.kSpeedAt12Volts);
-
+                        new GyroIOPigeon2(CompTunerConstants.DrivetrainConstants.Pigeon2Id, CompTunerConstants.kCANBus),
+                        ModuleIOTalonFX::new,
+                        CompTunerConstants.FrontLeft,
+                        CompTunerConstants.FrontRight,
+                        CompTunerConstants.BackLeft,
+                        CompTunerConstants.BackRight,
+                        CompTunerConstants.kSpeedAt12Volts
+                    );
+
+                    questnav_ = new MotionTrackerVision(new TrackerIOQuest(), drivebase_::addVisionMeasurement);
+
                     vision_ = new AprilTagVision(
                         drivebase_::addVisionMeasurement,
                         new CameraIOLimelight4(VisionConstants.frontLimelightName, drivebase_::getRotation)
                     );
+                    // vision_.setTagFilterDistance(Meters.of(1.2));
 
                     try {
                         manipulator_ = new ManipulatorSubsystem(new ManipulatorIOHardware());
@@ -257,14 +265,6 @@ private RobotContainer() {
                             CompTunerConstants.BackRight,
                             CompTunerConstants.kSpeedAt12Volts);
 
-                    vision_ = new AprilTagVision(
-                        PoseEstimateConsumer.ignore(),
-                        new CameraIOPhotonSim("Front", VisionConstants.frontTransform,
-                        drivebase_::getPose, true),
-                        new CameraIOPhotonSim("Back", VisionConstants.backTransform,
-                        drivebase_::getPose, true)
-                    );
-
                     try {
                         manipulator_ = new ManipulatorSubsystem(new ManipulatorIOHardware());
                     } catch (Exception ex) {
@@ -327,7 +327,7 @@ private RobotContainer() {
 
         if (vision_ == null) {
             int numCams = switch (Constants.getRobot()) {
-                default -> 3;
+                default -> 1;
             };
 
             CameraIO[] cams = new CameraIO[numCams];
@@ -339,6 +339,10 @@ private RobotContainer() {
                     cams);
         }
 
+        if (questnav_ == null) {
+            questnav_ = new MotionTrackerVision(new TrackerIO() {}, drivebase_::addVisionMeasurement);
+        }
+
         if (manipulator_ == null) {
             manipulator_ = new ManipulatorSubsystem(new ManipulatorIO() {
             });
@@ -369,7 +373,7 @@ private RobotContainer() {
             () -> -gamepad_.getLeftY(),
             () -> -gamepad_.getLeftX(),
             () -> -gamepad_.getRightX()
-        );        
+        );
 
         // Shuffleboard Tabs
         ShuffleboardTab autonomousTab = Shuffleboard.getTab("Autonomous");
@@ -416,6 +420,10 @@ public Drive drivebase() {
         return drivebase_;
     }
 
+    public MotionTrackerVision quest() {
+        return questnav_;
+    }
+
     public XeroGamepad gamepad() {
         return gamepad_;
     }
@@ -456,6 +464,8 @@ public void setupAutos() {
         odometryTest.addCommands(DriveCommands.initialFollowPathCommand(drivebase_, "Odom Test"));
 
         autoChooser_.addOption("Odom Test (aka Kachow)", odometryTest);
+
+        autoChooser_.addOption("Zero Pose Reset", new AutoModeBaseCmd("Zero Pose", Pose2d.kZero));
     }
 
     private void subsystemCreateException(Exception ex) {
@@ -599,28 +609,11 @@ private void configureDriveBindings() {
      * @return the command to run in autonomous
      */
     public Command getAutonomousCommand() {
-        Command ret = null;
-
         if (Robot.useXeroSimulator()) {
-            //
-            // In the Xero simulator, set the auto mode you want to run
-            // Note: the auto used here must match the simulation stimulus file set in the
-            // Robot.java file.
-            //
-
-            // ret = AutoCommands.oneCoralAuto(brain_, drivebase_, manipulator_, grabber_) ;
-            ret = AutoCommands.threeCoralSideAuto(brain_, vision_, drivebase_, manipulator_, grabber_, funnel_, true) ;
-            // ret = AutoCommands.oneCoralOneAlgaeAuto(brain_, drivebase_, manipulator_, grabber_) ;
-            // ret = AutoCommands.twoCoralCenterAuto(brain_, drivebase_, manipulator_, grabber_, funnel_, true);
-
-            // Command autoChosen = autoChooser_.get();
-            // ret = autoChosen != null ? autoChosen : tuningChooser_.get();
-
-        } else {
-            Command autoChosen = autoChooser_.get();
-            ret = autoChosen != null ? autoChosen : tuningChooser_.get();
+            return AutoCommands.threeCoralSideAuto(brain_, vision_, drivebase_, manipulator_, grabber_, funnel_, true);
         }
 
-        return ret;
+        Command autoChosen = autoChooser_.get();
+        return autoChosen != null ? autoChosen : tuningChooser_.get();
     }
 }

src/main/java/frc/robot/subsystems/drive/Drive.java

@@ -280,6 +280,10 @@ public void periodic() {
 
         ChassisSpeeds spd = getChassisSpeeds() ;
         Logger.recordOutput("drive/velocity", Math.hypot(spd.vxMetersPerSecond, spd.vyMetersPerSecond)) ;
+
+        Pose2d pose = getPose();
+        Logger.recordOutput("Odometry/Individual/X", pose.getX());
+        Logger.recordOutput("Odometry/Individual/Y", pose.getY());
     }
 
     /**
@@ -472,8 +476,7 @@ public void addVisionMeasurement(
     double timestampSeconds,
     Matrix<N3, N1> visionMeasurementStdDevs) {
         Logger.recordOutput("Odometry/VisionMeasurement", visionRobotPoseMeters);
-        poseEstimator.addVisionMeasurement(
-        visionRobotPoseMeters, timestampSeconds, visionMeasurementStdDevs);
+        poseEstimator.addVisionMeasurement(visionRobotPoseMeters, timestampSeconds, visionMeasurementStdDevs);
     }
 
     /** Returns the maximum linear speed in meters per sec. */

src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java

@@ -17,6 +17,7 @@
 
 import com.ctre.phoenix6.BaseStatusSignal;
 import com.ctre.phoenix6.CANBus;
+import com.ctre.phoenix6.StatusCode;
 import com.ctre.phoenix6.StatusSignal;
 import com.ctre.phoenix6.configs.GyroTrimConfigs;
 import com.ctre.phoenix6.configs.Pigeon2Configuration;
@@ -52,7 +53,7 @@ public GyroIOPigeon2(int Pigeon2Id, CANBus bus) {
 
   @Override
   public void updateInputs(GyroIOInputs inputs) {
-    inputs.connected = BaseStatusSignal.refreshAll(yaw).isOK() ;
+    inputs.connected = BaseStatusSignal.refreshAll(yaw).equals(StatusCode.OK);
     inputs.yawPosition = Rotation2d.fromDegrees(yaw.getValueAsDouble());
 
     inputs.odometryYawTimestamps =

src/main/java/frc/robot/subsystems/vision/MotionTrackerVision.java

@@ -0,0 +1,100 @@
+
+package frc.robot.subsystems.vision;
+
+import static edu.wpi.first.units.Units.Degrees;
+import static edu.wpi.first.units.Units.Meters;
+
+import org.littletonrobotics.junction.Logger;
+
+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.Rotation2d;
+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.wpilibj.Alert;
+import edu.wpi.first.wpilibj.Alert.AlertType;
+import edu.wpi.first.wpilibj.RobotState;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import gg.questnav.questnav.PoseFrame;
+
+public class MotionTrackerVision extends SubsystemBase {
+
+    private static final Matrix<N3, N1> stdDevs = VecBuilder.fill(
+        0.01,
+        0.01,
+        0.01
+    );
+
+    private boolean useQuestNav = false;
+    private static final boolean calibration = false;
+    private static final boolean useQueuedFrames = true;
+
+    private static final Transform2d robotToQuest = new Transform2d(
+        !calibration ? Meters.of(0.199) : Meters.zero(),
+        !calibration ? Meters.of(0.282) : Meters.zero(),
+        new Rotation2d(Degrees.of(45))
+    );
+
+    // x: -0.216 y: -0.261
+
+    // time: 565-593
+    // time: 794-831
+    private final TrackerIO io_;
+    private final TrackerInputsAutoLogged inputs_;
+
+    private final Alert disconnectedAlert_ = new Alert("Motion Tracker Disconnected!", AlertType.kError);
+
+    private final Alert lowBatteryAlert_ = new Alert("Quest battery is low!", AlertType.kWarning);
+
+    private final PoseEstimateConsumer estimateConsumer_;
+
+    public MotionTrackerVision(TrackerIO io, PoseEstimateConsumer estimateConsumer) {
+        io_ = io;
+        inputs_ = new TrackerInputsAutoLogged();
+        estimateConsumer_ = estimateConsumer;
+    }
+
+    @Override
+    public void periodic() {
+        io_.updateInputs(inputs_);
+        Logger.processInputs("MotionTrackerVision", inputs_);
+
+        boolean disconnected = !inputs_.connected;
+        disconnectedAlert_.set(disconnected);
+
+        lowBatteryAlert_.set(inputs_.batteryPercent < 20);
+
+        if (
+            disconnected ||
+            !inputs_.isTracking ||
+            RobotState.isDisabled() ||
+            !useQuestNav
+        ) return;
+
+        if (useQueuedFrames) {
+            for (PoseFrame frame : inputs_.unreadFrames) {
+                Pose2d robotPose = frame.questPose().transformBy(robotToQuest.inverse());
+                double timestamp = frame.dataTimestamp();
+                estimateConsumer_.integrate(robotPose, timestamp, stdDevs);
+            }
+        } else {
+            PoseFrame frame = inputs_.unreadFrames[inputs_.unreadFrames.length - 1];
+            estimateConsumer_.integrate(
+                frame.questPose().transformBy(robotToQuest.inverse()),
+                frame.dataTimestamp(),
+                stdDevs
+            );
+        }
+    }
+
+    public void setPose(Pose2d pose) {
+        io_.setPose(pose.transformBy(robotToQuest));
+    }
+
+    public boolean isConnected() {
+        return inputs_.connected;
+    }
+
+}

src/main/java/frc/robot/subsystems/vision/TrackerIO.java

@@ -0,0 +1,23 @@
+package frc.robot.subsystems.vision;
+
+import org.littletonrobotics.junction.AutoLog;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import gg.questnav.questnav.PoseFrame;
+
+public interface TrackerIO {
+    @AutoLog
+    public static class TrackerInputs {
+        public boolean connected = false;
+        public double timestamp = 0.0;
+        public double batteryPercent = 0.0;
+        public long frameCount = 0;
+        public boolean isTracking = false;
+        public long trackingLostCount = 0;
+
+        public PoseFrame[] unreadFrames = {};
+    }
+
+    public default void updateInputs(TrackerInputs inputs) {}
+    public default void setPose(Pose2d pose) {}
+}