Merge pull request #66 from 5152Alotobots/questnav-cleanup

QuestNav works

.run/deploy.run.xml

@@ -1,11 +1,10 @@
 <component name="ProjectRunConfigurationManager">
-  <configuration default="false" name="deploy"
-    type="GradleRunConfiguration" factoryName="Gradle">
+  <configuration default="false" name="deploy" type="GradleRunConfiguration" factoryName="Gradle">
     <ExternalSystemSettings>
       <option name="executionName" />
       <option name="externalProjectPath" value="$PROJECT_DIR$" />
       <option name="externalSystemIdString" value="GRADLE" />
-      <option name="scriptParameters" value="" />
+      <option name="scriptParameters" value="--stacktrace" />
       <option name="taskDescriptions">
         <list />
       </option>
@@ -22,4 +21,4 @@
     <RunAsTest>false</RunAsTest>
     <method v="2" />
   </configuration>
-</component>
+</component>

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)

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)

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)

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)

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

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 <team>    - 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 <path>   - 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 <team> 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

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;

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();
 }