diff --git a/src/main/java/frc/robot/subsystems/quest/Quest.java b/src/main/java/frc/robot/subsystems/quest/Quest.java index ee37cab..1c7c1bb 100644 --- a/src/main/java/frc/robot/subsystems/quest/Quest.java +++ b/src/main/java/frc/robot/subsystems/quest/Quest.java @@ -40,8 +40,21 @@ public void periodic() { Pose2d fieldToRobot = getFieldToRobot(); + /* + * "Does the Quest leave the field?" + * + * Check previous Global Pose and compare to the new compensated Quest pose. + */ + double measuredPoseDelta = + fieldToRobot.minus(BobotState.getGlobalPose()).getTranslation().getNorm(); + boolean isPoseWithinTolerance = measuredPoseDelta < QuestConstants.acceptableDistanceTolerance; + + Logger.recordOutput("Oculus/MeasuredPoseDelta", measuredPoseDelta); + Logger.recordOutput("Oculus/IsPoseWithinTolerance", isPoseWithinTolerance); + // // Only enable this when we know we're ready - // if (DriverStation.isEnabled() && isPoseReset && Constants.currentMode == Constants.Mode.REAL) + // if (DriverStation.isEnabled() && isPoseReset && isPoseWithinTolerance && + // Constants.currentMode == Constants.Mode.REAL) // { // BobotState.offerQuestMeasurement(new TimestampedPose(fieldToRobot, inputs.timestamp)); // } diff --git a/src/main/java/frc/robot/subsystems/quest/QuestConstants.java b/src/main/java/frc/robot/subsystems/quest/QuestConstants.java index c3aed4a..85a622f 100644 --- a/src/main/java/frc/robot/subsystems/quest/QuestConstants.java +++ b/src/main/java/frc/robot/subsystems/quest/QuestConstants.java @@ -16,4 +16,18 @@ public class QuestConstants { Rotation2d.kCCW_90deg); public static final Matrix stdDevs = VecBuilder.fill(0, 0, 0); + + /** + * "Tolerance" for how far QuestNav should travel relative to the previous known Global Pose. This + * number will need to be tuned. However, we can guess that with the following math we should find + * the maximum wheel odometry travel distance and use that as our starting point. + * + *

maxSpeedMetersPerSec (4.4) / 1000 * 20 -> (0.088 meters / 3.465 inches) /robot loop. + * + *

Make sure to compensate for other robots pushing around. + * + *

This an incredibly hacky solution for checking if the headset somehow "teleports off the + * field" again. + */ + public static final double acceptableDistanceTolerance = Units.inchesToMeters(4); }