From 9dc0a58cfef45dcf71a4c31d70dfb0c14d4fe89e Mon Sep 17 00:00:00 2001 From: SeanErn Date: Sun, 29 Dec 2024 12:09:36 -0500 Subject: [PATCH 01/23] not working! --- .run/deploy.run.xml | 7 +- quest-tools.sh | 234 ++++++++++++ .../deploy/pathplanner/paths/New Path.path | 70 ++++ src/main/java/frc/alotobots/Constants.java | 2 +- src/main/java/frc/alotobots/OI.java | 6 + .../java/frc/alotobots/RobotContainer.java | 20 +- .../vision/questnav/OculusSubsystem.java | 338 ++++++++++++++++++ .../CalculateCameraOffsetCommand.java | 72 ++++ .../commands/ResetOculusPoseCommand.java | 63 ++++ .../questnav/constants/OculusConstants.java | 24 ++ .../vision/questnav/io/OculusIO.java | 37 ++ .../vision/questnav/io/OculusIOReal.java | 68 ++++ .../vision/questnav/io/OculusIOSim.java | 33 ++ 13 files changed, 967 insertions(+), 7 deletions(-) create mode 100755 quest-tools.sh create mode 100644 src/main/deploy/pathplanner/paths/New Path.path create mode 100644 src/main/java/frc/alotobots/library/subsystems/vision/questnav/OculusSubsystem.java create mode 100644 src/main/java/frc/alotobots/library/subsystems/vision/questnav/commands/CalculateCameraOffsetCommand.java create mode 100644 src/main/java/frc/alotobots/library/subsystems/vision/questnav/commands/ResetOculusPoseCommand.java create mode 100644 src/main/java/frc/alotobots/library/subsystems/vision/questnav/constants/OculusConstants.java create mode 100644 src/main/java/frc/alotobots/library/subsystems/vision/questnav/io/OculusIO.java create mode 100644 src/main/java/frc/alotobots/library/subsystems/vision/questnav/io/OculusIOReal.java create mode 100644 src/main/java/frc/alotobots/library/subsystems/vision/questnav/io/OculusIOSim.java diff --git a/.run/deploy.run.xml b/.run/deploy.run.xml index 3c2dda37..9d76a291 100644 --- a/.run/deploy.run.xml +++ b/.run/deploy.run.xml @@ -1,11 +1,10 @@ - + @@ -22,4 +21,4 @@ false - + \ No newline at end of file diff --git a/quest-tools.sh b/quest-tools.sh new file mode 100755 index 00000000..b5f8b1fb --- /dev/null +++ b/quest-tools.sh @@ -0,0 +1,234 @@ +#!/bin/bash + +APP_PACKAGE="com.DerpyCatAviationLLC.QuestNav" +QUEST_PORT=5555 + +# Colors for output +RED='\033[0;31m' +GREEN='\033[0;32m' +YELLOW='\033[1;33m' +NC='\033[0m' # No Color + +print_help() { + echo -e "${YELLOW}Quest Development Tools${NC}" + echo "Usage: ./quest-tools.sh [command] [team_number]" + echo "" + echo "Commands:" + echo " connect - Find and connect to Quest on robot network (e.g., 5152)" + echo " stopwireless - Disable WiFi and Bluetooth and reboot" + echo " setup - Disable guardian and restart app" + echo " restart - Restart the QuestNav app" + echo " redeploy - Install APK from path and restart app" + echo " screen - Launch scrcpy for screen mirroring" + echo " reboot - Reboot the Quest" + echo " shutdown - Shutdown the Quest" + echo " status - Check ADB connection status" + echo " logs [filter] - Show app logs, optionally filtered by string" +} + +check_adb() { + if ! command -v adb &> /dev/null; then + echo -e "${RED}Error: ADB not found. Please install Android platform tools.${NC}" + exit 1 + fi +} + +ensure_connected() { + if ! adb devices | grep -q "$QUEST_PORT"; then + echo -e "${RED}Error: Quest not connected. Use 'connect' command first.${NC}" + exit 1 + fi +} + +find_quest() { + local team=$1 + local subnet="10.${team:0:2}.${team:2:2}" + + echo -e "${GREEN}Scanning network ${subnet}.0/24 for Quest...${NC}" + + # Check if nmap is installed + if ! command -v nmap &> /dev/null; then + echo -e "${RED}Error: nmap not found. Please install nmap first.${NC}" + exit 1 + fi + + # Kill any existing ADB server first + adb kill-server &> /dev/null + adb start-server &> /dev/null + + # Use nmap to quickly scan network, excluding .1 and .2 + echo "Running quick network scan..." + local devices=$(nmap -n -sn --exclude ${subnet}.1,${subnet}.2 ${subnet}.0/24 | grep "report for" | cut -d " " -f 5) + + for ip in $devices; do + echo -e "${YELLOW}Found device at ${ip}, attempting ADB connection...${NC}" + + # Try ADB connect with timeout + timeout 3 adb connect "${ip}:$QUEST_PORT" &> /dev/null + + # Quick check if device is connected + if timeout 1 adb devices | grep -q "${ip}:$QUEST_PORT"; then + echo -e "${GREEN}Successfully connected to Quest!${NC}" + return 0 + fi + + adb disconnect "${ip}:$QUEST_PORT" &> /dev/null + done + + echo -e "${RED}Could not find Quest on network${NC}" + return 1 +} + +stopwireless() { + ensure_connected + echo "Disabling wireless connections..." + adb shell settings put global bluetooth_on 0 + adb shell settings put global wifi_on 0 + echo "Rebooting..." + adb reboot +} + +setup_quest() { + ensure_connected + echo "Disabling Guardian..." + adb shell setprop debug.oculus.guardian_pause 1 + restart_app +} + +restart_app() { + ensure_connected + echo "Restarting QuestNav..." + + # Store last known device IP + local device_ip=$(adb devices | grep -m 1 "${QUEST_PORT}" | cut -f1 | cut -d: -f1) + + # Force stop the app + adb shell am force-stop $APP_PACKAGE + + # Give USB Ethernet interface time to settle + sleep 3 + + # Start the app directly + adb shell monkey -p $APP_PACKAGE 1 + + # Wait for potential network reset + sleep 5 + + # Check if we need to reconnect + if ! adb devices | grep -q "$QUEST_PORT"; then + echo "Network reset detected, reconnecting..." + adb disconnect + sleep 2 + + # Try to reconnect multiple times with increasing delays + for i in {1..5}; do + echo "Reconnection attempt $i..." + adb connect "${device_ip}:${QUEST_PORT}" + sleep $(($i * 2)) # Increasing delay between attempts + if adb devices | grep -q "$QUEST_PORT"; then + echo -e "${GREEN}Successfully reconnected!${NC}" + return 0 + fi + done + echo -e "${RED}Failed to reconnect. The network interface might need more time to stabilize.${NC}" + echo "You can try: ./quest-tools.sh connect to reconnect manually." + return 1 + fi + + echo -e "${GREEN}App restarted successfully${NC}" +} + +redeploy() { + ensure_connected + local apk_path="$1" + + if [ -z "$apk_path" ]; then + echo -e "${RED}Error: APK path required${NC}" + echo "Usage: ./quest-tools.sh redeploy path/to/app.apk" + exit 1 + fi + + if [ ! -f "$apk_path" ]; then + echo -e "${RED}Error: APK file not found: $apk_path${NC}" + exit 1 + fi + + echo "Installing APK from: $apk_path" + adb install -r -d "$apk_path" + restart_app +} + +launch_scrcpy() { + ensure_connected + if ! command -v scrcpy &> /dev/null; then + echo -e "${RED}Error: scrcpy not found. Please install it first.${NC}" + exit 1 + fi + scrcpy --crop 1920:1080:0:0 -b 10M +} + +tail_logs() { + ensure_connected + local filter="$1" + echo "Starting log stream..." + if [ -z "$filter" ]; then + # No filter, show all Unity logs + adb logcat -s Unity:* + else + # Use exact working command format + adb logcat -s Unity:* | grep "\\${filter}" + fi +} + +# Main command handler +case "$1" in + "connect") + if [ -z "$2" ]; then + echo -e "${RED}Error: Team number required${NC}" + exit 1 + fi + check_adb + find_quest "$2" + ;; + "stopwireless") + check_adb + stopwireless + ;; + "setup") + check_adb + setup_quest + ;; + "restart") + check_adb + restart_app + ;; + "redeploy") + check_adb + redeploy "$2" + ;; + "screen") + check_adb + launch_scrcpy + ;; + "reboot") + check_adb + ensure_connected + adb reboot + ;; + "shutdown") + check_adb + ensure_connected + adb shell reboot -p + ;; + "status") + check_adb + adb devices + ;; + "logs") + check_adb + tail_logs "$2" + ;; + *|"help") + print_help + ;; +esac diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/New Path.path new file mode 100644 index 00000000..b729d7b9 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/New Path.path @@ -0,0 +1,70 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.0, + "y": 7.0 + }, + "prevControl": null, + "nextControl": { + "x": 3.0, + "y": 7.0 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.0, + "y": 6.0 + }, + "prevControl": { + "x": 3.0, + "y": 6.0 + }, + "nextControl": { + "x": 5.0, + "y": 6.0 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 15.292488516315062, + "y": 5.5195165628007326 + }, + "prevControl": { + "x": 10.127923968435965, + "y": 5.754338411405699 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.2, + "maxAcceleration": 3.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 460.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/frc/alotobots/Constants.java b/src/main/java/frc/alotobots/Constants.java index ab614c12..acea753b 100644 --- a/src/main/java/frc/alotobots/Constants.java +++ b/src/main/java/frc/alotobots/Constants.java @@ -23,7 +23,7 @@ */ public final class Constants { /** The simulation mode to use when not running on real hardware. */ - public static final Mode simMode = Mode.SIM; + public static final Mode simMode = Mode.REPLAY; /** The current runtime mode, determined by whether running on real hardware or in simulation. */ public static final Mode currentMode = RobotBase.isReal() ? Mode.REAL : simMode; diff --git a/src/main/java/frc/alotobots/OI.java b/src/main/java/frc/alotobots/OI.java index ba1ab1c2..a2c97d7f 100644 --- a/src/main/java/frc/alotobots/OI.java +++ b/src/main/java/frc/alotobots/OI.java @@ -80,4 +80,10 @@ public static double getTurboSpeedTrigger() { /** Button for activating the pathfind to best object command. */ public static Trigger pathfindToBestObjectButton = driverController.b(); + + /** A temporary test button. */ + public static Trigger testButton = driverController.y(); + + /** A temporary test button. */ + public static Trigger testButton2 = driverController.x(); } diff --git a/src/main/java/frc/alotobots/RobotContainer.java b/src/main/java/frc/alotobots/RobotContainer.java index a3b7d2a2..75314e43 100644 --- a/src/main/java/frc/alotobots/RobotContainer.java +++ b/src/main/java/frc/alotobots/RobotContainer.java @@ -12,11 +12,12 @@ */ package frc.alotobots; -import static frc.alotobots.OI.driveFacingBestObjectButton; -import static frc.alotobots.OI.pathfindToBestObjectButton; +import static frc.alotobots.OI.*; import static frc.alotobots.library.subsystems.vision.photonvision.objectdetection.constants.ObjectDetectionConstants.NOTE; import com.pathplanner.lib.auto.AutoBuilder; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.alotobots.library.subsystems.bling.BlingSubsystem; @@ -42,6 +43,12 @@ import frc.alotobots.library.subsystems.vision.photonvision.objectdetection.constants.ObjectDetectionConstants; import frc.alotobots.library.subsystems.vision.photonvision.objectdetection.io.ObjectDetectionIO; import frc.alotobots.library.subsystems.vision.photonvision.objectdetection.io.ObjectDetectionIOPhotonVision; +import frc.alotobots.library.subsystems.vision.questnav.OculusSubsystem; +import frc.alotobots.library.subsystems.vision.questnav.commands.CalculateCameraOffsetCommand; +import frc.alotobots.library.subsystems.vision.questnav.commands.ResetOculusPoseCommand; +import frc.alotobots.library.subsystems.vision.questnav.io.OculusIO; +import frc.alotobots.library.subsystems.vision.questnav.io.OculusIOReal; +import frc.alotobots.library.subsystems.vision.questnav.io.OculusIOSim; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; /** @@ -62,6 +69,8 @@ public class RobotContainer { /** The LED control subsystem for robot status indication. */ private final BlingSubsystem blingSubsystem; + private final OculusSubsystem oculusSubsystem; + /** Dashboard chooser for selecting autonomous routines. */ private final LoggedDashboardChooser autoChooser; @@ -91,6 +100,7 @@ public RobotContainer() { swerveDriveSubsystem::getPose, new ObjectDetectionIOPhotonVision(ObjectDetectionConstants.CAMERA_CONFIGS[0])); blingSubsystem = new BlingSubsystem(new BlingIOReal()); + oculusSubsystem = new OculusSubsystem(new OculusIOReal()); break; case SIM: @@ -112,6 +122,7 @@ public RobotContainer() { objectDetectionSubsystem = new ObjectDetectionSubsystem(swerveDriveSubsystem::getPose, new ObjectDetectionIO() {}); blingSubsystem = new BlingSubsystem(new BlingIOSim()); + oculusSubsystem = new OculusSubsystem(new OculusIOSim()); break; default: @@ -131,6 +142,7 @@ public RobotContainer() { objectDetectionSubsystem = new ObjectDetectionSubsystem(swerveDriveSubsystem::getPose, new ObjectDetectionIO() {}); blingSubsystem = new BlingSubsystem(new BlingIO() {}); + oculusSubsystem = new OculusSubsystem(new OculusIO() {}); break; } @@ -176,6 +188,10 @@ private void configureLogicCommands() { new DriveFacingBestObject(objectDetectionSubsystem, swerveDriveSubsystem, NOTE)); pathfindToBestObjectButton.onTrue( new PathfindToBestObject(objectDetectionSubsystem, swerveDriveSubsystem, NOTE)); + testButton.onTrue( + new ResetOculusPoseCommand( + oculusSubsystem, new Pose2d(15.3, 5.5, Rotation2d.fromDegrees(0.0)))); + testButton2.whileTrue(new CalculateCameraOffsetCommand(oculusSubsystem)); } /** diff --git a/src/main/java/frc/alotobots/library/subsystems/vision/questnav/OculusSubsystem.java b/src/main/java/frc/alotobots/library/subsystems/vision/questnav/OculusSubsystem.java new file mode 100644 index 00000000..15fe1d8e --- /dev/null +++ b/src/main/java/frc/alotobots/library/subsystems/vision/questnav/OculusSubsystem.java @@ -0,0 +1,338 @@ +/* +* ALOTOBOTS - FRC Team 5152 + https://github.com/5152Alotobots +* Copyright (C) 2024 ALOTOBOTS +* +* This program is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* Source code must be publicly available on GitHub or an alternative web accessible site +*/ +package frc.alotobots.library.subsystems.vision.questnav; + +import static frc.alotobots.library.subsystems.vision.questnav.constants.OculusConstants.*; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.alotobots.library.subsystems.vision.questnav.io.OculusIO; +import frc.alotobots.library.subsystems.vision.questnav.io.OculusIOInputsAutoLogged; +import lombok.Getter; +import org.littletonrobotics.junction.Logger; + +public class OculusSubsystem extends SubsystemBase { + private final OculusIO io; + private final OculusIOInputsAutoLogged inputs = new OculusIOInputsAutoLogged(); + + @Getter private boolean poseResetInProgress = false; + @Getter private boolean headingResetInProgress = false; + + private double resetStartTime = 0; + private int currentResetAttempt = 0; + private Pose2d pendingResetPose = null; + + private float yawOffset = 0.0f; + + public OculusSubsystem(OculusIO io) { + this.io = io; + zeroHeading(); + Logger.recordOutput("Oculus/debug/status", "Initialized"); + Logger.recordOutput("Oculus/debug/yawOffset", yawOffset); + } + + @Override + public void periodic() { + io.updateInputs(inputs); + Logger.processInputs("Oculus", inputs); + + // Log all raw input values + Logger.recordOutput("Oculus/debug/raw/eulerAngles", inputs.eulerAngles); + Logger.recordOutput("Oculus/debug/raw/position", inputs.position); + Logger.recordOutput("Oculus/debug/raw/misoValue", inputs.misoValue); + Logger.recordOutput("Oculus/debug/state/poseResetInProgress", poseResetInProgress); + Logger.recordOutput("Oculus/debug/state/headingResetInProgress", headingResetInProgress); + Logger.recordOutput("Oculus/debug/state/currentResetAttempt", currentResetAttempt); + Logger.recordOutput("Oculus/debug/state/resetStartTime", resetStartTime); + + // Log timing information + double currentTime = Timer.getTimestamp(); + Logger.recordOutput("Oculus/debug/timing/currentTime", currentTime); + Logger.recordOutput("Oculus/debug/timing/timeSinceReset", currentTime - resetStartTime); + Logger.recordOutput( + "Oculus/debug/timing/timeoutStatus", + (currentTime - resetStartTime) > RESET_TIMEOUT_SECONDS); + + // Handle reset timeout + if (currentTime - resetStartTime > RESET_TIMEOUT_SECONDS) { + if (headingResetInProgress) { + Logger.recordOutput("Oculus/debug/timeout", "Heading reset timeout occurred"); + handleResetTimeoutHeading(); + } + if (poseResetInProgress) { + Logger.recordOutput("Oculus/debug/timeout", "Pose reset timeout occurred"); + handleResetTimeoutPose(); + } + } + + // Log reset completion status + Logger.recordOutput( + "Oculus/debug/resetCompletion/headingReset", + headingResetInProgress && inputs.misoValue == 99); + Logger.recordOutput( + "Oculus/debug/resetCompletion/poseReset", poseResetInProgress && inputs.misoValue == 98); + + // Handle reset completion + if (headingResetInProgress && inputs.misoValue == 99) { + Logger.recordOutput( + "Oculus/debug/status", + "Heading Reset completed successfully on attempt " + (currentResetAttempt + 1)); + clearHeadingResetState(); + } + if (poseResetInProgress && inputs.misoValue == 98) { + Logger.recordOutput( + "Oculus/debug/status", + "Pose Reset completed successfully on attempt " + (currentResetAttempt + 1)); + clearPoseResetState(); + } + + // Handle ping response + if (inputs.misoValue == 97) { + Logger.recordOutput("Oculus/debug/status", "Ping response received"); + Logger.recordOutput("Oculus/debug/ping/responseTime", Timer.getTimestamp()); + io.setMosi(0); + } + + // Log all pose calculations + Translation2d oculusPos = getOculusPosition(); + Logger.recordOutput("Oculus/debug/calculations/oculusPosition/x", oculusPos.getX()); + Logger.recordOutput("Oculus/debug/calculations/oculusPosition/y", oculusPos.getY()); + + double heading = getHeading(); + Logger.recordOutput("Oculus/debug/calculations/heading", heading); + Logger.recordOutput("Oculus/debug/calculations/turnRate", getTurnRate()); + Logger.recordOutput("Oculus/debug/calculations/oculusYaw", getOculusYaw()); + + // Log poses as objects + Logger.recordOutput("Oculus/debug/poses/oculus", getOculusPose()); + Logger.recordOutput("Oculus/debug/poses/robot", getRobotPose()); + Logger.recordOutput("Oculus/debug/transforms/oculusToRobot", OCULUS_TO_ROBOT); + + // Original logging + Logger.recordOutput("Oculus/debug/position", getOculusPosition()); + Logger.recordOutput("Oculus/Pose", getOculusPose()); + Logger.recordOutput("Oculus/RobotPose", getRobotPose()); + } + + private void handleResetTimeoutPose() { + Logger.recordOutput("Oculus/debug/resetTimeout/pose/attemptNumber", currentResetAttempt); + Logger.recordOutput("Oculus/debug/resetTimeout/pose/maxAttempts", MAX_RESET_ATTEMPTS); + + if (currentResetAttempt < MAX_RESET_ATTEMPTS) { + Logger.recordOutput( + "Oculus/debug/status", + "Pose Reset attempt " + (currentResetAttempt + 1) + " timed out, retrying..."); + currentResetAttempt++; + resetStartTime = Timer.getTimestamp(); + + // Log pending reset pose + if (pendingResetPose != null) { + Logger.recordOutput( + "Oculus/debug/resetTimeout/pose/pendingReset/x", pendingResetPose.getX()); + Logger.recordOutput( + "Oculus/debug/resetTimeout/pose/pendingReset/y", pendingResetPose.getY()); + Logger.recordOutput( + "Oculus/debug/resetTimeout/pose/pendingReset/rotation", + pendingResetPose.getRotation().getDegrees()); + } + + // Reset Mosi + io.setMosi(0); + // Retry the reset + io.setResetPose( + pendingResetPose.getX(), + pendingResetPose.getY(), + pendingResetPose.getRotation().getDegrees()); + io.setMosi(2); + } else { + Logger.recordOutput( + "Oculus/debug/status", "Pose Reset failed after " + MAX_RESET_ATTEMPTS + " attempts"); + clearPoseResetState(); + } + } + + private void handleResetTimeoutHeading() { + Logger.recordOutput("Oculus/debug/resetTimeout/heading/attemptNumber", currentResetAttempt); + Logger.recordOutput("Oculus/debug/resetTimeout/heading/maxAttempts", MAX_RESET_ATTEMPTS); + + if (currentResetAttempt < MAX_RESET_ATTEMPTS) { + Logger.recordOutput( + "Oculus/debug/status", + "Heading Reset attempt " + (currentResetAttempt + 1) + " timed out, retrying..."); + currentResetAttempt++; + resetStartTime = Timer.getTimestamp(); + + // Reset Mosi + io.setMosi(0); + // Retry the reset + io.setMosi(1); + } else { + Logger.recordOutput( + "Oculus/debug/status", "Heading Reset failed after " + MAX_RESET_ATTEMPTS + " attempts"); + clearHeadingResetState(); + } + } + + private void clearPoseResetState() { + Logger.recordOutput("Oculus/debug/clearState", "Clearing pose reset state"); + poseResetInProgress = false; + pendingResetPose = null; + currentResetAttempt = 0; + io.setMosi(0); + } + + private void clearHeadingResetState() { + Logger.recordOutput("Oculus/debug/clearState", "Clearing heading reset state"); + headingResetInProgress = false; + currentResetAttempt = 0; + io.setMosi(0); + } + + public boolean resetToPose(Pose2d targetPose) { + Logger.recordOutput("Oculus/debug/resetToPose/called", true); + Logger.recordOutput("Oculus/debug/resetToPose/initial/x", targetPose.getX()); + Logger.recordOutput("Oculus/debug/resetToPose/initial/y", targetPose.getY()); + Logger.recordOutput( + "Oculus/debug/resetToPose/initial/rotation", targetPose.getRotation().getDegrees()); + + if (poseResetInProgress) { + Logger.recordOutput("Oculus/debug/status", "Cannot reset pose - reset already in progress"); + return false; + } + + if (inputs.misoValue != 0) { + Logger.recordOutput( + "Oculus/debug/status", "Cannot reset pose - Quest busy (MISO=" + inputs.misoValue + ")"); + return false; + } + + targetPose = targetPose.plus(OCULUS_TO_ROBOT); + Logger.recordOutput("Oculus/debug/resetToPose/transformed/x", targetPose.getX()); + Logger.recordOutput("Oculus/debug/resetToPose/transformed/y", targetPose.getY()); + Logger.recordOutput( + "Oculus/debug/resetToPose/transformed/rotation", targetPose.getRotation().getDegrees()); + + pendingResetPose = targetPose; + + Logger.recordOutput( + "Oculus/debug/status", + String.format( + "Initiating pose reset to X:%.2f Y:%.2f Rot:%.2f°", + targetPose.getX(), targetPose.getY(), targetPose.getRotation().getDegrees())); + + // Start reset process + io.setResetPose(targetPose.getX(), targetPose.getY(), targetPose.getRotation().getDegrees()); + poseResetInProgress = true; + resetStartTime = Timer.getTimestamp(); + currentResetAttempt = 0; + io.setMosi(2); + + return true; + } + + public void zeroHeading() { + Logger.recordOutput("Oculus/debug/zeroHeading/called", true); + Logger.recordOutput("Oculus/debug/zeroHeading/currentMisoValue", inputs.misoValue); + + if (inputs.misoValue == 0) { + Logger.recordOutput("Oculus/debug/status", "Zeroing heading"); + Logger.recordOutput("Oculus/debug/zeroHeading/previousYawOffset", yawOffset); + yawOffset = inputs.eulerAngles[1]; + Logger.recordOutput("Oculus/debug/zeroHeading/newYawOffset", yawOffset); + headingResetInProgress = true; + io.setMosi(1); + resetStartTime = Timer.getTimestamp(); + } else { + Logger.recordOutput( + "Oculus/debug/status", + "Cannot zero heading - system busy (MISO=" + inputs.misoValue + ")"); + } + } + + public double getHeading() { + double heading = Rotation2d.fromDegrees(getOculusYaw()).getDegrees(); + Logger.recordOutput("Oculus/debug/getHeading/value", heading); + return heading; + } + + public double getTurnRate() { + double turnRate = -getOculusYaw(); + Logger.recordOutput("Oculus/debug/getTurnRate/value", turnRate); + return turnRate; + } + + private float getOculusYaw() { + float[] eulerAngles = inputs.eulerAngles; + Logger.recordOutput("Oculus/debug/getOculusYaw/rawEulerAngle", eulerAngles[1]); + Logger.recordOutput("Oculus/debug/getOculusYaw/yawOffset", yawOffset); + + var ret = eulerAngles[1] - yawOffset; + ret %= 360; + if (ret < 0) { + ret += 360; + } + Logger.recordOutput("Oculus/debug/getOculusYaw/calculatedValue", ret); + return ret; + } + + private Translation2d getOculusPosition() { + Logger.recordOutput("Oculus/debug/getOculusPosition/rawZ", inputs.position[2]); + Logger.recordOutput("Oculus/debug/getOculusPosition/rawX", inputs.position[0]); + return new Translation2d(inputs.position[2], -inputs.position[0]); + } + + public Pose2d getOculusPose() { + var oculusPositionCompensated = getOculusPosition(); + Logger.recordOutput("Oculus/debug/poses/compensatedPosition", oculusPositionCompensated); + + var pose = new Pose2d(oculusPositionCompensated, Rotation2d.fromDegrees(getOculusYaw())); + Logger.recordOutput("Oculus/debug/poses/rawPose", pose); + + var transformedPose = pose.transformBy(OCULUS_TO_ROBOT); + Logger.recordOutput("Oculus/debug/poses/transformedPose", transformedPose); + + return pose; + } + + public Pose2d getRobotPose() { + var oculusPose = getOculusPose(); + Logger.recordOutput("Oculus/debug/poses/currentOculusPose", oculusPose); + + var robotPose = + new Pose2d( + getOculusPosition().minus(OCULUS_TO_ROBOT.getTranslation()), + Rotation2d.fromDegrees(getOculusYaw())); + Logger.recordOutput("Oculus/debug/poses/finalRobotPose", robotPose); + + return robotPose; + } + + public boolean ping() { + Logger.recordOutput("Oculus/debug/ping/attempt", true); + Logger.recordOutput("Oculus/debug/ping/currentMisoValue", inputs.misoValue); + + if (inputs.misoValue == 0) { + Logger.recordOutput("Oculus/debug/status", "Sending ping..."); + Logger.recordOutput("Oculus/debug/ping/sendTime", Timer.getTimestamp()); + io.setMosi(3); + return true; + } else { + Logger.recordOutput( + "Oculus/debug/status", "Cannot ping - system busy (MISO=" + inputs.misoValue + ")"); + return false; + } + } +} diff --git a/src/main/java/frc/alotobots/library/subsystems/vision/questnav/commands/CalculateCameraOffsetCommand.java b/src/main/java/frc/alotobots/library/subsystems/vision/questnav/commands/CalculateCameraOffsetCommand.java new file mode 100644 index 00000000..b68d1181 --- /dev/null +++ b/src/main/java/frc/alotobots/library/subsystems/vision/questnav/commands/CalculateCameraOffsetCommand.java @@ -0,0 +1,72 @@ +/* +* ALOTOBOTS - FRC Team 5152 + https://github.com/5152Alotobots +* Copyright (C) 2024 ALOTOBOTS +* +* This program is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* Source code must be publicly available on GitHub or an alternative web accessible site +*/ +package frc.alotobots.library.subsystems.vision.questnav.commands; + +import static frc.alotobots.library.subsystems.vision.questnav.constants.OculusConstants.OCULUS_TO_ROBOT; + +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.geometry.Translation2d; +import edu.wpi.first.wpilibj2.command.Command; +import frc.alotobots.library.subsystems.vision.questnav.OculusSubsystem; +import org.littletonrobotics.junction.Logger; + +public class CalculateCameraOffsetCommand extends Command { + private final OculusSubsystem oculusSubsystem; + private Pose2d pose1; + + public CalculateCameraOffsetCommand(OculusSubsystem oculusSubsystem) { + this.oculusSubsystem = oculusSubsystem; + addRequirements(oculusSubsystem); + } + + @Override + public void initialize() { + pose1 = oculusSubsystem.getOculusPose(); + } + + @Override + public void end(boolean interrupted) { + Pose2d pose2 = oculusSubsystem.getOculusPose(); + Transform2d offset = solveRobotToCamera(pose1, pose2, OCULUS_TO_ROBOT.getRotation()); + Logger.recordOutput("Oculus/debug/empiricalOffset", offset); + } + + private static Transform2d solveRobotToCamera( + Pose2d cameraPose1, Pose2d cameraPose2, Rotation2d angleOnRobot) { + // Extract the camera positions and rotations + double x1 = cameraPose1.getTranslation().getX(); + double y1 = cameraPose1.getTranslation().getY(); + double x2 = cameraPose2.getTranslation().getX(); + double y2 = cameraPose2.getTranslation().getY(); + double theta1 = cameraPose1.getRotation().getRadians(); + double theta2 = cameraPose2.getRotation().getRadians(); + + // Compute the coefficients for x and y + double cos1 = Math.cos(theta1); + double sin1 = Math.sin(theta1); + double cos2 = Math.cos(theta2); + double sin2 = Math.sin(theta2); + + // Compute the determinant (denominator) for solving the system + double denominator = (cos1 - cos2) * (cos1 - cos2) + (sin1 - sin2) * (sin1 - sin2); + + // Calculate x and y + double x = ((x2 - x1) * (cos1 - cos2) + (y2 - y1) * (sin1 - sin2)) / -denominator; + double y = ((x2 - x1) * (sin1 - sin2) + (y2 - y1) * (cos2 - cos1)) / denominator; + + // Return the robotToCamera transform as a Transform2d + return new Transform2d(new Translation2d(x, y).rotateBy(angleOnRobot), angleOnRobot); + } +} diff --git a/src/main/java/frc/alotobots/library/subsystems/vision/questnav/commands/ResetOculusPoseCommand.java b/src/main/java/frc/alotobots/library/subsystems/vision/questnav/commands/ResetOculusPoseCommand.java new file mode 100644 index 00000000..53e1e926 --- /dev/null +++ b/src/main/java/frc/alotobots/library/subsystems/vision/questnav/commands/ResetOculusPoseCommand.java @@ -0,0 +1,63 @@ +/* +* ALOTOBOTS - FRC Team 5152 + https://github.com/5152Alotobots +* Copyright (C) 2024 ALOTOBOTS +* +* This program is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* Source code must be publicly available on GitHub or an alternative web accessible site +*/ +package frc.alotobots.library.subsystems.vision.questnav.commands; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj2.command.Command; +import frc.alotobots.library.subsystems.vision.questnav.OculusSubsystem; + +public class ResetOculusPoseCommand extends Command { + private final OculusSubsystem oculusSubsystem; + private final Pose2d targetPose; + private boolean resetInitiated = false; + + /** + * Creates a new ResetOculusPoseCommand. + * + * @param oculusSubsystem The subsystem to reset + * @param targetPose The pose to reset to + */ + public ResetOculusPoseCommand(OculusSubsystem oculusSubsystem, Pose2d targetPose) { + this.oculusSubsystem = oculusSubsystem; + this.targetPose = targetPose; + addRequirements(oculusSubsystem); + } + + @Override + public void initialize() { + resetInitiated = oculusSubsystem.resetToPose(targetPose); + if (!resetInitiated) { + System.out.println("[ResetOculusPoseCommand] Failed to initiate pose reset"); + } + } + + @Override + public boolean isFinished() { + // End command if: + // 1. Reset failed to initiate + // 2. Reset completed (no longer in progress) + return !resetInitiated || !oculusSubsystem.isPoseResetInProgress(); + } + + @Override + public void end(boolean interrupted) { + if (interrupted) { + System.out.println("[ResetOculusPoseCommand] Reset interrupted or timed out"); + } + } + + @Override + public boolean runsWhenDisabled() { + return true; // Allow pose reset when robot disabled + } +} diff --git a/src/main/java/frc/alotobots/library/subsystems/vision/questnav/constants/OculusConstants.java b/src/main/java/frc/alotobots/library/subsystems/vision/questnav/constants/OculusConstants.java new file mode 100644 index 00000000..0882c50c --- /dev/null +++ b/src/main/java/frc/alotobots/library/subsystems/vision/questnav/constants/OculusConstants.java @@ -0,0 +1,24 @@ +/* +* ALOTOBOTS - FRC Team 5152 + https://github.com/5152Alotobots +* Copyright (C) 2024 ALOTOBOTS +* +* This program is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* Source code must be publicly available on GitHub or an alternative web accessible site +*/ +package frc.alotobots.library.subsystems.vision.questnav.constants; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; + +public class OculusConstants { + public static final Transform2d OCULUS_TO_ROBOT = + new Transform2d(0, 0, Rotation2d.fromDegrees(0)); // X: +Forwards, Y: + Left, ROT: + CCW .16 x + + public static final double RESET_TIMEOUT_SECONDS = 0.2; + public static final int MAX_RESET_ATTEMPTS = 3; +} diff --git a/src/main/java/frc/alotobots/library/subsystems/vision/questnav/io/OculusIO.java b/src/main/java/frc/alotobots/library/subsystems/vision/questnav/io/OculusIO.java new file mode 100644 index 00000000..923c1b34 --- /dev/null +++ b/src/main/java/frc/alotobots/library/subsystems/vision/questnav/io/OculusIO.java @@ -0,0 +1,37 @@ +/* +* ALOTOBOTS - FRC Team 5152 + https://github.com/5152Alotobots +* Copyright (C) 2024 ALOTOBOTS +* +* This program is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* Source code must be publicly available on GitHub or an alternative web accessible site +*/ +package frc.alotobots.library.subsystems.vision.questnav.io; + +import org.littletonrobotics.junction.AutoLog; + +public interface OculusIO { + @AutoLog + public static class OculusIOInputs { + public float[] position = new float[] {0.0f, 0.0f, 0.0f}; + public float[] quaternion = new float[] {0.0f, 0.0f, 0.0f, 0.0f}; + public float[] eulerAngles = new float[] {0.0f, 0.0f, 0.0f}; + public double timestamp = 0.0; + public int frameCount = 0; + public double batteryPercent = 0.0; + public int misoValue = 0; + } + + /** Updates the set of loggable inputs. */ + public default void updateInputs(OculusIOInputs inputs) {} + + /** Sets MOSI value for Quest communication */ + public default void setMosi(int value) {} + + /** Sets the pose components for pose reset */ + public default void setResetPose(double x, double y, double rotation) {} +} diff --git a/src/main/java/frc/alotobots/library/subsystems/vision/questnav/io/OculusIOReal.java b/src/main/java/frc/alotobots/library/subsystems/vision/questnav/io/OculusIOReal.java new file mode 100644 index 00000000..0684c68f --- /dev/null +++ b/src/main/java/frc/alotobots/library/subsystems/vision/questnav/io/OculusIOReal.java @@ -0,0 +1,68 @@ +/* +* ALOTOBOTS - FRC Team 5152 + https://github.com/5152Alotobots +* Copyright (C) 2024 ALOTOBOTS +* +* This program is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* Source code must be publicly available on GitHub or an alternative web accessible site +*/ +package frc.alotobots.library.subsystems.vision.questnav.io; + +import edu.wpi.first.networktables.*; + +public class OculusIOReal implements OculusIO { + private final NetworkTable nt4Table; + private final IntegerSubscriber questMiso; + private final IntegerPublisher questMosi; + private final IntegerSubscriber questFrameCount; + private final DoubleSubscriber questTimestamp; + private final FloatArraySubscriber questPosition; + private final FloatArraySubscriber questQuaternion; + private final FloatArraySubscriber questEulerAngles; + private final DoubleSubscriber questBatteryPercent; + private final DoubleArrayPublisher resetPosePub; + + public OculusIOReal() { + nt4Table = NetworkTableInstance.getDefault().getTable("questnav"); + questMiso = nt4Table.getIntegerTopic("miso").subscribe(0); + questMosi = nt4Table.getIntegerTopic("mosi").publish(); + questFrameCount = nt4Table.getIntegerTopic("frameCount").subscribe(0); + questTimestamp = nt4Table.getDoubleTopic("timestamp").subscribe(0.0); + questPosition = + nt4Table.getFloatArrayTopic("position").subscribe(new float[] {0.0f, 0.0f, 0.0f}); + questQuaternion = + nt4Table.getFloatArrayTopic("quaternion").subscribe(new float[] {0.0f, 0.0f, 0.0f, 0.0f}); + questEulerAngles = + nt4Table.getFloatArrayTopic("eulerAngles").subscribe(new float[] {0.0f, 0.0f, 0.0f}); + questBatteryPercent = nt4Table.getDoubleTopic("batteryPercent").subscribe(0.0); + + // Publishers for pose reset + resetPosePub = nt4Table.getDoubleArrayTopic("resetpose").publish(); + } + + @Override + public void updateInputs(OculusIOInputs inputs) { + inputs.position = questPosition.get(); + inputs.quaternion = questQuaternion.get(); + inputs.eulerAngles = questEulerAngles.get(); + inputs.timestamp = questTimestamp.get(); + inputs.frameCount = (int) questFrameCount.get(); + inputs.batteryPercent = questBatteryPercent.get(); + inputs.misoValue = (int) questMiso.get(); + } + + @Override + public void setMosi(int value) { + questMosi.set(value); + } + + @Override + public void setResetPose(double x, double y, double rotation) { + // First value of 1 indicates "ready" + resetPosePub.set(new double[] {x, y, rotation}); + } +} diff --git a/src/main/java/frc/alotobots/library/subsystems/vision/questnav/io/OculusIOSim.java b/src/main/java/frc/alotobots/library/subsystems/vision/questnav/io/OculusIOSim.java new file mode 100644 index 00000000..8ad17d68 --- /dev/null +++ b/src/main/java/frc/alotobots/library/subsystems/vision/questnav/io/OculusIOSim.java @@ -0,0 +1,33 @@ +/* +* ALOTOBOTS - FRC Team 5152 + https://github.com/5152Alotobots +* Copyright (C) 2024 ALOTOBOTS +* +* This program is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* Source code must be publicly available on GitHub or an alternative web accessible site +*/ +package frc.alotobots.library.subsystems.vision.questnav.io; + +public class OculusIOSim implements OculusIO { + @Override + public void updateInputs(OculusIOInputs inputs) { + // Simulate static position for testing + inputs.position = new float[] {0.0f, 0.0f, 0.0f}; + inputs.quaternion = new float[] {1.0f, 0.0f, 0.0f, 0.0f}; + inputs.eulerAngles = new float[] {0.0f, 0.0f, 0.0f}; + inputs.timestamp = 0.0; + inputs.frameCount = 0; + inputs.batteryPercent = 100.0; + inputs.misoValue = 0; + } + + @Override + public void setMosi(int value) {} + + @Override + public void setResetPose(double x, double y, double rotation) {} +} From c327734a416070dce53c2ba4bd63a1f0c7e4be25 Mon Sep 17 00:00:00 2001 From: SeanErn Date: Sun, 29 Dec 2024 20:37:17 -0500 Subject: [PATCH 02/23] move camera position --- .../subsystems/vision/questnav/constants/OculusConstants.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/alotobots/library/subsystems/vision/questnav/constants/OculusConstants.java b/src/main/java/frc/alotobots/library/subsystems/vision/questnav/constants/OculusConstants.java index 0882c50c..27e2f385 100644 --- a/src/main/java/frc/alotobots/library/subsystems/vision/questnav/constants/OculusConstants.java +++ b/src/main/java/frc/alotobots/library/subsystems/vision/questnav/constants/OculusConstants.java @@ -17,7 +17,8 @@ public class OculusConstants { public static final Transform2d OCULUS_TO_ROBOT = - new Transform2d(0, 0, Rotation2d.fromDegrees(0)); // X: +Forwards, Y: + Left, ROT: + CCW .16 x + new Transform2d( + 0.16, 0, Rotation2d.fromDegrees(0)); // X: +Forwards, Y: + Left, ROT: + CCW .16 x public static final double RESET_TIMEOUT_SECONDS = 0.2; public static final int MAX_RESET_ATTEMPTS = 3; From 9523f571bbb252acb65237125ec3304ccfdf5711 Mon Sep 17 00:00:00 2001 From: SeanErn Date: Sun, 29 Dec 2024 20:38:12 -0500 Subject: [PATCH 03/23] transforms --- .../subsystems/vision/questnav/OculusSubsystem.java | 8 +------- 1 file changed, 1 insertion(+), 7 deletions(-) diff --git a/src/main/java/frc/alotobots/library/subsystems/vision/questnav/OculusSubsystem.java b/src/main/java/frc/alotobots/library/subsystems/vision/questnav/OculusSubsystem.java index 15fe1d8e..554b6187 100644 --- a/src/main/java/frc/alotobots/library/subsystems/vision/questnav/OculusSubsystem.java +++ b/src/main/java/frc/alotobots/library/subsystems/vision/questnav/OculusSubsystem.java @@ -301,9 +301,6 @@ public Pose2d getOculusPose() { var pose = new Pose2d(oculusPositionCompensated, Rotation2d.fromDegrees(getOculusYaw())); Logger.recordOutput("Oculus/debug/poses/rawPose", pose); - var transformedPose = pose.transformBy(OCULUS_TO_ROBOT); - Logger.recordOutput("Oculus/debug/poses/transformedPose", transformedPose); - return pose; } @@ -311,10 +308,7 @@ public Pose2d getRobotPose() { var oculusPose = getOculusPose(); Logger.recordOutput("Oculus/debug/poses/currentOculusPose", oculusPose); - var robotPose = - new Pose2d( - getOculusPosition().minus(OCULUS_TO_ROBOT.getTranslation()), - Rotation2d.fromDegrees(getOculusYaw())); + var robotPose = getOculusPose().transformBy(OCULUS_TO_ROBOT); Logger.recordOutput("Oculus/debug/poses/finalRobotPose", robotPose); return robotPose; From ba3d154d8780d151109dcedc6762896f60a354a1 Mon Sep 17 00:00:00 2001 From: SeanErn Date: Mon, 30 Dec 2024 16:08:58 -0500 Subject: [PATCH 04/23] update offset --- .../subsystems/vision/questnav/constants/OculusConstants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/alotobots/library/subsystems/vision/questnav/constants/OculusConstants.java b/src/main/java/frc/alotobots/library/subsystems/vision/questnav/constants/OculusConstants.java index 27e2f385..58cc3c88 100644 --- a/src/main/java/frc/alotobots/library/subsystems/vision/questnav/constants/OculusConstants.java +++ b/src/main/java/frc/alotobots/library/subsystems/vision/questnav/constants/OculusConstants.java @@ -18,7 +18,7 @@ public class OculusConstants { public static final Transform2d OCULUS_TO_ROBOT = new Transform2d( - 0.16, 0, Rotation2d.fromDegrees(0)); // X: +Forwards, Y: + Left, ROT: + CCW .16 x + 0.17, 0, Rotation2d.fromDegrees(0)); // X: +Forwards, Y: + Left, ROT: + CCW .16 x public static final double RESET_TIMEOUT_SECONDS = 0.2; public static final int MAX_RESET_ATTEMPTS = 3; From e447cab95c29a5d8ebb66f74a34be0d74a22cafa Mon Sep 17 00:00:00 2001 From: SeanErn Date: Mon, 30 Dec 2024 16:17:36 -0500 Subject: [PATCH 05/23] Make the subsystem actually readable --- .../vision/questnav/OculusSubsystem.java | 94 ++----------------- 1 file changed, 6 insertions(+), 88 deletions(-) diff --git a/src/main/java/frc/alotobots/library/subsystems/vision/questnav/OculusSubsystem.java b/src/main/java/frc/alotobots/library/subsystems/vision/questnav/OculusSubsystem.java index 554b6187..1eed92c8 100644 --- a/src/main/java/frc/alotobots/library/subsystems/vision/questnav/OculusSubsystem.java +++ b/src/main/java/frc/alotobots/library/subsystems/vision/questnav/OculusSubsystem.java @@ -49,42 +49,19 @@ public void periodic() { io.updateInputs(inputs); Logger.processInputs("Oculus", inputs); - // Log all raw input values - Logger.recordOutput("Oculus/debug/raw/eulerAngles", inputs.eulerAngles); - Logger.recordOutput("Oculus/debug/raw/position", inputs.position); - Logger.recordOutput("Oculus/debug/raw/misoValue", inputs.misoValue); - Logger.recordOutput("Oculus/debug/state/poseResetInProgress", poseResetInProgress); - Logger.recordOutput("Oculus/debug/state/headingResetInProgress", headingResetInProgress); - Logger.recordOutput("Oculus/debug/state/currentResetAttempt", currentResetAttempt); - Logger.recordOutput("Oculus/debug/state/resetStartTime", resetStartTime); - // Log timing information double currentTime = Timer.getTimestamp(); - Logger.recordOutput("Oculus/debug/timing/currentTime", currentTime); - Logger.recordOutput("Oculus/debug/timing/timeSinceReset", currentTime - resetStartTime); - Logger.recordOutput( - "Oculus/debug/timing/timeoutStatus", - (currentTime - resetStartTime) > RESET_TIMEOUT_SECONDS); // Handle reset timeout if (currentTime - resetStartTime > RESET_TIMEOUT_SECONDS) { if (headingResetInProgress) { - Logger.recordOutput("Oculus/debug/timeout", "Heading reset timeout occurred"); handleResetTimeoutHeading(); } if (poseResetInProgress) { - Logger.recordOutput("Oculus/debug/timeout", "Pose reset timeout occurred"); handleResetTimeoutPose(); } } - // Log reset completion status - Logger.recordOutput( - "Oculus/debug/resetCompletion/headingReset", - headingResetInProgress && inputs.misoValue == 99); - Logger.recordOutput( - "Oculus/debug/resetCompletion/poseReset", poseResetInProgress && inputs.misoValue == 98); - // Handle reset completion if (headingResetInProgress && inputs.misoValue == 99) { Logger.recordOutput( @@ -102,35 +79,11 @@ public void periodic() { // Handle ping response if (inputs.misoValue == 97) { Logger.recordOutput("Oculus/debug/status", "Ping response received"); - Logger.recordOutput("Oculus/debug/ping/responseTime", Timer.getTimestamp()); io.setMosi(0); } - - // Log all pose calculations - Translation2d oculusPos = getOculusPosition(); - Logger.recordOutput("Oculus/debug/calculations/oculusPosition/x", oculusPos.getX()); - Logger.recordOutput("Oculus/debug/calculations/oculusPosition/y", oculusPos.getY()); - - double heading = getHeading(); - Logger.recordOutput("Oculus/debug/calculations/heading", heading); - Logger.recordOutput("Oculus/debug/calculations/turnRate", getTurnRate()); - Logger.recordOutput("Oculus/debug/calculations/oculusYaw", getOculusYaw()); - - // Log poses as objects - Logger.recordOutput("Oculus/debug/poses/oculus", getOculusPose()); - Logger.recordOutput("Oculus/debug/poses/robot", getRobotPose()); - Logger.recordOutput("Oculus/debug/transforms/oculusToRobot", OCULUS_TO_ROBOT); - - // Original logging - Logger.recordOutput("Oculus/debug/position", getOculusPosition()); - Logger.recordOutput("Oculus/Pose", getOculusPose()); - Logger.recordOutput("Oculus/RobotPose", getRobotPose()); } private void handleResetTimeoutPose() { - Logger.recordOutput("Oculus/debug/resetTimeout/pose/attemptNumber", currentResetAttempt); - Logger.recordOutput("Oculus/debug/resetTimeout/pose/maxAttempts", MAX_RESET_ATTEMPTS); - if (currentResetAttempt < MAX_RESET_ATTEMPTS) { Logger.recordOutput( "Oculus/debug/status", @@ -148,7 +101,6 @@ private void handleResetTimeoutPose() { "Oculus/debug/resetTimeout/pose/pendingReset/rotation", pendingResetPose.getRotation().getDegrees()); } - // Reset Mosi io.setMosi(0); // Retry the reset @@ -165,9 +117,6 @@ private void handleResetTimeoutPose() { } private void handleResetTimeoutHeading() { - Logger.recordOutput("Oculus/debug/resetTimeout/heading/attemptNumber", currentResetAttempt); - Logger.recordOutput("Oculus/debug/resetTimeout/heading/maxAttempts", MAX_RESET_ATTEMPTS); - if (currentResetAttempt < MAX_RESET_ATTEMPTS) { Logger.recordOutput( "Oculus/debug/status", @@ -187,7 +136,7 @@ private void handleResetTimeoutHeading() { } private void clearPoseResetState() { - Logger.recordOutput("Oculus/debug/clearState", "Clearing pose reset state"); + Logger.recordOutput("Oculus/debug/status", "Clearing pose reset state"); poseResetInProgress = false; pendingResetPose = null; currentResetAttempt = 0; @@ -195,19 +144,13 @@ private void clearPoseResetState() { } private void clearHeadingResetState() { - Logger.recordOutput("Oculus/debug/clearState", "Clearing heading reset state"); + Logger.recordOutput("Oculus/debug/status", "Clearing heading reset state"); headingResetInProgress = false; currentResetAttempt = 0; io.setMosi(0); } public boolean resetToPose(Pose2d targetPose) { - Logger.recordOutput("Oculus/debug/resetToPose/called", true); - Logger.recordOutput("Oculus/debug/resetToPose/initial/x", targetPose.getX()); - Logger.recordOutput("Oculus/debug/resetToPose/initial/y", targetPose.getY()); - Logger.recordOutput( - "Oculus/debug/resetToPose/initial/rotation", targetPose.getRotation().getDegrees()); - if (poseResetInProgress) { Logger.recordOutput("Oculus/debug/status", "Cannot reset pose - reset already in progress"); return false; @@ -220,13 +163,7 @@ public boolean resetToPose(Pose2d targetPose) { } targetPose = targetPose.plus(OCULUS_TO_ROBOT); - Logger.recordOutput("Oculus/debug/resetToPose/transformed/x", targetPose.getX()); - Logger.recordOutput("Oculus/debug/resetToPose/transformed/y", targetPose.getY()); - Logger.recordOutput( - "Oculus/debug/resetToPose/transformed/rotation", targetPose.getRotation().getDegrees()); - pendingResetPose = targetPose; - Logger.recordOutput( "Oculus/debug/status", String.format( @@ -244,14 +181,9 @@ public boolean resetToPose(Pose2d targetPose) { } public void zeroHeading() { - Logger.recordOutput("Oculus/debug/zeroHeading/called", true); - Logger.recordOutput("Oculus/debug/zeroHeading/currentMisoValue", inputs.misoValue); - if (inputs.misoValue == 0) { Logger.recordOutput("Oculus/debug/status", "Zeroing heading"); - Logger.recordOutput("Oculus/debug/zeroHeading/previousYawOffset", yawOffset); yawOffset = inputs.eulerAngles[1]; - Logger.recordOutput("Oculus/debug/zeroHeading/newYawOffset", yawOffset); headingResetInProgress = true; io.setMosi(1); resetStartTime = Timer.getTimestamp(); @@ -264,21 +196,15 @@ public void zeroHeading() { public double getHeading() { double heading = Rotation2d.fromDegrees(getOculusYaw()).getDegrees(); - Logger.recordOutput("Oculus/debug/getHeading/value", heading); return heading; } public double getTurnRate() { - double turnRate = -getOculusYaw(); - Logger.recordOutput("Oculus/debug/getTurnRate/value", turnRate); - return turnRate; + return -getOculusYaw(); // Negative bc we are neg gyro } private float getOculusYaw() { float[] eulerAngles = inputs.eulerAngles; - Logger.recordOutput("Oculus/debug/getOculusYaw/rawEulerAngle", eulerAngles[1]); - Logger.recordOutput("Oculus/debug/getOculusYaw/yawOffset", yawOffset); - var ret = eulerAngles[1] - yawOffset; ret %= 360; if (ret < 0) { @@ -295,22 +221,14 @@ private Translation2d getOculusPosition() { } public Pose2d getOculusPose() { - var oculusPositionCompensated = getOculusPosition(); - Logger.recordOutput("Oculus/debug/poses/compensatedPosition", oculusPositionCompensated); - - var pose = new Pose2d(oculusPositionCompensated, Rotation2d.fromDegrees(getOculusYaw())); - Logger.recordOutput("Oculus/debug/poses/rawPose", pose); - + var pose = new Pose2d(getOculusPosition(), Rotation2d.fromDegrees(getOculusYaw())); + Logger.recordOutput("Oculus/debug/poses/headsetPose", pose); return pose; } public Pose2d getRobotPose() { - var oculusPose = getOculusPose(); - Logger.recordOutput("Oculus/debug/poses/currentOculusPose", oculusPose); - var robotPose = getOculusPose().transformBy(OCULUS_TO_ROBOT); - Logger.recordOutput("Oculus/debug/poses/finalRobotPose", robotPose); - + Logger.recordOutput("Oculus/debug/poses/robotPose", robotPose); return robotPose; } From 1fd3812b089315fb60ef29130e58d1c8a271a1d9 Mon Sep 17 00:00:00 2001 From: SeanErn Date: Mon, 30 Dec 2024 16:19:16 -0500 Subject: [PATCH 06/23] remove old comment --- .../library/subsystems/vision/questnav/io/OculusIOReal.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/frc/alotobots/library/subsystems/vision/questnav/io/OculusIOReal.java b/src/main/java/frc/alotobots/library/subsystems/vision/questnav/io/OculusIOReal.java index 0684c68f..d809b0da 100644 --- a/src/main/java/frc/alotobots/library/subsystems/vision/questnav/io/OculusIOReal.java +++ b/src/main/java/frc/alotobots/library/subsystems/vision/questnav/io/OculusIOReal.java @@ -62,7 +62,6 @@ public void setMosi(int value) { @Override public void setResetPose(double x, double y, double rotation) { - // First value of 1 indicates "ready" resetPosePub.set(new double[] {x, y, rotation}); } } From cee07921df9642272b4fa5f47802dd09b9645249 Mon Sep 17 00:00:00 2001 From: SeanErn Date: Tue, 31 Dec 2024 15:41:53 -0500 Subject: [PATCH 07/23] WIP --- .../vision/questnav/OculusSubsystem.java | 30 +++++++++++-------- .../questnav/constants/OculusConstants.java | 3 +- 2 files changed, 19 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/alotobots/library/subsystems/vision/questnav/OculusSubsystem.java b/src/main/java/frc/alotobots/library/subsystems/vision/questnav/OculusSubsystem.java index 1eed92c8..8f8cbdc1 100644 --- a/src/main/java/frc/alotobots/library/subsystems/vision/questnav/OculusSubsystem.java +++ b/src/main/java/frc/alotobots/library/subsystems/vision/questnav/OculusSubsystem.java @@ -49,6 +49,7 @@ public void periodic() { io.updateInputs(inputs); Logger.processInputs("Oculus", inputs); + getRobotPose(); // Log timing information double currentTime = Timer.getTimestamp(); @@ -181,17 +182,19 @@ public boolean resetToPose(Pose2d targetPose) { } public void zeroHeading() { - if (inputs.misoValue == 0) { - Logger.recordOutput("Oculus/debug/status", "Zeroing heading"); - yawOffset = inputs.eulerAngles[1]; - headingResetInProgress = true; - io.setMosi(1); - resetStartTime = Timer.getTimestamp(); - } else { - Logger.recordOutput( - "Oculus/debug/status", - "Cannot zero heading - system busy (MISO=" + inputs.misoValue + ")"); - } + float[] eulerAngles = inputs.eulerAngles; + yawOffset = eulerAngles[1]; + // if (inputs.misoValue == 0) { + // Logger.recordOutput("Oculus/debug/status", "Zeroing heading"); + // yawOffset = inputs.eulerAngles[1]; + // headingResetInProgress = true; + // io.setMosi(1); + // resetStartTime = Timer.getTimestamp(); + // } else { + // Logger.recordOutput( + // "Oculus/debug/status", + // "Cannot zero heading - system busy (MISO=" + inputs.misoValue + ")"); + // } } public double getHeading() { @@ -227,7 +230,10 @@ public Pose2d getOculusPose() { } public Pose2d getRobotPose() { - var robotPose = getOculusPose().transformBy(OCULUS_TO_ROBOT); + var robotPose = + new Pose2d( + getOculusPose().getTranslation().minus(OCULUS_TO_ROBOT.getTranslation()), + Rotation2d.fromDegrees(getOculusYaw())); Logger.recordOutput("Oculus/debug/poses/robotPose", robotPose); return robotPose; } diff --git a/src/main/java/frc/alotobots/library/subsystems/vision/questnav/constants/OculusConstants.java b/src/main/java/frc/alotobots/library/subsystems/vision/questnav/constants/OculusConstants.java index 58cc3c88..bc86ab13 100644 --- a/src/main/java/frc/alotobots/library/subsystems/vision/questnav/constants/OculusConstants.java +++ b/src/main/java/frc/alotobots/library/subsystems/vision/questnav/constants/OculusConstants.java @@ -17,8 +17,7 @@ public class OculusConstants { public static final Transform2d OCULUS_TO_ROBOT = - new Transform2d( - 0.17, 0, Rotation2d.fromDegrees(0)); // X: +Forwards, Y: + Left, ROT: + CCW .16 x + new Transform2d(0.17, 0, new Rotation2d()); // X: +Forwards, Y: + Left, ROT: + CCW .16 x public static final double RESET_TIMEOUT_SECONDS = 0.2; public static final int MAX_RESET_ATTEMPTS = 3; From 63c00f752347b24d377d76a4f96bb6373b12a6e8 Mon Sep 17 00:00:00 2001 From: Sean Ernstes Date: Tue, 31 Dec 2024 16:25:48 -0500 Subject: [PATCH 08/23] WIP --- gradlew | 0 quest-tools.sh | 0 2 files changed, 0 insertions(+), 0 deletions(-) mode change 100755 => 100644 gradlew mode change 100755 => 100644 quest-tools.sh diff --git a/gradlew b/gradlew old mode 100755 new mode 100644 diff --git a/quest-tools.sh b/quest-tools.sh old mode 100755 new mode 100644 From ae2ee920052efa5f73bb09a7af7aea9b50301244 Mon Sep 17 00:00:00 2001 From: Sean Ernstes Date: Wed, 1 Jan 2025 15:38:02 -0500 Subject: [PATCH 09/23] OMFG IT WORKS --- .../java/frc/alotobots/RobotContainer.java | 9 +- .../vision/questnav/OculusSubsystem.java | 280 +++++------------- .../CalculateCameraOffsetCommand.java | 72 ----- .../vision/questnav/commands/PingCommand.java | 114 +++++++ .../commands/ResetOculusPoseCommand.java | 63 ---- .../questnav/commands/ResetPoseCommand.java | 120 ++++++++ .../questnav/commands/ZeroHeadingCommand.java | 113 +++++++ .../questnav/constants/OculusConstants.java | 20 +- .../vision/questnav/io/OculusIO.java | 35 ++- .../vision/questnav/io/OculusIOReal.java | 25 +- .../vision/questnav/io/OculusIOSim.java | 9 +- 11 files changed, 513 insertions(+), 347 deletions(-) delete mode 100644 src/main/java/frc/alotobots/library/subsystems/vision/questnav/commands/CalculateCameraOffsetCommand.java create mode 100644 src/main/java/frc/alotobots/library/subsystems/vision/questnav/commands/PingCommand.java delete mode 100644 src/main/java/frc/alotobots/library/subsystems/vision/questnav/commands/ResetOculusPoseCommand.java create mode 100644 src/main/java/frc/alotobots/library/subsystems/vision/questnav/commands/ResetPoseCommand.java create mode 100644 src/main/java/frc/alotobots/library/subsystems/vision/questnav/commands/ZeroHeadingCommand.java diff --git a/src/main/java/frc/alotobots/RobotContainer.java b/src/main/java/frc/alotobots/RobotContainer.java index 75314e43..4a4e7c85 100644 --- a/src/main/java/frc/alotobots/RobotContainer.java +++ b/src/main/java/frc/alotobots/RobotContainer.java @@ -44,8 +44,7 @@ import frc.alotobots.library.subsystems.vision.photonvision.objectdetection.io.ObjectDetectionIO; import frc.alotobots.library.subsystems.vision.photonvision.objectdetection.io.ObjectDetectionIOPhotonVision; import frc.alotobots.library.subsystems.vision.questnav.OculusSubsystem; -import frc.alotobots.library.subsystems.vision.questnav.commands.CalculateCameraOffsetCommand; -import frc.alotobots.library.subsystems.vision.questnav.commands.ResetOculusPoseCommand; +import frc.alotobots.library.subsystems.vision.questnav.commands.ResetPoseCommand; import frc.alotobots.library.subsystems.vision.questnav.io.OculusIO; import frc.alotobots.library.subsystems.vision.questnav.io.OculusIOReal; import frc.alotobots.library.subsystems.vision.questnav.io.OculusIOSim; @@ -189,9 +188,9 @@ private void configureLogicCommands() { pathfindToBestObjectButton.onTrue( new PathfindToBestObject(objectDetectionSubsystem, swerveDriveSubsystem, NOTE)); testButton.onTrue( - new ResetOculusPoseCommand( - oculusSubsystem, new Pose2d(15.3, 5.5, Rotation2d.fromDegrees(0.0)))); - testButton2.whileTrue(new CalculateCameraOffsetCommand(oculusSubsystem)); + new ResetPoseCommand(oculusSubsystem, new Pose2d(15.3, 5.5, Rotation2d.fromDegrees(0.0)))); + testButton2.onTrue( + new ResetPoseCommand(oculusSubsystem, new Pose2d(5.3, 2.5, Rotation2d.fromDegrees(180.0)))); } /** diff --git a/src/main/java/frc/alotobots/library/subsystems/vision/questnav/OculusSubsystem.java b/src/main/java/frc/alotobots/library/subsystems/vision/questnav/OculusSubsystem.java index 8f8cbdc1..fe44a57f 100644 --- a/src/main/java/frc/alotobots/library/subsystems/vision/questnav/OculusSubsystem.java +++ b/src/main/java/frc/alotobots/library/subsystems/vision/questnav/OculusSubsystem.java @@ -17,31 +17,30 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.alotobots.library.subsystems.vision.questnav.io.OculusIO; import frc.alotobots.library.subsystems.vision.questnav.io.OculusIOInputsAutoLogged; -import lombok.Getter; import org.littletonrobotics.junction.Logger; +/** + * The OculusSubsystem manages communication and data handling for the Oculus Quest VR headset used + * for robot navigation and positioning. + */ public class OculusSubsystem extends SubsystemBase { + /** The IO interface for communicating with the Oculus hardware. */ private final OculusIO io; - private final OculusIOInputsAutoLogged inputs = new OculusIOInputsAutoLogged(); - - @Getter private boolean poseResetInProgress = false; - @Getter private boolean headingResetInProgress = false; - private double resetStartTime = 0; - private int currentResetAttempt = 0; - private Pose2d pendingResetPose = null; - - private float yawOffset = 0.0f; + /** Logged inputs from the Oculus device. */ + private final OculusIOInputsAutoLogged inputs = new OculusIOInputsAutoLogged(); + /** + * Creates a new OculusSubsystem. + * + * @param io The IO interface for communicating with the Oculus hardware + */ public OculusSubsystem(OculusIO io) { this.io = io; - zeroHeading(); - Logger.recordOutput("Oculus/debug/status", "Initialized"); - Logger.recordOutput("Oculus/debug/yawOffset", yawOffset); + Logger.recordOutput("Oculus/status", "Initialized"); } @Override @@ -49,208 +48,91 @@ public void periodic() { io.updateInputs(inputs); Logger.processInputs("Oculus", inputs); - getRobotPose(); - // Log timing information - double currentTime = Timer.getTimestamp(); - - // Handle reset timeout - if (currentTime - resetStartTime > RESET_TIMEOUT_SECONDS) { - if (headingResetInProgress) { - handleResetTimeoutHeading(); - } - if (poseResetInProgress) { - handleResetTimeoutPose(); - } - } - - // Handle reset completion - if (headingResetInProgress && inputs.misoValue == 99) { - Logger.recordOutput( - "Oculus/debug/status", - "Heading Reset completed successfully on attempt " + (currentResetAttempt + 1)); - clearHeadingResetState(); - } - if (poseResetInProgress && inputs.misoValue == 98) { - Logger.recordOutput( - "Oculus/debug/status", - "Pose Reset completed successfully on attempt " + (currentResetAttempt + 1)); - clearPoseResetState(); - } - - // Handle ping response - if (inputs.misoValue == 97) { - Logger.recordOutput("Oculus/debug/status", "Ping response received"); - io.setMosi(0); - } - } - - private void handleResetTimeoutPose() { - if (currentResetAttempt < MAX_RESET_ATTEMPTS) { - Logger.recordOutput( - "Oculus/debug/status", - "Pose Reset attempt " + (currentResetAttempt + 1) + " timed out, retrying..."); - currentResetAttempt++; - resetStartTime = Timer.getTimestamp(); - - // Log pending reset pose - if (pendingResetPose != null) { - Logger.recordOutput( - "Oculus/debug/resetTimeout/pose/pendingReset/x", pendingResetPose.getX()); - Logger.recordOutput( - "Oculus/debug/resetTimeout/pose/pendingReset/y", pendingResetPose.getY()); - Logger.recordOutput( - "Oculus/debug/resetTimeout/pose/pendingReset/rotation", - pendingResetPose.getRotation().getDegrees()); - } - // Reset Mosi - io.setMosi(0); - // Retry the reset - io.setResetPose( - pendingResetPose.getX(), - pendingResetPose.getY(), - pendingResetPose.getRotation().getDegrees()); - io.setMosi(2); - } else { - Logger.recordOutput( - "Oculus/debug/status", "Pose Reset failed after " + MAX_RESET_ATTEMPTS + " attempts"); - clearPoseResetState(); - } - } - - private void handleResetTimeoutHeading() { - if (currentResetAttempt < MAX_RESET_ATTEMPTS) { - Logger.recordOutput( - "Oculus/debug/status", - "Heading Reset attempt " + (currentResetAttempt + 1) + " timed out, retrying..."); - currentResetAttempt++; - resetStartTime = Timer.getTimestamp(); - - // Reset Mosi - io.setMosi(0); - // Retry the reset - io.setMosi(1); - } else { - Logger.recordOutput( - "Oculus/debug/status", "Heading Reset failed after " + MAX_RESET_ATTEMPTS + " attempts"); - clearHeadingResetState(); - } - } - - private void clearPoseResetState() { - Logger.recordOutput("Oculus/debug/status", "Clearing pose reset state"); - poseResetInProgress = false; - pendingResetPose = null; - currentResetAttempt = 0; - io.setMosi(0); - } - - private void clearHeadingResetState() { - Logger.recordOutput("Oculus/debug/status", "Clearing heading reset state"); - headingResetInProgress = false; - currentResetAttempt = 0; - io.setMosi(0); + var oculusPose = getOculusPose(); + var robotPose = oculusPose.transformBy(OCULUS_TO_ROBOT.inverse()); + Logger.recordOutput("Oculus/poses/headsetPose", oculusPose); + Logger.recordOutput("Oculus/poses/robotPose", robotPose); } - public boolean resetToPose(Pose2d targetPose) { - if (poseResetInProgress) { - Logger.recordOutput("Oculus/debug/status", "Cannot reset pose - reset already in progress"); - return false; - } - - if (inputs.misoValue != 0) { - Logger.recordOutput( - "Oculus/debug/status", "Cannot reset pose - Quest busy (MISO=" + inputs.misoValue + ")"); - return false; - } - - targetPose = targetPose.plus(OCULUS_TO_ROBOT); - pendingResetPose = targetPose; - Logger.recordOutput( - "Oculus/debug/status", - String.format( - "Initiating pose reset to X:%.2f Y:%.2f Rot:%.2f°", - targetPose.getX(), targetPose.getY(), targetPose.getRotation().getDegrees())); - - // Start reset process - io.setResetPose(targetPose.getX(), targetPose.getY(), targetPose.getRotation().getDegrees()); - poseResetInProgress = true; - resetStartTime = Timer.getTimestamp(); - currentResetAttempt = 0; - io.setMosi(2); - - return true; + /** + * Gets the current yaw (rotation) of the Oculus headset. + * + * @return The yaw as a Rotation2d + */ + private Rotation2d getOculusYaw() { + return Rotation2d.fromDegrees(-inputs.eulerAngles[1]); } - public void zeroHeading() { - float[] eulerAngles = inputs.eulerAngles; - yawOffset = eulerAngles[1]; - // if (inputs.misoValue == 0) { - // Logger.recordOutput("Oculus/debug/status", "Zeroing heading"); - // yawOffset = inputs.eulerAngles[1]; - // headingResetInProgress = true; - // io.setMosi(1); - // resetStartTime = Timer.getTimestamp(); - // } else { - // Logger.recordOutput( - // "Oculus/debug/status", - // "Cannot zero heading - system busy (MISO=" + inputs.misoValue + ")"); - // } + /** + * Gets the current position of the Oculus headset. + * + * @return The position as a Translation2d + */ + private Translation2d getOculusPosition() { + return new Translation2d(inputs.position[2], -inputs.position[0]); } - public double getHeading() { - double heading = Rotation2d.fromDegrees(getOculusYaw()).getDegrees(); - return heading; + /** + * Gets the current pose (position and rotation) of the Oculus headset. + * + * @return The pose of the Oculus headset + */ + public Pose2d getOculusPose() { + return new Pose2d(getOculusPosition(), getOculusYaw()); } - public double getTurnRate() { - return -getOculusYaw(); // Negative bc we are neg gyro + /** + * Gets the current pose of the robot based on the Oculus headset position. + * + * @return The calculated robot pose + */ + public Pose2d getRobotPose() { + return getOculusPose().transformBy(OCULUS_TO_ROBOT.inverse()); } - private float getOculusYaw() { - float[] eulerAngles = inputs.eulerAngles; - var ret = eulerAngles[1] - yawOffset; - ret %= 360; - if (ret < 0) { - ret += 360; - } - Logger.recordOutput("Oculus/debug/getOculusYaw/calculatedValue", ret); - return ret; + /** + * Gets the current MISO (Master In Slave Out) value from the Oculus. + * + * @return The current MISO value + */ + public int getMisoValue() { + return inputs.misoValue; } - private Translation2d getOculusPosition() { - Logger.recordOutput("Oculus/debug/getOculusPosition/rawZ", inputs.position[2]); - Logger.recordOutput("Oculus/debug/getOculusPosition/rawX", inputs.position[0]); - return new Translation2d(inputs.position[2], -inputs.position[0]); + /** + * Sets the MOSI (Master Out Slave In) value for the Oculus. + * + * @param value The MOSI value to set + */ + public void setMosi(int value) { + io.setMosi(value); } - public Pose2d getOculusPose() { - var pose = new Pose2d(getOculusPosition(), Rotation2d.fromDegrees(getOculusYaw())); - Logger.recordOutput("Oculus/debug/poses/headsetPose", pose); - return pose; + /** + * Sets a new reset pose for the Oculus tracking system. + * + * @param pose The target pose to reset to + */ + public void setResetPose(Pose2d pose) { + var targetPose = pose.plus(OCULUS_TO_ROBOT); + io.setResetPose(targetPose.getX(), targetPose.getY(), targetPose.getRotation().getDegrees()); } - public Pose2d getRobotPose() { - var robotPose = - new Pose2d( - getOculusPose().getTranslation().minus(OCULUS_TO_ROBOT.getTranslation()), - Rotation2d.fromDegrees(getOculusYaw())); - Logger.recordOutput("Oculus/debug/poses/robotPose", robotPose); - return robotPose; + /** + * Logs a status message for the Oculus subsystem. + * + * @param message The status message to log + */ + public void logStatus(String message) { + Logger.recordOutput("Oculus/status", message); } - public boolean ping() { - Logger.recordOutput("Oculus/debug/ping/attempt", true); - Logger.recordOutput("Oculus/debug/ping/currentMisoValue", inputs.misoValue); - - if (inputs.misoValue == 0) { - Logger.recordOutput("Oculus/debug/status", "Sending ping..."); - Logger.recordOutput("Oculus/debug/ping/sendTime", Timer.getTimestamp()); - io.setMosi(3); - return true; - } else { - Logger.recordOutput( - "Oculus/debug/status", "Cannot ping - system busy (MISO=" + inputs.misoValue + ")"); - return false; - } + /** + * Logs the ping time for communication with the Oculus. + * + * @param timestamp The timestamp to log + */ + public void logPingTime(double timestamp) { + Logger.recordOutput("Oculus/ping/sendTime", timestamp); } } diff --git a/src/main/java/frc/alotobots/library/subsystems/vision/questnav/commands/CalculateCameraOffsetCommand.java b/src/main/java/frc/alotobots/library/subsystems/vision/questnav/commands/CalculateCameraOffsetCommand.java deleted file mode 100644 index b68d1181..00000000 --- a/src/main/java/frc/alotobots/library/subsystems/vision/questnav/commands/CalculateCameraOffsetCommand.java +++ /dev/null @@ -1,72 +0,0 @@ -/* -* ALOTOBOTS - FRC Team 5152 - https://github.com/5152Alotobots -* Copyright (C) 2024 ALOTOBOTS -* -* This program is free software: you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation, either version 3 of the License, or -* (at your option) any later version. -* -* Source code must be publicly available on GitHub or an alternative web accessible site -*/ -package frc.alotobots.library.subsystems.vision.questnav.commands; - -import static frc.alotobots.library.subsystems.vision.questnav.constants.OculusConstants.OCULUS_TO_ROBOT; - -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.geometry.Translation2d; -import edu.wpi.first.wpilibj2.command.Command; -import frc.alotobots.library.subsystems.vision.questnav.OculusSubsystem; -import org.littletonrobotics.junction.Logger; - -public class CalculateCameraOffsetCommand extends Command { - private final OculusSubsystem oculusSubsystem; - private Pose2d pose1; - - public CalculateCameraOffsetCommand(OculusSubsystem oculusSubsystem) { - this.oculusSubsystem = oculusSubsystem; - addRequirements(oculusSubsystem); - } - - @Override - public void initialize() { - pose1 = oculusSubsystem.getOculusPose(); - } - - @Override - public void end(boolean interrupted) { - Pose2d pose2 = oculusSubsystem.getOculusPose(); - Transform2d offset = solveRobotToCamera(pose1, pose2, OCULUS_TO_ROBOT.getRotation()); - Logger.recordOutput("Oculus/debug/empiricalOffset", offset); - } - - private static Transform2d solveRobotToCamera( - Pose2d cameraPose1, Pose2d cameraPose2, Rotation2d angleOnRobot) { - // Extract the camera positions and rotations - double x1 = cameraPose1.getTranslation().getX(); - double y1 = cameraPose1.getTranslation().getY(); - double x2 = cameraPose2.getTranslation().getX(); - double y2 = cameraPose2.getTranslation().getY(); - double theta1 = cameraPose1.getRotation().getRadians(); - double theta2 = cameraPose2.getRotation().getRadians(); - - // Compute the coefficients for x and y - double cos1 = Math.cos(theta1); - double sin1 = Math.sin(theta1); - double cos2 = Math.cos(theta2); - double sin2 = Math.sin(theta2); - - // Compute the determinant (denominator) for solving the system - double denominator = (cos1 - cos2) * (cos1 - cos2) + (sin1 - sin2) * (sin1 - sin2); - - // Calculate x and y - double x = ((x2 - x1) * (cos1 - cos2) + (y2 - y1) * (sin1 - sin2)) / -denominator; - double y = ((x2 - x1) * (sin1 - sin2) + (y2 - y1) * (cos2 - cos1)) / denominator; - - // Return the robotToCamera transform as a Transform2d - return new Transform2d(new Translation2d(x, y).rotateBy(angleOnRobot), angleOnRobot); - } -} diff --git a/src/main/java/frc/alotobots/library/subsystems/vision/questnav/commands/PingCommand.java b/src/main/java/frc/alotobots/library/subsystems/vision/questnav/commands/PingCommand.java new file mode 100644 index 00000000..b35b442e --- /dev/null +++ b/src/main/java/frc/alotobots/library/subsystems/vision/questnav/commands/PingCommand.java @@ -0,0 +1,114 @@ +/* +* ALOTOBOTS - FRC Team 5152 + https://github.com/5152Alotobots +* Copyright (C) 2024 ALOTOBOTS +* +* This program is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* Source code must be publicly available on GitHub or an alternative web accessible site +*/ +package frc.alotobots.library.subsystems.vision.questnav.commands; + +import static frc.alotobots.library.subsystems.vision.questnav.constants.OculusConstants.*; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import frc.alotobots.library.subsystems.vision.questnav.OculusSubsystem; +import org.littletonrobotics.junction.Logger; + +/** + * Command that sends a ping request to the Oculus headset and waits for a response. Used to verify + * communication with the headset is working properly. + */ +public class PingCommand extends Command { + /** Status code indicating the Oculus is ready for commands */ + private static final int STATUS_READY = 0; + + /** Status code indicating the Oculus has responded to ping */ + private static final int STATUS_PING_RESPONSE = 97; + + /** The Oculus subsystem instance */ + private final OculusSubsystem oculus; + + /** Flag indicating if ping sequence has started */ + private boolean hasStarted; + + /** Counter for ping attempts */ + private int currentAttempt; + + /** Timestamp when current ping attempt started */ + private double startTime; + + /** + * Creates a new PingCommand. + * + * @param oculus The Oculus subsystem to ping + */ + public PingCommand(OculusSubsystem oculus) { + this.oculus = oculus; + addRequirements(oculus); + } + + @Override + public void initialize() { + startPing(); + hasStarted = false; + currentAttempt = 0; + startTime = 0; + } + + /** Initiates a new ping attempt if the Oculus is ready. */ + private void startPing() { + if (oculus.getMisoValue() == STATUS_READY) { + oculus.logStatus("Starting ping attempt " + currentAttempt); + oculus.logPingTime(Timer.getTimestamp()); + oculus.setMosi(3); // Ping + hasStarted = true; + startTime = Timer.getTimestamp(); + currentAttempt++; + } + } + + @Override + public void execute() { + if (!hasStarted) { + startPing(); + return; + } + + // Check for timeout + if (Timer.getTimestamp() - startTime > RESET_TIMEOUT_SECONDS) { + if (currentAttempt < MAX_RESET_ATTEMPTS) { + Logger.recordOutput( + "Oculus/status", "Ping attempt " + currentAttempt + " timed out, retrying..."); + oculus.setMosi(0); // Clear + startPing(); + } else { + Logger.recordOutput( + "Oculus/status", "Ping failed after " + MAX_RESET_ATTEMPTS + " attempts"); + hasStarted = false; + } + } + } + + @Override + public boolean isFinished() { + if (!hasStarted) return true; + return oculus.getMisoValue() == STATUS_PING_RESPONSE; + } + + @Override + public void end(boolean interrupted) { + if (hasStarted) { + oculus.setMosi(0); // Clear + Logger.recordOutput( + "Oculus/status", + interrupted + ? "Ping interrupted" + : "Ping completed successfully on attempt " + currentAttempt); + } + } +} diff --git a/src/main/java/frc/alotobots/library/subsystems/vision/questnav/commands/ResetOculusPoseCommand.java b/src/main/java/frc/alotobots/library/subsystems/vision/questnav/commands/ResetOculusPoseCommand.java deleted file mode 100644 index 53e1e926..00000000 --- a/src/main/java/frc/alotobots/library/subsystems/vision/questnav/commands/ResetOculusPoseCommand.java +++ /dev/null @@ -1,63 +0,0 @@ -/* -* ALOTOBOTS - FRC Team 5152 - https://github.com/5152Alotobots -* Copyright (C) 2024 ALOTOBOTS -* -* This program is free software: you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation, either version 3 of the License, or -* (at your option) any later version. -* -* Source code must be publicly available on GitHub or an alternative web accessible site -*/ -package frc.alotobots.library.subsystems.vision.questnav.commands; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.wpilibj2.command.Command; -import frc.alotobots.library.subsystems.vision.questnav.OculusSubsystem; - -public class ResetOculusPoseCommand extends Command { - private final OculusSubsystem oculusSubsystem; - private final Pose2d targetPose; - private boolean resetInitiated = false; - - /** - * Creates a new ResetOculusPoseCommand. - * - * @param oculusSubsystem The subsystem to reset - * @param targetPose The pose to reset to - */ - public ResetOculusPoseCommand(OculusSubsystem oculusSubsystem, Pose2d targetPose) { - this.oculusSubsystem = oculusSubsystem; - this.targetPose = targetPose; - addRequirements(oculusSubsystem); - } - - @Override - public void initialize() { - resetInitiated = oculusSubsystem.resetToPose(targetPose); - if (!resetInitiated) { - System.out.println("[ResetOculusPoseCommand] Failed to initiate pose reset"); - } - } - - @Override - public boolean isFinished() { - // End command if: - // 1. Reset failed to initiate - // 2. Reset completed (no longer in progress) - return !resetInitiated || !oculusSubsystem.isPoseResetInProgress(); - } - - @Override - public void end(boolean interrupted) { - if (interrupted) { - System.out.println("[ResetOculusPoseCommand] Reset interrupted or timed out"); - } - } - - @Override - public boolean runsWhenDisabled() { - return true; // Allow pose reset when robot disabled - } -} diff --git a/src/main/java/frc/alotobots/library/subsystems/vision/questnav/commands/ResetPoseCommand.java b/src/main/java/frc/alotobots/library/subsystems/vision/questnav/commands/ResetPoseCommand.java new file mode 100644 index 00000000..f60f806b --- /dev/null +++ b/src/main/java/frc/alotobots/library/subsystems/vision/questnav/commands/ResetPoseCommand.java @@ -0,0 +1,120 @@ +/* +* ALOTOBOTS - FRC Team 5152 + https://github.com/5152Alotobots +* Copyright (C) 2024 ALOTOBOTS +* +* This program is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* Source code must be publicly available on GitHub or an alternative web accessible site +*/ +package frc.alotobots.library.subsystems.vision.questnav.commands; + +import static frc.alotobots.library.subsystems.vision.questnav.constants.OculusConstants.*; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import frc.alotobots.library.subsystems.vision.questnav.OculusSubsystem; +import org.littletonrobotics.junction.Logger; + +/** + * Command that resets the Oculus headset's position tracking to a specified pose. This is used to + * align the Oculus coordinate system with the field coordinate system. + */ +public class ResetPoseCommand extends Command { + /** Status code indicating the Oculus is ready for commands */ + private static final int STATUS_READY = 0; + + /** Status code indicating the pose reset is complete */ + private static final int STATUS_POSE_RESET_COMPLETE = 98; + + /** The Oculus subsystem instance */ + private final OculusSubsystem oculus; + + /** The target pose to reset to */ + private final Pose2d targetPose; + + /** Flag indicating if reset sequence has started */ + private boolean hasStarted; + + /** Counter for reset attempts */ + private int currentAttempt; + + /** Timestamp when current reset attempt started */ + private double startTime; + + /** + * Creates a new ResetPoseCommand. + * + * @param oculus The Oculus subsystem to reset + * @param targetPose The target pose to reset to + */ + public ResetPoseCommand(OculusSubsystem oculus, Pose2d targetPose) { + this.oculus = oculus; + this.targetPose = targetPose; + addRequirements(oculus); + } + + @Override + public void initialize() { + hasStarted = false; + currentAttempt = 0; + startTime = 0; + startReset(); + } + + /** Initiates a new pose reset attempt if the Oculus is ready. */ + private void startReset() { + if (oculus.getMisoValue() == STATUS_READY) { + oculus.setResetPose(targetPose); + oculus.setMosi(2); // Pose Reset + hasStarted = true; + startTime = Timer.getTimestamp(); + currentAttempt++; + Logger.recordOutput("Oculus/status", "Starting pose reset attempt " + currentAttempt); + } + } + + @Override + public void execute() { + if (!hasStarted) { + startReset(); + return; + } + + // Check for timeout + if (Timer.getTimestamp() - startTime > RESET_TIMEOUT_SECONDS) { + if (currentAttempt < MAX_RESET_ATTEMPTS) { + Logger.recordOutput( + "Oculus/status", "Pose reset attempt " + currentAttempt + " timed out, retrying..."); + oculus.setMosi(0); // Clear + startReset(); + } else { + Logger.recordOutput( + "Oculus/status", "Pose reset failed after " + MAX_RESET_ATTEMPTS + " attempts"); + hasStarted = false; + } + } + } + + @Override + public boolean isFinished() { + if (!hasStarted) return true; + return oculus.getMisoValue() == STATUS_POSE_RESET_COMPLETE; + } + + @Override + public void end(boolean interrupted) { + if (hasStarted) { + oculus.setMosi(0); // Clear + Logger.recordOutput( + "Oculus/status", + interrupted + ? "Pose reset interrupted" + : "Pose reset completed successfully on attempt " + currentAttempt); + } + } +} diff --git a/src/main/java/frc/alotobots/library/subsystems/vision/questnav/commands/ZeroHeadingCommand.java b/src/main/java/frc/alotobots/library/subsystems/vision/questnav/commands/ZeroHeadingCommand.java new file mode 100644 index 00000000..efcb4d63 --- /dev/null +++ b/src/main/java/frc/alotobots/library/subsystems/vision/questnav/commands/ZeroHeadingCommand.java @@ -0,0 +1,113 @@ +/* +* ALOTOBOTS - FRC Team 5152 + https://github.com/5152Alotobots +* Copyright (C) 2024 ALOTOBOTS +* +* This program is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* Source code must be publicly available on GitHub or an alternative web accessible site +*/ +package frc.alotobots.library.subsystems.vision.questnav.commands; + +import static frc.alotobots.library.subsystems.vision.questnav.constants.OculusConstants.*; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import frc.alotobots.library.subsystems.vision.questnav.OculusSubsystem; +import org.littletonrobotics.junction.Logger; + +/** + * Command that zeros the heading (yaw) of the Oculus headset. This sets the current heading as the + * zero reference point for rotation tracking. + */ +public class ZeroHeadingCommand extends Command { + /** Status code indicating the Oculus is ready for commands */ + private static final int STATUS_READY = 0; + + /** Status code indicating the heading reset is complete */ + private static final int STATUS_HEADING_RESET_COMPLETE = 99; + + /** The Oculus subsystem instance */ + private final OculusSubsystem oculus; + + /** Flag indicating if reset sequence has started */ + private boolean hasStarted; + + /** Counter for reset attempts */ + private int currentAttempt; + + /** Timestamp when current reset attempt started */ + private double startTime; + + /** + * Creates a new ZeroHeadingCommand. + * + * @param oculus The Oculus subsystem to reset + */ + public ZeroHeadingCommand(OculusSubsystem oculus) { + this.oculus = oculus; + addRequirements(oculus); + } + + @Override + public void initialize() { + startReset(); + hasStarted = false; + currentAttempt = 0; + startTime = 0; + } + + /** Initiates a new heading reset attempt if the Oculus is ready. */ + private void startReset() { + if (oculus.getMisoValue() == STATUS_READY) { + oculus.setMosi(1); // Heading Reset + hasStarted = true; + startTime = Timer.getTimestamp(); + currentAttempt++; + Logger.recordOutput("Oculus/status", "Starting heading reset attempt " + currentAttempt); + } + } + + @Override + public void execute() { + if (!hasStarted) { + startReset(); + return; + } + + // Check for timeout + if (Timer.getTimestamp() - startTime > RESET_TIMEOUT_SECONDS) { + if (currentAttempt < MAX_RESET_ATTEMPTS) { + Logger.recordOutput( + "Oculus/status", "Heading reset attempt " + currentAttempt + " timed out, retrying..."); + oculus.setMosi(0); // Clear + startReset(); + } else { + Logger.recordOutput( + "Oculus/status", "Heading reset failed after " + MAX_RESET_ATTEMPTS + " attempts"); + hasStarted = false; + } + } + } + + @Override + public boolean isFinished() { + if (!hasStarted) return true; + return oculus.getMisoValue() == STATUS_HEADING_RESET_COMPLETE; + } + + @Override + public void end(boolean interrupted) { + if (hasStarted) { + oculus.setMosi(0); // Clear + Logger.recordOutput( + "Oculus/status", + interrupted + ? "Heading reset interrupted" + : "Heading reset completed successfully on attempt " + currentAttempt); + } + } +} diff --git a/src/main/java/frc/alotobots/library/subsystems/vision/questnav/constants/OculusConstants.java b/src/main/java/frc/alotobots/library/subsystems/vision/questnav/constants/OculusConstants.java index bc86ab13..842eb670 100644 --- a/src/main/java/frc/alotobots/library/subsystems/vision/questnav/constants/OculusConstants.java +++ b/src/main/java/frc/alotobots/library/subsystems/vision/questnav/constants/OculusConstants.java @@ -15,10 +15,26 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform2d; +/** + * Constants used by the Oculus Quest navigation subsystem. Contains configuration values for + * physical setup and operation parameters. + */ public class OculusConstants { - public static final Transform2d OCULUS_TO_ROBOT = - new Transform2d(0.17, 0, new Rotation2d()); // X: +Forwards, Y: + Left, ROT: + CCW .16 x + /** + * Transform from the Oculus headset position to the robot's center. Coordinate system: - X: + * Positive is forwards - Y: Positive is left - Rotation: Positive is counter-clockwise + */ + public static final Transform2d OCULUS_TO_ROBOT = new Transform2d(0.075, 0.0, new Rotation2d()); + /** + * Timeout duration in seconds for reset operations (pose reset, heading reset, ping). If a reset + * operation takes longer than this time, it will be considered failed. + */ public static final double RESET_TIMEOUT_SECONDS = 0.2; + + /** + * Maximum number of attempts for reset operations. If a reset operation fails this many times, + * the command will terminate. + */ public static final int MAX_RESET_ATTEMPTS = 3; } diff --git a/src/main/java/frc/alotobots/library/subsystems/vision/questnav/io/OculusIO.java b/src/main/java/frc/alotobots/library/subsystems/vision/questnav/io/OculusIO.java index 923c1b34..804b99a3 100644 --- a/src/main/java/frc/alotobots/library/subsystems/vision/questnav/io/OculusIO.java +++ b/src/main/java/frc/alotobots/library/subsystems/vision/questnav/io/OculusIO.java @@ -14,24 +14,53 @@ import org.littletonrobotics.junction.AutoLog; +/** Interface for handling input/output operations with the Oculus Quest hardware. */ public interface OculusIO { + /** Data structure for Oculus inputs that can be automatically logged. */ @AutoLog public static class OculusIOInputs { + /** 3D position coordinates [x, y, z] */ public float[] position = new float[] {0.0f, 0.0f, 0.0f}; + + /** Quaternion orientation [w, x, y, z] */ public float[] quaternion = new float[] {0.0f, 0.0f, 0.0f, 0.0f}; + + /** Euler angles [roll, pitch, yaw] in degrees */ public float[] eulerAngles = new float[] {0.0f, 0.0f, 0.0f}; + + /** Current timestamp from the Oculus */ public double timestamp = 0.0; + + /** Frame counter from the Oculus */ public int frameCount = 0; + + /** Battery level percentage */ public double batteryPercent = 0.0; + + /** Current MISO (Master In Slave Out) value */ public int misoValue = 0; } - /** Updates the set of loggable inputs. */ + /** + * Updates the set of loggable inputs from the Oculus. + * + * @param inputs The input object to update with current values + */ public default void updateInputs(OculusIOInputs inputs) {} - /** Sets MOSI value for Quest communication */ + /** + * Sets MOSI (Master Out Slave In) value for Quest communication. + * + * @param value The MOSI value to set + */ public default void setMosi(int value) {} - /** Sets the pose components for pose reset */ + /** + * Sets the pose components for resetting the Oculus position tracking. + * + * @param x The X coordinate + * @param y The Y coordinate + * @param rotation The rotation in degrees + */ public default void setResetPose(double x, double y, double rotation) {} } diff --git a/src/main/java/frc/alotobots/library/subsystems/vision/questnav/io/OculusIOReal.java b/src/main/java/frc/alotobots/library/subsystems/vision/questnav/io/OculusIOReal.java index d809b0da..e99b9818 100644 --- a/src/main/java/frc/alotobots/library/subsystems/vision/questnav/io/OculusIOReal.java +++ b/src/main/java/frc/alotobots/library/subsystems/vision/questnav/io/OculusIOReal.java @@ -14,18 +14,42 @@ import edu.wpi.first.networktables.*; +/** Implementation of OculusIO for real hardware communication via NetworkTables. */ public class OculusIOReal implements OculusIO { + /** NetworkTable for Oculus communication */ private final NetworkTable nt4Table; + + /** Subscriber for MISO (Master In Slave Out) values */ private final IntegerSubscriber questMiso; + + /** Publisher for MOSI (Master Out Slave In) values */ private final IntegerPublisher questMosi; + + /** Subscriber for frame count updates */ private final IntegerSubscriber questFrameCount; + + /** Subscriber for timestamp updates */ private final DoubleSubscriber questTimestamp; + + /** Subscriber for position updates */ private final FloatArraySubscriber questPosition; + + /** Subscriber for quaternion orientation updates */ private final FloatArraySubscriber questQuaternion; + + /** Subscriber for Euler angle updates */ private final FloatArraySubscriber questEulerAngles; + + /** Subscriber for battery percentage updates */ private final DoubleSubscriber questBatteryPercent; + + /** Publisher for pose reset commands */ private final DoubleArrayPublisher resetPosePub; + /** + * Creates a new OculusIOReal instance and initializes all NetworkTable publishers and + * subscribers. + */ public OculusIOReal() { nt4Table = NetworkTableInstance.getDefault().getTable("questnav"); questMiso = nt4Table.getIntegerTopic("miso").subscribe(0); @@ -40,7 +64,6 @@ public OculusIOReal() { nt4Table.getFloatArrayTopic("eulerAngles").subscribe(new float[] {0.0f, 0.0f, 0.0f}); questBatteryPercent = nt4Table.getDoubleTopic("batteryPercent").subscribe(0.0); - // Publishers for pose reset resetPosePub = nt4Table.getDoubleArrayTopic("resetpose").publish(); } diff --git a/src/main/java/frc/alotobots/library/subsystems/vision/questnav/io/OculusIOSim.java b/src/main/java/frc/alotobots/library/subsystems/vision/questnav/io/OculusIOSim.java index 8ad17d68..ec42a6ed 100644 --- a/src/main/java/frc/alotobots/library/subsystems/vision/questnav/io/OculusIOSim.java +++ b/src/main/java/frc/alotobots/library/subsystems/vision/questnav/io/OculusIOSim.java @@ -12,6 +12,7 @@ */ package frc.alotobots.library.subsystems.vision.questnav.io; +/** Simulation implementation of OculusIO that provides static test values. */ public class OculusIOSim implements OculusIO { @Override public void updateInputs(OculusIOInputs inputs) { @@ -26,8 +27,12 @@ public void updateInputs(OculusIOInputs inputs) { } @Override - public void setMosi(int value) {} + public void setMosi(int value) { + // Simulation does not need to handle MOSI values + } @Override - public void setResetPose(double x, double y, double rotation) {} + public void setResetPose(double x, double y, double rotation) { + // Simulation does not need to handle pose reset + } } From 5cfbb7a104d4b1aaf27ae98fd63197bdff4f3ead Mon Sep 17 00:00:00 2001 From: Sean Ernstes Date: Wed, 1 Jan 2025 15:41:18 -0500 Subject: [PATCH 10/23] refactor: change name to be more descriptive --- .../library/subsystems/vision/questnav/OculusSubsystem.java | 6 +++--- .../vision/questnav/constants/OculusConstants.java | 4 ++-- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/alotobots/library/subsystems/vision/questnav/OculusSubsystem.java b/src/main/java/frc/alotobots/library/subsystems/vision/questnav/OculusSubsystem.java index fe44a57f..deaa7102 100644 --- a/src/main/java/frc/alotobots/library/subsystems/vision/questnav/OculusSubsystem.java +++ b/src/main/java/frc/alotobots/library/subsystems/vision/questnav/OculusSubsystem.java @@ -49,7 +49,7 @@ public void periodic() { Logger.processInputs("Oculus", inputs); var oculusPose = getOculusPose(); - var robotPose = oculusPose.transformBy(OCULUS_TO_ROBOT.inverse()); + var robotPose = oculusPose.transformBy(ROBOT_TO_OCULUS.inverse()); Logger.recordOutput("Oculus/poses/headsetPose", oculusPose); Logger.recordOutput("Oculus/poses/robotPose", robotPose); } @@ -87,7 +87,7 @@ public Pose2d getOculusPose() { * @return The calculated robot pose */ public Pose2d getRobotPose() { - return getOculusPose().transformBy(OCULUS_TO_ROBOT.inverse()); + return getOculusPose().transformBy(ROBOT_TO_OCULUS.inverse()); } /** @@ -114,7 +114,7 @@ public void setMosi(int value) { * @param pose The target pose to reset to */ public void setResetPose(Pose2d pose) { - var targetPose = pose.plus(OCULUS_TO_ROBOT); + var targetPose = pose.plus(ROBOT_TO_OCULUS); io.setResetPose(targetPose.getX(), targetPose.getY(), targetPose.getRotation().getDegrees()); } diff --git a/src/main/java/frc/alotobots/library/subsystems/vision/questnav/constants/OculusConstants.java b/src/main/java/frc/alotobots/library/subsystems/vision/questnav/constants/OculusConstants.java index 842eb670..10d56f69 100644 --- a/src/main/java/frc/alotobots/library/subsystems/vision/questnav/constants/OculusConstants.java +++ b/src/main/java/frc/alotobots/library/subsystems/vision/questnav/constants/OculusConstants.java @@ -21,10 +21,10 @@ */ public class OculusConstants { /** - * Transform from the Oculus headset position to the robot's center. Coordinate system: - X: + * Transform from the robot center to the headset. Coordinate system: - X: * Positive is forwards - Y: Positive is left - Rotation: Positive is counter-clockwise */ - public static final Transform2d OCULUS_TO_ROBOT = new Transform2d(0.075, 0.0, new Rotation2d()); + public static final Transform2d ROBOT_TO_OCULUS = new Transform2d(0.075, 0.0, new Rotation2d()); /** * Timeout duration in seconds for reset operations (pose reset, heading reset, ping). If a reset From 1154d605fc00fdd74127b4b39465777b12b800c1 Mon Sep 17 00:00:00 2001 From: Sean Ernstes Date: Wed, 1 Jan 2025 16:12:26 -0500 Subject: [PATCH 11/23] docs --- .../library/commands/vision/oculus/ping.md | 19 ++++++++++ .../commands/vision/oculus/resetpose.md | 20 +++++++++++ .../commands/vision/oculus/zeroheading.md | 19 ++++++++++ .../docs/library/subsystems/vision/oculus.md | 36 +++++++++++++++++++ docs/mkdocs/mkdocs.yml | 5 +++ 5 files changed, 99 insertions(+) create mode 100644 docs/mkdocs/docs/library/commands/vision/oculus/ping.md create mode 100644 docs/mkdocs/docs/library/commands/vision/oculus/resetpose.md create mode 100644 docs/mkdocs/docs/library/commands/vision/oculus/zeroheading.md create mode 100644 docs/mkdocs/docs/library/subsystems/vision/oculus.md diff --git a/docs/mkdocs/docs/library/commands/vision/oculus/ping.md b/docs/mkdocs/docs/library/commands/vision/oculus/ping.md new file mode 100644 index 00000000..af9df876 --- /dev/null +++ b/docs/mkdocs/docs/library/commands/vision/oculus/ping.md @@ -0,0 +1,19 @@ +# Ping Command + +A command that verifies communication with the Oculus headset by sending a ping request and waiting for a response. + +## Required Subsystems +- [Oculus Navigation Subsystem](/5152_Template/library/subsystems/vision/questnav) + +## Constructor Parameters +```java +public PingCommand(OculusSubsystem oculus) +``` +- `oculus`: The Oculus subsystem instance to ping + +## Configuration +No additional configuration required. + +## Reference Documentation + +[JavaDoc Reference](/5152_Template/javadoc/frc/alotobots/library/subsystems/vision/questnav/commands/PingCommand.html) \ No newline at end of file diff --git a/docs/mkdocs/docs/library/commands/vision/oculus/resetpose.md b/docs/mkdocs/docs/library/commands/vision/oculus/resetpose.md new file mode 100644 index 00000000..377a7979 --- /dev/null +++ b/docs/mkdocs/docs/library/commands/vision/oculus/resetpose.md @@ -0,0 +1,20 @@ +# Reset Pose Command + +A command that resets the Oculus headset's position tracking to align with a specified pose on the field. + +## Required Subsystems +- [Oculus Navigation Subsystem](/5152_Template/library/subsystems/vision/questnav) + +## Constructor Parameters +```java +public ResetPoseCommand(OculusSubsystem oculus, Pose2d targetPose) +``` +- `oculus`: The Oculus subsystem instance to reset +- `targetPose`: The target pose to reset the tracking to + +## Configuration +The target pose should be specified in field coordinates. + +## Reference Documentation + +[JavaDoc Reference](/5152_Template/javadoc/frc/alotobots/library/subsystems/vision/questnav/commands/ResetPoseCommand.html) \ No newline at end of file diff --git a/docs/mkdocs/docs/library/commands/vision/oculus/zeroheading.md b/docs/mkdocs/docs/library/commands/vision/oculus/zeroheading.md new file mode 100644 index 00000000..7ef81837 --- /dev/null +++ b/docs/mkdocs/docs/library/commands/vision/oculus/zeroheading.md @@ -0,0 +1,19 @@ +# Zero Heading Command + +A command that zeros the heading (yaw) of the Oculus headset, setting the current heading as the zero reference point. + +## Required Subsystems +- [Oculus Navigation Subsystem](/5152_Template/library/subsystems/vision/questnav) + +## Constructor Parameters +```java +public ZeroHeadingCommand(OculusSubsystem oculus) +``` +- `oculus`: The Oculus subsystem instance to reset + +## Configuration +No additional configuration required. + +## Reference Documentation + +[JavaDoc Reference](/5152_Template/javadoc/frc/alotobots/library/subsystems/vision/questnav/commands/ZeroHeadingCommand.html) \ No newline at end of file diff --git a/docs/mkdocs/docs/library/subsystems/vision/oculus.md b/docs/mkdocs/docs/library/subsystems/vision/oculus.md new file mode 100644 index 00000000..bebbb04c --- /dev/null +++ b/docs/mkdocs/docs/library/subsystems/vision/oculus.md @@ -0,0 +1,36 @@ +# Oculus Navigation Subsystem + +The Oculus Navigation subsystem provides robot positioning and orientation tracking using an Oculus Quest VR headset. This system enables high-precision robot navigation and positioning by leveraging the Oculus Quest's advanced tracking capabilities. + +## Constructor and Configuration + +The subsystem is constructed with an OculusIO interface implementation: + +```java +public OculusSubsystem(OculusIO io) +``` + +The io parameter can be either: +- `OculusIOReal` for real hardware operation +- `OculusIOSim` for simulation testing + +## Commands + +The following commands interact with the Oculus subsystem: + +- [Ping Command](/5152_Template/library/commands/questnav/pingcommand) - Tests communication with the headset +- [Reset Pose Command](/5152_Template/library/commands/questnav/resetposecommand) - Resets the headset's position tracking +- [Zero Heading Command](/5152_Template/library/commands/questnav/zeroheadingcommand) - Zeros the headset's rotation tracking + +## Required Configuration + +1. Software Setup: + - [QuestNav](https://github.com/juchong/QuestNav/tree/main) must be deployed and running on the Oculus Quest headset + +2. Physical Setup: + - Oculus Quest headset must be mounted securely to the robot + - Headset position relative to robot center must be configured in OculusConstants.OCULUS_TO_ROBOT + +## Reference Documentation + +Full JavaDoc reference: [Package Documentation](/5152_Template/javadoc/frc/alotobots/library/subsystems/vision/questnav/package-summary.html) \ No newline at end of file diff --git a/docs/mkdocs/mkdocs.yml b/docs/mkdocs/mkdocs.yml index 8de7989c..cd18b01d 100644 --- a/docs/mkdocs/mkdocs.yml +++ b/docs/mkdocs/mkdocs.yml @@ -24,12 +24,17 @@ nav: - Object Detection: - Drive Facing Best Object: library/commands/vision/objectdetection/drivefacingbestobject.md - Pathfind to Best Object: library/commands/vision/objectdetection/pathfindtobestobject.md + - Oculus QuestNav: + - Ping: library/commands/vision/oculus/ping.md + - Reset Pose: library/commands/vision/oculus/resetpose.md + - Zero Heading: library/commands/vision/oculus/zeroheading.md - Subsystems: - Swerve: library/subsystems/swerve.md - Bling: library/subsystems/bling.md - Vision: - Object Detection: library/subsystems/vision/objectdetection.md - AprilTag: library/subsystems/vision/apriltag.md + - Oculus: library/subsystems/vision/oculus.md - Core: - Controller Bindings: core/bindings.md - Auto Named Commands: core/autonamedcommands.md From de2065a3353567daf4f645c5bbc3c664189df574 Mon Sep 17 00:00:00 2001 From: Sean Ernstes Date: Wed, 1 Jan 2025 16:35:56 -0500 Subject: [PATCH 12/23] refactor: update package name to match subsystem name --- .../mkdocs/docs/library/commands/vision/oculus/ping.md | 2 +- .../docs/library/commands/vision/oculus/resetpose.md | 2 +- .../docs/library/commands/vision/oculus/zeroheading.md | 2 +- docs/mkdocs/docs/library/subsystems/vision/oculus.md | 2 +- src/main/java/frc/alotobots/RobotContainer.java | 10 +++++----- .../vision/{questnav => oculus}/OculusSubsystem.java | 8 ++++---- .../{questnav => oculus}/commands/PingCommand.java | 6 +++--- .../commands/ResetPoseCommand.java | 6 +++--- .../commands/ZeroHeadingCommand.java | 6 +++--- .../constants/OculusConstants.java | 6 +++--- .../vision/{questnav => oculus}/io/OculusIO.java | 2 +- .../vision/{questnav => oculus}/io/OculusIOReal.java | 2 +- .../vision/{questnav => oculus}/io/OculusIOSim.java | 2 +- 13 files changed, 28 insertions(+), 28 deletions(-) rename src/main/java/frc/alotobots/library/subsystems/vision/{questnav => oculus}/OculusSubsystem.java (92%) rename src/main/java/frc/alotobots/library/subsystems/vision/{questnav => oculus}/commands/PingCommand.java (93%) rename src/main/java/frc/alotobots/library/subsystems/vision/{questnav => oculus}/commands/ResetPoseCommand.java (93%) rename src/main/java/frc/alotobots/library/subsystems/vision/{questnav => oculus}/commands/ZeroHeadingCommand.java (93%) rename src/main/java/frc/alotobots/library/subsystems/vision/{questnav => oculus}/constants/OculusConstants.java (88%) rename src/main/java/frc/alotobots/library/subsystems/vision/{questnav => oculus}/io/OculusIO.java (97%) rename src/main/java/frc/alotobots/library/subsystems/vision/{questnav => oculus}/io/OculusIOReal.java (98%) rename src/main/java/frc/alotobots/library/subsystems/vision/{questnav => oculus}/io/OculusIOSim.java (95%) diff --git a/docs/mkdocs/docs/library/commands/vision/oculus/ping.md b/docs/mkdocs/docs/library/commands/vision/oculus/ping.md index af9df876..0d566a1f 100644 --- a/docs/mkdocs/docs/library/commands/vision/oculus/ping.md +++ b/docs/mkdocs/docs/library/commands/vision/oculus/ping.md @@ -16,4 +16,4 @@ No additional configuration required. ## Reference Documentation -[JavaDoc Reference](/5152_Template/javadoc/frc/alotobots/library/subsystems/vision/questnav/commands/PingCommand.html) \ No newline at end of file +[JavaDoc Reference](/5152_Template/javadoc/frc/alotobots/library/subsystems/vision/questnav/commands/PingCommand.html) diff --git a/docs/mkdocs/docs/library/commands/vision/oculus/resetpose.md b/docs/mkdocs/docs/library/commands/vision/oculus/resetpose.md index 377a7979..223f25ee 100644 --- a/docs/mkdocs/docs/library/commands/vision/oculus/resetpose.md +++ b/docs/mkdocs/docs/library/commands/vision/oculus/resetpose.md @@ -17,4 +17,4 @@ The target pose should be specified in field coordinates. ## Reference Documentation -[JavaDoc Reference](/5152_Template/javadoc/frc/alotobots/library/subsystems/vision/questnav/commands/ResetPoseCommand.html) \ No newline at end of file +[JavaDoc Reference](/5152_Template/javadoc/frc/alotobots/library/subsystems/vision/questnav/commands/ResetPoseCommand.html) diff --git a/docs/mkdocs/docs/library/commands/vision/oculus/zeroheading.md b/docs/mkdocs/docs/library/commands/vision/oculus/zeroheading.md index 7ef81837..344eb110 100644 --- a/docs/mkdocs/docs/library/commands/vision/oculus/zeroheading.md +++ b/docs/mkdocs/docs/library/commands/vision/oculus/zeroheading.md @@ -16,4 +16,4 @@ No additional configuration required. ## Reference Documentation -[JavaDoc Reference](/5152_Template/javadoc/frc/alotobots/library/subsystems/vision/questnav/commands/ZeroHeadingCommand.html) \ No newline at end of file +[JavaDoc Reference](/5152_Template/javadoc/frc/alotobots/library/subsystems/vision/questnav/commands/ZeroHeadingCommand.html) diff --git a/docs/mkdocs/docs/library/subsystems/vision/oculus.md b/docs/mkdocs/docs/library/subsystems/vision/oculus.md index bebbb04c..2e6df9da 100644 --- a/docs/mkdocs/docs/library/subsystems/vision/oculus.md +++ b/docs/mkdocs/docs/library/subsystems/vision/oculus.md @@ -33,4 +33,4 @@ The following commands interact with the Oculus subsystem: ## Reference Documentation -Full JavaDoc reference: [Package Documentation](/5152_Template/javadoc/frc/alotobots/library/subsystems/vision/questnav/package-summary.html) \ No newline at end of file +Full JavaDoc reference: [Package Documentation](/5152_Template/javadoc/frc/alotobots/library/subsystems/vision/questnav/package-summary.html) diff --git a/src/main/java/frc/alotobots/RobotContainer.java b/src/main/java/frc/alotobots/RobotContainer.java index 4a4e7c85..3e057ef2 100644 --- a/src/main/java/frc/alotobots/RobotContainer.java +++ b/src/main/java/frc/alotobots/RobotContainer.java @@ -32,6 +32,11 @@ import frc.alotobots.library.subsystems.swervedrive.commands.FeedforwardCharacterization; import frc.alotobots.library.subsystems.swervedrive.commands.WheelRadiusCharacterization; import frc.alotobots.library.subsystems.swervedrive.io.*; +import frc.alotobots.library.subsystems.vision.oculus.OculusSubsystem; +import frc.alotobots.library.subsystems.vision.oculus.commands.ResetPoseCommand; +import frc.alotobots.library.subsystems.vision.oculus.io.OculusIO; +import frc.alotobots.library.subsystems.vision.oculus.io.OculusIOReal; +import frc.alotobots.library.subsystems.vision.oculus.io.OculusIOSim; import frc.alotobots.library.subsystems.vision.photonvision.apriltag.AprilTagSubsystem; import frc.alotobots.library.subsystems.vision.photonvision.apriltag.constants.AprilTagConstants; import frc.alotobots.library.subsystems.vision.photonvision.apriltag.io.AprilTagIO; @@ -43,11 +48,6 @@ import frc.alotobots.library.subsystems.vision.photonvision.objectdetection.constants.ObjectDetectionConstants; import frc.alotobots.library.subsystems.vision.photonvision.objectdetection.io.ObjectDetectionIO; import frc.alotobots.library.subsystems.vision.photonvision.objectdetection.io.ObjectDetectionIOPhotonVision; -import frc.alotobots.library.subsystems.vision.questnav.OculusSubsystem; -import frc.alotobots.library.subsystems.vision.questnav.commands.ResetPoseCommand; -import frc.alotobots.library.subsystems.vision.questnav.io.OculusIO; -import frc.alotobots.library.subsystems.vision.questnav.io.OculusIOReal; -import frc.alotobots.library.subsystems.vision.questnav.io.OculusIOSim; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; /** diff --git a/src/main/java/frc/alotobots/library/subsystems/vision/questnav/OculusSubsystem.java b/src/main/java/frc/alotobots/library/subsystems/vision/oculus/OculusSubsystem.java similarity index 92% rename from src/main/java/frc/alotobots/library/subsystems/vision/questnav/OculusSubsystem.java rename to src/main/java/frc/alotobots/library/subsystems/vision/oculus/OculusSubsystem.java index deaa7102..4975d459 100644 --- a/src/main/java/frc/alotobots/library/subsystems/vision/questnav/OculusSubsystem.java +++ b/src/main/java/frc/alotobots/library/subsystems/vision/oculus/OculusSubsystem.java @@ -10,16 +10,16 @@ * * Source code must be publicly available on GitHub or an alternative web accessible site */ -package frc.alotobots.library.subsystems.vision.questnav; +package frc.alotobots.library.subsystems.vision.oculus; -import static frc.alotobots.library.subsystems.vision.questnav.constants.OculusConstants.*; +import static frc.alotobots.library.subsystems.vision.oculus.constants.OculusConstants.*; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.alotobots.library.subsystems.vision.questnav.io.OculusIO; -import frc.alotobots.library.subsystems.vision.questnav.io.OculusIOInputsAutoLogged; +import frc.alotobots.library.subsystems.vision.oculus.io.OculusIO; +import frc.alotobots.library.subsystems.vision.oculus.io.OculusIOInputsAutoLogged; import org.littletonrobotics.junction.Logger; /** diff --git a/src/main/java/frc/alotobots/library/subsystems/vision/questnav/commands/PingCommand.java b/src/main/java/frc/alotobots/library/subsystems/vision/oculus/commands/PingCommand.java similarity index 93% rename from src/main/java/frc/alotobots/library/subsystems/vision/questnav/commands/PingCommand.java rename to src/main/java/frc/alotobots/library/subsystems/vision/oculus/commands/PingCommand.java index b35b442e..15d79afe 100644 --- a/src/main/java/frc/alotobots/library/subsystems/vision/questnav/commands/PingCommand.java +++ b/src/main/java/frc/alotobots/library/subsystems/vision/oculus/commands/PingCommand.java @@ -10,13 +10,13 @@ * * Source code must be publicly available on GitHub or an alternative web accessible site */ -package frc.alotobots.library.subsystems.vision.questnav.commands; +package frc.alotobots.library.subsystems.vision.oculus.commands; -import static frc.alotobots.library.subsystems.vision.questnav.constants.OculusConstants.*; +import static frc.alotobots.library.subsystems.vision.oculus.constants.OculusConstants.*; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; -import frc.alotobots.library.subsystems.vision.questnav.OculusSubsystem; +import frc.alotobots.library.subsystems.vision.oculus.OculusSubsystem; import org.littletonrobotics.junction.Logger; /** diff --git a/src/main/java/frc/alotobots/library/subsystems/vision/questnav/commands/ResetPoseCommand.java b/src/main/java/frc/alotobots/library/subsystems/vision/oculus/commands/ResetPoseCommand.java similarity index 93% rename from src/main/java/frc/alotobots/library/subsystems/vision/questnav/commands/ResetPoseCommand.java rename to src/main/java/frc/alotobots/library/subsystems/vision/oculus/commands/ResetPoseCommand.java index f60f806b..a00687b1 100644 --- a/src/main/java/frc/alotobots/library/subsystems/vision/questnav/commands/ResetPoseCommand.java +++ b/src/main/java/frc/alotobots/library/subsystems/vision/oculus/commands/ResetPoseCommand.java @@ -10,14 +10,14 @@ * * Source code must be publicly available on GitHub or an alternative web accessible site */ -package frc.alotobots.library.subsystems.vision.questnav.commands; +package frc.alotobots.library.subsystems.vision.oculus.commands; -import static frc.alotobots.library.subsystems.vision.questnav.constants.OculusConstants.*; +import static frc.alotobots.library.subsystems.vision.oculus.constants.OculusConstants.*; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; -import frc.alotobots.library.subsystems.vision.questnav.OculusSubsystem; +import frc.alotobots.library.subsystems.vision.oculus.OculusSubsystem; import org.littletonrobotics.junction.Logger; /** diff --git a/src/main/java/frc/alotobots/library/subsystems/vision/questnav/commands/ZeroHeadingCommand.java b/src/main/java/frc/alotobots/library/subsystems/vision/oculus/commands/ZeroHeadingCommand.java similarity index 93% rename from src/main/java/frc/alotobots/library/subsystems/vision/questnav/commands/ZeroHeadingCommand.java rename to src/main/java/frc/alotobots/library/subsystems/vision/oculus/commands/ZeroHeadingCommand.java index efcb4d63..71bbe688 100644 --- a/src/main/java/frc/alotobots/library/subsystems/vision/questnav/commands/ZeroHeadingCommand.java +++ b/src/main/java/frc/alotobots/library/subsystems/vision/oculus/commands/ZeroHeadingCommand.java @@ -10,13 +10,13 @@ * * Source code must be publicly available on GitHub or an alternative web accessible site */ -package frc.alotobots.library.subsystems.vision.questnav.commands; +package frc.alotobots.library.subsystems.vision.oculus.commands; -import static frc.alotobots.library.subsystems.vision.questnav.constants.OculusConstants.*; +import static frc.alotobots.library.subsystems.vision.oculus.constants.OculusConstants.*; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; -import frc.alotobots.library.subsystems.vision.questnav.OculusSubsystem; +import frc.alotobots.library.subsystems.vision.oculus.OculusSubsystem; import org.littletonrobotics.junction.Logger; /** diff --git a/src/main/java/frc/alotobots/library/subsystems/vision/questnav/constants/OculusConstants.java b/src/main/java/frc/alotobots/library/subsystems/vision/oculus/constants/OculusConstants.java similarity index 88% rename from src/main/java/frc/alotobots/library/subsystems/vision/questnav/constants/OculusConstants.java rename to src/main/java/frc/alotobots/library/subsystems/vision/oculus/constants/OculusConstants.java index 10d56f69..a160bf3b 100644 --- a/src/main/java/frc/alotobots/library/subsystems/vision/questnav/constants/OculusConstants.java +++ b/src/main/java/frc/alotobots/library/subsystems/vision/oculus/constants/OculusConstants.java @@ -10,7 +10,7 @@ * * Source code must be publicly available on GitHub or an alternative web accessible site */ -package frc.alotobots.library.subsystems.vision.questnav.constants; +package frc.alotobots.library.subsystems.vision.oculus.constants; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform2d; @@ -21,8 +21,8 @@ */ public class OculusConstants { /** - * Transform from the robot center to the headset. Coordinate system: - X: - * Positive is forwards - Y: Positive is left - Rotation: Positive is counter-clockwise + * Transform from the robot center to the headset. Coordinate system: - X: Positive is forwards - + * Y: Positive is left - Rotation: Positive is counter-clockwise */ public static final Transform2d ROBOT_TO_OCULUS = new Transform2d(0.075, 0.0, new Rotation2d()); diff --git a/src/main/java/frc/alotobots/library/subsystems/vision/questnav/io/OculusIO.java b/src/main/java/frc/alotobots/library/subsystems/vision/oculus/io/OculusIO.java similarity index 97% rename from src/main/java/frc/alotobots/library/subsystems/vision/questnav/io/OculusIO.java rename to src/main/java/frc/alotobots/library/subsystems/vision/oculus/io/OculusIO.java index 804b99a3..9ddbbdb7 100644 --- a/src/main/java/frc/alotobots/library/subsystems/vision/questnav/io/OculusIO.java +++ b/src/main/java/frc/alotobots/library/subsystems/vision/oculus/io/OculusIO.java @@ -10,7 +10,7 @@ * * Source code must be publicly available on GitHub or an alternative web accessible site */ -package frc.alotobots.library.subsystems.vision.questnav.io; +package frc.alotobots.library.subsystems.vision.oculus.io; import org.littletonrobotics.junction.AutoLog; diff --git a/src/main/java/frc/alotobots/library/subsystems/vision/questnav/io/OculusIOReal.java b/src/main/java/frc/alotobots/library/subsystems/vision/oculus/io/OculusIOReal.java similarity index 98% rename from src/main/java/frc/alotobots/library/subsystems/vision/questnav/io/OculusIOReal.java rename to src/main/java/frc/alotobots/library/subsystems/vision/oculus/io/OculusIOReal.java index e99b9818..6bc7daf6 100644 --- a/src/main/java/frc/alotobots/library/subsystems/vision/questnav/io/OculusIOReal.java +++ b/src/main/java/frc/alotobots/library/subsystems/vision/oculus/io/OculusIOReal.java @@ -10,7 +10,7 @@ * * Source code must be publicly available on GitHub or an alternative web accessible site */ -package frc.alotobots.library.subsystems.vision.questnav.io; +package frc.alotobots.library.subsystems.vision.oculus.io; import edu.wpi.first.networktables.*; diff --git a/src/main/java/frc/alotobots/library/subsystems/vision/questnav/io/OculusIOSim.java b/src/main/java/frc/alotobots/library/subsystems/vision/oculus/io/OculusIOSim.java similarity index 95% rename from src/main/java/frc/alotobots/library/subsystems/vision/questnav/io/OculusIOSim.java rename to src/main/java/frc/alotobots/library/subsystems/vision/oculus/io/OculusIOSim.java index ec42a6ed..aef56094 100644 --- a/src/main/java/frc/alotobots/library/subsystems/vision/questnav/io/OculusIOSim.java +++ b/src/main/java/frc/alotobots/library/subsystems/vision/oculus/io/OculusIOSim.java @@ -10,7 +10,7 @@ * * Source code must be publicly available on GitHub or an alternative web accessible site */ -package frc.alotobots.library.subsystems.vision.questnav.io; +package frc.alotobots.library.subsystems.vision.oculus.io; /** Simulation implementation of OculusIO that provides static test values. */ public class OculusIOSim implements OculusIO { From 1f1fab31e11738338061c7f2b9406d9fc1b53e16 Mon Sep 17 00:00:00 2001 From: Sean Ernstes Date: Wed, 1 Jan 2025 20:17:18 -0500 Subject: [PATCH 13/23] Follow WPILib subsystem conventions --- .../java/frc/alotobots/RobotContainer.java | 10 +- .../vision/oculus/OculusSubsystem.java | 411 +++++++++++++----- .../vision/oculus/commands/PingCommand.java | 81 +--- .../oculus/commands/ResetPoseCommand.java | 84 +--- .../oculus/commands/ZeroHeadingCommand.java | 82 +--- .../oculus/constants/OculusConstants.java | 43 +- 6 files changed, 361 insertions(+), 350 deletions(-) diff --git a/src/main/java/frc/alotobots/RobotContainer.java b/src/main/java/frc/alotobots/RobotContainer.java index 3e057ef2..e9ec4d2d 100644 --- a/src/main/java/frc/alotobots/RobotContainer.java +++ b/src/main/java/frc/alotobots/RobotContainer.java @@ -19,6 +19,7 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.alotobots.library.subsystems.bling.BlingSubsystem; import frc.alotobots.library.subsystems.bling.commands.NoAllianceWaiting; @@ -188,9 +189,12 @@ private void configureLogicCommands() { pathfindToBestObjectButton.onTrue( new PathfindToBestObject(objectDetectionSubsystem, swerveDriveSubsystem, NOTE)); testButton.onTrue( - new ResetPoseCommand(oculusSubsystem, new Pose2d(15.3, 5.5, Rotation2d.fromDegrees(0.0)))); - testButton2.onTrue( - new ResetPoseCommand(oculusSubsystem, new Pose2d(5.3, 2.5, Rotation2d.fromDegrees(180.0)))); + new ResetPoseCommand(oculusSubsystem, new Pose2d(15.3, 5.5, Rotation2d.fromDegrees(0.0))) + .andThen( + new InstantCommand( + () -> + swerveDriveSubsystem.setPose( + new Pose2d(15.3, 5.5, Rotation2d.fromDegrees(0.0)))))); } /** diff --git a/src/main/java/frc/alotobots/library/subsystems/vision/oculus/OculusSubsystem.java b/src/main/java/frc/alotobots/library/subsystems/vision/oculus/OculusSubsystem.java index 4975d459..383857e3 100644 --- a/src/main/java/frc/alotobots/library/subsystems/vision/oculus/OculusSubsystem.java +++ b/src/main/java/frc/alotobots/library/subsystems/vision/oculus/OculusSubsystem.java @@ -17,122 +17,311 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.alotobots.library.subsystems.vision.oculus.io.OculusIO; import frc.alotobots.library.subsystems.vision.oculus.io.OculusIOInputsAutoLogged; +import lombok.Getter; import org.littletonrobotics.junction.Logger; /** - * The OculusSubsystem manages communication and data handling for the Oculus Quest VR headset used - * for robot navigation and positioning. + * The OculusSubsystem manages communication and pose estimation with an Oculus Quest VR headset. + * This subsystem provides robot positioning using the Quest's inside-out tracking system. */ public class OculusSubsystem extends SubsystemBase { - /** The IO interface for communicating with the Oculus hardware. */ - private final OculusIO io; - - /** Logged inputs from the Oculus device. */ - private final OculusIOInputsAutoLogged inputs = new OculusIOInputsAutoLogged(); - - /** - * Creates a new OculusSubsystem. - * - * @param io The IO interface for communicating with the Oculus hardware - */ - public OculusSubsystem(OculusIO io) { - this.io = io; - Logger.recordOutput("Oculus/status", "Initialized"); - } - - @Override - public void periodic() { - io.updateInputs(inputs); - Logger.processInputs("Oculus", inputs); - - var oculusPose = getOculusPose(); - var robotPose = oculusPose.transformBy(ROBOT_TO_OCULUS.inverse()); - Logger.recordOutput("Oculus/poses/headsetPose", oculusPose); - Logger.recordOutput("Oculus/poses/robotPose", robotPose); - } - - /** - * Gets the current yaw (rotation) of the Oculus headset. - * - * @return The yaw as a Rotation2d - */ - private Rotation2d getOculusYaw() { - return Rotation2d.fromDegrees(-inputs.eulerAngles[1]); - } - - /** - * Gets the current position of the Oculus headset. - * - * @return The position as a Translation2d - */ - private Translation2d getOculusPosition() { - return new Translation2d(inputs.position[2], -inputs.position[0]); - } - - /** - * Gets the current pose (position and rotation) of the Oculus headset. - * - * @return The pose of the Oculus headset - */ - public Pose2d getOculusPose() { - return new Pose2d(getOculusPosition(), getOculusYaw()); - } - - /** - * Gets the current pose of the robot based on the Oculus headset position. - * - * @return The calculated robot pose - */ - public Pose2d getRobotPose() { - return getOculusPose().transformBy(ROBOT_TO_OCULUS.inverse()); - } - - /** - * Gets the current MISO (Master In Slave Out) value from the Oculus. - * - * @return The current MISO value - */ - public int getMisoValue() { - return inputs.misoValue; - } - - /** - * Sets the MOSI (Master Out Slave In) value for the Oculus. - * - * @param value The MOSI value to set - */ - public void setMosi(int value) { - io.setMosi(value); - } - - /** - * Sets a new reset pose for the Oculus tracking system. - * - * @param pose The target pose to reset to - */ - public void setResetPose(Pose2d pose) { - var targetPose = pose.plus(ROBOT_TO_OCULUS); - io.setResetPose(targetPose.getX(), targetPose.getY(), targetPose.getRotation().getDegrees()); - } - - /** - * Logs a status message for the Oculus subsystem. - * - * @param message The status message to log - */ - public void logStatus(String message) { - Logger.recordOutput("Oculus/status", message); - } - - /** - * Logs the ping time for communication with the Oculus. - * - * @param timestamp The timestamp to log - */ - public void logPingTime(double timestamp) { - Logger.recordOutput("Oculus/ping/sendTime", timestamp); - } -} + /** Status code indicating the Oculus system is ready for commands */ + private static final int STATUS_READY = 0; + /** Status code indicating a heading reset operation has completed */ + private static final int STATUS_HEADING_RESET_COMPLETE = 99; + /** Status code indicating a pose reset operation has completed */ + private static final int STATUS_POSE_RESET_COMPLETE = 98; + /** Status code indicating a ping response has been received */ + private static final int STATUS_PING_RESPONSE = 97; + + /** IO interface for communicating with the Oculus hardware */ + private final OculusIO io; + /** Logged inputs from the Oculus system */ + private final OculusIOInputsAutoLogged inputs = new OculusIOInputsAutoLogged(); + + /** Flag indicating if a pose reset operation is currently in progress */ + @Getter private boolean poseResetInProgress = false; + /** Flag indicating if a heading reset operation is currently in progress */ + @Getter private boolean headingResetInProgress = false; + /** Flag indicating if a ping operation is currently in progress */ + @Getter private boolean pingInProgress = false; + /** Previous connection state for detecting status changes */ + @Getter private boolean wasConnected = false; + + /** Timestamp when the current reset operation started */ + private double resetStartTime = 0; + /** Counter for the number of reset attempts made */ + private int currentResetAttempt = 0; + /** Previous timestamp from the Oculus Quest */ + private double lastTimestamp = 0.0; + /** Most recent timestamp from the Oculus Quest */ + private double currentTimestamp = 0.0; + /** Target pose for the current reset operation */ + private Pose2d pendingResetPose = null; + + /** + * Creates a new OculusSubsystem with the specified IO interface. + * + * @param io The IO interface for communicating with the Oculus hardware + */ + public OculusSubsystem(OculusIO io) { + this.io = io; + Logger.recordOutput("Oculus/status", "Initialized"); + } + + @Override + public void periodic() { + io.updateInputs(inputs); + Logger.processInputs("Oculus", inputs); + + var oculusPose = getOculusPose(); + Logger.recordOutput("Oculus/status/poses/headsetPose", oculusPose); + var robotPose = oculusPose.transformBy(ROBOT_TO_OCULUS.inverse()); + Logger.recordOutput("Oculus/status/poses/robotPose", robotPose); + + handleResetTimeout(); + handleResetCompletion(); + handlePingResponse(); + } + + /** + * Checks if any reset operations have timed out and handles them accordingly. + */ + private void handleResetTimeout() { + double currentTime = Timer.getTimestamp(); + if (currentTime - resetStartTime > RESET_TIMEOUT_SECONDS) { + if (headingResetInProgress) handleReset(true); + if (poseResetInProgress) handleReset(false); + } + } + + /** + * Handles reset operation retries and failures. + * + * @param isHeadingReset True if handling a heading reset, false if handling a pose reset + */ + private void handleReset(boolean isHeadingReset) { + if (currentResetAttempt < MAX_RESET_ATTEMPTS) { + String resetType = isHeadingReset ? "Heading" : "Pose"; + Logger.recordOutput( + "Oculus/status", + resetType + " Reset attempt " + (currentResetAttempt + 1) + " timed out, retrying..."); + currentResetAttempt++; + resetStartTime = Timer.getTimestamp(); + + io.setMosi(0); // Clear + + if (isHeadingReset) { + io.setMosi(1); // Heading Reset + } else { + io.setResetPose( + pendingResetPose.getX(), + pendingResetPose.getY(), + pendingResetPose.getRotation().getDegrees()); + io.setMosi(2); // Pose Reset + } + } else { + Logger.recordOutput( + "Oculus/status", + (isHeadingReset ? "Heading" : "Pose") + + " Reset failed after " + + MAX_RESET_ATTEMPTS + + " attempts"); + if (isHeadingReset) { + clearHeadingResetState(); + } else { + clearPoseResetState(); + } + } + } + + /** + * Checks for and handles completion of reset operations. + */ + private void handleResetCompletion() { + if (headingResetInProgress && inputs.misoValue == STATUS_HEADING_RESET_COMPLETE) { + Logger.recordOutput( + "Oculus/status", + "Heading Reset completed successfully on attempt " + (currentResetAttempt + 1)); + clearHeadingResetState(); + } + if (poseResetInProgress && inputs.misoValue == STATUS_POSE_RESET_COMPLETE) { + Logger.recordOutput( + "Oculus/status", + "Pose Reset completed successfully on attempt " + (currentResetAttempt + 1)); + clearPoseResetState(); + } + } + + /** + * Handles ping response from the Oculus system. + */ + private void handlePingResponse() { + if (inputs.misoValue == STATUS_PING_RESPONSE) { + Logger.recordOutput("Oculus/status", "Ping response received"); + io.setMosi(0); // Clear + pingInProgress = false; + } + } + + /** + * Clears the state associated with a pose reset operation. + */ + private void clearPoseResetState() { + Logger.recordOutput("Oculus/status", "Clearing pose reset state"); + poseResetInProgress = false; + pendingResetPose = null; + currentResetAttempt = 0; + io.setMosi(0); // Clear + } + + /** + * Clears the state associated with a heading reset operation. + */ + private void clearHeadingResetState() { + Logger.recordOutput("Oculus/status", "Clearing heading reset state"); + headingResetInProgress = false; + currentResetAttempt = 0; + io.setMosi(0); // Clear + } + + /** + * Gets the current yaw (rotation) of the Oculus headset. + * + * @return The headset's yaw as a Rotation2d + */ + private Rotation2d getOculusYaw() { + return Rotation2d.fromDegrees(-inputs.eulerAngles[1]); + } + + /** + * Gets the current position of the Oculus headset. + * + * @return The headset's position as a Translation2d + */ + private Translation2d getOculusPosition() { + return new Translation2d(inputs.position[2], -inputs.position[0]); + } + + /** + * Gets the current pose (position and rotation) of the Oculus headset. + * + * @return The headset's pose + */ + public Pose2d getOculusPose() { + return new Pose2d(getOculusPosition(), getOculusYaw()); + } + + /** + * Gets the current pose of the robot, derived from the Oculus headset's pose. + * + * @return The robot's pose + */ + public Pose2d getRobotPose() { + return getOculusPose().transformBy(ROBOT_TO_OCULUS.inverse()); + } + + /** + * Initiates a pose reset operation to a specified target pose. + * + * @param targetPose The desired pose to reset to + * @return True if the reset was initiated successfully, false otherwise + */ + public boolean resetToPose(Pose2d targetPose) { + if (poseResetInProgress) { + Logger.recordOutput("Oculus/status", "Cannot reset pose - reset already in progress"); + return false; + } + + if (inputs.misoValue != STATUS_READY) { + Logger.recordOutput( + "Oculus/status", "Cannot reset pose - Quest busy (MISO=" + inputs.misoValue + ")"); + return false; + } + + targetPose = targetPose.plus(ROBOT_TO_OCULUS); + pendingResetPose = targetPose; + Logger.recordOutput( + "Oculus/status", + String.format( + "Initiating pose reset to X:%.2f Y:%.2f Rot:%.2f°", + targetPose.getX(), targetPose.getY(), targetPose.getRotation().getDegrees())); + + io.setResetPose(targetPose.getX(), targetPose.getY(), targetPose.getRotation().getDegrees()); + poseResetInProgress = true; + resetStartTime = Timer.getTimestamp(); + currentResetAttempt = 0; + io.setMosi(2); // Pose Reset + + return true; + } + + /** + * Initiates a heading reset operation to zero the current heading. + * + * @return True if the reset was initiated successfully, false otherwise + */ + public boolean zeroHeading() { + if (headingResetInProgress) { + Logger.recordOutput("Oculus/status", "Cannot zero heading - reset already in progress"); + return false; + } + + if (inputs.misoValue != STATUS_READY) { + Logger.recordOutput( + "Oculus/status", "Cannot zero heading - Quest busy (MISO=" + inputs.misoValue + ")"); + return false; + } + + Logger.recordOutput("Oculus/status", "Zeroing heading"); + headingResetInProgress = true; + resetStartTime = Timer.getTimestamp(); + currentResetAttempt = 0; + io.setMosi(1); // Heading Reset + return true; + } + + /** + * Sends a ping command to the Oculus system to verify communication. + * + * @return True if the ping was initiated successfully, false otherwise + */ + public boolean ping() { + if (pingInProgress) { + Logger.recordOutput("Oculus/status", "Cannot ping - ping already in progress"); + return false; + } + + if (inputs.misoValue != STATUS_READY) { + Logger.recordOutput( + "Oculus/status", "Cannot ping - system busy (MISO=" + inputs.misoValue + ")"); + return false; + } + + Logger.recordOutput("Oculus/status", "Sending ping..."); + Logger.recordOutput("Oculus/ping/sendTime", Timer.getTimestamp()); + pingInProgress = true; + io.setMosi(3); // Ping + return true; + } + + /** + * Checks if the Oculus Quest is currently connected and sending valid data. + * This is determined by monitoring if the timestamp values are increasing. + * + * @return true if the Quest is connected and sending data, false otherwise + */ + public boolean isConnected() { + double currentTime = Timer.getTimestamp(); + + // Check if timestamps are increasing and within timeout window + boolean timestampValid = currentTimestamp > lastTimestamp; + boolean timeoutValid = (currentTime - currentTimestamp) < CONNECTION_TIMEOUT; + + return timestampValid && timeoutValid; + } +} \ No newline at end of file diff --git a/src/main/java/frc/alotobots/library/subsystems/vision/oculus/commands/PingCommand.java b/src/main/java/frc/alotobots/library/subsystems/vision/oculus/commands/PingCommand.java index 15d79afe..0815edb1 100644 --- a/src/main/java/frc/alotobots/library/subsystems/vision/oculus/commands/PingCommand.java +++ b/src/main/java/frc/alotobots/library/subsystems/vision/oculus/commands/PingCommand.java @@ -12,36 +12,17 @@ */ package frc.alotobots.library.subsystems.vision.oculus.commands; -import static frc.alotobots.library.subsystems.vision.oculus.constants.OculusConstants.*; - -import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import frc.alotobots.library.subsystems.vision.oculus.OculusSubsystem; -import org.littletonrobotics.junction.Logger; /** - * Command that sends a ping request to the Oculus headset and waits for a response. Used to verify - * communication with the headset is working properly. + * Command that sends a ping to the Oculus system and waits for a response. + * Used to verify communication with the Oculus headset. */ public class PingCommand extends Command { - /** Status code indicating the Oculus is ready for commands */ - private static final int STATUS_READY = 0; - - /** Status code indicating the Oculus has responded to ping */ - private static final int STATUS_PING_RESPONSE = 97; - - /** The Oculus subsystem instance */ + /** The Oculus subsystem instance to use for pinging */ private final OculusSubsystem oculus; - /** Flag indicating if ping sequence has started */ - private boolean hasStarted; - - /** Counter for ping attempts */ - private int currentAttempt; - - /** Timestamp when current ping attempt started */ - private double startTime; - /** * Creates a new PingCommand. * @@ -54,61 +35,11 @@ public PingCommand(OculusSubsystem oculus) { @Override public void initialize() { - startPing(); - hasStarted = false; - currentAttempt = 0; - startTime = 0; - } - - /** Initiates a new ping attempt if the Oculus is ready. */ - private void startPing() { - if (oculus.getMisoValue() == STATUS_READY) { - oculus.logStatus("Starting ping attempt " + currentAttempt); - oculus.logPingTime(Timer.getTimestamp()); - oculus.setMosi(3); // Ping - hasStarted = true; - startTime = Timer.getTimestamp(); - currentAttempt++; - } - } - - @Override - public void execute() { - if (!hasStarted) { - startPing(); - return; - } - - // Check for timeout - if (Timer.getTimestamp() - startTime > RESET_TIMEOUT_SECONDS) { - if (currentAttempt < MAX_RESET_ATTEMPTS) { - Logger.recordOutput( - "Oculus/status", "Ping attempt " + currentAttempt + " timed out, retrying..."); - oculus.setMosi(0); // Clear - startPing(); - } else { - Logger.recordOutput( - "Oculus/status", "Ping failed after " + MAX_RESET_ATTEMPTS + " attempts"); - hasStarted = false; - } - } + oculus.ping(); } @Override public boolean isFinished() { - if (!hasStarted) return true; - return oculus.getMisoValue() == STATUS_PING_RESPONSE; - } - - @Override - public void end(boolean interrupted) { - if (hasStarted) { - oculus.setMosi(0); // Clear - Logger.recordOutput( - "Oculus/status", - interrupted - ? "Ping interrupted" - : "Ping completed successfully on attempt " + currentAttempt); - } + return !oculus.isPingInProgress(); } -} +} \ No newline at end of file diff --git a/src/main/java/frc/alotobots/library/subsystems/vision/oculus/commands/ResetPoseCommand.java b/src/main/java/frc/alotobots/library/subsystems/vision/oculus/commands/ResetPoseCommand.java index a00687b1..b1983c04 100644 --- a/src/main/java/frc/alotobots/library/subsystems/vision/oculus/commands/ResetPoseCommand.java +++ b/src/main/java/frc/alotobots/library/subsystems/vision/oculus/commands/ResetPoseCommand.java @@ -12,45 +12,25 @@ */ package frc.alotobots.library.subsystems.vision.oculus.commands; -import static frc.alotobots.library.subsystems.vision.oculus.constants.OculusConstants.*; - import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import frc.alotobots.library.subsystems.vision.oculus.OculusSubsystem; -import org.littletonrobotics.junction.Logger; /** - * Command that resets the Oculus headset's position tracking to a specified pose. This is used to - * align the Oculus coordinate system with the field coordinate system. + * Command that resets the Oculus system's pose estimation to a specified target pose. + * This is useful for initializing or correcting the robot's position tracking. */ public class ResetPoseCommand extends Command { - /** Status code indicating the Oculus is ready for commands */ - private static final int STATUS_READY = 0; - - /** Status code indicating the pose reset is complete */ - private static final int STATUS_POSE_RESET_COMPLETE = 98; - - /** The Oculus subsystem instance */ + /** The Oculus subsystem instance to reset */ private final OculusSubsystem oculus; - /** The target pose to reset to */ private final Pose2d targetPose; - /** Flag indicating if reset sequence has started */ - private boolean hasStarted; - - /** Counter for reset attempts */ - private int currentAttempt; - - /** Timestamp when current reset attempt started */ - private double startTime; - /** * Creates a new ResetPoseCommand. * * @param oculus The Oculus subsystem to reset - * @param targetPose The target pose to reset to + * @param targetPose The desired pose to reset to */ public ResetPoseCommand(OculusSubsystem oculus, Pose2d targetPose) { this.oculus = oculus; @@ -60,61 +40,11 @@ public ResetPoseCommand(OculusSubsystem oculus, Pose2d targetPose) { @Override public void initialize() { - hasStarted = false; - currentAttempt = 0; - startTime = 0; - startReset(); - } - - /** Initiates a new pose reset attempt if the Oculus is ready. */ - private void startReset() { - if (oculus.getMisoValue() == STATUS_READY) { - oculus.setResetPose(targetPose); - oculus.setMosi(2); // Pose Reset - hasStarted = true; - startTime = Timer.getTimestamp(); - currentAttempt++; - Logger.recordOutput("Oculus/status", "Starting pose reset attempt " + currentAttempt); - } - } - - @Override - public void execute() { - if (!hasStarted) { - startReset(); - return; - } - - // Check for timeout - if (Timer.getTimestamp() - startTime > RESET_TIMEOUT_SECONDS) { - if (currentAttempt < MAX_RESET_ATTEMPTS) { - Logger.recordOutput( - "Oculus/status", "Pose reset attempt " + currentAttempt + " timed out, retrying..."); - oculus.setMosi(0); // Clear - startReset(); - } else { - Logger.recordOutput( - "Oculus/status", "Pose reset failed after " + MAX_RESET_ATTEMPTS + " attempts"); - hasStarted = false; - } - } + oculus.resetToPose(targetPose); } @Override public boolean isFinished() { - if (!hasStarted) return true; - return oculus.getMisoValue() == STATUS_POSE_RESET_COMPLETE; - } - - @Override - public void end(boolean interrupted) { - if (hasStarted) { - oculus.setMosi(0); // Clear - Logger.recordOutput( - "Oculus/status", - interrupted - ? "Pose reset interrupted" - : "Pose reset completed successfully on attempt " + currentAttempt); - } + return !oculus.isPoseResetInProgress(); } -} +} \ No newline at end of file diff --git a/src/main/java/frc/alotobots/library/subsystems/vision/oculus/commands/ZeroHeadingCommand.java b/src/main/java/frc/alotobots/library/subsystems/vision/oculus/commands/ZeroHeadingCommand.java index 71bbe688..903d2f81 100644 --- a/src/main/java/frc/alotobots/library/subsystems/vision/oculus/commands/ZeroHeadingCommand.java +++ b/src/main/java/frc/alotobots/library/subsystems/vision/oculus/commands/ZeroHeadingCommand.java @@ -12,40 +12,21 @@ */ package frc.alotobots.library.subsystems.vision.oculus.commands; -import static frc.alotobots.library.subsystems.vision.oculus.constants.OculusConstants.*; - -import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import frc.alotobots.library.subsystems.vision.oculus.OculusSubsystem; -import org.littletonrobotics.junction.Logger; /** - * Command that zeros the heading (yaw) of the Oculus headset. This sets the current heading as the - * zero reference point for rotation tracking. + * Command that zeros the heading (rotation) of the Oculus system. + * This is useful for aligning the robot's coordinate system with the field. */ public class ZeroHeadingCommand extends Command { - /** Status code indicating the Oculus is ready for commands */ - private static final int STATUS_READY = 0; - - /** Status code indicating the heading reset is complete */ - private static final int STATUS_HEADING_RESET_COMPLETE = 99; - - /** The Oculus subsystem instance */ + /** The Oculus subsystem instance to zero */ private final OculusSubsystem oculus; - /** Flag indicating if reset sequence has started */ - private boolean hasStarted; - - /** Counter for reset attempts */ - private int currentAttempt; - - /** Timestamp when current reset attempt started */ - private double startTime; - /** * Creates a new ZeroHeadingCommand. * - * @param oculus The Oculus subsystem to reset + * @param oculus The Oculus subsystem to zero */ public ZeroHeadingCommand(OculusSubsystem oculus) { this.oculus = oculus; @@ -54,60 +35,11 @@ public ZeroHeadingCommand(OculusSubsystem oculus) { @Override public void initialize() { - startReset(); - hasStarted = false; - currentAttempt = 0; - startTime = 0; - } - - /** Initiates a new heading reset attempt if the Oculus is ready. */ - private void startReset() { - if (oculus.getMisoValue() == STATUS_READY) { - oculus.setMosi(1); // Heading Reset - hasStarted = true; - startTime = Timer.getTimestamp(); - currentAttempt++; - Logger.recordOutput("Oculus/status", "Starting heading reset attempt " + currentAttempt); - } - } - - @Override - public void execute() { - if (!hasStarted) { - startReset(); - return; - } - - // Check for timeout - if (Timer.getTimestamp() - startTime > RESET_TIMEOUT_SECONDS) { - if (currentAttempt < MAX_RESET_ATTEMPTS) { - Logger.recordOutput( - "Oculus/status", "Heading reset attempt " + currentAttempt + " timed out, retrying..."); - oculus.setMosi(0); // Clear - startReset(); - } else { - Logger.recordOutput( - "Oculus/status", "Heading reset failed after " + MAX_RESET_ATTEMPTS + " attempts"); - hasStarted = false; - } - } + oculus.zeroHeading(); } @Override public boolean isFinished() { - if (!hasStarted) return true; - return oculus.getMisoValue() == STATUS_HEADING_RESET_COMPLETE; - } - - @Override - public void end(boolean interrupted) { - if (hasStarted) { - oculus.setMosi(0); // Clear - Logger.recordOutput( - "Oculus/status", - interrupted - ? "Heading reset interrupted" - : "Heading reset completed successfully on attempt " + currentAttempt); - } + return !oculus.isHeadingResetInProgress(); } -} +} \ No newline at end of file diff --git a/src/main/java/frc/alotobots/library/subsystems/vision/oculus/constants/OculusConstants.java b/src/main/java/frc/alotobots/library/subsystems/vision/oculus/constants/OculusConstants.java index a160bf3b..f4104e87 100644 --- a/src/main/java/frc/alotobots/library/subsystems/vision/oculus/constants/OculusConstants.java +++ b/src/main/java/frc/alotobots/library/subsystems/vision/oculus/constants/OculusConstants.java @@ -12,29 +12,54 @@ */ package frc.alotobots.library.subsystems.vision.oculus.constants; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; 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; /** - * Constants used by the Oculus Quest navigation subsystem. Contains configuration values for - * physical setup and operation parameters. + * Constants used by the Oculus Quest navigation subsystem. + * Contains configuration values for physical setup and operation parameters. */ public class OculusConstants { /** - * Transform from the robot center to the headset. Coordinate system: - X: Positive is forwards - - * Y: Positive is left - Rotation: Positive is counter-clockwise + * Transform from the robot center to the headset. + * Coordinate system: + * - X: Positive is forwards + * - Y: Positive is left + * - Rotation: Positive is counter-clockwise */ public static final Transform2d ROBOT_TO_OCULUS = new Transform2d(0.075, 0.0, new Rotation2d()); /** - * Timeout duration in seconds for reset operations (pose reset, heading reset, ping). If a reset - * operation takes longer than this time, it will be considered failed. + * Timeout duration in seconds for reset operations (pose reset, heading reset, ping). + * If a reset operation takes longer than this time, it will be considered failed. */ public static final double RESET_TIMEOUT_SECONDS = 0.2; /** - * Maximum number of attempts for reset operations. If a reset operation fails this many times, - * the command will terminate. + * Maximum number of attempts for reset operations. + * If a reset operation fails this many times, the command will terminate. */ public static final int MAX_RESET_ATTEMPTS = 3; -} + + /** Timeout threshold for considering Quest disconnected (seconds) */ + public static final double CONNECTION_TIMEOUT = 0.1; + + /** + * Standard deviations representing how much we "trust" the position from the Oculus. + * By default, the Quest 3 provides sub-millimeter accuracy. + * Values represent: + * [0]: X position trust (5mm) + * [1]: Y position trust (5mm) + * [2]: Rotation trust (~2.87 degrees) + */ + public static final Matrix OCULUS_STD_DEVS = + VecBuilder.fill( + 0.005, // Trust down to 5mm + 0.005, // Trust down to 5mm + 0.05 // Trust down to ~2.87deg + ); +} \ No newline at end of file From 1841af4bd6b5c73a132cc45ba211cbd14fb7eea6 Mon Sep 17 00:00:00 2001 From: Sean Ernstes Date: Thu, 2 Jan 2025 03:36:47 -0500 Subject: [PATCH 14/23] rough concept --- .../deploy/pathplanner/paths/New Path.path | 70 -- .../java/frc/alotobots/RobotContainer.java | 133 ++-- .../swervedrive/SwerveDriveSubsystem.java | 43 +- .../commands/FeedforwardCharacterization.java | 2 +- .../swervedrive/util/PathPlannerManager.java | 117 ++++ .../LocalizationFusion.java | 333 +++++++++ .../localizationfusion/LocalizationState.java | 123 ++++ .../vision/localizationfusion/PoseSource.java | 78 +++ .../StateTransitionLogger.java | 47 ++ .../vision/oculus/OculusSubsystem.java | 647 ++++++++++-------- .../vision/oculus/commands/PingCommand.java | 6 +- .../oculus/commands/ResetPoseCommand.java | 7 +- .../oculus/commands/ZeroHeadingCommand.java | 6 +- .../oculus/constants/OculusConstants.java | 38 +- .../vision/oculus/util/OculusPoseSource.java | 84 +++ .../apriltag/AprilTagSubsystem.java | 289 ++++---- .../apriltag/constants/AprilTagConstants.java | 3 + .../apriltag/util/AprilTagPoseSource.java | 83 +++ .../commands/PathfindToBestObject.java | 8 +- 19 files changed, 1478 insertions(+), 639 deletions(-) delete mode 100644 src/main/deploy/pathplanner/paths/New Path.path create mode 100644 src/main/java/frc/alotobots/library/subsystems/swervedrive/util/PathPlannerManager.java create mode 100644 src/main/java/frc/alotobots/library/subsystems/vision/localizationfusion/LocalizationFusion.java create mode 100644 src/main/java/frc/alotobots/library/subsystems/vision/localizationfusion/LocalizationState.java create mode 100644 src/main/java/frc/alotobots/library/subsystems/vision/localizationfusion/PoseSource.java create mode 100644 src/main/java/frc/alotobots/library/subsystems/vision/localizationfusion/StateTransitionLogger.java create mode 100644 src/main/java/frc/alotobots/library/subsystems/vision/oculus/util/OculusPoseSource.java create mode 100644 src/main/java/frc/alotobots/library/subsystems/vision/photonvision/apriltag/util/AprilTagPoseSource.java diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/New Path.path deleted file mode 100644 index b729d7b9..00000000 --- a/src/main/deploy/pathplanner/paths/New Path.path +++ /dev/null @@ -1,70 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 2.0, - "y": 7.0 - }, - "prevControl": null, - "nextControl": { - "x": 3.0, - "y": 7.0 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 4.0, - "y": 6.0 - }, - "prevControl": { - "x": 3.0, - "y": 6.0 - }, - "nextControl": { - "x": 5.0, - "y": 6.0 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 15.292488516315062, - "y": 5.5195165628007326 - }, - "prevControl": { - "x": 10.127923968435965, - "y": 5.754338411405699 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 5.2, - "maxAcceleration": 3.5, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 460.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 0.0 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": 0.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/java/frc/alotobots/RobotContainer.java b/src/main/java/frc/alotobots/RobotContainer.java index e9ec4d2d..d00d82de 100644 --- a/src/main/java/frc/alotobots/RobotContainer.java +++ b/src/main/java/frc/alotobots/RobotContainer.java @@ -16,69 +16,39 @@ import static frc.alotobots.library.subsystems.vision.photonvision.objectdetection.constants.ObjectDetectionConstants.NOTE; import com.pathplanner.lib.auto.AutoBuilder; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.alotobots.library.subsystems.bling.BlingSubsystem; -import frc.alotobots.library.subsystems.bling.commands.NoAllianceWaiting; -import frc.alotobots.library.subsystems.bling.commands.SetToAllianceColor; -import frc.alotobots.library.subsystems.bling.io.BlingIO; -import frc.alotobots.library.subsystems.bling.io.BlingIOReal; -import frc.alotobots.library.subsystems.bling.io.BlingIOSim; -import frc.alotobots.library.subsystems.swervedrive.ModulePosition; -import frc.alotobots.library.subsystems.swervedrive.SwerveDriveSubsystem; -import frc.alotobots.library.subsystems.swervedrive.commands.DefaultDrive; -import frc.alotobots.library.subsystems.swervedrive.commands.FeedforwardCharacterization; -import frc.alotobots.library.subsystems.swervedrive.commands.WheelRadiusCharacterization; +import frc.alotobots.library.subsystems.bling.commands.*; +import frc.alotobots.library.subsystems.bling.io.*; +import frc.alotobots.library.subsystems.swervedrive.*; +import frc.alotobots.library.subsystems.swervedrive.commands.*; import frc.alotobots.library.subsystems.swervedrive.io.*; +import frc.alotobots.library.subsystems.swervedrive.util.PathPlannerManager; +import frc.alotobots.library.subsystems.vision.localizationfusion.LocalizationFusion; import frc.alotobots.library.subsystems.vision.oculus.OculusSubsystem; -import frc.alotobots.library.subsystems.vision.oculus.commands.ResetPoseCommand; -import frc.alotobots.library.subsystems.vision.oculus.io.OculusIO; -import frc.alotobots.library.subsystems.vision.oculus.io.OculusIOReal; -import frc.alotobots.library.subsystems.vision.oculus.io.OculusIOSim; +import frc.alotobots.library.subsystems.vision.oculus.io.*; +import frc.alotobots.library.subsystems.vision.oculus.util.OculusPoseSource; import frc.alotobots.library.subsystems.vision.photonvision.apriltag.AprilTagSubsystem; import frc.alotobots.library.subsystems.vision.photonvision.apriltag.constants.AprilTagConstants; -import frc.alotobots.library.subsystems.vision.photonvision.apriltag.io.AprilTagIO; -import frc.alotobots.library.subsystems.vision.photonvision.apriltag.io.AprilTagIOPhotonVision; -import frc.alotobots.library.subsystems.vision.photonvision.apriltag.io.AprilTagIOPhotonVisionSim; +import frc.alotobots.library.subsystems.vision.photonvision.apriltag.io.*; +import frc.alotobots.library.subsystems.vision.photonvision.apriltag.util.AprilTagPoseSource; import frc.alotobots.library.subsystems.vision.photonvision.objectdetection.ObjectDetectionSubsystem; -import frc.alotobots.library.subsystems.vision.photonvision.objectdetection.commands.DriveFacingBestObject; -import frc.alotobots.library.subsystems.vision.photonvision.objectdetection.commands.PathfindToBestObject; +import frc.alotobots.library.subsystems.vision.photonvision.objectdetection.commands.*; import frc.alotobots.library.subsystems.vision.photonvision.objectdetection.constants.ObjectDetectionConstants; -import frc.alotobots.library.subsystems.vision.photonvision.objectdetection.io.ObjectDetectionIO; -import frc.alotobots.library.subsystems.vision.photonvision.objectdetection.io.ObjectDetectionIOPhotonVision; +import frc.alotobots.library.subsystems.vision.photonvision.objectdetection.io.*; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; -/** - * The main container class for the robot. This class is responsible for initializing all - * subsystems, configuring button bindings, and setting up autonomous commands. It follows a - * dependency injection pattern for hardware IO to support simulation and replay modes. - */ public class RobotContainer { - /** The swerve drive subsystem that handles robot movement. */ private final SwerveDriveSubsystem swerveDriveSubsystem; - - /** The AprilTag vision subsystem for robot localization. */ + private final OculusSubsystem oculusSubsystem; private final AprilTagSubsystem aprilTagSubsystem; - - /** The object detection subsystem for game piece tracking. */ + private final LocalizationFusion localizationFusion; private final ObjectDetectionSubsystem objectDetectionSubsystem; - - /** The LED control subsystem for robot status indication. */ private final BlingSubsystem blingSubsystem; - - private final OculusSubsystem oculusSubsystem; - - /** Dashboard chooser for selecting autonomous routines. */ + private final PathPlannerManager pathPlannerManager; private final LoggedDashboardChooser autoChooser; - /** - * Constructs the RobotContainer and initializes all robot subsystems and commands. Different IO - * implementations are used based on whether the robot is running in real, simulation, or replay - * mode. - */ public RobotContainer() { switch (Constants.currentMode) { case REAL: @@ -90,17 +60,29 @@ public RobotContainer() { new ModuleIOTalonFX(ModulePosition.FRONT_RIGHT.index), new ModuleIOTalonFX(ModulePosition.BACK_LEFT.index), new ModuleIOTalonFX(ModulePosition.BACK_RIGHT.index)); + + oculusSubsystem = new OculusSubsystem(new OculusIOReal()); aprilTagSubsystem = new AprilTagSubsystem( - swerveDriveSubsystem::addVisionMeasurement, new AprilTagIOPhotonVision(AprilTagConstants.CAMERA_CONFIGS[0]), new AprilTagIOPhotonVision(AprilTagConstants.CAMERA_CONFIGS[1])); + + // Create pose sources + OculusPoseSource oculusPoseSource = new OculusPoseSource(oculusSubsystem); + AprilTagPoseSource aprilTagPoseSource = new AprilTagPoseSource(aprilTagSubsystem); + + // Initialize localization fusion with pose sources + LocalizationFusion.PoseVisionConsumer poseConsumer = + swerveDriveSubsystem::addVisionMeasurement; + localizationFusion = + new LocalizationFusion(poseConsumer, oculusPoseSource, aprilTagPoseSource); + pathPlannerManager = new PathPlannerManager(swerveDriveSubsystem, localizationFusion); + objectDetectionSubsystem = new ObjectDetectionSubsystem( swerveDriveSubsystem::getPose, new ObjectDetectionIOPhotonVision(ObjectDetectionConstants.CAMERA_CONFIGS[0])); blingSubsystem = new BlingSubsystem(new BlingIOReal()); - oculusSubsystem = new OculusSubsystem(new OculusIOReal()); break; case SIM: @@ -112,17 +94,28 @@ public RobotContainer() { new ModuleIOSim(ModulePosition.FRONT_RIGHT.index), new ModuleIOSim(ModulePosition.BACK_LEFT.index), new ModuleIOSim(ModulePosition.BACK_RIGHT.index)); + + oculusSubsystem = new OculusSubsystem(new OculusIOSim()); aprilTagSubsystem = new AprilTagSubsystem( - swerveDriveSubsystem::addVisionMeasurement, new AprilTagIOPhotonVisionSim( AprilTagConstants.CAMERA_CONFIGS[0], swerveDriveSubsystem::getPose), new AprilTagIOPhotonVisionSim( AprilTagConstants.CAMERA_CONFIGS[1], swerveDriveSubsystem::getPose)); + + // Create pose sources + oculusPoseSource = new OculusPoseSource(oculusSubsystem); + aprilTagPoseSource = new AprilTagPoseSource(aprilTagSubsystem); + + // Initialize localization fusion with pose sources + poseConsumer = swerveDriveSubsystem::addVisionMeasurement; + localizationFusion = + new LocalizationFusion(poseConsumer, oculusPoseSource, aprilTagPoseSource); + pathPlannerManager = new PathPlannerManager(swerveDriveSubsystem, localizationFusion); + objectDetectionSubsystem = new ObjectDetectionSubsystem(swerveDriveSubsystem::getPose, new ObjectDetectionIO() {}); blingSubsystem = new BlingSubsystem(new BlingIOSim()); - oculusSubsystem = new OculusSubsystem(new OculusIOSim()); break; default: @@ -134,22 +127,30 @@ public RobotContainer() { new ModuleIO() {}, new ModuleIO() {}, new ModuleIO() {}); - aprilTagSubsystem = - new AprilTagSubsystem( - swerveDriveSubsystem::addVisionMeasurement, - new AprilTagIO() {}, - new AprilTagIO() {}); + + oculusSubsystem = new OculusSubsystem(new OculusIO() {}); + aprilTagSubsystem = new AprilTagSubsystem(new AprilTagIO() {}, new AprilTagIO() {}); + + // Create pose sources + oculusPoseSource = new OculusPoseSource(oculusSubsystem); + aprilTagPoseSource = new AprilTagPoseSource(aprilTagSubsystem); + + // Initialize localization fusion with pose sources + poseConsumer = swerveDriveSubsystem::addVisionMeasurement; + localizationFusion = + new LocalizationFusion(poseConsumer, oculusPoseSource, aprilTagPoseSource); + pathPlannerManager = new PathPlannerManager(swerveDriveSubsystem, localizationFusion); + objectDetectionSubsystem = new ObjectDetectionSubsystem(swerveDriveSubsystem::getPose, new ObjectDetectionIO() {}); blingSubsystem = new BlingSubsystem(new BlingIO() {}); - oculusSubsystem = new OculusSubsystem(new OculusIO() {}); break; } // Set up auto routines autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser()); - // Set up SysId routines + // Add SysId routines autoChooser.addOption( "Drive Wheel Radius Characterization", new WheelRadiusCharacterization(swerveDriveSubsystem)); @@ -172,36 +173,22 @@ public RobotContainer() { configureLogicCommands(); } - /** - * Configures the default commands for each subsystem. These commands run automatically when no - * other commands are scheduled for a subsystem. - */ private void configureDefaultCommands() { swerveDriveSubsystem.setDefaultCommand(new DefaultDrive(swerveDriveSubsystem).getCommand()); blingSubsystem.setDefaultCommand( new NoAllianceWaiting(blingSubsystem).andThen(new SetToAllianceColor(blingSubsystem))); } - /** Configures commands that are triggered by button presses or other logic conditions. */ private void configureLogicCommands() { driveFacingBestObjectButton.toggleOnTrue( new DriveFacingBestObject(objectDetectionSubsystem, swerveDriveSubsystem, NOTE)); pathfindToBestObjectButton.onTrue( - new PathfindToBestObject(objectDetectionSubsystem, swerveDriveSubsystem, NOTE)); + new PathfindToBestObject( + objectDetectionSubsystem, swerveDriveSubsystem, pathPlannerManager, NOTE)); testButton.onTrue( - new ResetPoseCommand(oculusSubsystem, new Pose2d(15.3, 5.5, Rotation2d.fromDegrees(0.0))) - .andThen( - new InstantCommand( - () -> - swerveDriveSubsystem.setPose( - new Pose2d(15.3, 5.5, Rotation2d.fromDegrees(0.0)))))); + localizationFusion.runOnce(localizationFusion::requestResetOculusPoseViaAprilTags)); } - /** - * Returns the command to run in autonomous mode. - * - * @return The command selected in the autonomous chooser - */ public Command getAutonomousCommand() { return autoChooser.get(); } diff --git a/src/main/java/frc/alotobots/library/subsystems/swervedrive/SwerveDriveSubsystem.java b/src/main/java/frc/alotobots/library/subsystems/swervedrive/SwerveDriveSubsystem.java index 8fbb752c..f5d5362f 100644 --- a/src/main/java/frc/alotobots/library/subsystems/swervedrive/SwerveDriveSubsystem.java +++ b/src/main/java/frc/alotobots/library/subsystems/swervedrive/SwerveDriveSubsystem.java @@ -14,10 +14,7 @@ import static edu.wpi.first.units.Units.*; -import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.pathfinding.Pathfinding; import com.pathplanner.lib.util.DriveFeedforwards; -import com.pathplanner.lib.util.PathPlannerLogging; import com.pathplanner.lib.util.swerve.SwerveSetpoint; import com.pathplanner.lib.util.swerve.SwerveSetpointGenerator; import edu.wpi.first.hal.FRCNetComm.tInstances; @@ -34,21 +31,17 @@ import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; -import edu.wpi.first.units.measure.LinearVelocity; import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import frc.alotobots.AutoNamedCommands; import frc.alotobots.Constants; import frc.alotobots.Constants.Mode; import frc.alotobots.library.subsystems.swervedrive.io.GyroIO; import frc.alotobots.library.subsystems.swervedrive.io.GyroIOInputsAutoLogged; import frc.alotobots.library.subsystems.swervedrive.io.ModuleIO; -import frc.alotobots.util.LocalADStarAK; import java.util.concurrent.locks.Lock; import java.util.concurrent.locks.ReentrantLock; import org.littletonrobotics.junction.AutoLogOutput; @@ -134,28 +127,6 @@ public SwerveDriveSubsystem( // Initialize and start odometry thread PhoenixOdometryThread.getInstance().start(); - // Configure AutoBuilder for PathPlanner - AutoBuilder.configure( - this::getPose, - this::setPose, - this::getChassisSpeeds, - this::runVelocity, - Constants.tunerConstants.getHolonomicDriveController(), - Constants.tunerConstants.getPathPlannerConfig(), - () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red, - this); - AutoNamedCommands.setupNamedCommands(); // Setup the named commands - Pathfinding.setPathfinder(new LocalADStarAK()); - PathPlannerLogging.setLogActivePathCallback( - (activePath) -> { - Logger.recordOutput( - "Odometry/Trajectory", activePath.toArray(new Pose2d[activePath.size()])); - }); - PathPlannerLogging.setLogTargetPoseCallback( - (targetPose) -> { - Logger.recordOutput("Odometry/TrajectorySetpoint", targetPose); - }); - setpointGenerator = new SwerveSetpointGenerator( Constants.tunerConstants.getPathPlannerConfig(), @@ -331,7 +302,7 @@ private SwerveModulePosition[] getModulePositions() { * @return Current chassis speeds */ @AutoLogOutput(key = "SwerveChassisSpeeds/Measured") - private ChassisSpeeds getChassisSpeeds() { + public ChassisSpeeds getChassisSpeeds() { return kinematics.toChassisSpeeds(getModuleStates()); } @@ -421,16 +392,4 @@ public double getMaxLinearSpeedMetersPerSec() { public double getMaxAngularSpeedRadPerSec() { return getMaxLinearSpeedMetersPerSec() / Constants.tunerConstants.getDriveBaseRadius(); } - - /** - * Creates a pathfinding command to the specified pose. - * - * @param target Target pose - * @param velocity Target velocity - * @return Pathfinding command - */ - public Command getPathFinderCommand(Pose2d target, LinearVelocity velocity) { - return AutoBuilder.pathfindToPose( - target, Constants.tunerConstants.getPathfindingConstraints(), velocity); - } } diff --git a/src/main/java/frc/alotobots/library/subsystems/swervedrive/commands/FeedforwardCharacterization.java b/src/main/java/frc/alotobots/library/subsystems/swervedrive/commands/FeedforwardCharacterization.java index a05954a6..8f57a788 100644 --- a/src/main/java/frc/alotobots/library/subsystems/swervedrive/commands/FeedforwardCharacterization.java +++ b/src/main/java/frc/alotobots/library/subsystems/swervedrive/commands/FeedforwardCharacterization.java @@ -51,7 +51,7 @@ public void initialize() { velocitySamples.clear(); voltageSamples.clear(); characterizationStarted = false; - startTime = Timer.getFPGATimestamp(); + startTime = Timer.getTimestamp(); timer.restart(); } diff --git a/src/main/java/frc/alotobots/library/subsystems/swervedrive/util/PathPlannerManager.java b/src/main/java/frc/alotobots/library/subsystems/swervedrive/util/PathPlannerManager.java new file mode 100644 index 00000000..17c15818 --- /dev/null +++ b/src/main/java/frc/alotobots/library/subsystems/swervedrive/util/PathPlannerManager.java @@ -0,0 +1,117 @@ +/* +* ALOTOBOTS - FRC Team 5152 + https://github.com/5152Alotobots +* Copyright (C) 2024 ALOTOBOTS +* +* This program is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* Source code must be publicly available on GitHub or an alternative web accessible site +*/ +package frc.alotobots.library.subsystems.swervedrive.util; + +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.pathfinding.Pathfinding; +import com.pathplanner.lib.util.PathPlannerLogging; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.units.measure.LinearVelocity; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj2.command.Command; +import frc.alotobots.AutoNamedCommands; +import frc.alotobots.Constants; +import frc.alotobots.library.subsystems.swervedrive.SwerveDriveSubsystem; +import frc.alotobots.library.subsystems.vision.localizationfusion.LocalizationFusion; +import frc.alotobots.util.LocalADStarAK; +import org.littletonrobotics.junction.Logger; + +/** + * Manages PathPlanner integration for autonomous path following and path finding. Abstracts path + * planning functionality from the SwerveDriveSubsystem. + */ +public class PathPlannerManager { + private final SwerveDriveSubsystem driveSubsystem; + private final LocalizationFusion localizationFusion; + + /** + * Creates a new PathPlannerManager. + * + * @param driveSubsystem The swerve drive subsystem to control + * @param localizationFusion The localizationFusion instance to be able to reset oculus pose + */ + public PathPlannerManager( + SwerveDriveSubsystem driveSubsystem, LocalizationFusion localizationFusion) { + this.driveSubsystem = driveSubsystem; + this.localizationFusion = localizationFusion; + configurePathPlanner(); + } + + /** Configures PathPlanner with necessary callbacks and settings. */ + private void configurePathPlanner() { + // Configure AutoBuilder for PathPlanner + AutoBuilder.configure( + driveSubsystem::getPose, + driveSubsystem::setPose, + driveSubsystem::getChassisSpeeds, + driveSubsystem::runVelocity, + Constants.tunerConstants.getHolonomicDriveController(), + Constants.tunerConstants.getPathPlannerConfig(), + () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red, + driveSubsystem); + + // Setup named commands for autonomous routines + AutoNamedCommands.setupNamedCommands(); + + // Configure pathfinding + Pathfinding.setPathfinder(new LocalADStarAK()); + + // Setup logging callbacks + configureLogging(); + } + + /** Configures PathPlanner logging callbacks. */ + private void configureLogging() { + PathPlannerLogging.setLogActivePathCallback( + (activePath) -> { + Logger.recordOutput( + "Odometry/Trajectory", activePath.toArray(new Pose2d[activePath.size()])); + }); + + PathPlannerLogging.setLogTargetPoseCallback( + (targetPose) -> { + Logger.recordOutput("Odometry/TrajectorySetpoint", targetPose); + }); + } + + /** + * Creates a pathfinding command to the specified pose. + * + * @param target Target pose + * @param velocity Target velocity + * @return Pathfinding command + */ + public Command getPathFinderCommand(Pose2d target, LinearVelocity velocity) { + return AutoBuilder.pathfindToPose( + target, Constants.tunerConstants.getPathfindingConstraints(), velocity); + } + + /** + * Gets maximum linear speed capability. + * + * @return Maximum speed in meters per second + */ + public double getMaxLinearSpeedMetersPerSec() { + return driveSubsystem.getMaxLinearSpeedMetersPerSec(); + } + + /** + * Gets maximum angular speed capability. + * + * @return Maximum angular speed in radians per second + */ + public double getMaxAngularSpeedRadPerSec() { + return driveSubsystem.getMaxAngularSpeedRadPerSec(); + } +} diff --git a/src/main/java/frc/alotobots/library/subsystems/vision/localizationfusion/LocalizationFusion.java b/src/main/java/frc/alotobots/library/subsystems/vision/localizationfusion/LocalizationFusion.java new file mode 100644 index 00000000..8cc3bcdf --- /dev/null +++ b/src/main/java/frc/alotobots/library/subsystems/vision/localizationfusion/LocalizationFusion.java @@ -0,0 +1,333 @@ +/* +* ALOTOBOTS - FRC Team 5152 + https://github.com/5152Alotobots +* Copyright (C) 2024 ALOTOBOTS +* +* This program is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* Source code must be publicly available on GitHub or an alternative web accessible site +*/ +package frc.alotobots.library.subsystems.vision.localizationfusion; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.alotobots.library.subsystems.vision.oculus.util.OculusPoseSource; +import frc.alotobots.library.subsystems.vision.photonvision.apriltag.util.AprilTagPoseSource; +import org.littletonrobotics.junction.Logger; + +/** + * A subsystem that fuses multiple pose sources for robot localization. + * + *

This system primarily uses the Meta Quest 3's SLAM capabilities for high-frequency (120Hz) + * pose updates, with AprilTag detection serving as a validation and initialization mechanism. The + * system follows a hierarchical approach: + * + *

1. Quest SLAM (Primary): Provides continuous, high-precision pose estimates 2. AprilTag System + * (Secondary): Used for initial pose acquisition and ongoing validation 3. Emergency Odometry + * (Fallback): Available when both primary systems fail + * + *

The system maintains a state machine to manage transitions between these sources based on + * availability and reliability metrics. + */ +public class LocalizationFusion extends SubsystemBase implements StateTransitionLogger { + /** Maximum acceptable difference between AprilTag and Quest poses for validation (meters) */ + private static final double APRILTAG_VALIDATION_THRESHOLD = 0.5; + + /** Update interval matching Quest's native 120Hz update rate (seconds) */ + private static final double POSE_UPDATE_INTERVAL = 1.0 / 120.0; + + /** Consumer interface for receiving pose updates */ + private final PoseVisionConsumer poseConsumer; + + /** Primary pose source using Quest SLAM */ + private final OculusPoseSource oculusSource; + + /** Secondary pose source using AprilTags */ + private final AprilTagPoseSource tagSource; + + /** State machine managing pose source transitions */ + private final LocalizationState state; + + /** Timestamp of last pose update */ + private double lastUpdateTime = 0.0; + + /** Timestamp of last valid Quest update */ + private double lastQuestUpdate = 0.0; + + /** Most recent validated pose from any source */ + private Pose2d lastValidatedPose = null; + + /** Previous DriverStation connection state */ + private boolean wasConnected = false; + + /** Functional interface for consuming pose updates from the fusion system. */ + @FunctionalInterface + public interface PoseVisionConsumer { + /** + * Accepts a new pose update from the fusion system. + * + * @param pose The current robot pose in field coordinates + * @param timestampSeconds The timestamp when this pose was measured + * @param stdDevs Standard deviations for x, y, and rotation measurements + */ + void accept(Pose2d pose, double timestampSeconds, Matrix stdDevs); + } + + /** + * Creates a new LocalizationFusion subsystem. + * + * @param poseConsumer Consumer for receiving pose updates + * @param oculusSource Primary pose source using Quest SLAM + * @param aprilTagSource Secondary pose source using AprilTags + */ + public LocalizationFusion( + PoseVisionConsumer poseConsumer, + OculusPoseSource oculusSource, + AprilTagPoseSource aprilTagSource) { + this.poseConsumer = poseConsumer; + this.oculusSource = oculusSource; + this.tagSource = aprilTagSource; + this.state = new LocalizationState(this); + } + + @Override + public void periodic() { + logSystemStatus(); + handleDriverStationConnection(); + updatePoses(); + updateState(); + } + + /** Logs current system status to NetworkTables for monitoring. */ + private void logSystemStatus() { + Logger.recordOutput("PoseEstimator/State", state.getCurrentState().getDescription()); + Logger.recordOutput("PoseEstimator/OculusConnected", oculusSource.isConnected()); + Logger.recordOutput("PoseEstimator/AprilTagConnected", tagSource.isConnected()); + } + + /** + * Handles DriverStation connection state changes. When reconnecting, attempts to reset to last + * known good pose. + */ + private void handleDriverStationConnection() { + boolean isConnected = DriverStation.isDSAttached(); + if (!wasConnected && isConnected && lastValidatedPose != null) { + Logger.recordOutput("PoseEstimator/Event", "DriverStation connected - initiating pose reset"); + if (resetToPose(lastValidatedPose)) { + state.transitionTo(LocalizationState.State.RESETTING); + } + } + wasConnected = isConnected; + } + + /** + * Updates pose estimates based on current state and available sources. Maintains rate limiting to + * match Quest update frequency. + */ + private void updatePoses() { + double currentTime = Timer.getTimestamp(); + if (currentTime - lastUpdateTime < POSE_UPDATE_INTERVAL) { + return; + } + lastUpdateTime = currentTime; + + switch (state.getCurrentState()) { + case UNINITIALIZED: + handleUninitializedState(); + break; + case RESETTING: + handleResettingState(); + break; + case QUEST_PRIMARY: + handleQuestPrimaryState(currentTime); + break; + case TAG_BACKUP: + handleTagBackupState(currentTime); + break; + case EMERGENCY: + Logger.recordOutput( + "PoseEstimator/Event", + "Failed to connect to any external pose source. Running with odometry only!"); + break; + } + } + + /** Handles system initialization using AprilTag poses. */ + private void handleUninitializedState() { + Pose2d tagPose = tagSource.getCurrentPose(); + if (tagPose != null) { + lastValidatedPose = tagPose; + if (DriverStation.isDSAttached() && resetToPose(tagPose)) { + state.transitionTo(LocalizationState.State.RESETTING); + Logger.recordOutput("PoseEstimator/Event", "Received initial AprilTag pose - resetting"); + } + } + } + + /** Handles pose reset operations and transitions. */ + private void handleResettingState() { + if (!oculusSource.isConnected()) { + state.transitionTo(LocalizationState.State.TAG_BACKUP); + Logger.recordOutput( + "PoseEstimator/Event", "Quest unavailable during reset - using AprilTags"); + return; + } + if (!isResetInProgress()) { + lastQuestUpdate = Timer.getTimestamp(); + state.transitionTo(LocalizationState.State.QUEST_PRIMARY); + Logger.recordOutput("PoseEstimator/Event", "Pose reset complete - using Quest primary"); + } + } + + /** + * Handles primary Quest-based pose updates with AprilTag validation. + * + * @param currentTime Current system timestamp + */ + private void handleQuestPrimaryState(double currentTime) { + if (!oculusSource.isConnected()) { + state.transitionTo(LocalizationState.State.TAG_BACKUP); + Logger.recordOutput("PoseEstimator/Event", "Quest connection lost - switching to AprilTags"); + return; + } + + Pose2d questPose = oculusSource.getCurrentPose(); + if (questPose != null) { + if (validatePose(questPose)) { + poseConsumer.accept(questPose, currentTime, oculusSource.getStdDevs()); + lastQuestUpdate = currentTime; + } else { + Logger.recordOutput("PoseEstimator/Event", "Quest pose validation failed"); + state.transitionTo(LocalizationState.State.TAG_BACKUP); + } + } + } + + /** + * Handles fallback AprilTag-based pose updates. + * + * @param currentTime Current system timestamp + */ + private void handleTagBackupState(double currentTime) { + if (oculusSource.isConnected()) { + state.transitionTo(LocalizationState.State.QUEST_PRIMARY); + Logger.recordOutput( + "PoseEstimator/Event", "Quest connection restored - switching to primary"); + return; + } + + Pose2d tagPose = tagSource.getCurrentPose(); + if (tagPose != null) { + poseConsumer.accept(tagPose, currentTime, tagSource.getStdDevs()); + lastValidatedPose = tagPose; + } + } + + /** + * Validates Quest pose against AprilTag measurements. + * + * @param pose Quest pose to validate + * @return true if pose is valid or no validation possible + */ + private boolean validatePose(Pose2d pose) { + Pose2d tagPose = tagSource.getCurrentPose(); + if (tagPose == null) { + return true; // No validation possible, assume valid + } + + double poseError = tagPose.getTranslation().getDistance(pose.getTranslation()); + return poseError <= APRILTAG_VALIDATION_THRESHOLD; + } + + /** Updates system state based on source availability. */ + private void updateState() { + if (!state.isInState( + LocalizationState.State.UNINITIALIZED, LocalizationState.State.RESETTING)) { + if (oculusSource.isConnected()) { + state.transitionTo(LocalizationState.State.QUEST_PRIMARY); + } else { + state.transitionTo(LocalizationState.State.TAG_BACKUP); + } + } + } + + /** + * Initiates a pose reset operation. + * + * @param pose Target pose to reset to + * @return true if reset was initiated successfully + */ + private boolean resetToPose(Pose2d pose) { + return oculusSource.subsystem.resetToPose(pose); + } + + /** + * Checks if a pose reset operation is in progress. + * + * @return true if reset is ongoing + */ + private boolean isResetInProgress() { + return oculusSource.subsystem.isPoseResetInProgress(); + } + + /** + * Manually triggers a pose reset using the most recent validated pose. This can be used as a + * callback for manual reset requests. + * + * @return true if reset was initiated successfully, false otherwise + */ + public boolean requestResetOculusPoseViaAprilTags() { + Pose2d currentTagPose = tagSource.getCurrentPose(); + if (currentTagPose == null) { + Logger.recordOutput( + "PoseEstimator/Event", "Manual reset failed - no AprilTag pose available"); + return false; + } + + Logger.recordOutput( + "PoseEstimator/Event", + "Manual reset requested using AprilTag pose - initiating pose reset"); + if (resetToPose(currentTagPose)) { + state.transitionTo(LocalizationState.State.RESETTING); + return true; + } + return false; + } + + /** + * Manually triggers a pose reset to a specific pose. Use this when you want to reset to a known + * position rather than the last validated pose. + * + * @param targetPose The pose to reset to + * @return true if reset was initiated successfully, false otherwise + */ + public boolean requestResetOculusPose(Pose2d targetPose) { + if (targetPose == null) { + Logger.recordOutput("PoseEstimator/Event", "Manual reset failed - null pose provided"); + return false; + } + + Logger.recordOutput( + "PoseEstimator/Event", "Manual reset requested with custom pose - initiating pose reset"); + if (resetToPose(targetPose)) { + state.transitionTo(LocalizationState.State.RESETTING); + return true; + } + return false; + } + + @Override + public void logTransition(LocalizationState.State from, LocalizationState.State to) { + Logger.recordOutput( + "PoseEstimator/StateTransition", + String.format("State transition: %s -> %s", from.name(), to.name())); + } +} diff --git a/src/main/java/frc/alotobots/library/subsystems/vision/localizationfusion/LocalizationState.java b/src/main/java/frc/alotobots/library/subsystems/vision/localizationfusion/LocalizationState.java new file mode 100644 index 00000000..5ba4acc2 --- /dev/null +++ b/src/main/java/frc/alotobots/library/subsystems/vision/localizationfusion/LocalizationState.java @@ -0,0 +1,123 @@ +/* +* ALOTOBOTS - FRC Team 5152 + https://github.com/5152Alotobots +* Copyright (C) 2024 ALOTOBOTS +* +* This program is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* Source code must be publicly available on GitHub or an alternative web accessible site +*/ +package frc.alotobots.library.subsystems.vision.localizationfusion; + +import lombok.Getter; + +/** + * Manages the state machine for the robot localization system. + * + *

This class implements a hierarchical state machine that controls transitions between different + * pose estimation sources based on their availability and reliability. The system follows this + * hierarchy: + * + *

1. Quest SLAM as primary source (120Hz updates) 2. AprilTags as backup source 3. Odometry as + * emergency fallback + * + *

State transitions are logged for debugging and monitoring purposes. + */ +public class LocalizationState { + + /** + * Represents the possible states of the localization system. States are ordered by initialization + * sequence and fallback hierarchy. + */ + public enum State { + /** + * System is waiting for initial pose acquisition. Valid transitions: - To RESETTING when + * initial AprilTag pose is acquired + */ + UNINITIALIZED("Waiting for initial pose"), + + /** + * System is resetting to a reference pose. Valid transitions: - To QUEST_PRIMARY when reset + * completes successfully - To TAG_BACKUP if Quest becomes unavailable during reset + */ + RESETTING("Resetting to reference pose"), + + /** + * System is using Quest SLAM as primary pose source. Valid transitions: - To TAG_BACKUP if + * Quest connection is lost or validation fails - To EMERGENCY if both Quest and AprilTags are + * unavailable + */ + QUEST_PRIMARY("Using Quest as primary source"), + + /** + * System is using AprilTags as backup pose source. Valid transitions: - To QUEST_PRIMARY when + * Quest connection is restored - To EMERGENCY if AprilTags become unavailable + */ + TAG_BACKUP("Using AprilTags as backup"), + + /** + * System is using only wheel odometry for pose estimation. Valid transitions: - To TAG_BACKUP + * when AprilTags become available - To QUEST_PRIMARY when Quest connection is restored + */ + EMERGENCY("Using only odometry"); + + @Getter private final String description; + + /** + * Creates a new State with the specified description. + * + * @param description Human-readable description of the state + */ + State(String description) { + this.description = description; + } + } + + /** Current state of the localization system */ + @Getter private State currentState = State.UNINITIALIZED; + + /** Logger for state transitions */ + @Getter private final StateTransitionLogger transitionLogger; + + /** + * Creates a new LocalizationState instance. + * + * @param logger Logger to record state transitions + */ + public LocalizationState(StateTransitionLogger logger) { + this.transitionLogger = logger; + } + + /** + * Attempts to transition to a new state. The transition will be logged through the + * StateTransitionLogger. Invalid transitions will be logged but not executed. + * + * @param newState The target state to transition to + */ + public void transitionTo(State newState) { + if (newState != currentState) { + // Log state transition + transitionLogger.logTransition(currentState, newState); + + // Execute transition + currentState = newState; + } + } + + /** + * Checks if the system is currently in any of the specified states. This is useful for checking + * multiple valid states at once. + * + * @param states Variable number of states to check against + * @return true if current state matches any of the specified states + */ + public boolean isInState(State... states) { + for (State state : states) { + if (currentState == state) return true; + } + return false; + } +} diff --git a/src/main/java/frc/alotobots/library/subsystems/vision/localizationfusion/PoseSource.java b/src/main/java/frc/alotobots/library/subsystems/vision/localizationfusion/PoseSource.java new file mode 100644 index 00000000..2d9c4348 --- /dev/null +++ b/src/main/java/frc/alotobots/library/subsystems/vision/localizationfusion/PoseSource.java @@ -0,0 +1,78 @@ +/* +* ALOTOBOTS - FRC Team 5152 + https://github.com/5152Alotobots +* Copyright (C) 2024 ALOTOBOTS +* +* This program is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* Source code must be publicly available on GitHub or an alternative web accessible site +*/ +package frc.alotobots.library.subsystems.vision.localizationfusion; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; + +/** + * Standardized interface for pose estimation sources in the robot localization system. + * + *

This interface defines the contract that all pose sources must fulfill to integrate with the + * localization fusion system. It supports various types of pose sources including: + * + *

- Meta Quest SLAM (primary source) - AprilTag vision (secondary source) - Wheel odometry + * (emergency backup) - Other potential sources (e.g., LiDAR, external cameras) + * + *

Each source must provide: - Connection status monitoring - Current pose estimation - + * Measurement uncertainty estimates - Source identification + */ +public interface PoseSource { + + /** + * Checks if the pose source is currently connected and providing valid data. + * + *

This method should: - Verify hardware connectivity (if applicable) - Check data freshness - + * Validate sensor readings - Monitor update frequency + * + * @return true if the source is connected and providing valid data + */ + boolean isConnected(); + + /** + * Gets the most recent pose estimate from this source. + * + *

The returned pose should be: - In field-relative coordinates - Using the standard FRC + * coordinate system: - Origin at field corner - +X towards opposite alliance wall - +Y towards + * driver station - CCW positive rotation + * + *

If no valid pose is available, returns null. + * + * @return The current robot pose estimate, or null if unavailable + */ + Pose2d getCurrentPose(); + + /** + * Gets the standard deviations of measurement uncertainty for this pose source. + * + *

Returns a 3x1 matrix containing standard deviations for: - X position (meters) - Y position + * (meters) - Rotation (radians) + * + *

These values should: - Reflect real measurement uncertainty - Scale with distance/conditions + * - Account for systematic errors - Consider environmental factors + * + * @return 3x1 matrix of standard deviations [x, y, theta] + */ + Matrix getStdDevs(); + + /** + * Gets a human-readable identifier for this pose source. + * + *

Used for: - Logging - Debugging - User interfaces - Status reporting + * + * @return String identifier for this pose source + */ + String getSourceName(); +} diff --git a/src/main/java/frc/alotobots/library/subsystems/vision/localizationfusion/StateTransitionLogger.java b/src/main/java/frc/alotobots/library/subsystems/vision/localizationfusion/StateTransitionLogger.java new file mode 100644 index 00000000..681021be --- /dev/null +++ b/src/main/java/frc/alotobots/library/subsystems/vision/localizationfusion/StateTransitionLogger.java @@ -0,0 +1,47 @@ +/* +* ALOTOBOTS - FRC Team 5152 + https://github.com/5152Alotobots +* Copyright (C) 2024 ALOTOBOTS +* +* This program is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* Source code must be publicly available on GitHub or an alternative web accessible site +*/ +package frc.alotobots.library.subsystems.vision.localizationfusion; + +/** + * Interface for logging state transitions in the localization system. + * + *

This interface provides a standardized way to record and monitor state machine transitions + * within the localization system. It enables: + * + *

- Real-time monitoring of system behavior - Post-match analysis of state changes - Debugging + * of unexpected transitions - Performance tracking and optimization - System health monitoring + * + *

Implementations should consider: - Logging to NetworkTables for real-time monitoring - + * Persistent storage for post-match analysis - Timestamp recording for timing analysis - Context + * preservation for debugging - Rate limiting for high-frequency transitions + */ +public interface StateTransitionLogger { + + /** + * Records a state transition in the localization system. + * + *

This method is called whenever the system transitions between states. Implementations + * should: - Record the timestamp of the transition - Log both the previous and new states - + * Include relevant context if available - Handle rapid transitions appropriately - Ensure thread + * safety + * + *

Common transitions to monitor: - UNINITIALIZED → RESETTING (Initial pose acquisition) - + * RESETTING → QUEST_PRIMARY (Successful initialization) - QUEST_PRIMARY → TAG_BACKUP (Quest + * failure or validation error) - TAG_BACKUP → QUEST_PRIMARY (Quest recovery) - Any → EMERGENCY + * (System failure) + * + * @param from The state transitioning from + * @param to The state transitioning to + */ + void logTransition(LocalizationState.State from, LocalizationState.State to); +} diff --git a/src/main/java/frc/alotobots/library/subsystems/vision/oculus/OculusSubsystem.java b/src/main/java/frc/alotobots/library/subsystems/vision/oculus/OculusSubsystem.java index 383857e3..e43d0620 100644 --- a/src/main/java/frc/alotobots/library/subsystems/vision/oculus/OculusSubsystem.java +++ b/src/main/java/frc/alotobots/library/subsystems/vision/oculus/OculusSubsystem.java @@ -14,314 +14,397 @@ import static frc.alotobots.library.subsystems.vision.oculus.constants.OculusConstants.*; +import edu.wpi.first.math.Matrix; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.alotobots.library.subsystems.vision.localizationfusion.PoseSource; import frc.alotobots.library.subsystems.vision.oculus.io.OculusIO; import frc.alotobots.library.subsystems.vision.oculus.io.OculusIOInputsAutoLogged; import lombok.Getter; import org.littletonrobotics.junction.Logger; /** - * The OculusSubsystem manages communication and pose estimation with an Oculus Quest VR headset. - * This subsystem provides robot positioning using the Quest's inside-out tracking system. + * Manages communication and pose estimation with a Meta Quest VR headset. + * + *

This subsystem leverages the Quest's inside-out SLAM tracking system to provide high-frequency + * (120Hz) robot pose estimation. Key features: + * + *

- Global SLAM-based localization - Field mapping and persistence - Sub-millimeter tracking + * precision - High update rate (120Hz) - Drift-free position tracking - Fast relocalization + * + *

The system operates in phases: 1. Pre-match mapping to capture field features 2. Initial pose + * acquisition and alignment 3. Continuous pose updates during match 4. Recovery handling if + * tracking is lost */ -public class OculusSubsystem extends SubsystemBase { - /** Status code indicating the Oculus system is ready for commands */ - private static final int STATUS_READY = 0; - /** Status code indicating a heading reset operation has completed */ - private static final int STATUS_HEADING_RESET_COMPLETE = 99; - /** Status code indicating a pose reset operation has completed */ - private static final int STATUS_POSE_RESET_COMPLETE = 98; - /** Status code indicating a ping response has been received */ - private static final int STATUS_PING_RESPONSE = 97; - - /** IO interface for communicating with the Oculus hardware */ - private final OculusIO io; - /** Logged inputs from the Oculus system */ - private final OculusIOInputsAutoLogged inputs = new OculusIOInputsAutoLogged(); - - /** Flag indicating if a pose reset operation is currently in progress */ - @Getter private boolean poseResetInProgress = false; - /** Flag indicating if a heading reset operation is currently in progress */ - @Getter private boolean headingResetInProgress = false; - /** Flag indicating if a ping operation is currently in progress */ - @Getter private boolean pingInProgress = false; - /** Previous connection state for detecting status changes */ - @Getter private boolean wasConnected = false; - - /** Timestamp when the current reset operation started */ - private double resetStartTime = 0; - /** Counter for the number of reset attempts made */ - private int currentResetAttempt = 0; - /** Previous timestamp from the Oculus Quest */ - private double lastTimestamp = 0.0; - /** Most recent timestamp from the Oculus Quest */ - private double currentTimestamp = 0.0; - /** Target pose for the current reset operation */ - private Pose2d pendingResetPose = null; - - /** - * Creates a new OculusSubsystem with the specified IO interface. - * - * @param io The IO interface for communicating with the Oculus hardware - */ - public OculusSubsystem(OculusIO io) { - this.io = io; - Logger.recordOutput("Oculus/status", "Initialized"); +public class OculusSubsystem extends SubsystemBase implements PoseSource { + /** Status indicating system is ready for commands */ + private static final int STATUS_READY = 0; + + /** Status indicating heading reset completion */ + private static final int STATUS_HEADING_RESET_COMPLETE = 99; + + /** Status indicating pose reset completion */ + private static final int STATUS_POSE_RESET_COMPLETE = 98; + + /** Status indicating ping response receipt */ + private static final int STATUS_PING_RESPONSE = 97; + + /** Connection timeout threshold (seconds) */ + private static final double CONNECTION_TIMEOUT = 1; + + /** Hardware communication interface */ + private final OculusIO io; + + /** Logged inputs from Quest hardware */ + private final OculusIOInputsAutoLogged inputs = new OculusIOInputsAutoLogged(); + + /** Flag indicating active pose reset */ + @Getter private boolean poseResetInProgress = false; + + /** Flag indicating active heading reset */ + @Getter private boolean headingResetInProgress = false; + + /** Flag indicating active ping operation */ + @Getter private boolean pingInProgress = false; + + /** Previous connection state */ + @Getter private boolean wasConnected = false; + + /** Reset operation start timestamp */ + private double resetStartTime = 0; + + /** Current reset attempt counter */ + private int currentResetAttempt = 0; + + /** Previous Quest timestamp */ + private double lastTimestamp = 0.0; + + /** Current Quest timestamp */ + private double currentTimestamp = 0.0; + + /** Target pose for reset operation */ + private Pose2d pendingResetPose = null; + + /** Current robot pose estimate */ + private Pose2d currentPose = null; + + /** + * Creates a new OculusSubsystem. + * + *

Initializes communication with Quest hardware and prepares logging systems. The subsystem + * starts in an uninitialized state requiring pose calibration. + * + * @param io Interface for Quest hardware communication + */ + public OculusSubsystem(OculusIO io) { + this.io = io; + Logger.recordOutput("Oculus/status", "Initialized"); + } + + /** + * Updates subsystem state and processes Quest data. + * + *

Called periodically by the command scheduler. This method: - Updates hardware inputs - + * Processes new pose data - Handles state transitions - Manages reset operations - Updates + * logging + */ + @Override + public void periodic() { + io.updateInputs(inputs); + Logger.processInputs("Oculus", inputs); + + lastTimestamp = currentTimestamp; + currentTimestamp = inputs.timestamp; + + // Update current pose + var oculusPose = getOculusPose(); + currentPose = oculusPose.transformBy(ROBOT_TO_OCULUS.inverse()); + Logger.recordOutput("Oculus/status/poses/headsetPose", oculusPose); + Logger.recordOutput("Oculus/status/poses/robotPose", currentPose); + + handleResetTimeout(); + handleResetCompletion(); + handlePingResponse(); + } + + /** + * Manages timeouts for reset operations. + * + *

Reset operations that exceed the timeout threshold are either: - Retried up to the maximum + * attempt limit - Abandoned with appropriate status logging + */ + private void handleResetTimeout() { + double currentTime = Timer.getTimestamp(); + if (currentTime - resetStartTime > RESET_TIMEOUT_SECONDS) { + if (headingResetInProgress) handleReset(true); + if (poseResetInProgress) handleReset(false); } - - @Override - public void periodic() { - io.updateInputs(inputs); - Logger.processInputs("Oculus", inputs); - - var oculusPose = getOculusPose(); - Logger.recordOutput("Oculus/status/poses/headsetPose", oculusPose); - var robotPose = oculusPose.transformBy(ROBOT_TO_OCULUS.inverse()); - Logger.recordOutput("Oculus/status/poses/robotPose", robotPose); - - handleResetTimeout(); - handleResetCompletion(); - handlePingResponse(); - } - - /** - * Checks if any reset operations have timed out and handles them accordingly. - */ - private void handleResetTimeout() { - double currentTime = Timer.getTimestamp(); - if (currentTime - resetStartTime > RESET_TIMEOUT_SECONDS) { - if (headingResetInProgress) handleReset(true); - if (poseResetInProgress) handleReset(false); - } - } - - /** - * Handles reset operation retries and failures. - * - * @param isHeadingReset True if handling a heading reset, false if handling a pose reset - */ - private void handleReset(boolean isHeadingReset) { - if (currentResetAttempt < MAX_RESET_ATTEMPTS) { - String resetType = isHeadingReset ? "Heading" : "Pose"; - Logger.recordOutput( - "Oculus/status", - resetType + " Reset attempt " + (currentResetAttempt + 1) + " timed out, retrying..."); - currentResetAttempt++; - resetStartTime = Timer.getTimestamp(); - - io.setMosi(0); // Clear - - if (isHeadingReset) { - io.setMosi(1); // Heading Reset - } else { - io.setResetPose( - pendingResetPose.getX(), - pendingResetPose.getY(), - pendingResetPose.getRotation().getDegrees()); - io.setMosi(2); // Pose Reset - } - } else { - Logger.recordOutput( - "Oculus/status", - (isHeadingReset ? "Heading" : "Pose") - + " Reset failed after " - + MAX_RESET_ATTEMPTS - + " attempts"); - if (isHeadingReset) { - clearHeadingResetState(); - } else { - clearPoseResetState(); - } - } - } - - /** - * Checks for and handles completion of reset operations. - */ - private void handleResetCompletion() { - if (headingResetInProgress && inputs.misoValue == STATUS_HEADING_RESET_COMPLETE) { - Logger.recordOutput( - "Oculus/status", - "Heading Reset completed successfully on attempt " + (currentResetAttempt + 1)); - clearHeadingResetState(); - } - if (poseResetInProgress && inputs.misoValue == STATUS_POSE_RESET_COMPLETE) { - Logger.recordOutput( - "Oculus/status", - "Pose Reset completed successfully on attempt " + (currentResetAttempt + 1)); - clearPoseResetState(); - } + } + + /** + * Processes reset operation outcomes. + * + * @param isHeadingReset true for heading reset, false for pose reset + */ + private void handleReset(boolean isHeadingReset) { + if (currentResetAttempt < MAX_RESET_ATTEMPTS) { + String resetType = isHeadingReset ? "Heading" : "Pose"; + Logger.recordOutput( + "Oculus/status", + resetType + " Reset attempt " + (currentResetAttempt + 1) + " timed out, retrying..."); + currentResetAttempt++; + resetStartTime = Timer.getTimestamp(); + + io.setMosi(0); // Clear command + + if (isHeadingReset) { + io.setMosi(1); // Request heading reset + } else { + io.setResetPose( + pendingResetPose.getX(), + pendingResetPose.getY(), + pendingResetPose.getRotation().getDegrees()); + io.setMosi(2); // Request pose reset + } + } else { + Logger.recordOutput( + "Oculus/status", + (isHeadingReset ? "Heading" : "Pose") + + " Reset failed after " + + MAX_RESET_ATTEMPTS + + " attempts"); + if (isHeadingReset) { + clearHeadingResetState(); + } else { + clearPoseResetState(); + } } - - /** - * Handles ping response from the Oculus system. - */ - private void handlePingResponse() { - if (inputs.misoValue == STATUS_PING_RESPONSE) { - Logger.recordOutput("Oculus/status", "Ping response received"); - io.setMosi(0); // Clear - pingInProgress = false; - } + } + + /** + * Handles completion of reset operations. + * + *

Processes successful reset confirmations from Quest and updates system state. + */ + private void handleResetCompletion() { + if (headingResetInProgress && inputs.misoValue == STATUS_HEADING_RESET_COMPLETE) { + Logger.recordOutput( + "Oculus/status", + "Heading Reset completed successfully on attempt " + (currentResetAttempt + 1)); + clearHeadingResetState(); } - - /** - * Clears the state associated with a pose reset operation. - */ - private void clearPoseResetState() { - Logger.recordOutput("Oculus/status", "Clearing pose reset state"); - poseResetInProgress = false; - pendingResetPose = null; - currentResetAttempt = 0; - io.setMosi(0); // Clear + if (poseResetInProgress && inputs.misoValue == STATUS_POSE_RESET_COMPLETE) { + Logger.recordOutput( + "Oculus/status", + "Pose Reset completed successfully on attempt " + (currentResetAttempt + 1)); + clearPoseResetState(); } - - /** - * Clears the state associated with a heading reset operation. - */ - private void clearHeadingResetState() { - Logger.recordOutput("Oculus/status", "Clearing heading reset state"); - headingResetInProgress = false; - currentResetAttempt = 0; - io.setMosi(0); // Clear + } + + /** + * Handles ping response from Quest. + * + *

Processes communication test responses and updates status. + */ + private void handlePingResponse() { + if (inputs.misoValue == STATUS_PING_RESPONSE) { + Logger.recordOutput("Oculus/status", "Ping response received"); + io.setMosi(0); // Clear command + pingInProgress = false; } - - /** - * Gets the current yaw (rotation) of the Oculus headset. - * - * @return The headset's yaw as a Rotation2d - */ - private Rotation2d getOculusYaw() { - return Rotation2d.fromDegrees(-inputs.eulerAngles[1]); + } + + /** + * Clears pose reset operation state. + * + *

Resets all state variables related to pose reset operations. + */ + private void clearPoseResetState() { + Logger.recordOutput("Oculus/status", "Clearing pose reset state"); + poseResetInProgress = false; + pendingResetPose = null; + currentResetAttempt = 0; + io.setMosi(0); // Clear command + } + + /** + * Clears heading reset operation state. + * + *

Resets all state variables related to heading reset operations. + */ + private void clearHeadingResetState() { + Logger.recordOutput("Oculus/status", "Clearing heading reset state"); + headingResetInProgress = false; + currentResetAttempt = 0; + io.setMosi(0); // Clear command + } + + /** + * Gets current Quest rotation in field frame. + * + *

Converts Quest IMU data to field-relative rotation. + * + * @return Current headset rotation + */ + private Rotation2d getOculusYaw() { + return Rotation2d.fromDegrees(-inputs.eulerAngles[1]); + } + + /** + * Gets current Quest position in field frame. + * + *

Converts Quest SLAM position to field coordinates. + * + * @return Current headset position + */ + private Translation2d getOculusPosition() { + return new Translation2d(inputs.position[2], -inputs.position[0]); + } + + /** + * Gets complete Quest pose in field frame. + * + *

Combines position and rotation data into a field-relative pose. + * + * @return Current headset pose + */ + private Pose2d getOculusPose() { + return new Pose2d(getOculusPosition(), getOculusYaw()); + } + + /** + * Initiates pose reset operation. + * + *

Attempts to reset Quest SLAM origin to align with target pose. + * + * @param targetPose Desired robot pose after reset + * @return true if reset initiated successfully + */ + public boolean resetToPose(Pose2d targetPose) { + if (poseResetInProgress) { + Logger.recordOutput("Oculus/status", "Cannot reset pose - reset already in progress"); + return false; } - /** - * Gets the current position of the Oculus headset. - * - * @return The headset's position as a Translation2d - */ - private Translation2d getOculusPosition() { - return new Translation2d(inputs.position[2], -inputs.position[0]); + if (inputs.misoValue != STATUS_READY) { + Logger.recordOutput( + "Oculus/status", "Cannot reset pose - Quest busy (MISO=" + inputs.misoValue + ")"); + return false; } - /** - * Gets the current pose (position and rotation) of the Oculus headset. - * - * @return The headset's pose - */ - public Pose2d getOculusPose() { - return new Pose2d(getOculusPosition(), getOculusYaw()); + targetPose = targetPose.plus(ROBOT_TO_OCULUS); + pendingResetPose = targetPose; + Logger.recordOutput( + "Oculus/status", + String.format( + "Initiating pose reset to X:%.2f Y:%.2f Rot:%.2f°", + targetPose.getX(), targetPose.getY(), targetPose.getRotation().getDegrees())); + + io.setResetPose(targetPose.getX(), targetPose.getY(), targetPose.getRotation().getDegrees()); + poseResetInProgress = true; + resetStartTime = Timer.getTimestamp(); + currentResetAttempt = 0; + io.setMosi(2); // Request pose reset + + return true; + } + + /** + * Initiates heading reset operation. + * + *

Attempts to zero current Quest heading measurement. + * + * @return true if reset initiated successfully + */ + public boolean zeroHeading() { + if (headingResetInProgress) { + Logger.recordOutput("Oculus/status", "Cannot zero heading - reset already in progress"); + return false; } - /** - * Gets the current pose of the robot, derived from the Oculus headset's pose. - * - * @return The robot's pose - */ - public Pose2d getRobotPose() { - return getOculusPose().transformBy(ROBOT_TO_OCULUS.inverse()); + if (inputs.misoValue != STATUS_READY) { + Logger.recordOutput( + "Oculus/status", "Cannot zero heading - Quest busy (MISO=" + inputs.misoValue + ")"); + return false; } - /** - * Initiates a pose reset operation to a specified target pose. - * - * @param targetPose The desired pose to reset to - * @return True if the reset was initiated successfully, false otherwise - */ - public boolean resetToPose(Pose2d targetPose) { - if (poseResetInProgress) { - Logger.recordOutput("Oculus/status", "Cannot reset pose - reset already in progress"); - return false; - } - - if (inputs.misoValue != STATUS_READY) { - Logger.recordOutput( - "Oculus/status", "Cannot reset pose - Quest busy (MISO=" + inputs.misoValue + ")"); - return false; - } - - targetPose = targetPose.plus(ROBOT_TO_OCULUS); - pendingResetPose = targetPose; - Logger.recordOutput( - "Oculus/status", - String.format( - "Initiating pose reset to X:%.2f Y:%.2f Rot:%.2f°", - targetPose.getX(), targetPose.getY(), targetPose.getRotation().getDegrees())); - - io.setResetPose(targetPose.getX(), targetPose.getY(), targetPose.getRotation().getDegrees()); - poseResetInProgress = true; - resetStartTime = Timer.getTimestamp(); - currentResetAttempt = 0; - io.setMosi(2); // Pose Reset - - return true; + Logger.recordOutput("Oculus/status", "Zeroing heading"); + headingResetInProgress = true; + resetStartTime = Timer.getTimestamp(); + currentResetAttempt = 0; + io.setMosi(1); // Request heading reset + return true; + } + + /** + * Tests Quest communication. + * + *

Sends ping command to verify hardware connectivity. + * + * @return true if ping initiated successfully + */ + public boolean ping() { + if (pingInProgress) { + Logger.recordOutput("Oculus/status", "Cannot ping - ping already in progress"); + return false; } - /** - * Initiates a heading reset operation to zero the current heading. - * - * @return True if the reset was initiated successfully, false otherwise - */ - public boolean zeroHeading() { - if (headingResetInProgress) { - Logger.recordOutput("Oculus/status", "Cannot zero heading - reset already in progress"); - return false; - } - - if (inputs.misoValue != STATUS_READY) { - Logger.recordOutput( - "Oculus/status", "Cannot zero heading - Quest busy (MISO=" + inputs.misoValue + ")"); - return false; - } - - Logger.recordOutput("Oculus/status", "Zeroing heading"); - headingResetInProgress = true; - resetStartTime = Timer.getTimestamp(); - currentResetAttempt = 0; - io.setMosi(1); // Heading Reset - return true; + if (inputs.misoValue != STATUS_READY) { + Logger.recordOutput( + "Oculus/status", "Cannot ping - system busy (MISO=" + inputs.misoValue + ")"); + return false; } - /** - * Sends a ping command to the Oculus system to verify communication. - * - * @return True if the ping was initiated successfully, false otherwise - */ - public boolean ping() { - if (pingInProgress) { - Logger.recordOutput("Oculus/status", "Cannot ping - ping already in progress"); - return false; - } - - if (inputs.misoValue != STATUS_READY) { - Logger.recordOutput( - "Oculus/status", "Cannot ping - system busy (MISO=" + inputs.misoValue + ")"); - return false; - } - - Logger.recordOutput("Oculus/status", "Sending ping..."); - Logger.recordOutput("Oculus/ping/sendTime", Timer.getTimestamp()); - pingInProgress = true; - io.setMosi(3); // Ping - return true; - } - - /** - * Checks if the Oculus Quest is currently connected and sending valid data. - * This is determined by monitoring if the timestamp values are increasing. - * - * @return true if the Quest is connected and sending data, false otherwise - */ - public boolean isConnected() { - double currentTime = Timer.getTimestamp(); - - // Check if timestamps are increasing and within timeout window - boolean timestampValid = currentTimestamp > lastTimestamp; - boolean timeoutValid = (currentTime - currentTimestamp) < CONNECTION_TIMEOUT; - - return timestampValid && timeoutValid; - } -} \ No newline at end of file + Logger.recordOutput("Oculus/status", "Sending ping..."); + Logger.recordOutput("Oculus/ping/sendTime", Timer.getTimestamp()); + pingInProgress = true; + io.setMosi(3); // Send ping + return true; + } + + // PoseSource interface implementation + + /** + * {@inheritDoc} + * + *

Quest connection requires: - Valid timestamps - Recent updates (within timeout) - Consistent + * update rate + */ + @Override + public boolean isConnected() { + double questTimeDelta = currentTimestamp - lastTimestamp; + boolean timeoutValid = (questTimeDelta >= 0) && (questTimeDelta < CONNECTION_TIMEOUT); + return timeoutValid; + } + + /** + * {@inheritDoc} + * + *

Returns field-relative robot pose from Quest SLAM. + */ + @Override + public Pose2d getCurrentPose() { + return currentPose; + } + + /** + * {@inheritDoc} + * + *

Returns constant measurement uncertainty for Quest tracking. + */ + @Override + public Matrix getStdDevs() { + return OCULUS_STD_DEVS; + } + + /** {@inheritDoc} */ + @Override + public String getSourceName() { + return "Oculus Quest"; + } +} diff --git a/src/main/java/frc/alotobots/library/subsystems/vision/oculus/commands/PingCommand.java b/src/main/java/frc/alotobots/library/subsystems/vision/oculus/commands/PingCommand.java index 0815edb1..463755cd 100644 --- a/src/main/java/frc/alotobots/library/subsystems/vision/oculus/commands/PingCommand.java +++ b/src/main/java/frc/alotobots/library/subsystems/vision/oculus/commands/PingCommand.java @@ -16,8 +16,8 @@ import frc.alotobots.library.subsystems.vision.oculus.OculusSubsystem; /** - * Command that sends a ping to the Oculus system and waits for a response. - * Used to verify communication with the Oculus headset. + * Command that sends a ping to the Oculus system and waits for a response. Used to verify + * communication with the Oculus headset. */ public class PingCommand extends Command { /** The Oculus subsystem instance to use for pinging */ @@ -42,4 +42,4 @@ public void initialize() { public boolean isFinished() { return !oculus.isPingInProgress(); } -} \ No newline at end of file +} diff --git a/src/main/java/frc/alotobots/library/subsystems/vision/oculus/commands/ResetPoseCommand.java b/src/main/java/frc/alotobots/library/subsystems/vision/oculus/commands/ResetPoseCommand.java index b1983c04..f5254d59 100644 --- a/src/main/java/frc/alotobots/library/subsystems/vision/oculus/commands/ResetPoseCommand.java +++ b/src/main/java/frc/alotobots/library/subsystems/vision/oculus/commands/ResetPoseCommand.java @@ -17,12 +17,13 @@ import frc.alotobots.library.subsystems.vision.oculus.OculusSubsystem; /** - * Command that resets the Oculus system's pose estimation to a specified target pose. - * This is useful for initializing or correcting the robot's position tracking. + * Command that resets the Oculus system's pose estimation to a specified target pose. This is + * useful for initializing or correcting the robot's position tracking. */ public class ResetPoseCommand extends Command { /** The Oculus subsystem instance to reset */ private final OculusSubsystem oculus; + /** The target pose to reset to */ private final Pose2d targetPose; @@ -47,4 +48,4 @@ public void initialize() { public boolean isFinished() { return !oculus.isPoseResetInProgress(); } -} \ No newline at end of file +} diff --git a/src/main/java/frc/alotobots/library/subsystems/vision/oculus/commands/ZeroHeadingCommand.java b/src/main/java/frc/alotobots/library/subsystems/vision/oculus/commands/ZeroHeadingCommand.java index 903d2f81..1c2a97c4 100644 --- a/src/main/java/frc/alotobots/library/subsystems/vision/oculus/commands/ZeroHeadingCommand.java +++ b/src/main/java/frc/alotobots/library/subsystems/vision/oculus/commands/ZeroHeadingCommand.java @@ -16,8 +16,8 @@ import frc.alotobots.library.subsystems.vision.oculus.OculusSubsystem; /** - * Command that zeros the heading (rotation) of the Oculus system. - * This is useful for aligning the robot's coordinate system with the field. + * Command that zeros the heading (rotation) of the Oculus system. This is useful for aligning the + * robot's coordinate system with the field. */ public class ZeroHeadingCommand extends Command { /** The Oculus subsystem instance to zero */ @@ -42,4 +42,4 @@ public void initialize() { public boolean isFinished() { return !oculus.isHeadingResetInProgress(); } -} \ No newline at end of file +} diff --git a/src/main/java/frc/alotobots/library/subsystems/vision/oculus/constants/OculusConstants.java b/src/main/java/frc/alotobots/library/subsystems/vision/oculus/constants/OculusConstants.java index f4104e87..13aa5e61 100644 --- a/src/main/java/frc/alotobots/library/subsystems/vision/oculus/constants/OculusConstants.java +++ b/src/main/java/frc/alotobots/library/subsystems/vision/oculus/constants/OculusConstants.java @@ -20,28 +20,25 @@ import edu.wpi.first.math.numbers.N3; /** - * Constants used by the Oculus Quest navigation subsystem. - * Contains configuration values for physical setup and operation parameters. + * Constants used by the Oculus Quest navigation subsystem. Contains configuration values for + * physical setup and operation parameters. */ public class OculusConstants { /** - * Transform from the robot center to the headset. - * Coordinate system: - * - X: Positive is forwards - * - Y: Positive is left - * - Rotation: Positive is counter-clockwise + * Transform from the robot center to the headset. Coordinate system: - X: Positive is forwards - + * Y: Positive is left - Rotation: Positive is counter-clockwise */ public static final Transform2d ROBOT_TO_OCULUS = new Transform2d(0.075, 0.0, new Rotation2d()); /** - * Timeout duration in seconds for reset operations (pose reset, heading reset, ping). - * If a reset operation takes longer than this time, it will be considered failed. + * Timeout duration in seconds for reset operations (pose reset, heading reset, ping). If a reset + * operation takes longer than this time, it will be considered failed. */ public static final double RESET_TIMEOUT_SECONDS = 0.2; /** - * Maximum number of attempts for reset operations. - * If a reset operation fails this many times, the command will terminate. + * Maximum number of attempts for reset operations. If a reset operation fails this many times, + * the command will terminate. */ public static final int MAX_RESET_ATTEMPTS = 3; @@ -49,17 +46,14 @@ public class OculusConstants { public static final double CONNECTION_TIMEOUT = 0.1; /** - * Standard deviations representing how much we "trust" the position from the Oculus. - * By default, the Quest 3 provides sub-millimeter accuracy. - * Values represent: - * [0]: X position trust (5mm) - * [1]: Y position trust (5mm) - * [2]: Rotation trust (~2.87 degrees) + * Standard deviations representing how much we "trust" the position from the Oculus. By default, + * the Quest 3 provides sub-millimeter accuracy. Values represent: [0]: X position trust (5mm) + * [1]: Y position trust (5mm) [2]: Rotation trust (~2.87 degrees) */ public static final Matrix OCULUS_STD_DEVS = - VecBuilder.fill( - 0.005, // Trust down to 5mm - 0.005, // Trust down to 5mm - 0.05 // Trust down to ~2.87deg + VecBuilder.fill( + 0.005, // Trust down to 5mm + 0.005, // Trust down to 5mm + 0.05 // Trust down to ~2.87deg ); -} \ No newline at end of file +} diff --git a/src/main/java/frc/alotobots/library/subsystems/vision/oculus/util/OculusPoseSource.java b/src/main/java/frc/alotobots/library/subsystems/vision/oculus/util/OculusPoseSource.java new file mode 100644 index 00000000..63beb2e0 --- /dev/null +++ b/src/main/java/frc/alotobots/library/subsystems/vision/oculus/util/OculusPoseSource.java @@ -0,0 +1,84 @@ +/* +* ALOTOBOTS - FRC Team 5152 + https://github.com/5152Alotobots +* Copyright (C) 2024 ALOTOBOTS +* +* This program is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* Source code must be publicly available on GitHub or an alternative web accessible site +*/ +package frc.alotobots.library.subsystems.vision.oculus.util; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import frc.alotobots.library.subsystems.vision.localizationfusion.PoseSource; +import frc.alotobots.library.subsystems.vision.oculus.OculusSubsystem; +import frc.alotobots.library.subsystems.vision.oculus.constants.OculusConstants; + +/** + * Adapts the Meta Quest SLAM system to the standardized PoseSource interface. + * + *

This class wraps the OculusSubsystem to provide high-frequency pose estimation through the + * standardized PoseSource interface. The Quest serves as: - Primary pose estimation source - + * High-frequency (120Hz) updates - Global SLAM-based localization - Drift-free position tracking + * + *

Features: - Sub-millimeter tracking precision - Building-scale SLAM mapping - Robust to + * reflective surfaces - Fast relocalization from saved maps - High-speed movement handling + */ +public class OculusPoseSource implements PoseSource { + /** The underlying Oculus subsystem */ + public final OculusSubsystem subsystem; + + /** + * Creates a new OculusPoseSource. + * + * @param subsystem The Oculus subsystem to wrap + */ + public OculusPoseSource(OculusSubsystem subsystem) { + this.subsystem = subsystem; + } + + /** + * {@inheritDoc} + * + *

For Quest SLAM, connection status depends on: - Quest hardware connection - SLAM tracking + * quality - Update frequency - Environment mapping status + */ + @Override + public boolean isConnected() { + return subsystem.isConnected(); + } + + /** + * {@inheritDoc} + * + *

Quest poses are: - Globally consistent via SLAM - Updated at 120Hz - Drift-compensated - + * Environment-referenced - Transform-corrected to field frame + */ + @Override + public Pose2d getCurrentPose() { + return subsystem.getCurrentPose(); + } + + /** + * {@inheritDoc} + * + *

Quest measurement uncertainty: - Sub-millimeter position accuracy - Factory-calibrated IMU - + * Environment-dependent SLAM quality - Constant across operating range + */ + @Override + public Matrix getStdDevs() { + return OculusConstants.OCULUS_STD_DEVS; + } + + /** {@inheritDoc} */ + @Override + public String getSourceName() { + return "Oculus Quest"; + } +} diff --git a/src/main/java/frc/alotobots/library/subsystems/vision/photonvision/apriltag/AprilTagSubsystem.java b/src/main/java/frc/alotobots/library/subsystems/vision/photonvision/apriltag/AprilTagSubsystem.java index 5913552b..fb3b9f82 100644 --- a/src/main/java/frc/alotobots/library/subsystems/vision/photonvision/apriltag/AprilTagSubsystem.java +++ b/src/main/java/frc/alotobots/library/subsystems/vision/photonvision/apriltag/AprilTagSubsystem.java @@ -16,56 +16,41 @@ import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.Vector; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation2d; 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.Timer; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.alotobots.library.subsystems.vision.localizationfusion.PoseSource; import frc.alotobots.library.subsystems.vision.photonvision.apriltag.io.AprilTagIO; import frc.alotobots.library.subsystems.vision.photonvision.apriltag.io.AprilTagIOInputsAutoLogged; import java.util.LinkedList; import java.util.List; import org.littletonrobotics.junction.Logger; -/** - * A subsystem that handles AprilTag detection and processing using PhotonVision. This subsystem - * processes AprilTag data from multiple cameras and provides pose estimation with statistical - * confidence measures. - */ -public class AprilTagSubsystem extends SubsystemBase { - /** Consumer interface for receiving AprilTag pose data with statistical confidence */ - private final AprilTagConsumer consumer; +public class AprilTagSubsystem extends SubsystemBase implements PoseSource { - /** Array of AprilTag IO interfaces for each camera */ private final AprilTagIO[] io; - - /** Array of input logs for each camera */ private final AprilTagIOInputsAutoLogged[] inputs; - - /** Array of alerts for disconnected cameras */ private final Alert[] disconnectedAlerts; - /** - * Constructs a new AprilTagSubsystem. - * - * @param consumer The consumer that will receive processed AprilTag data - * @param io Array of AprilTagIO interfaces for each camera - * @throws IllegalArgumentException if camera standard deviation factors are invalid - */ - public AprilTagSubsystem(AprilTagConsumer consumer, AprilTagIO... io) { - this.consumer = consumer; - this.io = io; + private Pose2d latestPose = null; + private Vector latestStdDevs = null; + private boolean hasValidPose = false; + private boolean isConnected = false; + private double lastPoseTimestamp = 0.0; - // Initialize inputs + public AprilTagSubsystem(AprilTagIO... io) { + this.io = io; this.inputs = new AprilTagIOInputsAutoLogged[io.length]; for (int i = 0; i < inputs.length; i++) { inputs[i] = new AprilTagIOInputsAutoLogged(); } - // Initialize disconnected alerts this.disconnectedAlerts = new Alert[io.length]; for (int i = 0; i < inputs.length; i++) { disconnectedAlerts[i] = @@ -74,34 +59,49 @@ public AprilTagSubsystem(AprilTagConsumer consumer, AprilTagIO... io) { AlertType.kWarning); } - // Ensure no illegal values for STD factors - for (double factor : CAMERA_STD_DEV_FACTORS) { - if (factor < 1.0) - throw new IllegalArgumentException( - "[AprilTagSubsystem] Value must be greater than or equal to 1, but was: " + factor); + validateConfiguration(); + } + + // PoseSource interface implementation + @Override + public boolean isConnected() { + return isConnected; + } + + @Override + public Pose2d getCurrentPose() { + double timeSinceLastPose = Timer.getTimestamp() - lastPoseTimestamp; + if (timeSinceLastPose > POSE_TIMEOUT) { + return null; // Return null if pose is stale } + return hasValidPose ? latestPose : null; } - /** - * Returns the X angle to the best target for simple vision-based servoing. - * - * @param cameraIndex The index of the camera to use - * @return The rotation angle to the best target - */ - public Rotation2d getTargetX(int cameraIndex) { - return inputs[cameraIndex].latestTargetObservation.tx(); + @Override + public Matrix getStdDevs() { + return latestStdDevs; + } + + @Override + public String getSourceName() { + return "AprilTag"; } - /** - * Periodic function that processes AprilTag data from all cameras. This method: - Updates camera - * inputs - Processes pose observations - Applies statistical confidence calculations - Logs - * vision data - Sends accepted pose estimates to the consumer - */ @Override public void periodic() { + hasValidPose = false; + isConnected = false; + + // Update inputs and check camera connections for (int i = 0; i < io.length; i++) { io[i].updateInputs(inputs[i]); Logger.processInputs("Vision/AprilTag/Camera" + CAMERA_CONFIGS[i].name(), inputs[i]); + + // If any camera is connected, consider the system connected + if (inputs[i].connected) { + isConnected = true; + } + disconnectedAlerts[i].set(!inputs[i].connected); } // Initialize logging values @@ -110,118 +110,129 @@ public void periodic() { List allRobotPosesAccepted = new LinkedList<>(); List allRobotPosesRejected = new LinkedList<>(); - // Loop over cameras - for (int cameraIndex = 0; cameraIndex < io.length; cameraIndex++) { - // Update disconnected alert - disconnectedAlerts[cameraIndex].set(!inputs[cameraIndex].connected); + // Process camera data + processCameraData(allTagPoses, allRobotPoses, allRobotPosesAccepted, allRobotPosesRejected); + + // Log summary data + logSummaryData(allTagPoses, allRobotPoses, allRobotPosesAccepted, allRobotPosesRejected); - // Initialize logging values + // Log pose staleness + double timeSinceLastPose = Timer.getTimestamp() - lastPoseTimestamp; + Logger.recordOutput("Vision/AprilTag/TimeSinceLastPose", timeSinceLastPose); + Logger.recordOutput("Vision/AprilTag/PoseStale", timeSinceLastPose > POSE_TIMEOUT); + } + + private void validateConfiguration() { + for (double factor : CAMERA_STD_DEV_FACTORS) { + if (factor < 1.0) { + throw new IllegalArgumentException( + "[AprilTagSubsystem] STD factor must be >= 1.0, but was: " + factor); + } + } + } + + private void processCameraData( + List allTagPoses, + List allRobotPoses, + List allRobotPosesAccepted, + List allRobotPosesRejected) { + + for (int cameraIndex = 0; cameraIndex < io.length; cameraIndex++) { List tagPoses = new LinkedList<>(); List robotPoses = new LinkedList<>(); List robotPosesAccepted = new LinkedList<>(); List robotPosesRejected = new LinkedList<>(); - // Add tag poses - for (int tagId : inputs[cameraIndex].tagIds) { - var tagPose = APRIL_TAG_LAYOUT.getTagPose(tagId); - if (tagPose.isPresent()) { - tagPoses.add(tagPose.get()); - } - } + processTagPoses(cameraIndex, tagPoses); + processPoseObservations(cameraIndex, robotPoses, robotPosesAccepted, robotPosesRejected); - // Loop over pose observations - for (var observation : inputs[cameraIndex].poseObservations) { - // Check whether to reject pose - boolean rejectPose = - observation.tagCount() == 0 // Must have at least one tag - || (observation.tagCount() == 1 - && observation.ambiguity() > MAX_AMBIGUITY) // Cannot be high ambiguity - || Math.abs(observation.pose().getZ()) - > MAX_Z_ERROR // Must have realistic Z coordinate - - // Must be within the field boundaries - || observation.pose().getX() < 0.0 - || observation.pose().getX() > APRIL_TAG_LAYOUT.getFieldLength() - || observation.pose().getY() < 0.0 - || observation.pose().getY() > APRIL_TAG_LAYOUT.getFieldWidth(); - - // Add pose to log - robotPoses.add(observation.pose()); - if (rejectPose) { - robotPosesRejected.add(observation.pose()); - } else { - robotPosesAccepted.add(observation.pose()); - } - - // Skip if rejected - if (rejectPose) { - continue; - } - - // Calculate standard deviations - double stdDevFactor = - Math.pow(observation.averageTagDistance(), 2.0) / observation.tagCount(); - double linearStdDev = LINEAR_STD_DEV_BASE * stdDevFactor; - double angularStdDev = ANGULAR_STD_DEV_BASE * stdDevFactor; - - if (cameraIndex < CAMERA_STD_DEV_FACTORS.length) { - linearStdDev *= CAMERA_STD_DEV_FACTORS[cameraIndex]; - angularStdDev *= CAMERA_STD_DEV_FACTORS[cameraIndex]; - } - - // Send vision observation - consumer.accept( - observation.pose().toPose2d(), - observation.timestamp(), - VecBuilder.fill(linearStdDev, linearStdDev, angularStdDev)); - } + logCameraData(cameraIndex, tagPoses, robotPoses, robotPosesAccepted, robotPosesRejected); - // Log camera data - Logger.recordOutput( - "Vision/AprilTag/Camera" + CAMERA_CONFIGS[cameraIndex].name() + "/TagPoses", - tagPoses.toArray(new Pose3d[tagPoses.size()])); - Logger.recordOutput( - "Vision/AprilTag/Camera" + CAMERA_CONFIGS[cameraIndex].name() + "/RobotPoses", - robotPoses.toArray(new Pose3d[robotPoses.size()])); - Logger.recordOutput( - "Vision/AprilTag/Camera" + CAMERA_CONFIGS[cameraIndex].name() + "/RobotPosesAccepted", - robotPosesAccepted.toArray(new Pose3d[robotPosesAccepted.size()])); - Logger.recordOutput( - "Vision/AprilTag/Camera" + CAMERA_CONFIGS[cameraIndex].name() + "/RobotPosesRejected", - robotPosesRejected.toArray(new Pose3d[robotPosesRejected.size()])); allTagPoses.addAll(tagPoses); allRobotPoses.addAll(robotPoses); allRobotPosesAccepted.addAll(robotPosesAccepted); allRobotPosesRejected.addAll(robotPosesRejected); } + } - // Log summary data - Logger.recordOutput( - "Vision/AprilTag/Summary/TagPoses", allTagPoses.toArray(new Pose3d[allTagPoses.size()])); - Logger.recordOutput( - "Vision/AprilTag/Summary/RobotPoses", - allRobotPoses.toArray(new Pose3d[allRobotPoses.size()])); + private void processTagPoses(int cameraIndex, List tagPoses) { + for (int tagId : inputs[cameraIndex].tagIds) { + var tagPose = APRIL_TAG_LAYOUT.getTagPose(tagId); + tagPose.ifPresent(tagPoses::add); + } + } + + private void processPoseObservations( + int cameraIndex, + List robotPoses, + List robotPosesAccepted, + List robotPosesRejected) { + + for (var observation : inputs[cameraIndex].poseObservations) { + boolean rejectPose = shouldRejectPose(observation); + + robotPoses.add(observation.pose()); + if (rejectPose) { + robotPosesRejected.add(observation.pose()); + continue; + } + + robotPosesAccepted.add(observation.pose()); + updateLatestPose(observation, cameraIndex); + hasValidPose = true; + lastPoseTimestamp = Timer.getTimestamp(); + } + } + + private boolean shouldRejectPose(AprilTagIO.PoseObservation observation) { + return observation.tagCount() == 0 + || (observation.tagCount() == 1 && observation.ambiguity() > MAX_AMBIGUITY) + || Math.abs(observation.pose().getZ()) > MAX_Z_ERROR + || observation.pose().getX() < 0.0 + || observation.pose().getX() > APRIL_TAG_LAYOUT.getFieldLength() + || observation.pose().getY() < 0.0 + || observation.pose().getY() > APRIL_TAG_LAYOUT.getFieldWidth(); + } + + private void updateLatestPose(AprilTagIO.PoseObservation observation, int cameraIndex) { + double stdDevFactor = Math.pow(observation.averageTagDistance(), 2.0) / observation.tagCount(); + double linearStdDev = LINEAR_STD_DEV_BASE * stdDevFactor; + double angularStdDev = ANGULAR_STD_DEV_BASE * stdDevFactor; + + if (cameraIndex < CAMERA_STD_DEV_FACTORS.length) { + linearStdDev *= CAMERA_STD_DEV_FACTORS[cameraIndex]; + angularStdDev *= CAMERA_STD_DEV_FACTORS[cameraIndex]; + } + + latestPose = observation.pose().toPose2d(); + latestStdDevs = VecBuilder.fill(linearStdDev, linearStdDev, angularStdDev); + } + + private void logCameraData( + int cameraIndex, + List tagPoses, + List robotPoses, + List robotPosesAccepted, + List robotPosesRejected) { + String cameraPrefix = "Vision/AprilTag/Camera" + CAMERA_CONFIGS[cameraIndex].name(); + Logger.recordOutput(cameraPrefix + "/TagPoses", tagPoses.toArray(new Pose3d[0])); + Logger.recordOutput(cameraPrefix + "/RobotPoses", robotPoses.toArray(new Pose3d[0])); Logger.recordOutput( - "Vision/AprilTag/Summary/RobotPosesAccepted", - allRobotPosesAccepted.toArray(new Pose3d[allRobotPosesAccepted.size()])); + cameraPrefix + "/RobotPosesAccepted", robotPosesAccepted.toArray(new Pose3d[0])); Logger.recordOutput( - "Vision/AprilTag/Summary/RobotPosesRejected", - allRobotPosesRejected.toArray(new Pose3d[allRobotPosesRejected.size()])); + cameraPrefix + "/RobotPosesRejected", robotPosesRejected.toArray(new Pose3d[0])); } - /** Functional interface for consuming AprilTag pose data with statistical confidence measures. */ - @FunctionalInterface - public static interface AprilTagConsumer { - /** - * Accepts AprilTag pose data with statistical confidence measures. - * - * @param visionRobotPoseMeters The estimated robot pose in field coordinates - * @param timestampSeconds The timestamp of the measurement in seconds - * @param visionMeasurementStdDevs Standard deviations for the measurement [x, y, theta] - */ - public void accept( - Pose2d visionRobotPoseMeters, - double timestampSeconds, - Matrix visionMeasurementStdDevs); + private void logSummaryData( + List allTagPoses, + List allRobotPoses, + List allRobotPosesAccepted, + List allRobotPosesRejected) { + Logger.recordOutput("Vision/AprilTag/Summary/TagPoses", allTagPoses.toArray(new Pose3d[0])); + Logger.recordOutput("Vision/AprilTag/Summary/RobotPoses", allRobotPoses.toArray(new Pose3d[0])); + Logger.recordOutput( + "Vision/AprilTag/Summary/RobotPosesAccepted", allRobotPosesAccepted.toArray(new Pose3d[0])); + Logger.recordOutput( + "Vision/AprilTag/Summary/RobotPosesRejected", allRobotPosesRejected.toArray(new Pose3d[0])); } } diff --git a/src/main/java/frc/alotobots/library/subsystems/vision/photonvision/apriltag/constants/AprilTagConstants.java b/src/main/java/frc/alotobots/library/subsystems/vision/photonvision/apriltag/constants/AprilTagConstants.java index e3bd7f47..1419a5c1 100644 --- a/src/main/java/frc/alotobots/library/subsystems/vision/photonvision/apriltag/constants/AprilTagConstants.java +++ b/src/main/java/frc/alotobots/library/subsystems/vision/photonvision/apriltag/constants/AprilTagConstants.java @@ -65,4 +65,7 @@ public class AprilTagConstants { // Basic Filtering public static double MAX_AMBIGUITY = 0.3; public static double MAX_Z_ERROR = 0.75; + + /** Time after which a pose is considered stale (seconds) */ + public static final double POSE_TIMEOUT = 0.06; } diff --git a/src/main/java/frc/alotobots/library/subsystems/vision/photonvision/apriltag/util/AprilTagPoseSource.java b/src/main/java/frc/alotobots/library/subsystems/vision/photonvision/apriltag/util/AprilTagPoseSource.java new file mode 100644 index 00000000..ff40e195 --- /dev/null +++ b/src/main/java/frc/alotobots/library/subsystems/vision/photonvision/apriltag/util/AprilTagPoseSource.java @@ -0,0 +1,83 @@ +/* +* ALOTOBOTS - FRC Team 5152 + https://github.com/5152Alotobots +* Copyright (C) 2024 ALOTOBOTS +* +* This program is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* Source code must be publicly available on GitHub or an alternative web accessible site +*/ +package frc.alotobots.library.subsystems.vision.photonvision.apriltag.util; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import frc.alotobots.library.subsystems.vision.localizationfusion.PoseSource; +import frc.alotobots.library.subsystems.vision.photonvision.apriltag.AprilTagSubsystem; + +/** + * Adapts the AprilTag vision system to the standardized PoseSource interface. + * + *

This class wraps the AprilTagSubsystem to provide pose estimation through the standardized + * PoseSource interface. AprilTags serve as: - Secondary pose validation source - Initial pose + * calibration reference - Backup pose estimation when Quest is unavailable + * + *

Pose accuracy depends on: - Number of visible AprilTags - Distance to detected tags - Camera + * calibration quality - Tag pose ambiguity factors + */ +public class AprilTagPoseSource implements PoseSource { + /** The underlying AprilTag subsystem */ + private final AprilTagSubsystem subsystem; + + /** + * Creates a new AprilTagPoseSource. + * + * @param subsystem The AprilTag subsystem to wrap + */ + public AprilTagPoseSource(AprilTagSubsystem subsystem) { + this.subsystem = subsystem; + } + + /** + * {@inheritDoc} + * + *

For AprilTags, connection status depends on: - Camera connection status - Valid pose + * availability - Recent detection history + */ + @Override + public boolean isConnected() { + return subsystem.isConnected(); + } + + /** + * {@inheritDoc} + * + *

AprilTag poses are: - Field-relative - Validated for ambiguity - Filtered for physical + * feasibility - Updated at camera frame rate + */ + @Override + public Pose2d getCurrentPose() { + return subsystem.getCurrentPose(); + } + + /** + * {@inheritDoc} + * + *

Measurement uncertainty scales with: - Tag distance - Number of visible tags - Viewing angle + * - Tag pose ambiguity + */ + @Override + public Matrix getStdDevs() { + return subsystem.getStdDevs(); + } + + /** {@inheritDoc} */ + @Override + public String getSourceName() { + return "AprilTag"; + } +} diff --git a/src/main/java/frc/alotobots/library/subsystems/vision/photonvision/objectdetection/commands/PathfindToBestObject.java b/src/main/java/frc/alotobots/library/subsystems/vision/photonvision/objectdetection/commands/PathfindToBestObject.java index 6008f691..c907db6e 100644 --- a/src/main/java/frc/alotobots/library/subsystems/vision/photonvision/objectdetection/commands/PathfindToBestObject.java +++ b/src/main/java/frc/alotobots/library/subsystems/vision/photonvision/objectdetection/commands/PathfindToBestObject.java @@ -25,6 +25,7 @@ import frc.alotobots.library.subsystems.swervedrive.SwerveDriveSubsystem; import frc.alotobots.library.subsystems.swervedrive.commands.DefaultDrive; import frc.alotobots.library.subsystems.swervedrive.commands.DriveFacingPose; +import frc.alotobots.library.subsystems.swervedrive.util.PathPlannerManager; import frc.alotobots.library.subsystems.vision.photonvision.objectdetection.ObjectDetectionSubsystem; import frc.alotobots.library.subsystems.vision.photonvision.objectdetection.io.ObjectDetectionIO; import frc.alotobots.library.subsystems.vision.photonvision.objectdetection.util.GameElement; @@ -42,6 +43,9 @@ public class PathfindToBestObject extends Command { /** Subsystem for controlling robot movement */ private final SwerveDriveSubsystem swerveDriveSubsystem; + /** The pathplanner instance */ + private final PathPlannerManager pathPlannerManager; + /** Array of game elements to target, in priority order */ private final GameElement[] targetGameElementNames; @@ -64,9 +68,11 @@ public class PathfindToBestObject extends Command { public PathfindToBestObject( ObjectDetectionSubsystem objectDetectionSubsystem, SwerveDriveSubsystem swerveDriveSubsystem, + PathPlannerManager pathPlannerManager, GameElement... targetGameElementNames) { this.objectDetectionSubsystem = objectDetectionSubsystem; this.swerveDriveSubsystem = swerveDriveSubsystem; + this.pathPlannerManager = pathPlannerManager; this.targetGameElementNames = targetGameElementNames; this.driveFacingPose = new DriveFacingPose(swerveDriveSubsystem); this.defaultDrive = new DefaultDrive(swerveDriveSubsystem); @@ -119,7 +125,7 @@ public void execute() { new Pose2d(objectPose.getTranslation().plus(offsetTranslation), objectPose.getRotation()); Command command = - swerveDriveSubsystem.getPathFinderCommand( + pathPlannerManager.getPathFinderCommand( targetPose, LinearVelocity.ofBaseUnits(0, Units.MetersPerSecond)); command.schedule(); } From 998d860fa3164fd2341d414fa06dd99818109cc6 Mon Sep 17 00:00:00 2001 From: Sean Ernstes Date: Fri, 3 Jan 2025 00:29:56 -0500 Subject: [PATCH 15/23] Move PoseVisionConsumer to its own file --- .../java/frc/alotobots/RobotContainer.java | 18 +- .../LocalizationFusion.java | 455 +++++++++++++----- .../PoseVisionConsumer.java | 31 ++ 3 files changed, 375 insertions(+), 129 deletions(-) create mode 100644 src/main/java/frc/alotobots/library/subsystems/vision/localizationfusion/PoseVisionConsumer.java diff --git a/src/main/java/frc/alotobots/RobotContainer.java b/src/main/java/frc/alotobots/RobotContainer.java index d00d82de..921057f0 100644 --- a/src/main/java/frc/alotobots/RobotContainer.java +++ b/src/main/java/frc/alotobots/RobotContainer.java @@ -71,11 +71,9 @@ public RobotContainer() { OculusPoseSource oculusPoseSource = new OculusPoseSource(oculusSubsystem); AprilTagPoseSource aprilTagPoseSource = new AprilTagPoseSource(aprilTagSubsystem); - // Initialize localization fusion with pose sources - LocalizationFusion.PoseVisionConsumer poseConsumer = - swerveDriveSubsystem::addVisionMeasurement; localizationFusion = - new LocalizationFusion(poseConsumer, oculusPoseSource, aprilTagPoseSource); + new LocalizationFusion( + swerveDriveSubsystem::addVisionMeasurement, oculusPoseSource, aprilTagPoseSource); pathPlannerManager = new PathPlannerManager(swerveDriveSubsystem, localizationFusion); objectDetectionSubsystem = @@ -106,11 +104,9 @@ public RobotContainer() { // Create pose sources oculusPoseSource = new OculusPoseSource(oculusSubsystem); aprilTagPoseSource = new AprilTagPoseSource(aprilTagSubsystem); - - // Initialize localization fusion with pose sources - poseConsumer = swerveDriveSubsystem::addVisionMeasurement; localizationFusion = - new LocalizationFusion(poseConsumer, oculusPoseSource, aprilTagPoseSource); + new LocalizationFusion( + swerveDriveSubsystem::addVisionMeasurement, oculusPoseSource, aprilTagPoseSource); pathPlannerManager = new PathPlannerManager(swerveDriveSubsystem, localizationFusion); objectDetectionSubsystem = @@ -134,11 +130,9 @@ public RobotContainer() { // Create pose sources oculusPoseSource = new OculusPoseSource(oculusSubsystem); aprilTagPoseSource = new AprilTagPoseSource(aprilTagSubsystem); - - // Initialize localization fusion with pose sources - poseConsumer = swerveDriveSubsystem::addVisionMeasurement; localizationFusion = - new LocalizationFusion(poseConsumer, oculusPoseSource, aprilTagPoseSource); + new LocalizationFusion( + swerveDriveSubsystem::addVisionMeasurement, oculusPoseSource, aprilTagPoseSource); pathPlannerManager = new PathPlannerManager(swerveDriveSubsystem, localizationFusion); objectDetectionSubsystem = diff --git a/src/main/java/frc/alotobots/library/subsystems/vision/localizationfusion/LocalizationFusion.java b/src/main/java/frc/alotobots/library/subsystems/vision/localizationfusion/LocalizationFusion.java index 8cc3bcdf..6d579f7b 100644 --- a/src/main/java/frc/alotobots/library/subsystems/vision/localizationfusion/LocalizationFusion.java +++ b/src/main/java/frc/alotobots/library/subsystems/vision/localizationfusion/LocalizationFusion.java @@ -12,10 +12,7 @@ */ package frc.alotobots.library.subsystems.vision.localizationfusion; -import edu.wpi.first.math.Matrix; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.numbers.N3; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -23,27 +20,31 @@ import frc.alotobots.library.subsystems.vision.photonvision.apriltag.util.AprilTagPoseSource; import org.littletonrobotics.junction.Logger; -/** - * A subsystem that fuses multiple pose sources for robot localization. - * - *

This system primarily uses the Meta Quest 3's SLAM capabilities for high-frequency (120Hz) - * pose updates, with AprilTag detection serving as a validation and initialization mechanism. The - * system follows a hierarchical approach: - * - *

1. Quest SLAM (Primary): Provides continuous, high-precision pose estimates 2. AprilTag System - * (Secondary): Used for initial pose acquisition and ongoing validation 3. Emergency Odometry - * (Fallback): Available when both primary systems fail - * - *

The system maintains a state machine to manage transitions between these sources based on - * availability and reliability metrics. - */ public class LocalizationFusion extends SubsystemBase implements StateTransitionLogger { /** Maximum acceptable difference between AprilTag and Quest poses for validation (meters) */ - private static final double APRILTAG_VALIDATION_THRESHOLD = 0.5; + private static final double APRILTAG_VALIDATION_THRESHOLD = 1.0; // More lenient with Quest + + /** Stricter threshold for validating poses during initialization */ + private static final double INIT_VALIDATION_THRESHOLD = 0.3; // Stricter during init /** Update interval matching Quest's native 120Hz update rate (seconds) */ private static final double POSE_UPDATE_INTERVAL = 1.0 / 120.0; + /** Time window for considering a Quest disconnect as temporary (seconds) */ + private static final double QUICK_RECONNECT_WINDOW = 5.0; + + /** Time required for Quest initialization (seconds) */ + private static final double QUEST_INIT_TIMEOUT = 2.0; + + /** Time required for AprilTag initialization (seconds) */ + private static final double TAG_INIT_TIMEOUT = 1.0; + + /** Minimum valid Quest updates required for initialization */ + private static final int MIN_QUEST_VALID_UPDATES = 10; + + /** Minimum valid AprilTag updates required for initialization */ + private static final int MIN_TAG_VALID_UPDATES = 3; + /** Consumer interface for receiving pose updates */ private final PoseVisionConsumer poseConsumer; @@ -68,26 +69,21 @@ public class LocalizationFusion extends SubsystemBase implements StateTransition /** Previous DriverStation connection state */ private boolean wasConnected = false; - /** Functional interface for consuming pose updates from the fusion system. */ - @FunctionalInterface - public interface PoseVisionConsumer { - /** - * Accepts a new pose update from the fusion system. - * - * @param pose The current robot pose in field coordinates - * @param timestampSeconds The timestamp when this pose was measured - * @param stdDevs Standard deviations for x, y, and rotation measurements - */ - void accept(Pose2d pose, double timestampSeconds, Matrix stdDevs); - } - - /** - * Creates a new LocalizationFusion subsystem. - * - * @param poseConsumer Consumer for receiving pose updates - * @param oculusSource Primary pose source using Quest SLAM - * @param aprilTagSource Secondary pose source using AprilTags - */ + // Quest initialization tracking + private double questInitStartTime = 0.0; + private int consecutiveValidQuestUpdates = 0; + private Pose2d lastQuestInitPose = null; + private boolean questInitialized = false; + private double lastQuestDisconnectTime = 0.0; + private boolean hadPreviousCalibration = false; + + // AprilTag initialization tracking + private double tagInitStartTime = 0.0; + private int consecutiveValidTagUpdates = 0; + private Pose2d lastTagInitPose = null; + private boolean tagInitialized = false; + + /** Creates a new LocalizationFusion subsystem. */ public LocalizationFusion( PoseVisionConsumer poseConsumer, OculusPoseSource oculusSource, @@ -102,36 +98,165 @@ public LocalizationFusion( public void periodic() { logSystemStatus(); handleDriverStationConnection(); - updatePoses(); - updateState(); - } - /** Logs current system status to NetworkTables for monitoring. */ - private void logSystemStatus() { - Logger.recordOutput("PoseEstimator/State", state.getCurrentState().getDescription()); - Logger.recordOutput("PoseEstimator/OculusConnected", oculusSource.isConnected()); - Logger.recordOutput("PoseEstimator/AprilTagConnected", tagSource.isConnected()); + // Handle initialization of both sources + if (!questInitialized && oculusSource.isConnected()) { + handleQuestInitialization(); + } + if (!tagInitialized && tagSource.isConnected()) { + handleTagInitialization(); + } + + // Only proceed with normal updates if at least one source is initialized + if (questInitialized || tagInitialized) { + updatePoses(); + updateState(); + } else if (!state.isInState(LocalizationState.State.EMERGENCY)) { + state.transitionTo(LocalizationState.State.EMERGENCY); + } + + // Log initialization and status + logDetailedStatus(); } /** - * Handles DriverStation connection state changes. When reconnecting, attempts to reset to last - * known good pose. + * Handles initialization of the Quest SLAM system. Differentiates between temporary disconnects + * and full reboots. */ - private void handleDriverStationConnection() { - boolean isConnected = DriverStation.isDSAttached(); - if (!wasConnected && isConnected && lastValidatedPose != null) { - Logger.recordOutput("PoseEstimator/Event", "DriverStation connected - initiating pose reset"); - if (resetToPose(lastValidatedPose)) { - state.transitionTo(LocalizationState.State.RESETTING); + private void handleQuestInitialization() { + if (!oculusSource.isConnected()) { + if (questInitialized) { + lastQuestDisconnectTime = Timer.getTimestamp(); + hadPreviousCalibration = true; } + resetQuestInitialization(); + return; + } + + // Check if this is a quick reconnect with maintained calibration + if (hadPreviousCalibration + && (Timer.getTimestamp() - lastQuestDisconnectTime) < QUICK_RECONNECT_WINDOW) { + + // Verify the pose hasn't jumped dramatically + Pose2d questPose = oculusSource.getCurrentPose(); + if (questPose != null && lastQuestInitPose != null) { + double poseJump = + questPose.getTranslation().getDistance(lastQuestInitPose.getTranslation()); + + if (poseJump < APRILTAG_VALIDATION_THRESHOLD) { + // Quick restore of previous calibration + questInitialized = true; + Logger.recordOutput( + "PoseEstimator/Event", + "Quest reconnected with valid calibration - resuming tracking"); + state.transitionTo(LocalizationState.State.QUEST_PRIMARY); + return; + } else { + Logger.recordOutput( + "PoseEstimator/Event", "Quest calibration invalid after reconnect - reinitializing"); + hadPreviousCalibration = false; + } + } + } + + // Otherwise proceed with full initialization + Pose2d questPose = oculusSource.getCurrentPose(); + if (questPose == null) { + return; + } + + // Start initialization process + if (questInitStartTime == 0.0) { + questInitStartTime = Timer.getTimestamp(); + lastQuestInitPose = questPose; + + // If we have a valid reference pose, use it + Pose2d referencePose = getValidReferencePose(); + if (referencePose != null) { + if (!resetToPose(referencePose)) { + Logger.recordOutput( + "PoseEstimator/Event", "Quest initialization failed - could not set initial pose"); + resetQuestInitialization(); + return; + } + } + + state.transitionTo(LocalizationState.State.RESETTING); + return; + } + + // Verify pose stability and consistency + if (isNewPoseValid(questPose, lastQuestInitPose, INIT_VALIDATION_THRESHOLD)) { + consecutiveValidQuestUpdates++; + lastQuestInitPose = questPose; + } else { + consecutiveValidQuestUpdates = 0; + } + + // Check initialization criteria + double initDuration = Timer.getTimestamp() - questInitStartTime; + if (consecutiveValidQuestUpdates >= MIN_QUEST_VALID_UPDATES + && initDuration >= QUEST_INIT_TIMEOUT) { + + questInitialized = true; + Logger.recordOutput("PoseEstimator/Event", "Quest initialization complete - system ready"); + + // Only switch to primary if AprilTags aren't providing better data + if (!tagInitialized || shouldPreferQuest()) { + state.transitionTo(LocalizationState.State.QUEST_PRIMARY); + } + } else if (initDuration > QUEST_INIT_TIMEOUT * 2) { + Logger.recordOutput("PoseEstimator/Event", "Quest initialization failed - timeout exceeded"); + resetQuestInitialization(); } - wasConnected = isConnected; } - /** - * Updates pose estimates based on current state and available sources. Maintains rate limiting to - * match Quest update frequency. - */ + /** Handles initialization of the AprilTag vision system. */ + private void handleTagInitialization() { + if (!tagSource.isConnected()) { + resetTagInitialization(); + return; + } + + Pose2d tagPose = tagSource.getCurrentPose(); + if (tagPose == null) { + return; + } + + // Start initialization process + if (tagInitStartTime == 0.0) { + tagInitStartTime = Timer.getTimestamp(); + lastTagInitPose = tagPose; + return; + } + + // Verify pose stability and consistency + if (isNewPoseValid(tagPose, lastTagInitPose, INIT_VALIDATION_THRESHOLD)) { + consecutiveValidTagUpdates++; + lastTagInitPose = tagPose; + } else { + consecutiveValidTagUpdates = 0; + } + + // Check initialization criteria + double initDuration = Timer.getTimestamp() - tagInitStartTime; + if (consecutiveValidTagUpdates >= MIN_TAG_VALID_UPDATES && initDuration >= TAG_INIT_TIMEOUT) { + + tagInitialized = true; + Logger.recordOutput("PoseEstimator/Event", "AprilTag initialization complete - system ready"); + + // Switch to TAG_BACKUP if Quest isn't ready or is less reliable + if (!questInitialized || !shouldPreferQuest()) { + state.transitionTo(LocalizationState.State.TAG_BACKUP); + } + } else if (initDuration > TAG_INIT_TIMEOUT * 2) { + Logger.recordOutput( + "PoseEstimator/Event", "AprilTag initialization failed - timeout exceeded"); + resetTagInitialization(); + } + } + + /** Updates pose estimates based on current state and available sources. */ private void updatePoses() { double currentTime = Timer.getTimestamp(); if (currentTime - lastUpdateTime < POSE_UPDATE_INTERVAL) { @@ -154,13 +279,92 @@ private void updatePoses() { break; case EMERGENCY: Logger.recordOutput( - "PoseEstimator/Event", - "Failed to connect to any external pose source. Running with odometry only!"); + "PoseEstimator/Event", "No valid pose sources available - emergency mode active"); break; } } - /** Handles system initialization using AprilTag poses. */ + private boolean shouldPreferQuest() { + if (!questInitialized) return false; + if (!tagInitialized) return true; + + // If we're in a mid-match initialization (DriverStation is enabled and enough time has passed) + boolean isMidMatch = DriverStation.isEnabled() && Timer.getMatchTime() > 5.0; + + // During mid-match initialization, prefer AprilTags until Quest fully validates + if (isMidMatch && Timer.getTimestamp() - questInitStartTime < QUEST_INIT_TIMEOUT * 3) { + return false; + } + + // Otherwise strongly prefer Quest as primary source + return validatePose(oculusSource.getCurrentPose()); + } + + private boolean validatePose(Pose2d pose) { + if (pose == null) return false; + + Pose2d tagPose = tagSource.getCurrentPose(); + if (tagPose == null) { + return true; // No validation possible, trust Quest + } + + double poseError = tagPose.getTranslation().getDistance(pose.getTranslation()); + + // During initialization, use stricter threshold + if (!questInitialized) { + return poseError <= INIT_VALIDATION_THRESHOLD; + } + + // Once initialized, be more lenient with Quest + return poseError <= APRILTAG_VALIDATION_THRESHOLD; + } + + private boolean isNewPoseValid(Pose2d newPose, Pose2d lastPose, double maxChange) { + if (lastPose == null) return true; + + double poseChange = newPose.getTranslation().getDistance(lastPose.getTranslation()); + double rotationChange = + Math.abs(newPose.getRotation().minus(lastPose.getRotation()).getDegrees()); + + return poseChange <= maxChange && rotationChange <= 45.0; // 45 degrees max rotation + } + + private Pose2d getValidReferencePose() { + if (tagInitialized) { + return tagSource.getCurrentPose(); + } + return null; + } + + private void resetQuestInitialization() { + questInitStartTime = 0.0; + consecutiveValidQuestUpdates = 0; + lastQuestInitPose = null; + questInitialized = false; + } + + private void resetTagInitialization() { + tagInitStartTime = 0.0; + consecutiveValidTagUpdates = 0; + lastTagInitPose = null; + tagInitialized = false; + } + + private void logDetailedStatus() { + Logger.recordOutput("PoseEstimator/QuestInitialized", questInitialized); + Logger.recordOutput("PoseEstimator/TagInitialized", tagInitialized); + Logger.recordOutput("PoseEstimator/QuestHadCalibration", hadPreviousCalibration); + + if (questInitialized) { + Logger.recordOutput("PoseEstimator/QuestValidUpdates", consecutiveValidQuestUpdates); + Logger.recordOutput("PoseEstimator/LastQuestUpdate", lastQuestUpdate); + } + if (tagInitialized) { + Logger.recordOutput("PoseEstimator/TagValidUpdates", consecutiveValidTagUpdates); + } + } + + /* Original state handling methods remain unchanged */ private void handleUninitializedState() { Pose2d tagPose = tagSource.getCurrentPose(); if (tagPose != null) { @@ -172,7 +376,6 @@ private void handleUninitializedState() { } } - /** Handles pose reset operations and transitions. */ private void handleResettingState() { if (!oculusSource.isConnected()) { state.transitionTo(LocalizationState.State.TAG_BACKUP); @@ -187,13 +390,14 @@ private void handleResettingState() { } } - /** - * Handles primary Quest-based pose updates with AprilTag validation. - * - * @param currentTime Current system timestamp - */ private void handleQuestPrimaryState(double currentTime) { if (!oculusSource.isConnected()) { + if (questInitialized) { + // Mark for potential quick reconnect + lastQuestDisconnectTime = Timer.getTimestamp(); + hadPreviousCalibration = true; + lastQuestInitPose = oculusSource.getCurrentPose(); + } state.transitionTo(LocalizationState.State.TAG_BACKUP); Logger.recordOutput("PoseEstimator/Event", "Quest connection lost - switching to AprilTags"); return; @@ -204,26 +408,45 @@ private void handleQuestPrimaryState(double currentTime) { if (validatePose(questPose)) { poseConsumer.accept(questPose, currentTime, oculusSource.getStdDevs()); lastQuestUpdate = currentTime; + lastValidatedPose = questPose; } else { Logger.recordOutput("PoseEstimator/Event", "Quest pose validation failed"); - state.transitionTo(LocalizationState.State.TAG_BACKUP); + // Don't immediately switch to backup if we have recent valid data + if (currentTime - lastQuestUpdate > QUEST_INIT_TIMEOUT) { + state.transitionTo(LocalizationState.State.TAG_BACKUP); + } } } } - /** - * Handles fallback AprilTag-based pose updates. - * - * @param currentTime Current system timestamp - */ private void handleTagBackupState(double currentTime) { - if (oculusSource.isConnected()) { - state.transitionTo(LocalizationState.State.QUEST_PRIMARY); - Logger.recordOutput( - "PoseEstimator/Event", "Quest connection restored - switching to primary"); + // Check for Quest quick reconnect opportunity + if (oculusSource.isConnected() + && hadPreviousCalibration + && (Timer.getTimestamp() - lastQuestDisconnectTime) < QUICK_RECONNECT_WINDOW) { + + Pose2d questPose = oculusSource.getCurrentPose(); + if (questPose != null && lastQuestInitPose != null) { + double poseJump = + questPose.getTranslation().getDistance(lastQuestInitPose.getTranslation()); + + if (poseJump < APRILTAG_VALIDATION_THRESHOLD) { + state.transitionTo(LocalizationState.State.QUEST_PRIMARY); + Logger.recordOutput( + "PoseEstimator/Event", + "Quest connection restored with valid calibration - resuming primary"); + return; + } + } + } + + // If Quest is available but needs full reinit + if (oculusSource.isConnected() && !hadPreviousCalibration) { + handleQuestInitialization(); return; } + // Otherwise use AprilTag data Pose2d tagPose = tagSource.getCurrentPose(); if (tagPose != null) { poseConsumer.accept(tagPose, currentTime, tagSource.getStdDevs()); @@ -231,58 +454,35 @@ private void handleTagBackupState(double currentTime) { } } - /** - * Validates Quest pose against AprilTag measurements. - * - * @param pose Quest pose to validate - * @return true if pose is valid or no validation possible - */ - private boolean validatePose(Pose2d pose) { - Pose2d tagPose = tagSource.getCurrentPose(); - if (tagPose == null) { - return true; // No validation possible, assume valid - } - - double poseError = tagPose.getTranslation().getDistance(pose.getTranslation()); - return poseError <= APRILTAG_VALIDATION_THRESHOLD; + private void logSystemStatus() { + Logger.recordOutput("PoseEstimator/State", state.getCurrentState().getDescription()); + Logger.recordOutput("PoseEstimator/OculusConnected", oculusSource.isConnected()); + Logger.recordOutput("PoseEstimator/AprilTagConnected", tagSource.isConnected()); } - /** Updates system state based on source availability. */ - private void updateState() { - if (!state.isInState( - LocalizationState.State.UNINITIALIZED, LocalizationState.State.RESETTING)) { - if (oculusSource.isConnected()) { - state.transitionTo(LocalizationState.State.QUEST_PRIMARY); - } else { - state.transitionTo(LocalizationState.State.TAG_BACKUP); + private void handleDriverStationConnection() { + boolean isConnected = DriverStation.isDSAttached(); + if (!wasConnected && isConnected && lastValidatedPose != null) { + Logger.recordOutput("PoseEstimator/Event", "DriverStation connected - initiating pose reset"); + if (resetToPose(lastValidatedPose)) { + state.transitionTo(LocalizationState.State.RESETTING); } } + wasConnected = isConnected; } - /** - * Initiates a pose reset operation. - * - * @param pose Target pose to reset to - * @return true if reset was initiated successfully - */ private boolean resetToPose(Pose2d pose) { return oculusSource.subsystem.resetToPose(pose); } - /** - * Checks if a pose reset operation is in progress. - * - * @return true if reset is ongoing - */ private boolean isResetInProgress() { return oculusSource.subsystem.isPoseResetInProgress(); } /** - * Manually triggers a pose reset using the most recent validated pose. This can be used as a - * callback for manual reset requests. + * Manually triggers a pose reset using the most recent validated pose. * - * @return true if reset was initiated successfully, false otherwise + * @return true if reset was initiated successfully */ public boolean requestResetOculusPoseViaAprilTags() { Pose2d currentTagPose = tagSource.getCurrentPose(); @@ -303,11 +503,10 @@ public boolean requestResetOculusPoseViaAprilTags() { } /** - * Manually triggers a pose reset to a specific pose. Use this when you want to reset to a known - * position rather than the last validated pose. + * Manually triggers a pose reset to a specific pose. * * @param targetPose The pose to reset to - * @return true if reset was initiated successfully, false otherwise + * @return true if reset was initiated successfully */ public boolean requestResetOculusPose(Pose2d targetPose) { if (targetPose == null) { @@ -324,6 +523,28 @@ public boolean requestResetOculusPose(Pose2d targetPose) { return false; } + /** Updates system state based on source availability and initialization status */ + private void updateState() { + // Skip state updates during initialization phases + if (state.isInState(LocalizationState.State.UNINITIALIZED, LocalizationState.State.RESETTING)) { + return; + } + + // Handle source availability + if (oculusSource.isConnected() && questInitialized) { + // If we have a good Quest connection and it's initialized, prefer it + state.transitionTo(LocalizationState.State.QUEST_PRIMARY); + } else if (tagSource.isConnected() && tagInitialized) { + // Fall back to AprilTags if available and initialized + state.transitionTo(LocalizationState.State.TAG_BACKUP); + } else { + // No valid sources available + state.transitionTo(LocalizationState.State.EMERGENCY); + Logger.recordOutput( + "PoseEstimator/Event", "No valid pose sources available - entering emergency mode"); + } + } + @Override public void logTransition(LocalizationState.State from, LocalizationState.State to) { Logger.recordOutput( diff --git a/src/main/java/frc/alotobots/library/subsystems/vision/localizationfusion/PoseVisionConsumer.java b/src/main/java/frc/alotobots/library/subsystems/vision/localizationfusion/PoseVisionConsumer.java new file mode 100644 index 00000000..fa6c11ae --- /dev/null +++ b/src/main/java/frc/alotobots/library/subsystems/vision/localizationfusion/PoseVisionConsumer.java @@ -0,0 +1,31 @@ +/* +* ALOTOBOTS - FRC Team 5152 + https://github.com/5152Alotobots +* Copyright (C) 2024 ALOTOBOTS +* +* This program is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* Source code must be publicly available on GitHub or an alternative web accessible site +*/ +package frc.alotobots.library.subsystems.vision.localizationfusion; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; + +/** Functional interface for consuming pose updates from the fusion system. */ +@FunctionalInterface +public interface PoseVisionConsumer { + /** + * Accepts a new pose update from the fusion system. + * + * @param pose The current robot pose in field coordinates + * @param timestampSeconds The timestamp when this pose was measured + * @param stdDevs Standard deviations for x, y, and rotation measurements + */ + void accept(Pose2d pose, double timestampSeconds, Matrix stdDevs); +} From e4ec7410776cc6e5f069480bad9b5c14178d0656 Mon Sep 17 00:00:00 2001 From: Sean Ernstes Date: Fri, 3 Jan 2025 16:23:57 -0500 Subject: [PATCH 16/23] Very strict and extremely robust pose handling --- .../LocalizationFusion.java | 832 +++++++++++------- .../LocalizationFusionConstants.java | 83 ++ .../vision/oculus/OculusSubsystem.java | 18 +- .../subsystems/vision/oculus/io/OculusIO.java | 6 +- .../vision/oculus/io/OculusIOReal.java | 6 +- .../vision/oculus/io/OculusIOSim.java | 4 +- 6 files changed, 638 insertions(+), 311 deletions(-) create mode 100644 src/main/java/frc/alotobots/library/subsystems/vision/localizationfusion/LocalizationFusionConstants.java diff --git a/src/main/java/frc/alotobots/library/subsystems/vision/localizationfusion/LocalizationFusion.java b/src/main/java/frc/alotobots/library/subsystems/vision/localizationfusion/LocalizationFusion.java index 6d579f7b..d2af7605 100644 --- a/src/main/java/frc/alotobots/library/subsystems/vision/localizationfusion/LocalizationFusion.java +++ b/src/main/java/frc/alotobots/library/subsystems/vision/localizationfusion/LocalizationFusion.java @@ -12,78 +12,117 @@ */ package frc.alotobots.library.subsystems.vision.localizationfusion; +import static frc.alotobots.library.subsystems.vision.localizationfusion.LocalizationFusionConstants.InitializationRequirements.*; +import static frc.alotobots.library.subsystems.vision.localizationfusion.LocalizationFusionConstants.Timing.*; +import static frc.alotobots.library.subsystems.vision.localizationfusion.LocalizationFusionConstants.ValidationThresholds.*; + import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.alotobots.library.subsystems.vision.oculus.util.OculusPoseSource; import frc.alotobots.library.subsystems.vision.photonvision.apriltag.util.AprilTagPoseSource; import org.littletonrobotics.junction.Logger; +/** + * The LocalizationFusion subsystem manages robot pose estimation by fusing data from multiple + * sources. It primarily uses Oculus Quest SLAM for continuous tracking, with AprilTag detection as + * a backup and validation source. The system handles initialization, validation, and switching + * between sources based on their availability and reliability. + */ public class LocalizationFusion extends SubsystemBase implements StateTransitionLogger { - /** Maximum acceptable difference between AprilTag and Quest poses for validation (meters) */ - private static final double APRILTAG_VALIDATION_THRESHOLD = 1.0; // More lenient with Quest - - /** Stricter threshold for validating poses during initialization */ - private static final double INIT_VALIDATION_THRESHOLD = 0.3; // Stricter during init - - /** Update interval matching Quest's native 120Hz update rate (seconds) */ - private static final double POSE_UPDATE_INTERVAL = 1.0 / 120.0; - - /** Time window for considering a Quest disconnect as temporary (seconds) */ - private static final double QUICK_RECONNECT_WINDOW = 5.0; - - /** Time required for Quest initialization (seconds) */ - private static final double QUEST_INIT_TIMEOUT = 2.0; - - /** Time required for AprilTag initialization (seconds) */ - private static final double TAG_INIT_TIMEOUT = 1.0; - - /** Minimum valid Quest updates required for initialization */ - private static final int MIN_QUEST_VALID_UPDATES = 10; - /** Minimum valid AprilTag updates required for initialization */ - private static final int MIN_TAG_VALID_UPDATES = 3; - - /** Consumer interface for receiving pose updates */ + // -------------------- Dependencies -------------------- + /** Interface for consuming pose updates from the localization system. */ private final PoseVisionConsumer poseConsumer; - /** Primary pose source using Quest SLAM */ + /** Primary pose source using Oculus Quest SLAM tracking. */ private final OculusPoseSource oculusSource; - /** Secondary pose source using AprilTags */ + /** Secondary pose source using AprilTag vision tracking. */ private final AprilTagPoseSource tagSource; - /** State machine managing pose source transitions */ + /** State machine managing transitions between different localization states. */ private final LocalizationState state; - /** Timestamp of last pose update */ + /** Tab for drivers to ensure readiness */ + private final ShuffleboardTab prematchTab = Shuffleboard.getTab("Prematch"); + + // -------------------- State Variables -------------------- + /** Timestamp of the last pose update processed. */ private double lastUpdateTime = 0.0; - /** Timestamp of last valid Quest update */ + /** Timestamp of the last valid Quest pose update. */ private double lastQuestUpdate = 0.0; - /** Most recent validated pose from any source */ - private Pose2d lastValidatedPose = null; + /** Timestamp when the current reset sequence started. */ + private double resetStartTime = 0.0; - /** Previous DriverStation connection state */ + // Connection State + /** Previous DriverStation connection state. */ private boolean wasConnected = false; - // Quest initialization tracking + /** Tracks whether we've had our initial DriverStation connection. */ + private boolean hadInitialConnection = false; + + /** Indicates if an initial reset is needed due to pre-connected DriverStation. */ + private boolean needsInitialReset = false; + + // Quest State + /** Timestamp when Quest initialization started. */ private double questInitStartTime = 0.0; + + /** Counter for consecutive valid Quest pose updates during initialization. */ private int consecutiveValidQuestUpdates = 0; + + /** Last valid Quest pose during initialization. */ private Pose2d lastQuestInitPose = null; + + /** Indicates if Quest initialization is complete. */ private boolean questInitialized = false; + + /** Timestamp of the last Quest disconnection. */ private double lastQuestDisconnectTime = 0.0; + + /** Indicates if Quest had a previous valid calibration before disconnection. */ private boolean hadPreviousCalibration = false; - // AprilTag initialization tracking + // AprilTag State + /** Timestamp when AprilTag initialization started. */ private double tagInitStartTime = 0.0; + + /** Counter for consecutive valid AprilTag pose updates during initialization. */ private int consecutiveValidTagUpdates = 0; + + /** Last valid AprilTag pose during initialization. */ private Pose2d lastTagInitPose = null; + + /** Indicates if AprilTag initialization is complete. */ private boolean tagInitialized = false; - /** Creates a new LocalizationFusion subsystem. */ + // Pose Validation + /** Timestamp when initial pose validation started. */ + private double initialPoseValidationStartTime = 0.0; + + /** Last validated pose during initialization or disabled state. */ + private Pose2d lastValidatedPose = null; + + /** Indicates if initial pose validation is complete. */ + private boolean initialPoseValidated = false; + + // TODO: Also, we should only reset for a significant change in pose while disabled until the + // TODO: match starts. Otherwise, when we transition from auto to teleop, we would reset + + // -------------------- Constructor -------------------- + /** + * Creates a new LocalizationFusion subsystem. + * + * @param poseConsumer The consumer interface that will receive pose updates + * @param oculusSource The primary pose source using Quest SLAM + * @param aprilTagSource The secondary pose source using AprilTags + */ public LocalizationFusion( PoseVisionConsumer poseConsumer, OculusPoseSource oculusSource, @@ -92,8 +131,60 @@ public LocalizationFusion( this.oculusSource = oculusSource; this.tagSource = aprilTagSource; this.state = new LocalizationState(this); + initializeConnectionState(); + setupShuffleboardLogging(); } + // -------------------- Core System Methods -------------------- + /** + * Sets up Shuffleboard logging for critical system status indicators in the /Prematch tab. + * Configures three status indicators arranged vertically: + * + *

1. "Quest Ready" - Indicates if the Oculus Quest SLAM system is connected and initialized + * + *

2. "Tags Ready" - Indicates if the AprilTag vision system is connected and initialized + * + *

3. "System Ready" - Indicates if both localization systems are ready and not in emergency + * state + * + *

Each indicator is configured with a size of 2x1 grid units and positioned vertically in the + * Prematch tab. The indicators automatically update based on system state changes and will show + * as green when true and red when false. + * + *

Layout: + * + * 

+   * [Quest Ready]
+   * [Tags Ready]
+   * [System Ready]
+   *
+ */ + private void setupShuffleboardLogging() { + prematchTab + .addBoolean("Quest Ready", () -> oculusSource.isConnected() && questInitialized) + .withSize(2, 1) + .withPosition(0, 0); + + prematchTab + .addBoolean("Tags Ready", () -> tagSource.isConnected() && tagInitialized) + .withSize(2, 1) + .withPosition(0, 1); + + prematchTab + .addBoolean( + "System Ready", + () -> + (oculusSource.isConnected() && questInitialized) + && (tagSource.isConnected() && tagInitialized) + && state.getCurrentState() != LocalizationState.State.EMERGENCY) + .withSize(2, 1) + .withPosition(0, 2); + } + + /** + * Periodic update method called by the command scheduler. Handles system state updates, source + * initialization, and pose processing. + */ @Override public void periodic() { logSystemStatus(); @@ -107,85 +198,215 @@ public void periodic() { handleTagInitialization(); } - // Only proceed with normal updates if at least one source is initialized - if (questInitialized || tagInitialized) { - updatePoses(); - updateState(); - } else if (!state.isInState(LocalizationState.State.EMERGENCY)) { - state.transitionTo(LocalizationState.State.EMERGENCY); + // State-specific handling + double currentTime = Timer.getTimestamp(); + switch (state.getCurrentState()) { + case RESETTING: + handleResettingState(); + break; + case EMERGENCY: + handleEmergencyState(); + break; + case QUEST_PRIMARY: + handleQuestPrimaryState(currentTime); + break; + case TAG_BACKUP: + handleTagBackupState(currentTime); + break; + case UNINITIALIZED: + handleUninitializedState(); + break; } - // Log initialization and status logDetailedStatus(); } + // -------------------- State Handlers -------------------- /** - * Handles initialization of the Quest SLAM system. Differentiates between temporary disconnects - * and full reboots. + * Handles the RESETTING state where the system is attempting to align Quest and AprilTag poses. + * Monitors timeout conditions and validates pose alignment before transitioning to primary state. */ - private void handleQuestInitialization() { + private void handleResettingState() { + if (resetStartTime > 0 && Timer.getTimestamp() - resetStartTime > RESET_TIMEOUT) { + Logger.recordOutput( + "LocalizationFusion/Event", "Reset sequence timed out - switching to emergency mode"); + resetStartTime = 0.0; + resetQuestInitialization(); + state.transitionTo(LocalizationState.State.EMERGENCY); + return; + } + + Pose2d tagPose = tagSource.getCurrentPose(); + if (tagPose == null) { + if (resetStartTime == 0.0) { + resetStartTime = Timer.getTimestamp(); + } + return; + } + + if (!oculusSource.isConnected() || !tagSource.isConnected()) { + return; + } + + if (isResetInProgress()) { + return; + } + + Pose2d questPose = oculusSource.getCurrentPose(); + if (questPose == null) { + return; + } + + double poseError = questPose.getTranslation().getDistance(tagPose.getTranslation()); + if (poseError <= INIT_VALIDATION_THRESHOLD) { + resetStartTime = 0.0; + lastQuestUpdate = Timer.getTimestamp(); + state.transitionTo(LocalizationState.State.QUEST_PRIMARY); + } + } + + /** + * Handles the EMERGENCY state where normal operation has failed. Attempts to recover using + * AprilTag detection and reinitialize the system. + */ + private void handleEmergencyState() { + if (tagSource.isConnected()) { + Pose2d tagPose = tagSource.getCurrentPose(); + if (tagPose != null) { + if (oculusSource.isConnected() && resetToPose(tagPose)) { + resetStartTime = Timer.getTimestamp(); + state.transitionTo(LocalizationState.State.RESETTING); + } else { + state.transitionTo(LocalizationState.State.TAG_BACKUP); + } + } + } + } + + /** + * Handles the QUEST_PRIMARY state where Quest SLAM is the primary pose source. Validates poses + * and handles Quest disconnection scenarios. + * + * @param currentTime The current system timestamp + */ + private void handleQuestPrimaryState(double currentTime) { + if (currentTime - lastUpdateTime < POSE_UPDATE_INTERVAL) { + return; + } + lastUpdateTime = currentTime; + if (!oculusSource.isConnected()) { if (questInitialized) { lastQuestDisconnectTime = Timer.getTimestamp(); hadPreviousCalibration = true; + lastQuestInitPose = oculusSource.getCurrentPose(); } - resetQuestInitialization(); + state.transitionTo(LocalizationState.State.TAG_BACKUP); return; } - // Check if this is a quick reconnect with maintained calibration - if (hadPreviousCalibration - && (Timer.getTimestamp() - lastQuestDisconnectTime) < QUICK_RECONNECT_WINDOW) { + Pose2d questPose = oculusSource.getCurrentPose(); + if (questPose != null) { + if (validatePose(questPose, false)) { + poseConsumer.accept(questPose, currentTime, oculusSource.getStdDevs()); + lastQuestUpdate = currentTime; + } else { + if (currentTime - lastQuestUpdate > QUEST_INIT_TIMEOUT) { + state.transitionTo(LocalizationState.State.TAG_BACKUP); + } + } + } + } + + /** + * Handles the TAG_BACKUP state where AprilTag detection is the primary pose source. Attempts + * Quest reconnection when possible and processes AprilTag poses. + * + * @param currentTime The current system timestamp + */ + private void handleTagBackupState(double currentTime) { + if (currentTime - lastUpdateTime < POSE_UPDATE_INTERVAL) { + return; + } + lastUpdateTime = currentTime; + + if (oculusSource.isConnected() && hadPreviousCalibration) { - // Verify the pose hasn't jumped dramatically Pose2d questPose = oculusSource.getCurrentPose(); - if (questPose != null && lastQuestInitPose != null) { - double poseJump = - questPose.getTranslation().getDistance(lastQuestInitPose.getTranslation()); - - if (poseJump < APRILTAG_VALIDATION_THRESHOLD) { - // Quick restore of previous calibration - questInitialized = true; - Logger.recordOutput( - "PoseEstimator/Event", - "Quest reconnected with valid calibration - resuming tracking"); - state.transitionTo(LocalizationState.State.QUEST_PRIMARY); - return; - } else { - Logger.recordOutput( - "PoseEstimator/Event", "Quest calibration invalid after reconnect - reinitializing"); - hadPreviousCalibration = false; - } + if (validatePose(questPose, true)) { + state.transitionTo(LocalizationState.State.QUEST_PRIMARY); + Logger.recordOutput( + "LocalizationFusion/Event", + "Quest Reconnected; Pose Validated - maintaining Quest tracking"); + return; + } else { + // Re-init as we couldn't validate pose + Logger.recordOutput( + "LocalizationFusion/Event", + "Quest Reconnected; Pose Failed to Validate - reinitializing Quest tracking"); + handleQuestInitialization(); } } - // Otherwise proceed with full initialization + if (oculusSource.isConnected() && !hadPreviousCalibration) { + handleQuestInitialization(); + return; + } + + Pose2d tagPose = tagSource.getCurrentPose(); + if (tagPose != null) { + poseConsumer.accept(tagPose, currentTime, tagSource.getStdDevs()); + } + } + + /** + * Handles the UNINITIALIZED state where the system is starting up. Attempts to initialize using + * AprilTag detection. + */ + private void handleUninitializedState() { + Pose2d tagPose = tagSource.getCurrentPose(); + if (tagPose != null) { + if (DriverStation.isDSAttached() && resetToPose(tagPose)) { + state.transitionTo(LocalizationState.State.RESETTING); + } + } + } + + // -------------------- Initialization Methods -------------------- + /** + * Handles the initialization of the Quest SLAM system. Manages calibration, validation, and state + * transitions during Quest startup. + */ + private void handleQuestInitialization() { + if (!oculusSource.isConnected()) { + if (questInitialized) { + lastQuestDisconnectTime = Timer.getTimestamp(); + hadPreviousCalibration = true; + lastQuestInitPose = oculusSource.getCurrentPose(); + } + resetQuestInitialization(); + return; + } + + // Skip validation during mid-match reboot + if (isMidMatchReboot()) { + questInitialized = true; + lastQuestUpdate = Timer.getTimestamp(); + state.transitionTo(LocalizationState.State.QUEST_PRIMARY); + return; + } + Pose2d questPose = oculusSource.getCurrentPose(); if (questPose == null) { return; } - // Start initialization process if (questInitStartTime == 0.0) { questInitStartTime = Timer.getTimestamp(); lastQuestInitPose = questPose; - - // If we have a valid reference pose, use it - Pose2d referencePose = getValidReferencePose(); - if (referencePose != null) { - if (!resetToPose(referencePose)) { - Logger.recordOutput( - "PoseEstimator/Event", "Quest initialization failed - could not set initial pose"); - resetQuestInitialization(); - return; - } - } - - state.transitionTo(LocalizationState.State.RESETTING); return; } - // Verify pose stability and consistency if (isNewPoseValid(questPose, lastQuestInitPose, INIT_VALIDATION_THRESHOLD)) { consecutiveValidQuestUpdates++; lastQuestInitPose = questPose; @@ -193,25 +414,24 @@ private void handleQuestInitialization() { consecutiveValidQuestUpdates = 0; } - // Check initialization criteria double initDuration = Timer.getTimestamp() - questInitStartTime; if (consecutiveValidQuestUpdates >= MIN_QUEST_VALID_UPDATES && initDuration >= QUEST_INIT_TIMEOUT) { - questInitialized = true; - Logger.recordOutput("PoseEstimator/Event", "Quest initialization complete - system ready"); - - // Only switch to primary if AprilTags aren't providing better data - if (!tagInitialized || shouldPreferQuest()) { - state.transitionTo(LocalizationState.State.QUEST_PRIMARY); + Pose2d referencePose = getValidReferencePose(); + if (referencePose != null && resetToPose(referencePose)) { + state.transitionTo(LocalizationState.State.RESETTING); + resetStartTime = Timer.getTimestamp(); } - } else if (initDuration > QUEST_INIT_TIMEOUT * 2) { - Logger.recordOutput("PoseEstimator/Event", "Quest initialization failed - timeout exceeded"); + } else if (initDuration > QUEST_INIT_TIMEOUT * QUEST_INIT_GRACE_MULTIPLIER) { resetQuestInitialization(); } } - /** Handles initialization of the AprilTag vision system. */ + /** + * Handles the initialization of the AprilTag vision system. Manages detection validation and + * state transitions during AprilTag startup. + */ private void handleTagInitialization() { if (!tagSource.isConnected()) { resetTagInitialization(); @@ -223,14 +443,12 @@ private void handleTagInitialization() { return; } - // Start initialization process if (tagInitStartTime == 0.0) { tagInitStartTime = Timer.getTimestamp(); lastTagInitPose = tagPose; return; } - // Verify pose stability and consistency if (isNewPoseValid(tagPose, lastTagInitPose, INIT_VALIDATION_THRESHOLD)) { consecutiveValidTagUpdates++; lastTagInitPose = tagPose; @@ -238,87 +456,176 @@ private void handleTagInitialization() { consecutiveValidTagUpdates = 0; } - // Check initialization criteria double initDuration = Timer.getTimestamp() - tagInitStartTime; if (consecutiveValidTagUpdates >= MIN_TAG_VALID_UPDATES && initDuration >= TAG_INIT_TIMEOUT) { - tagInitialized = true; - Logger.recordOutput("PoseEstimator/Event", "AprilTag initialization complete - system ready"); - - // Switch to TAG_BACKUP if Quest isn't ready or is less reliable if (!questInitialized || !shouldPreferQuest()) { state.transitionTo(LocalizationState.State.TAG_BACKUP); } - } else if (initDuration > TAG_INIT_TIMEOUT * 2) { - Logger.recordOutput( - "PoseEstimator/Event", "AprilTag initialization failed - timeout exceeded"); + } else if (initDuration > TAG_INIT_TIMEOUT * QUEST_INIT_GRACE_MULTIPLIER) { resetTagInitialization(); } } - /** Updates pose estimates based on current state and available sources. */ - private void updatePoses() { - double currentTime = Timer.getTimestamp(); - if (currentTime - lastUpdateTime < POSE_UPDATE_INTERVAL) { - return; + // -------------------- Connection Management -------------------- + /** + * Checks if we're rebooting during an active match by examining match time. A non-zero match time + * indicates the FMS->DS->Robot connection is active and we're in a match that was already + * running. + * + * @return true if this appears to be a mid-match reboot + */ + private boolean isMidMatchReboot() { + return DriverStation.isEnabled() && Timer.getMatchTime() > 0; + } + + /** + * Initializes the DriverStation connection state tracking. Called during subsystem construction + * to handle pre-connected scenarios and mid-match reboots. + */ + private void initializeConnectionState() { + wasConnected = DriverStation.isDSAttached(); + hadInitialConnection = wasConnected; + + if (isMidMatchReboot()) { + Logger.recordOutput( + "LocalizationFusion/Event", "Mid-match reboot detected - maintaining Quest tracking"); + // Skip initialization and trust Quest's maintained tracking + questInitialized = true; + hadPreviousCalibration = true; + + if (oculusSource.isConnected()) { + // Go straight to Quest primary if available + state.transitionTo(LocalizationState.State.QUEST_PRIMARY); + } else { + // Fallback to tags if Quest isn't ready yet + state.transitionTo(LocalizationState.State.TAG_BACKUP); + } + } else if (hadInitialConnection) { + state.transitionTo(LocalizationState.State.UNINITIALIZED); + needsInitialReset = true; } - lastUpdateTime = currentTime; + } - switch (state.getCurrentState()) { - case UNINITIALIZED: - handleUninitializedState(); - break; - case RESETTING: - handleResettingState(); - break; - case QUEST_PRIMARY: - handleQuestPrimaryState(currentTime); - break; - case TAG_BACKUP: - handleTagBackupState(currentTime); - break; - case EMERGENCY: + /** + * Handles DriverStation connection state changes and triggers appropriate system responses. + * Manages initial connection validation and pose recalibration during disabled state. + */ + private void handleDriverStationConnection() { + boolean isConnected = DriverStation.isDSAttached(); + + if (!wasConnected && isConnected) { + if (!hadInitialConnection) { + Pose2d tagPose = tagSource.getCurrentPose(); + if (tagPose != null) { + lastValidatedPose = tagPose; + initialPoseValidationStartTime = Timer.getTimestamp(); + initialPoseValidated = false; + } + hadInitialConnection = true; + } else { Logger.recordOutput( - "PoseEstimator/Event", "No valid pose sources available - emergency mode active"); - break; + "LocalizationFusion/Event", "DriverStation reconnected - maintaining current pose"); + } } + + if (isConnected && DriverStation.isDisabled()) { + handleDisabledPoseValidation(); + } + + wasConnected = isConnected; } - private boolean shouldPreferQuest() { - if (!questInitialized) return false; - if (!tagInitialized) return true; + // -------------------- Validation Methods -------------------- + /** + * Handles pose validation during the disabled state. Monitors pose stability and triggers + * recalibration if significant changes are detected. + */ + private void handleDisabledPoseValidation() { + Pose2d currentTagPose = tagSource.getCurrentPose(); + if (currentTagPose == null) return; + + if (!initialPoseValidated) { + if (Timer.getTimestamp() - initialPoseValidationStartTime >= INITIAL_POSE_STABILITY_TIME) { + if (lastValidatedPose != null) { + double poseChange = + currentTagPose.getTranslation().getDistance(lastValidatedPose.getTranslation()); + + if (poseChange <= INIT_VALIDATION_THRESHOLD) { + Logger.recordOutput( + "LocalizationFusion/Event", + "Initial pose validated after stability period - initiating reset"); + if (resetToPose(currentTagPose)) { + state.transitionTo(LocalizationState.State.RESETTING); + initialPoseValidated = true; + } + } else { + Logger.recordOutput( + "LocalizationFusion/Event", + "Pose changed during validation - restarting stability timer"); + initialPoseValidationStartTime = Timer.getTimestamp(); + lastValidatedPose = currentTagPose; + } + } + } + lastValidatedPose = currentTagPose; + return; + } - // If we're in a mid-match initialization (DriverStation is enabled and enough time has passed) - boolean isMidMatch = DriverStation.isEnabled() && Timer.getMatchTime() > 5.0; + if (lastValidatedPose != null) { + double poseChange = + currentTagPose.getTranslation().getDistance(lastValidatedPose.getTranslation()); - // During mid-match initialization, prefer AprilTags until Quest fully validates - if (isMidMatch && Timer.getTimestamp() - questInitStartTime < QUEST_INIT_TIMEOUT * 3) { - return false; + if (poseChange >= DISABLED_RECALIBRATION_THRESHOLD) { + Logger.recordOutput( + "LocalizationFusion/Event", + String.format( + "Significant pose change detected while disabled (%.2fm) - recalibrating", + poseChange)); + + initialPoseValidationStartTime = Timer.getTimestamp(); + lastValidatedPose = currentTagPose; + initialPoseValidated = false; + } } - // Otherwise strongly prefer Quest as primary source - return validatePose(oculusSource.getCurrentPose()); + lastValidatedPose = currentTagPose; } - private boolean validatePose(Pose2d pose) { + /** + * Validates a pose against the current AprilTag reference. Uses different thresholds for + * initialization and normal operation. + * + * @param pose The pose to validate + * @param strict True if we require tags to be present for validation, false to assume valid with + * no tags + * @return true if the pose is valid, false otherwise + */ + private boolean validatePose(Pose2d pose, boolean strict) { if (pose == null) return false; Pose2d tagPose = tagSource.getCurrentPose(); if (tagPose == null) { - return true; // No validation possible, trust Quest + return !strict; } double poseError = tagPose.getTranslation().getDistance(pose.getTranslation()); - // During initialization, use stricter threshold if (!questInitialized) { return poseError <= INIT_VALIDATION_THRESHOLD; } - // Once initialized, be more lenient with Quest return poseError <= APRILTAG_VALIDATION_THRESHOLD; } + /** + * Validates a new pose against a previous pose using maximum change thresholds. + * + * @param newPose The new pose to validate + * @param lastPose The previous pose to compare against + * @param maxChange The maximum allowed position change in meters + * @return true if the pose change is within acceptable limits + */ private boolean isNewPoseValid(Pose2d newPose, Pose2d lastPose, double maxChange) { if (lastPose == null) return true; @@ -326,9 +633,14 @@ private boolean isNewPoseValid(Pose2d newPose, Pose2d lastPose, double maxChange double rotationChange = Math.abs(newPose.getRotation().minus(lastPose.getRotation()).getDegrees()); - return poseChange <= maxChange && rotationChange <= 45.0; // 45 degrees max rotation + return poseChange <= maxChange && rotationChange <= MAX_ROTATION_CHANGE_DEGREES; } + /** + * Gets a valid reference pose from the AprilTag system if available. + * + * @return A valid reference Pose2d or null if unavailable + */ private Pose2d getValidReferencePose() { if (tagInitialized) { return tagSource.getCurrentPose(); @@ -336,151 +648,28 @@ private Pose2d getValidReferencePose() { return null; } - private void resetQuestInitialization() { - questInitStartTime = 0.0; - consecutiveValidQuestUpdates = 0; - lastQuestInitPose = null; - questInitialized = false; - } - - private void resetTagInitialization() { - tagInitStartTime = 0.0; - consecutiveValidTagUpdates = 0; - lastTagInitPose = null; - tagInitialized = false; - } - - private void logDetailedStatus() { - Logger.recordOutput("PoseEstimator/QuestInitialized", questInitialized); - Logger.recordOutput("PoseEstimator/TagInitialized", tagInitialized); - Logger.recordOutput("PoseEstimator/QuestHadCalibration", hadPreviousCalibration); - - if (questInitialized) { - Logger.recordOutput("PoseEstimator/QuestValidUpdates", consecutiveValidQuestUpdates); - Logger.recordOutput("PoseEstimator/LastQuestUpdate", lastQuestUpdate); - } - if (tagInitialized) { - Logger.recordOutput("PoseEstimator/TagValidUpdates", consecutiveValidTagUpdates); - } - } - - /* Original state handling methods remain unchanged */ - private void handleUninitializedState() { - Pose2d tagPose = tagSource.getCurrentPose(); - if (tagPose != null) { - lastValidatedPose = tagPose; - if (DriverStation.isDSAttached() && resetToPose(tagPose)) { - state.transitionTo(LocalizationState.State.RESETTING); - Logger.recordOutput("PoseEstimator/Event", "Received initial AprilTag pose - resetting"); - } - } - } - - private void handleResettingState() { - if (!oculusSource.isConnected()) { - state.transitionTo(LocalizationState.State.TAG_BACKUP); - Logger.recordOutput( - "PoseEstimator/Event", "Quest unavailable during reset - using AprilTags"); - return; - } - if (!isResetInProgress()) { - lastQuestUpdate = Timer.getTimestamp(); - state.transitionTo(LocalizationState.State.QUEST_PRIMARY); - Logger.recordOutput("PoseEstimator/Event", "Pose reset complete - using Quest primary"); - } - } - - private void handleQuestPrimaryState(double currentTime) { - if (!oculusSource.isConnected()) { - if (questInitialized) { - // Mark for potential quick reconnect - lastQuestDisconnectTime = Timer.getTimestamp(); - hadPreviousCalibration = true; - lastQuestInitPose = oculusSource.getCurrentPose(); - } - state.transitionTo(LocalizationState.State.TAG_BACKUP); - Logger.recordOutput("PoseEstimator/Event", "Quest connection lost - switching to AprilTags"); - return; - } - - Pose2d questPose = oculusSource.getCurrentPose(); - if (questPose != null) { - if (validatePose(questPose)) { - poseConsumer.accept(questPose, currentTime, oculusSource.getStdDevs()); - lastQuestUpdate = currentTime; - lastValidatedPose = questPose; - } else { - Logger.recordOutput("PoseEstimator/Event", "Quest pose validation failed"); - // Don't immediately switch to backup if we have recent valid data - if (currentTime - lastQuestUpdate > QUEST_INIT_TIMEOUT) { - state.transitionTo(LocalizationState.State.TAG_BACKUP); - } - } - } - } - - private void handleTagBackupState(double currentTime) { - // Check for Quest quick reconnect opportunity - if (oculusSource.isConnected() - && hadPreviousCalibration - && (Timer.getTimestamp() - lastQuestDisconnectTime) < QUICK_RECONNECT_WINDOW) { - - Pose2d questPose = oculusSource.getCurrentPose(); - if (questPose != null && lastQuestInitPose != null) { - double poseJump = - questPose.getTranslation().getDistance(lastQuestInitPose.getTranslation()); - - if (poseJump < APRILTAG_VALIDATION_THRESHOLD) { - state.transitionTo(LocalizationState.State.QUEST_PRIMARY); - Logger.recordOutput( - "PoseEstimator/Event", - "Quest connection restored with valid calibration - resuming primary"); - return; - } - } - } - - // If Quest is available but needs full reinit - if (oculusSource.isConnected() && !hadPreviousCalibration) { - handleQuestInitialization(); - return; - } - - // Otherwise use AprilTag data - Pose2d tagPose = tagSource.getCurrentPose(); - if (tagPose != null) { - poseConsumer.accept(tagPose, currentTime, tagSource.getStdDevs()); - lastValidatedPose = tagPose; - } - } - - private void logSystemStatus() { - Logger.recordOutput("PoseEstimator/State", state.getCurrentState().getDescription()); - Logger.recordOutput("PoseEstimator/OculusConnected", oculusSource.isConnected()); - Logger.recordOutput("PoseEstimator/AprilTagConnected", tagSource.isConnected()); - } - - private void handleDriverStationConnection() { - boolean isConnected = DriverStation.isDSAttached(); - if (!wasConnected && isConnected && lastValidatedPose != null) { - Logger.recordOutput("PoseEstimator/Event", "DriverStation connected - initiating pose reset"); - if (resetToPose(lastValidatedPose)) { - state.transitionTo(LocalizationState.State.RESETTING); - } - } - wasConnected = isConnected; - } - + // -------------------- Reset Methods -------------------- + /** + * Attempts to reset the Quest pose to match a target pose. + * + * @param pose The target pose to reset to + * @return true if reset was initiated successfully + */ private boolean resetToPose(Pose2d pose) { return oculusSource.subsystem.resetToPose(pose); } + /** + * Checks if a pose reset operation is currently in progress. + * + * @return true if a reset is in progress + */ private boolean isResetInProgress() { return oculusSource.subsystem.isPoseResetInProgress(); } /** - * Manually triggers a pose reset using the most recent validated pose. + * Manually triggers a pose reset using the current AprilTag pose. * * @return true if reset was initiated successfully */ @@ -488,12 +677,12 @@ public boolean requestResetOculusPoseViaAprilTags() { Pose2d currentTagPose = tagSource.getCurrentPose(); if (currentTagPose == null) { Logger.recordOutput( - "PoseEstimator/Event", "Manual reset failed - no AprilTag pose available"); + "LocalizationFusion/Event", "Manual reset failed - no AprilTag pose available"); return false; } Logger.recordOutput( - "PoseEstimator/Event", + "LocalizationFusion/Event", "Manual reset requested using AprilTag pose - initiating pose reset"); if (resetToPose(currentTagPose)) { state.transitionTo(LocalizationState.State.RESETTING); @@ -510,12 +699,13 @@ public boolean requestResetOculusPoseViaAprilTags() { */ public boolean requestResetOculusPose(Pose2d targetPose) { if (targetPose == null) { - Logger.recordOutput("PoseEstimator/Event", "Manual reset failed - null pose provided"); + Logger.recordOutput("LocalizationFusion/Event", "Manual reset failed - null pose provided"); return false; } Logger.recordOutput( - "PoseEstimator/Event", "Manual reset requested with custom pose - initiating pose reset"); + "LocalizationFusion/Event", + "Manual reset requested with custom pose - initiating pose reset"); if (resetToPose(targetPose)) { state.transitionTo(LocalizationState.State.RESETTING); return true; @@ -523,32 +713,78 @@ public boolean requestResetOculusPose(Pose2d targetPose) { return false; } - /** Updates system state based on source availability and initialization status */ - private void updateState() { - // Skip state updates during initialization phases - if (state.isInState(LocalizationState.State.UNINITIALIZED, LocalizationState.State.RESETTING)) { - return; + // -------------------- State Management Helpers -------------------- + /** Resets all Quest initialization state variables. */ + private void resetQuestInitialization() { + questInitStartTime = 0.0; + consecutiveValidQuestUpdates = 0; + lastQuestInitPose = null; + questInitialized = false; + } + + /** Resets all AprilTag initialization state variables. */ + private void resetTagInitialization() { + tagInitStartTime = 0.0; + consecutiveValidTagUpdates = 0; + lastTagInitPose = null; + tagInitialized = false; + } + + /** + * Determines whether Quest should be preferred as the primary pose source. Considers + * initialization status, match state, and pose validity. + * + * @return true if Quest should be the primary source + */ + private boolean shouldPreferQuest() { + if (!questInitialized) return false; + if (!tagInitialized) return true; + + boolean isMidMatch = + DriverStation.isEnabled() && Timer.getMatchTime() > MATCH_STARTUP_PERIOD_SECONDS; + + if (isMidMatch + && Timer.getTimestamp() - questInitStartTime + < QUEST_INIT_TIMEOUT * QUEST_INIT_GRACE_MULTIPLIER) { + return false; } - // Handle source availability - if (oculusSource.isConnected() && questInitialized) { - // If we have a good Quest connection and it's initialized, prefer it - state.transitionTo(LocalizationState.State.QUEST_PRIMARY); - } else if (tagSource.isConnected() && tagInitialized) { - // Fall back to AprilTags if available and initialized - state.transitionTo(LocalizationState.State.TAG_BACKUP); - } else { - // No valid sources available - state.transitionTo(LocalizationState.State.EMERGENCY); - Logger.recordOutput( - "PoseEstimator/Event", "No valid pose sources available - entering emergency mode"); + return validatePose(oculusSource.getCurrentPose(), false); + } + + // -------------------- Logging Methods -------------------- + /** Logs basic system status information including state and connection status. */ + private void logSystemStatus() { + Logger.recordOutput("LocalizationFusion/State", state.getCurrentState().getDescription()); + Logger.recordOutput("LocalizationFusion/OculusConnected", oculusSource.isConnected()); + Logger.recordOutput("LocalizationFusion/AprilTagConnected", tagSource.isConnected()); + } + + /** Logs detailed system status including initialization states and update counts. */ + private void logDetailedStatus() { + Logger.recordOutput("LocalizationFusion/QuestInitialized", questInitialized); + Logger.recordOutput("LocalizationFusion/TagInitialized", tagInitialized); + Logger.recordOutput("LocalizationFusion/QuestHadCalibration", hadPreviousCalibration); + + if (questInitialized) { + Logger.recordOutput("LocalizationFusion/QuestValidUpdates", consecutiveValidQuestUpdates); + Logger.recordOutput("LocalizationFusion/LastQuestUpdate", lastQuestUpdate); + } + if (tagInitialized) { + Logger.recordOutput("LocalizationFusion/TagValidUpdates", consecutiveValidTagUpdates); } } + /** + * Logs state transitions with descriptive messages. + * + * @param from The previous state + * @param to The new state + */ @Override public void logTransition(LocalizationState.State from, LocalizationState.State to) { Logger.recordOutput( - "PoseEstimator/StateTransition", + "LocalizationFusion/StateTransition", String.format("State transition: %s -> %s", from.name(), to.name())); } } diff --git a/src/main/java/frc/alotobots/library/subsystems/vision/localizationfusion/LocalizationFusionConstants.java b/src/main/java/frc/alotobots/library/subsystems/vision/localizationfusion/LocalizationFusionConstants.java new file mode 100644 index 00000000..e0e52ab8 --- /dev/null +++ b/src/main/java/frc/alotobots/library/subsystems/vision/localizationfusion/LocalizationFusionConstants.java @@ -0,0 +1,83 @@ +/* +* ALOTOBOTS - FRC Team 5152 + https://github.com/5152Alotobots +* Copyright (C) 2024 ALOTOBOTS +* +* This program is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* Source code must be publicly available on GitHub or an alternative web accessible site +*/ +package frc.alotobots.library.subsystems.vision.localizationfusion; + +import lombok.experimental.UtilityClass; + +/** + * Constants used by the LocalizationFusion subsystem for robot pose estimation and tracking. Groups + * related constants into inner classes for better organization. + */ +@UtilityClass +public class LocalizationFusionConstants { + + /** + * Constants related to pose validation and thresholds. Defines distance and rotation thresholds + * for validating poses from different sources. + */ + @UtilityClass + public static class ValidationThresholds { + /** Maximum acceptable difference between AprilTag and Quest poses for validation (meters). */ + public static final double APRILTAG_VALIDATION_THRESHOLD = 1.0; + + /** Stricter threshold used during initialization phase for validating poses (meters). */ + public static final double INIT_VALIDATION_THRESHOLD = 0.3; + + /** Maximum allowed pose change during disabled state to trigger recalibration (meters). */ + public static final double DISABLED_RECALIBRATION_THRESHOLD = 0.5; + + /** Maximum allowed rotation change between poses (degrees). */ + public static final double MAX_ROTATION_CHANGE_DEGREES = 45.0; + } + + /** + * Constants related to timing and update intervals. Defines various timeouts, intervals, and + * timing windows used in the localization system. + */ + @UtilityClass + public static class Timing { + /** Update interval matching Quest's native 120Hz update rate (seconds). */ + public static final double POSE_UPDATE_INTERVAL = 1.0 / 120.0; + + /** Time required for Quest initialization to complete (seconds). */ + public static final double QUEST_INIT_TIMEOUT = 2.0; + + /** Time required for AprilTag initialization to complete (seconds). */ + public static final double TAG_INIT_TIMEOUT = 1.0; + + /** Minimum time required to validate initial pose stability (seconds). */ + public static final double INITIAL_POSE_STABILITY_TIME = 3.0; + + /** Maximum time to wait for reset sequence to complete (seconds). */ + public static final double RESET_TIMEOUT = 5.0; + + /** Time after match start to consider it in mid-match state (seconds). */ + public static final double MATCH_STARTUP_PERIOD_SECONDS = 5.0; + } + + /** + * Constants related to initialization requirements and validation counts. Defines minimum update + * counts and multipliers for system initialization. + */ + @UtilityClass + public static class InitializationRequirements { + /** Minimum number of valid Quest updates required for successful initialization. */ + public static final int MIN_QUEST_VALID_UPDATES = 10; + + /** Minimum number of valid AprilTag updates required for successful initialization. */ + public static final int MIN_TAG_VALID_UPDATES = 3; + + /** Multiplier for extended Quest initialization grace period. */ + public static final double QUEST_INIT_GRACE_MULTIPLIER = 3.0; + } +} diff --git a/src/main/java/frc/alotobots/library/subsystems/vision/oculus/OculusSubsystem.java b/src/main/java/frc/alotobots/library/subsystems/vision/oculus/OculusSubsystem.java index e43d0620..f4602b55 100644 --- a/src/main/java/frc/alotobots/library/subsystems/vision/oculus/OculusSubsystem.java +++ b/src/main/java/frc/alotobots/library/subsystems/vision/oculus/OculusSubsystem.java @@ -93,6 +93,8 @@ public class OculusSubsystem extends SubsystemBase implements PoseSource { /** Current robot pose estimate */ private Pose2d currentPose = null; + private double lastQuestUpdateTime = Timer.getTimestamp(); + /** * Creates a new OculusSubsystem. * @@ -372,14 +374,20 @@ public boolean ping() { /** * {@inheritDoc} * - *

Quest connection requires: - Valid timestamps - Recent updates (within timeout) - Consistent - * update rate + *

Quest connection is determined by checking if we've received any new Quest timestamp updates + * within our timeout window. Small variations in update timing are allowed, only triggering + * disconnect on significant delays. */ @Override public boolean isConnected() { - double questTimeDelta = currentTimestamp - lastTimestamp; - boolean timeoutValid = (questTimeDelta >= 0) && (questTimeDelta < CONNECTION_TIMEOUT); - return timeoutValid; + // If timestamp has changed since last check, update our last update time + if (inputs.timestamp != lastTimestamp) { + lastQuestUpdateTime = Timer.getTimestamp(); + } + + // Only consider disconnected if we haven't seen ANY new timestamps + // for longer than our timeout period + return (Timer.getTimestamp() - lastQuestUpdateTime) < CONNECTION_TIMEOUT; } /** diff --git a/src/main/java/frc/alotobots/library/subsystems/vision/oculus/io/OculusIO.java b/src/main/java/frc/alotobots/library/subsystems/vision/oculus/io/OculusIO.java index 9ddbbdb7..20d44949 100644 --- a/src/main/java/frc/alotobots/library/subsystems/vision/oculus/io/OculusIO.java +++ b/src/main/java/frc/alotobots/library/subsystems/vision/oculus/io/OculusIO.java @@ -29,13 +29,13 @@ public static class OculusIOInputs { public float[] eulerAngles = new float[] {0.0f, 0.0f, 0.0f}; /** Current timestamp from the Oculus */ - public double timestamp = 0.0; + public double timestamp = -1.0; /** Frame counter from the Oculus */ - public int frameCount = 0; + public int frameCount = -1; /** Battery level percentage */ - public double batteryPercent = 0.0; + public double batteryPercent = -1.0; /** Current MISO (Master In Slave Out) value */ public int misoValue = 0; diff --git a/src/main/java/frc/alotobots/library/subsystems/vision/oculus/io/OculusIOReal.java b/src/main/java/frc/alotobots/library/subsystems/vision/oculus/io/OculusIOReal.java index 6bc7daf6..f1e0b7f9 100644 --- a/src/main/java/frc/alotobots/library/subsystems/vision/oculus/io/OculusIOReal.java +++ b/src/main/java/frc/alotobots/library/subsystems/vision/oculus/io/OculusIOReal.java @@ -54,15 +54,15 @@ public OculusIOReal() { nt4Table = NetworkTableInstance.getDefault().getTable("questnav"); questMiso = nt4Table.getIntegerTopic("miso").subscribe(0); questMosi = nt4Table.getIntegerTopic("mosi").publish(); - questFrameCount = nt4Table.getIntegerTopic("frameCount").subscribe(0); - questTimestamp = nt4Table.getDoubleTopic("timestamp").subscribe(0.0); + questFrameCount = nt4Table.getIntegerTopic("frameCount").subscribe(-1); + questTimestamp = nt4Table.getDoubleTopic("timestamp").subscribe(-1.0); questPosition = nt4Table.getFloatArrayTopic("position").subscribe(new float[] {0.0f, 0.0f, 0.0f}); questQuaternion = nt4Table.getFloatArrayTopic("quaternion").subscribe(new float[] {0.0f, 0.0f, 0.0f, 0.0f}); questEulerAngles = nt4Table.getFloatArrayTopic("eulerAngles").subscribe(new float[] {0.0f, 0.0f, 0.0f}); - questBatteryPercent = nt4Table.getDoubleTopic("batteryPercent").subscribe(0.0); + questBatteryPercent = nt4Table.getDoubleTopic("batteryPercent").subscribe(-1.0); resetPosePub = nt4Table.getDoubleArrayTopic("resetpose").publish(); } diff --git a/src/main/java/frc/alotobots/library/subsystems/vision/oculus/io/OculusIOSim.java b/src/main/java/frc/alotobots/library/subsystems/vision/oculus/io/OculusIOSim.java index aef56094..8a2dacf1 100644 --- a/src/main/java/frc/alotobots/library/subsystems/vision/oculus/io/OculusIOSim.java +++ b/src/main/java/frc/alotobots/library/subsystems/vision/oculus/io/OculusIOSim.java @@ -20,8 +20,8 @@ public void updateInputs(OculusIOInputs inputs) { inputs.position = new float[] {0.0f, 0.0f, 0.0f}; inputs.quaternion = new float[] {1.0f, 0.0f, 0.0f, 0.0f}; inputs.eulerAngles = new float[] {0.0f, 0.0f, 0.0f}; - inputs.timestamp = 0.0; - inputs.frameCount = 0; + inputs.timestamp = -1.0; + inputs.frameCount = -1; inputs.batteryPercent = 100.0; inputs.misoValue = 0; } From 391d5341e0a9594424e2a0522fa3671393a98ee7 Mon Sep 17 00:00:00 2001 From: Sean Ernstes Date: Fri, 3 Jan 2025 17:09:45 -0500 Subject: [PATCH 17/23] edit some constants --- .../vision/localizationfusion/LocalizationFusion.java | 3 +-- .../localizationfusion/LocalizationFusionConstants.java | 6 +++--- 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/alotobots/library/subsystems/vision/localizationfusion/LocalizationFusion.java b/src/main/java/frc/alotobots/library/subsystems/vision/localizationfusion/LocalizationFusion.java index d2af7605..c098b1f7 100644 --- a/src/main/java/frc/alotobots/library/subsystems/vision/localizationfusion/LocalizationFusion.java +++ b/src/main/java/frc/alotobots/library/subsystems/vision/localizationfusion/LocalizationFusion.java @@ -112,8 +112,7 @@ public class LocalizationFusion extends SubsystemBase implements StateTransition /** Indicates if initial pose validation is complete. */ private boolean initialPoseValidated = false; - // TODO: Also, we should only reset for a significant change in pose while disabled until the - // TODO: match starts. Otherwise, when we transition from auto to teleop, we would reset + // -------------------- Constructor -------------------- /** diff --git a/src/main/java/frc/alotobots/library/subsystems/vision/localizationfusion/LocalizationFusionConstants.java b/src/main/java/frc/alotobots/library/subsystems/vision/localizationfusion/LocalizationFusionConstants.java index e0e52ab8..dc2fd245 100644 --- a/src/main/java/frc/alotobots/library/subsystems/vision/localizationfusion/LocalizationFusionConstants.java +++ b/src/main/java/frc/alotobots/library/subsystems/vision/localizationfusion/LocalizationFusionConstants.java @@ -34,10 +34,10 @@ public static class ValidationThresholds { public static final double INIT_VALIDATION_THRESHOLD = 0.3; /** Maximum allowed pose change during disabled state to trigger recalibration (meters). */ - public static final double DISABLED_RECALIBRATION_THRESHOLD = 0.5; + public static final double DISABLED_RECALIBRATION_THRESHOLD = 0.05; /** Maximum allowed rotation change between poses (degrees). */ - public static final double MAX_ROTATION_CHANGE_DEGREES = 45.0; + public static final double MAX_ROTATION_CHANGE_DEGREES = 35.0; } /** @@ -56,7 +56,7 @@ public static class Timing { public static final double TAG_INIT_TIMEOUT = 1.0; /** Minimum time required to validate initial pose stability (seconds). */ - public static final double INITIAL_POSE_STABILITY_TIME = 3.0; + public static final double INITIAL_POSE_STABILITY_TIME = 15.0; /** Maximum time to wait for reset sequence to complete (seconds). */ public static final double RESET_TIMEOUT = 5.0; From 654ed1f85ea5a6d90dbe6268c2f79c985c51d824 Mon Sep 17 00:00:00 2001 From: Sean Ernstes Date: Sat, 4 Jan 2025 00:09:11 -0500 Subject: [PATCH 18/23] Alerts for everything! --- .../LocalizationFusion.java | 149 +++++++++++++++++- 1 file changed, 147 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/alotobots/library/subsystems/vision/localizationfusion/LocalizationFusion.java b/src/main/java/frc/alotobots/library/subsystems/vision/localizationfusion/LocalizationFusion.java index c098b1f7..d0a21f42 100644 --- a/src/main/java/frc/alotobots/library/subsystems/vision/localizationfusion/LocalizationFusion.java +++ b/src/main/java/frc/alotobots/library/subsystems/vision/localizationfusion/LocalizationFusion.java @@ -24,6 +24,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.alotobots.library.subsystems.vision.oculus.util.OculusPoseSource; import frc.alotobots.library.subsystems.vision.photonvision.apriltag.util.AprilTagPoseSource; +import frc.alotobots.util.Elastic; import org.littletonrobotics.junction.Logger; /** @@ -112,8 +113,6 @@ public class LocalizationFusion extends SubsystemBase implements StateTransition /** Indicates if initial pose validation is complete. */ private boolean initialPoseValidated = false; - - // -------------------- Constructor -------------------- /** * Creates a new LocalizationFusion subsystem. @@ -229,6 +228,12 @@ private void handleResettingState() { if (resetStartTime > 0 && Timer.getTimestamp() - resetStartTime > RESET_TIMEOUT) { Logger.recordOutput( "LocalizationFusion/Event", "Reset sequence timed out - switching to emergency mode"); + Elastic.sendAlert( + new Elastic.ElasticNotification() + .withLevel(Elastic.ElasticNotification.NotificationLevel.ERROR) + .withTitle("Reset Failed") + .withDescription("Reset sequence timed out - Check vision systems") + .withDisplaySeconds(10.0)); resetStartTime = 0.0; resetQuestInitialization(); state.transitionTo(LocalizationState.State.EMERGENCY); @@ -337,12 +342,24 @@ private void handleTagBackupState(double currentTime) { Logger.recordOutput( "LocalizationFusion/Event", "Quest Reconnected; Pose Validated - maintaining Quest tracking"); + Elastic.sendAlert( + new Elastic.ElasticNotification() + .withLevel(Elastic.ElasticNotification.NotificationLevel.INFO) + .withTitle("Quest Reconnected") + .withDescription("Quest tracking restored - Pose validated") + .withDisplaySeconds(3.0)); return; } else { // Re-init as we couldn't validate pose Logger.recordOutput( "LocalizationFusion/Event", "Quest Reconnected; Pose Failed to Validate - reinitializing Quest tracking"); + Elastic.sendAlert( + new Elastic.ElasticNotification() + .withLevel(Elastic.ElasticNotification.NotificationLevel.WARNING) + .withTitle("Quest Validation Failed") + .withDescription("Quest reconnected but pose invalid - Reinitializing") + .withDisplaySeconds(3.0)); handleQuestInitialization(); } } @@ -382,6 +399,12 @@ private void handleQuestInitialization() { lastQuestDisconnectTime = Timer.getTimestamp(); hadPreviousCalibration = true; lastQuestInitPose = oculusSource.getCurrentPose(); + Elastic.sendAlert( + new Elastic.ElasticNotification() + .withLevel(Elastic.ElasticNotification.NotificationLevel.ERROR) + .withTitle("Quest Disconnected") + .withDescription("Quest tracking system disconnected - Check USB connection") + .withDisplaySeconds(10.0)); } resetQuestInitialization(); return; @@ -417,12 +440,24 @@ private void handleQuestInitialization() { if (consecutiveValidQuestUpdates >= MIN_QUEST_VALID_UPDATES && initDuration >= QUEST_INIT_TIMEOUT) { questInitialized = true; + Elastic.sendAlert( + new Elastic.ElasticNotification() + .withLevel(Elastic.ElasticNotification.NotificationLevel.INFO) + .withTitle("Quest Initialized") + .withDescription("Quest tracking system successfully initialized") + .withDisplaySeconds(3.0)); Pose2d referencePose = getValidReferencePose(); if (referencePose != null && resetToPose(referencePose)) { state.transitionTo(LocalizationState.State.RESETTING); resetStartTime = Timer.getTimestamp(); } } else if (initDuration > QUEST_INIT_TIMEOUT * QUEST_INIT_GRACE_MULTIPLIER) { + Elastic.sendAlert( + new Elastic.ElasticNotification() + .withLevel(Elastic.ElasticNotification.NotificationLevel.ERROR) + .withTitle("Quest Init Failed") + .withDescription("Quest initialization timed out - Check headset") + .withDisplaySeconds(10.0)); resetQuestInitialization(); } } @@ -433,6 +468,14 @@ private void handleQuestInitialization() { */ private void handleTagInitialization() { if (!tagSource.isConnected()) { + if (tagInitialized) { + Elastic.sendAlert( + new Elastic.ElasticNotification() + .withLevel(Elastic.ElasticNotification.NotificationLevel.ERROR) + .withTitle("AprilTag System Disconnected") + .withDescription("AprilTag tracking system disconnected - Check camera connection") + .withDisplaySeconds(10.0)); + } resetTagInitialization(); return; } @@ -458,10 +501,23 @@ private void handleTagInitialization() { double initDuration = Timer.getTimestamp() - tagInitStartTime; if (consecutiveValidTagUpdates >= MIN_TAG_VALID_UPDATES && initDuration >= TAG_INIT_TIMEOUT) { tagInitialized = true; + Elastic.sendAlert( + new Elastic.ElasticNotification() + .withLevel(Elastic.ElasticNotification.NotificationLevel.INFO) + .withTitle("AprilTag System Ready") + .withDescription("AprilTag tracking system successfully initialized") + .withDisplaySeconds(3.0)); + if (!questInitialized || !shouldPreferQuest()) { state.transitionTo(LocalizationState.State.TAG_BACKUP); } } else if (initDuration > TAG_INIT_TIMEOUT * QUEST_INIT_GRACE_MULTIPLIER) { + Elastic.sendAlert( + new Elastic.ElasticNotification() + .withLevel(Elastic.ElasticNotification.NotificationLevel.ERROR) + .withTitle("AprilTag Init Failed") + .withDescription("AprilTag initialization timed out - Check camera view of tags") + .withDisplaySeconds(3.0)); resetTagInitialization(); } } @@ -489,6 +545,12 @@ private void initializeConnectionState() { if (isMidMatchReboot()) { Logger.recordOutput( "LocalizationFusion/Event", "Mid-match reboot detected - maintaining Quest tracking"); + Elastic.sendAlert( + new Elastic.ElasticNotification() + .withLevel(Elastic.ElasticNotification.NotificationLevel.WARNING) + .withTitle("Mid-Match Reboot") + .withDescription("Maintaining Quest tracking after reboot") + .withDisplaySeconds(3.0)); // Skip initialization and trust Quest's maintained tracking questInitialized = true; hadPreviousCalibration = true; @@ -515,6 +577,12 @@ private void handleDriverStationConnection() { if (!wasConnected && isConnected) { if (!hadInitialConnection) { + Elastic.sendAlert( + new Elastic.ElasticNotification() + .withLevel(Elastic.ElasticNotification.NotificationLevel.INFO) + .withTitle("Initial Connection") + .withDescription("Driver Station connected - Starting pose validation") + .withDisplaySeconds(3.0)); Pose2d tagPose = tagSource.getCurrentPose(); if (tagPose != null) { lastValidatedPose = tagPose; @@ -525,6 +593,12 @@ private void handleDriverStationConnection() { } else { Logger.recordOutput( "LocalizationFusion/Event", "DriverStation reconnected - maintaining current pose"); + Elastic.sendAlert( + new Elastic.ElasticNotification() + .withLevel(Elastic.ElasticNotification.NotificationLevel.WARNING) + .withTitle("Connection Restored") + .withDescription("Driver Station reconnected - Maintaining current pose") + .withDisplaySeconds(3.0)); } } @@ -554,6 +628,12 @@ private void handleDisabledPoseValidation() { Logger.recordOutput( "LocalizationFusion/Event", "Initial pose validated after stability period - initiating reset"); + Elastic.sendAlert( + new Elastic.ElasticNotification() + .withLevel(Elastic.ElasticNotification.NotificationLevel.INFO) + .withTitle("Initial Pose Validated") + .withDescription("Robot position confirmed stable - Starting tracking") + .withDisplaySeconds(3.0)); if (resetToPose(currentTagPose)) { state.transitionTo(LocalizationState.State.RESETTING); initialPoseValidated = true; @@ -562,6 +642,12 @@ private void handleDisabledPoseValidation() { Logger.recordOutput( "LocalizationFusion/Event", "Pose changed during validation - restarting stability timer"); + Elastic.sendAlert( + new Elastic.ElasticNotification() + .withLevel(Elastic.ElasticNotification.NotificationLevel.WARNING) + .withTitle("Position Changed") + .withDescription("Robot moved during validation - Restarting stability check") + .withDisplaySeconds(3.0)); initialPoseValidationStartTime = Timer.getTimestamp(); lastValidatedPose = currentTagPose; } @@ -581,6 +667,14 @@ private void handleDisabledPoseValidation() { String.format( "Significant pose change detected while disabled (%.2fm) - recalibrating", poseChange)); + Elastic.sendAlert( + new Elastic.ElasticNotification() + .withLevel(Elastic.ElasticNotification.NotificationLevel.WARNING) + .withTitle("Position Changed") + .withDescription( + String.format( + "Robot moved %.2f meters while disabled - Recalibrating", poseChange)) + .withDisplaySeconds(3.0)); initialPoseValidationStartTime = Timer.getTimestamp(); lastValidatedPose = currentTagPose; @@ -677,6 +771,12 @@ public boolean requestResetOculusPoseViaAprilTags() { if (currentTagPose == null) { Logger.recordOutput( "LocalizationFusion/Event", "Manual reset failed - no AprilTag pose available"); + Elastic.sendAlert( + new Elastic.ElasticNotification() + .withLevel(Elastic.ElasticNotification.NotificationLevel.ERROR) + .withTitle("Reset Failed") + .withDescription("Manual reset failed - No AprilTags visible") + .withDisplaySeconds(3.0)); return false; } @@ -699,6 +799,12 @@ public boolean requestResetOculusPoseViaAprilTags() { public boolean requestResetOculusPose(Pose2d targetPose) { if (targetPose == null) { Logger.recordOutput("LocalizationFusion/Event", "Manual reset failed - null pose provided"); + Elastic.sendAlert( + new Elastic.ElasticNotification() + .withLevel(Elastic.ElasticNotification.NotificationLevel.ERROR) + .withTitle("Reset Failed") + .withDescription("Manual reset failed - Invalid pose provided") + .withDisplaySeconds(3.0)); return false; } @@ -785,5 +891,44 @@ public void logTransition(LocalizationState.State from, LocalizationState.State Logger.recordOutput( "LocalizationFusion/StateTransition", String.format("State transition: %s -> %s", from.name(), to.name())); + + // Send appropriate notifications based on state transitions + switch (to) { + case EMERGENCY: + Elastic.sendAlert( + new Elastic.ElasticNotification() + .withLevel(Elastic.ElasticNotification.NotificationLevel.ERROR) + .withTitle("Localization Emergency") + .withDescription( + "Localization system entered emergency state - Check vision systems") + .withDisplaySeconds(10.0)); + break; + case RESETTING: + Elastic.sendAlert( + new Elastic.ElasticNotification() + .withLevel(Elastic.ElasticNotification.NotificationLevel.WARNING) + .withTitle("Localization Reset") + .withDescription("Realigning Quest and AprilTag poses") + .withDisplaySeconds(3.0)); + break; + case QUEST_PRIMARY: + if (from == LocalizationState.State.TAG_BACKUP) { + Elastic.sendAlert( + new Elastic.ElasticNotification() + .withLevel(Elastic.ElasticNotification.NotificationLevel.INFO) + .withTitle("Quest Tracking Restored") + .withDescription("Switched back to primary Quest-based tracking") + .withDisplaySeconds(3.0)); + } + break; + case TAG_BACKUP: + Elastic.sendAlert( + new Elastic.ElasticNotification() + .withLevel(Elastic.ElasticNotification.NotificationLevel.WARNING) + .withTitle("Using Backup Tracking") + .withDescription("Switched to AprilTag-based tracking") + .withDisplaySeconds(10.0)); + break; + } } } From b4596403e954373faed11fc8b3ed1f560826057b Mon Sep 17 00:00:00 2001 From: Sean Ernstes Date: Sat, 4 Jan 2025 00:50:07 -0500 Subject: [PATCH 19/23] New command for reset that runs when disabled --- .../java/frc/alotobots/RobotContainer.java | 4 +- .../commands/RequestPositionResetViaTags.java | 46 +++++++++++++++++++ 2 files changed, 48 insertions(+), 2 deletions(-) create mode 100644 src/main/java/frc/alotobots/library/subsystems/vision/localizationfusion/commands/RequestPositionResetViaTags.java diff --git a/src/main/java/frc/alotobots/RobotContainer.java b/src/main/java/frc/alotobots/RobotContainer.java index 921057f0..f34309ed 100644 --- a/src/main/java/frc/alotobots/RobotContainer.java +++ b/src/main/java/frc/alotobots/RobotContainer.java @@ -26,6 +26,7 @@ import frc.alotobots.library.subsystems.swervedrive.io.*; import frc.alotobots.library.subsystems.swervedrive.util.PathPlannerManager; import frc.alotobots.library.subsystems.vision.localizationfusion.LocalizationFusion; +import frc.alotobots.library.subsystems.vision.localizationfusion.commands.RequestPositionResetViaTags; import frc.alotobots.library.subsystems.vision.oculus.OculusSubsystem; import frc.alotobots.library.subsystems.vision.oculus.io.*; import frc.alotobots.library.subsystems.vision.oculus.util.OculusPoseSource; @@ -179,8 +180,7 @@ private void configureLogicCommands() { pathfindToBestObjectButton.onTrue( new PathfindToBestObject( objectDetectionSubsystem, swerveDriveSubsystem, pathPlannerManager, NOTE)); - testButton.onTrue( - localizationFusion.runOnce(localizationFusion::requestResetOculusPoseViaAprilTags)); + testButton.onTrue(new RequestPositionResetViaTags(localizationFusion)); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/alotobots/library/subsystems/vision/localizationfusion/commands/RequestPositionResetViaTags.java b/src/main/java/frc/alotobots/library/subsystems/vision/localizationfusion/commands/RequestPositionResetViaTags.java new file mode 100644 index 00000000..b273025c --- /dev/null +++ b/src/main/java/frc/alotobots/library/subsystems/vision/localizationfusion/commands/RequestPositionResetViaTags.java @@ -0,0 +1,46 @@ +/* +* ALOTOBOTS - FRC Team 5152 + https://github.com/5152Alotobots +* Copyright (C) 2024 ALOTOBOTS +* +* This program is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* Source code must be publicly available on GitHub or an alternative web accessible site +*/ +package frc.alotobots.library.subsystems.vision.localizationfusion.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.alotobots.library.subsystems.vision.localizationfusion.LocalizationFusion; + +public class RequestPositionResetViaTags extends Command { + boolean reset = false; + LocalizationFusion localizationFusion; + + public RequestPositionResetViaTags(LocalizationFusion localizationFusion) { + this.localizationFusion = localizationFusion; + addRequirements(localizationFusion); + } + + @Override + public void initialize() { + reset = false; + } + + @Override + public void execute() { + reset = localizationFusion.requestResetOculusPoseViaAprilTags(); + } + + @Override + public boolean isFinished() { + return reset; + } + + @Override + public boolean runsWhenDisabled() { + return true; + } +} From 5413fa5b01d582e9e841d91d3e0d30f86f045416 Mon Sep 17 00:00:00 2001 From: Sean Ernstes Date: Sat, 4 Jan 2025 00:50:48 -0500 Subject: [PATCH 20/23] Only allow disabled pose autoresets if match has not started --- .../localizationfusion/LocalizationFusion.java | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/alotobots/library/subsystems/vision/localizationfusion/LocalizationFusion.java b/src/main/java/frc/alotobots/library/subsystems/vision/localizationfusion/LocalizationFusion.java index d0a21f42..1ab5f69d 100644 --- a/src/main/java/frc/alotobots/library/subsystems/vision/localizationfusion/LocalizationFusion.java +++ b/src/main/java/frc/alotobots/library/subsystems/vision/localizationfusion/LocalizationFusion.java @@ -611,10 +611,15 @@ private void handleDriverStationConnection() { // -------------------- Validation Methods -------------------- /** - * Handles pose validation during the disabled state. Monitors pose stability and triggers - * recalibration if significant changes are detected. + * Handles pose validation during the disabled state. Only performs validation and resets before + * the match starts to avoid disrupting auto-to-teleop transitions. */ private void handleDisabledPoseValidation() { + // Skip disabled validation if match has started + if (DriverStation.getMatchTime() > 0) { + return; + } + Pose2d currentTagPose = tagSource.getCurrentPose(); if (currentTagPose == null) return; @@ -657,6 +662,7 @@ private void handleDisabledPoseValidation() { return; } + // Only check for pose changes requiring recalibration before match starts if (lastValidatedPose != null) { double poseChange = currentTagPose.getTranslation().getDistance(lastValidatedPose.getTranslation()); @@ -665,12 +671,12 @@ private void handleDisabledPoseValidation() { Logger.recordOutput( "LocalizationFusion/Event", String.format( - "Significant pose change detected while disabled (%.2fm) - recalibrating", + "Significant pre-match pose change detected while disabled (%.2fm) - recalibrating", poseChange)); Elastic.sendAlert( new Elastic.ElasticNotification() .withLevel(Elastic.ElasticNotification.NotificationLevel.WARNING) - .withTitle("Position Changed") + .withTitle("Pre-Match Position Changed") .withDescription( String.format( "Robot moved %.2f meters while disabled - Recalibrating", poseChange)) From 4d18753c786ac93606b721ba4302aa7db9ffd414 Mon Sep 17 00:00:00 2001 From: Sean Ernstes Date: Sat, 4 Jan 2025 01:25:23 -0500 Subject: [PATCH 21/23] runs when disabled --- .../commands/RequestPositionResetViaTags.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/alotobots/library/subsystems/vision/localizationfusion/commands/RequestPositionResetViaTags.java b/src/main/java/frc/alotobots/library/subsystems/vision/localizationfusion/commands/RequestPositionResetViaTags.java index b273025c..5b0fb5cf 100644 --- a/src/main/java/frc/alotobots/library/subsystems/vision/localizationfusion/commands/RequestPositionResetViaTags.java +++ b/src/main/java/frc/alotobots/library/subsystems/vision/localizationfusion/commands/RequestPositionResetViaTags.java @@ -39,8 +39,8 @@ public boolean isFinished() { return reset; } - @Override - public boolean runsWhenDisabled() { - return true; - } + @Override + public boolean runsWhenDisabled() { + return true; + } } From 7f94b43cfeb9ae4105e7f4b83b2962b2b30c5e4f Mon Sep 17 00:00:00 2001 From: Sean Ernstes Date: Sat, 4 Jan 2025 02:41:47 -0500 Subject: [PATCH 22/23] Use selected auto's starting pose until apriltags --- .../java/frc/alotobots/RobotContainer.java | 64 ++++--- .../swervedrive/util/PathPlannerManager.java | 7 +- .../LocalizationFusion.java | 168 +++++++++++++++++- 3 files changed, 205 insertions(+), 34 deletions(-) diff --git a/src/main/java/frc/alotobots/RobotContainer.java b/src/main/java/frc/alotobots/RobotContainer.java index f34309ed..b02ef6ec 100644 --- a/src/main/java/frc/alotobots/RobotContainer.java +++ b/src/main/java/frc/alotobots/RobotContainer.java @@ -48,9 +48,10 @@ public class RobotContainer { private final ObjectDetectionSubsystem objectDetectionSubsystem; private final BlingSubsystem blingSubsystem; private final PathPlannerManager pathPlannerManager; - private final LoggedDashboardChooser autoChooser; + private LoggedDashboardChooser autoChooser; public RobotContainer() { + switch (Constants.currentMode) { case REAL: // Real robot hardware initialization @@ -61,6 +62,8 @@ public RobotContainer() { new ModuleIOTalonFX(ModulePosition.FRONT_RIGHT.index), new ModuleIOTalonFX(ModulePosition.BACK_LEFT.index), new ModuleIOTalonFX(ModulePosition.BACK_RIGHT.index)); + pathPlannerManager = new PathPlannerManager(swerveDriveSubsystem); + configureAutoChooser(); oculusSubsystem = new OculusSubsystem(new OculusIOReal()); aprilTagSubsystem = @@ -74,8 +77,10 @@ public RobotContainer() { localizationFusion = new LocalizationFusion( - swerveDriveSubsystem::addVisionMeasurement, oculusPoseSource, aprilTagPoseSource); - pathPlannerManager = new PathPlannerManager(swerveDriveSubsystem, localizationFusion); + swerveDriveSubsystem::addVisionMeasurement, + oculusPoseSource, + aprilTagPoseSource, + autoChooser); objectDetectionSubsystem = new ObjectDetectionSubsystem( @@ -93,6 +98,8 @@ public RobotContainer() { new ModuleIOSim(ModulePosition.FRONT_RIGHT.index), new ModuleIOSim(ModulePosition.BACK_LEFT.index), new ModuleIOSim(ModulePosition.BACK_RIGHT.index)); + pathPlannerManager = new PathPlannerManager(swerveDriveSubsystem); + configureAutoChooser(); oculusSubsystem = new OculusSubsystem(new OculusIOSim()); aprilTagSubsystem = @@ -107,8 +114,10 @@ public RobotContainer() { aprilTagPoseSource = new AprilTagPoseSource(aprilTagSubsystem); localizationFusion = new LocalizationFusion( - swerveDriveSubsystem::addVisionMeasurement, oculusPoseSource, aprilTagPoseSource); - pathPlannerManager = new PathPlannerManager(swerveDriveSubsystem, localizationFusion); + swerveDriveSubsystem::addVisionMeasurement, + oculusPoseSource, + aprilTagPoseSource, + autoChooser); objectDetectionSubsystem = new ObjectDetectionSubsystem(swerveDriveSubsystem::getPose, new ObjectDetectionIO() {}); @@ -124,6 +133,8 @@ public RobotContainer() { new ModuleIO() {}, new ModuleIO() {}, new ModuleIO() {}); + pathPlannerManager = new PathPlannerManager(swerveDriveSubsystem); + configureAutoChooser(); oculusSubsystem = new OculusSubsystem(new OculusIO() {}); aprilTagSubsystem = new AprilTagSubsystem(new AprilTagIO() {}, new AprilTagIO() {}); @@ -133,15 +144,36 @@ public RobotContainer() { aprilTagPoseSource = new AprilTagPoseSource(aprilTagSubsystem); localizationFusion = new LocalizationFusion( - swerveDriveSubsystem::addVisionMeasurement, oculusPoseSource, aprilTagPoseSource); - pathPlannerManager = new PathPlannerManager(swerveDriveSubsystem, localizationFusion); + swerveDriveSubsystem::addVisionMeasurement, + oculusPoseSource, + aprilTagPoseSource, + autoChooser); objectDetectionSubsystem = new ObjectDetectionSubsystem(swerveDriveSubsystem::getPose, new ObjectDetectionIO() {}); blingSubsystem = new BlingSubsystem(new BlingIO() {}); break; } + configureDefaultCommands(); + configureLogicCommands(); + } + + private void configureDefaultCommands() { + swerveDriveSubsystem.setDefaultCommand(new DefaultDrive(swerveDriveSubsystem).getCommand()); + blingSubsystem.setDefaultCommand( + new NoAllianceWaiting(blingSubsystem).andThen(new SetToAllianceColor(blingSubsystem))); + } + + private void configureLogicCommands() { + driveFacingBestObjectButton.toggleOnTrue( + new DriveFacingBestObject(objectDetectionSubsystem, swerveDriveSubsystem, NOTE)); + pathfindToBestObjectButton.onTrue( + new PathfindToBestObject( + objectDetectionSubsystem, swerveDriveSubsystem, pathPlannerManager, NOTE)); + testButton.onTrue(new RequestPositionResetViaTags(localizationFusion).withTimeout(1)); + } + private void configureAutoChooser() { // Set up auto routines autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser()); @@ -163,24 +195,6 @@ public RobotContainer() { autoChooser.addOption( "Drive SysId (Dynamic Reverse)", swerveDriveSubsystem.sysIdDynamic(SysIdRoutine.Direction.kReverse)); - - configureDefaultCommands(); - configureLogicCommands(); - } - - private void configureDefaultCommands() { - swerveDriveSubsystem.setDefaultCommand(new DefaultDrive(swerveDriveSubsystem).getCommand()); - blingSubsystem.setDefaultCommand( - new NoAllianceWaiting(blingSubsystem).andThen(new SetToAllianceColor(blingSubsystem))); - } - - private void configureLogicCommands() { - driveFacingBestObjectButton.toggleOnTrue( - new DriveFacingBestObject(objectDetectionSubsystem, swerveDriveSubsystem, NOTE)); - pathfindToBestObjectButton.onTrue( - new PathfindToBestObject( - objectDetectionSubsystem, swerveDriveSubsystem, pathPlannerManager, NOTE)); - testButton.onTrue(new RequestPositionResetViaTags(localizationFusion)); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/alotobots/library/subsystems/swervedrive/util/PathPlannerManager.java b/src/main/java/frc/alotobots/library/subsystems/swervedrive/util/PathPlannerManager.java index 17c15818..92178c46 100644 --- a/src/main/java/frc/alotobots/library/subsystems/swervedrive/util/PathPlannerManager.java +++ b/src/main/java/frc/alotobots/library/subsystems/swervedrive/util/PathPlannerManager.java @@ -23,7 +23,6 @@ import frc.alotobots.AutoNamedCommands; import frc.alotobots.Constants; import frc.alotobots.library.subsystems.swervedrive.SwerveDriveSubsystem; -import frc.alotobots.library.subsystems.vision.localizationfusion.LocalizationFusion; import frc.alotobots.util.LocalADStarAK; import org.littletonrobotics.junction.Logger; @@ -33,18 +32,14 @@ */ public class PathPlannerManager { private final SwerveDriveSubsystem driveSubsystem; - private final LocalizationFusion localizationFusion; /** * Creates a new PathPlannerManager. * * @param driveSubsystem The swerve drive subsystem to control - * @param localizationFusion The localizationFusion instance to be able to reset oculus pose */ - public PathPlannerManager( - SwerveDriveSubsystem driveSubsystem, LocalizationFusion localizationFusion) { + public PathPlannerManager(SwerveDriveSubsystem driveSubsystem) { this.driveSubsystem = driveSubsystem; - this.localizationFusion = localizationFusion; configurePathPlanner(); } diff --git a/src/main/java/frc/alotobots/library/subsystems/vision/localizationfusion/LocalizationFusion.java b/src/main/java/frc/alotobots/library/subsystems/vision/localizationfusion/LocalizationFusion.java index 1ab5f69d..f55af342 100644 --- a/src/main/java/frc/alotobots/library/subsystems/vision/localizationfusion/LocalizationFusion.java +++ b/src/main/java/frc/alotobots/library/subsystems/vision/localizationfusion/LocalizationFusion.java @@ -16,16 +16,21 @@ import static frc.alotobots.library.subsystems.vision.localizationfusion.LocalizationFusionConstants.Timing.*; import static frc.alotobots.library.subsystems.vision.localizationfusion.LocalizationFusionConstants.ValidationThresholds.*; +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.commands.PathPlannerAuto; +import com.pathplanner.lib.util.FlippingUtil; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.alotobots.library.subsystems.vision.oculus.util.OculusPoseSource; import frc.alotobots.library.subsystems.vision.photonvision.apriltag.util.AprilTagPoseSource; import frc.alotobots.util.Elastic; import org.littletonrobotics.junction.Logger; +import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; /** * The LocalizationFusion subsystem manages robot pose estimation by fusing data from multiple @@ -48,6 +53,9 @@ public class LocalizationFusion extends SubsystemBase implements StateTransition /** State machine managing transitions between different localization states. */ private final LocalizationState state; + /** Auto chooser for fallback pose setting */ + LoggedDashboardChooser autoChooser; + /** Tab for drivers to ensure readiness */ private final ShuffleboardTab prematchTab = Shuffleboard.getTab("Prematch"); @@ -113,6 +121,15 @@ public class LocalizationFusion extends SubsystemBase implements StateTransition /** Indicates if initial pose validation is complete. */ private boolean initialPoseValidated = false; + /** Stores the previous and current auto selection state for comparison */ + private String previousAutoSelection = ""; + + private String currentAutoSelection = ""; + + // Separate flags for pose validation sources + private boolean hasTagValidatedPose = false; // Set only when we get AprilTag validation + private boolean hasAutoPose = false; // Set when we accept an auto pose + // -------------------- Constructor -------------------- /** * Creates a new LocalizationFusion subsystem. @@ -124,10 +141,12 @@ public class LocalizationFusion extends SubsystemBase implements StateTransition public LocalizationFusion( PoseVisionConsumer poseConsumer, OculusPoseSource oculusSource, - AprilTagPoseSource aprilTagSource) { + AprilTagPoseSource aprilTagSource, + LoggedDashboardChooser autoChooser) { this.poseConsumer = poseConsumer; this.oculusSource = oculusSource; this.tagSource = aprilTagSource; + this.autoChooser = autoChooser; this.state = new LocalizationState(this); initializeConnectionState(); setupShuffleboardLogging(); @@ -179,6 +198,46 @@ private void setupShuffleboardLogging() { .withPosition(0, 2); } + /** + * Gets the starting pose from the currently selected autonomous routine in PathPlanner. + * + * @return The starting Pose2d from the selected auto, or null if none available + */ + private Pose2d getAutoStartingPose() { + try { + String autoName = autoChooser.getSendableChooser().getSelected(); + if (autoName.equals("None")) { + Logger.recordOutput("LocalizationFusion/Event", "No auto selected"); + return null; + } + + Logger.recordOutput("LocalizationFusion/SelectedAuto", autoName); + + var autoPath = PathPlannerAuto.getPathGroupFromAutoFile(autoName); + if (autoPath.isEmpty()) { + Logger.recordOutput("LocalizationFusion/Event", "No paths found in auto: " + autoName); + return null; + } + + var startPose = autoPath.get(0).getStartingHolonomicPose(); + if (startPose.isPresent()) { + if (AutoBuilder.shouldFlip()) { + return FlippingUtil.flipFieldPose(startPose.get()); + } else { + return startPose.get(); + } + } else { + Logger.recordOutput( + "LocalizationFusion/Event", "No starting pose defined in auto: " + autoName); + return null; + } + } catch (Exception e) { + Logger.recordOutput( + "LocalizationFusion/Event", "Error getting auto starting pose: " + e.getMessage()); + return null; + } + } + /** * Periodic update method called by the command scheduler. Handles system state updates, source * initialization, and pose processing. @@ -187,6 +246,8 @@ private void setupShuffleboardLogging() { public void periodic() { logSystemStatus(); handleDriverStationConnection(); + updateAutoSelection(); // Automatically set pose to auto path start pose if we don't have any + // other valid poses // Handle initialization of both sources if (!questInitialized && oculusSource.isConnected()) { @@ -240,8 +301,21 @@ private void handleResettingState() { return; } + // If we have no tag pose, but Quest is connected and initialized Pose2d tagPose = tagSource.getCurrentPose(); if (tagPose == null) { + // If this was triggered by auto selection and Quest is ready, + // we can proceed without tag validation + if (!hasTagValidatedPose && oculusSource.isConnected() && !isResetInProgress()) { + Pose2d questPose = oculusSource.getCurrentPose(); + if (questPose != null) { + resetStartTime = 0.0; + lastQuestUpdate = Timer.getTimestamp(); + state.transitionTo(LocalizationState.State.QUEST_PRIMARY); + return; + } + } + if (resetStartTime == 0.0) { resetStartTime = Timer.getTimestamp(); } @@ -265,6 +339,7 @@ private void handleResettingState() { if (poseError <= INIT_VALIDATION_THRESHOLD) { resetStartTime = 0.0; lastQuestUpdate = Timer.getTimestamp(); + hasTagValidatedPose = true; // Now we have tag validation state.transitionTo(LocalizationState.State.QUEST_PRIMARY); } } @@ -277,11 +352,28 @@ private void handleEmergencyState() { if (tagSource.isConnected()) { Pose2d tagPose = tagSource.getCurrentPose(); if (tagPose != null) { + Logger.recordOutput( + "LocalizationFusion/Event", + "Valid AprilTag pose detected in EMERGENCY state - attempting recovery"); + if (oculusSource.isConnected() && resetToPose(tagPose)) { resetStartTime = Timer.getTimestamp(); state.transitionTo(LocalizationState.State.RESETTING); + Elastic.sendAlert( + new Elastic.ElasticNotification() + .withLevel(Elastic.ElasticNotification.NotificationLevel.INFO) + .withTitle("Recovery Attempt") + .withDescription("Valid AprilTags found - Attempting to restore tracking") + .withDisplaySeconds(3.0)); } else { + // If Quest isn't available, fall back to tag-only tracking state.transitionTo(LocalizationState.State.TAG_BACKUP); + Elastic.sendAlert( + new Elastic.ElasticNotification() + .withLevel(Elastic.ElasticNotification.NotificationLevel.WARNING) + .withTitle("Partial Recovery") + .withDescription("Switching to AprilTag-only tracking") + .withDisplaySeconds(3.0)); } } } @@ -621,6 +713,19 @@ private void handleDisabledPoseValidation() { } Pose2d currentTagPose = tagSource.getCurrentPose(); + + // If we're in EMERGENCY state and we see tags, attempt to recover + if (state.getCurrentState() == LocalizationState.State.EMERGENCY && currentTagPose != null) { + Logger.recordOutput( + "LocalizationFusion/Event", + "Tags detected while in EMERGENCY state during disabled - attempting recovery"); + if (resetToPose(currentTagPose)) { + state.transitionTo(LocalizationState.State.RESETTING); + resetStartTime = Timer.getTimestamp(); + return; + } + } + if (currentTagPose == null) return; if (!initialPoseValidated) { @@ -641,6 +746,7 @@ private void handleDisabledPoseValidation() { .withDisplaySeconds(3.0)); if (resetToPose(currentTagPose)) { state.transitionTo(LocalizationState.State.RESETTING); + resetStartTime = Timer.getTimestamp(); initialPoseValidated = true; } } else { @@ -685,6 +791,12 @@ private void handleDisabledPoseValidation() { initialPoseValidationStartTime = Timer.getTimestamp(); lastValidatedPose = currentTagPose; initialPoseValidated = false; + + // Attempt immediate reset with new pose + if (resetToPose(currentTagPose)) { + state.transitionTo(LocalizationState.State.RESETTING); + resetStartTime = Timer.getTimestamp(); + } } } @@ -709,12 +821,19 @@ private boolean validatePose(Pose2d pose, boolean strict) { } double poseError = tagPose.getTranslation().getDistance(pose.getTranslation()); + boolean isValid; if (!questInitialized) { - return poseError <= INIT_VALIDATION_THRESHOLD; + isValid = poseError <= INIT_VALIDATION_THRESHOLD; + } else { + isValid = poseError <= APRILTAG_VALIDATION_THRESHOLD; + } + + if (isValid) { + hasTagValidatedPose = true; // Update this instead of hasValidPoseOffset } - return poseError <= APRILTAG_VALIDATION_THRESHOLD; + return isValid; } /** @@ -791,6 +910,7 @@ public boolean requestResetOculusPoseViaAprilTags() { "Manual reset requested using AprilTag pose - initiating pose reset"); if (resetToPose(currentTagPose)) { state.transitionTo(LocalizationState.State.RESETTING); + hasTagValidatedPose = true; return true; } return false; @@ -819,12 +939,54 @@ public boolean requestResetOculusPose(Pose2d targetPose) { "Manual reset requested with custom pose - initiating pose reset"); if (resetToPose(targetPose)) { state.transitionTo(LocalizationState.State.RESETTING); + hasTagValidatedPose = true; // Set on manual reset return true; } return false; } // -------------------- State Management Helpers -------------------- + /** Updates the stored auto selection and initiates reset if needed */ + private void updateAutoSelection() { + String selectedAuto = autoChooser.get().getName(); + currentAutoSelection = (selectedAuto != null) ? selectedAuto : ""; + + // Update pose if either: + // 1. We don't have any pose source (!hasTagValidatedPose && !hasAutoPose) + // 2. We only have an auto pose (!hasTagValidatedPose && hasAutoPose) and auto changed + if (!hasTagValidatedPose + && (!hasAutoPose || !currentAutoSelection.equals(previousAutoSelection))) { + + Logger.recordOutput( + "LocalizationFusion/Event", + "Auto selection changed from '" + + previousAutoSelection + + "' to '" + + currentAutoSelection + + "'"); + + Pose2d autoPose = getAutoStartingPose(); + if (autoPose != null) { + Logger.recordOutput( + "LocalizationFusion/Event", "Updating pose to new auto starting position"); + Elastic.sendAlert( + new Elastic.ElasticNotification() + .withLevel(Elastic.ElasticNotification.NotificationLevel.INFO) + .withTitle("Auto Selection Changed") + .withDescription("Updating robot position for new auto: " + currentAutoSelection) + .withDisplaySeconds(3.0)); + + if (resetToPose(autoPose)) { + state.transitionTo(LocalizationState.State.RESETTING); + lastValidatedPose = autoPose; + hasAutoPose = true; + } + } + } + + previousAutoSelection = currentAutoSelection; + } + /** Resets all Quest initialization state variables. */ private void resetQuestInitialization() { questInitStartTime = 0.0; From 8d8c1d1b6ab1651e027a254d9517b3e1fc81e3b0 Mon Sep 17 00:00:00 2001 From: Sean Ernstes Date: Sat, 4 Jan 2025 04:19:03 -0500 Subject: [PATCH 23/23] auto realign --- .../LocalizationFusion.java | 113 +++++++++++++++++- .../LocalizationFusionConstants.java | 23 ++++ 2 files changed, 134 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/alotobots/library/subsystems/vision/localizationfusion/LocalizationFusion.java b/src/main/java/frc/alotobots/library/subsystems/vision/localizationfusion/LocalizationFusion.java index f55af342..e9512eab 100644 --- a/src/main/java/frc/alotobots/library/subsystems/vision/localizationfusion/LocalizationFusion.java +++ b/src/main/java/frc/alotobots/library/subsystems/vision/localizationfusion/LocalizationFusion.java @@ -126,6 +126,15 @@ public class LocalizationFusion extends SubsystemBase implements StateTransition private String currentAutoSelection = ""; + /** Timestamp when stability check started. */ + private double stabilityStartTime = 0.0; + + /** Timestamp of the last auto-realignment. */ + private double lastAutoRealignTime = 0.0; + + /** Pose when stability check started. */ + private Pose2d stabilityStartPose = null; + // Separate flags for pose validation sources private boolean hasTagValidatedPose = false; // Set only when we get AprilTag validation private boolean hasAutoPose = false; // Set when we accept an auto pose @@ -404,6 +413,11 @@ private void handleQuestPrimaryState(double currentTime) { Pose2d questPose = oculusSource.getCurrentPose(); if (questPose != null) { if (validatePose(questPose, false)) { + // Try auto realign if its enabled + if (LocalizationFusionConstants.AutoRealignConstants.ENABLED) { + tryAutoRealign(currentTime); + } + poseConsumer.accept(questPose, currentTime, oculusSource.getStdDevs()); lastQuestUpdate = currentTime; } else { @@ -867,6 +881,71 @@ private Pose2d getValidReferencePose() { } // -------------------- Reset Methods -------------------- + /** + * Attempts to perform an auto-realignment if conditions are met. + * + * @param currentTime the current system timestamp + * @return true if realignment was attempted + */ + private boolean tryAutoRealign(double currentTime) { + + if (!LocalizationFusionConstants.AutoRealignConstants.ENABLED + || !DriverStation.isTeleopEnabled()) { + return false; + } + + // Check cooldown period + if (currentTime - lastAutoRealignTime + < LocalizationFusionConstants.AutoRealignConstants.COOLDOWN) { + return false; + } + + // Get current poses + Pose2d questPose = oculusSource.getCurrentPose(); + Pose2d tagPose = tagSource.getCurrentPose(); + + if (questPose == null || tagPose == null) { + lastAutoRealignTime = currentTime; // Rate limit so we arent dumping calls + return false; + } + + // Verify stability + if (!checkStability(questPose, currentTime)) { + lastAutoRealignTime = currentTime; // Rate limit so we arent dumping calls + return false; + } + + // Check pose error + double poseError = questPose.getTranslation().getDistance(tagPose.getTranslation()); + if (poseError > LocalizationFusionConstants.AutoRealignConstants.THRESHOLD) { + String message = String.format("Auto-realignment triggered (error: %.2fm)", poseError); + Logger.recordOutput("LocalizationFusion/Event", message); + Elastic.sendAlert( + new Elastic.ElasticNotification() + .withLevel(Elastic.ElasticNotification.NotificationLevel.WARNING) + .withTitle("Auto Realignment") + .withDescription(message) + .withDisplaySeconds(3.0)); + + if (resetToPose(tagPose)) { + lastAutoRealignTime = currentTime; // Rate limit so we arent dumping calls + state.transitionTo(LocalizationState.State.RESETTING); + resetStartTime = currentTime; + // Reset stability tracking after realignment + stabilityStartTime = 0.0; + stabilityStartPose = null; + return true; + } + } else { + lastAutoRealignTime = currentTime; // Rate limit so we arent dumping calls + Logger.recordOutput( + "LocalizationFusion/Event", + String.format("Auto-realignment skipped! (error: %.2fm)", poseError)); + } + + return false; + } + /** * Attempts to reset the Quest pose to match a target pose. * @@ -946,6 +1025,36 @@ public boolean requestResetOculusPose(Pose2d targetPose) { } // -------------------- State Management Helpers -------------------- + /** + * Checks if the robot has been stable (minimal pose change) for the required time period. + * + * @param currentPose the current robot pose + * @param currentTime the current system timestamp + * @return true if the robot has been stable for the required duration + */ + private boolean checkStability(Pose2d currentPose, double currentTime) { + // Start stability check if not already started + if (stabilityStartTime == 0.0) { + stabilityStartTime = currentTime; + stabilityStartPose = currentPose; + return false; + } + + // Check if we've moved too much since starting stability check + double poseChange = + currentPose.getTranslation().getDistance(stabilityStartPose.getTranslation()); + if (poseChange > LocalizationFusionConstants.AutoRealignConstants.MAX_MOVEMENT) { + // Reset stability check + stabilityStartTime = currentTime; + stabilityStartPose = currentPose; + return false; + } + + // Check if we've been stable long enough + return (currentTime - stabilityStartTime) + >= LocalizationFusionConstants.AutoRealignConstants.STABILITY_TIME; + } + /** Updates the stored auto selection and initiates reset if needed */ private void updateAutoSelection() { String selectedAuto = autoChooser.get().getName(); @@ -1069,7 +1178,7 @@ public void logTransition(LocalizationState.State from, LocalizationState.State .withTitle("Localization Emergency") .withDescription( "Localization system entered emergency state - Check vision systems") - .withDisplaySeconds(10.0)); + .withDisplaySeconds(3.0)); break; case RESETTING: Elastic.sendAlert( @@ -1095,7 +1204,7 @@ public void logTransition(LocalizationState.State from, LocalizationState.State .withLevel(Elastic.ElasticNotification.NotificationLevel.WARNING) .withTitle("Using Backup Tracking") .withDescription("Switched to AprilTag-based tracking") - .withDisplaySeconds(10.0)); + .withDisplaySeconds(3.0)); break; } } diff --git a/src/main/java/frc/alotobots/library/subsystems/vision/localizationfusion/LocalizationFusionConstants.java b/src/main/java/frc/alotobots/library/subsystems/vision/localizationfusion/LocalizationFusionConstants.java index dc2fd245..e6a9a8b0 100644 --- a/src/main/java/frc/alotobots/library/subsystems/vision/localizationfusion/LocalizationFusionConstants.java +++ b/src/main/java/frc/alotobots/library/subsystems/vision/localizationfusion/LocalizationFusionConstants.java @@ -40,6 +40,29 @@ public static class ValidationThresholds { public static final double MAX_ROTATION_CHANGE_DEGREES = 35.0; } + /** + * Constants related to the auto-realignment feature for correcting Quest drift using AprilTags. + * This system monitors robot stability and pose error to trigger automatic realignments when the + * robot is stationary and significant drift is detected. + */ + @UtilityClass + public class AutoRealignConstants { + /** Whether auto-realignment should be enabled. */ + public static final boolean ENABLED = true; + + /** Threshold for auto-realignment when pose error exceeds this value (meters). */ + public static final double THRESHOLD = 0.1; + + /** Maximum pose change over stability period to be considered stable (meters). */ + public static final double MAX_MOVEMENT = 0.05; + + /** Time robot must be stable before realigning (seconds). */ + public static final double STABILITY_TIME = 1.0; + + /** Minimum time between auto-realignments (seconds). */ + public static final double COOLDOWN = 10.0; + } + /** * Constants related to timing and update intervals. Defines various timeouts, intervals, and * timing windows used in the localization system.