Initial commit of magicbot code base

.github/CODEOWNERS

@@ -1 +0,0 @@
-@tywilly

.github/workflows/gradle.yml

@@ -1,36 +0,0 @@
-# This workflow uses actions that are not certified by GitHub.
-# They are provided by a third-party and are governed by
-# separate terms of service, privacy policy, and support
-# documentation.
-# This workflow will build a Java project with Gradle and cache/restore any dependencies to improve the workflow execution time
-# For more information see: https://docs.github.com/en/actions/automating-builds-and-tests/building-and-testing-java-with-gradle
-
-name: Java CI with Gradle
-
-on:
-  push:
-    branches: [ ]
-  pull_request:
-    branches: [ "main" ]
-
-permissions:
-  contents: read
-
-jobs:
-  build:
-
-    runs-on: ubuntu-latest
-
-    steps:
-    - uses: actions/checkout@v4
-      with:
-        submodules: true
-    - name: Set up JDK 21
-      uses: actions/setup-java@v3
-      with:
-        java-version: '21'
-        distribution: 'temurin'
-    - name: Setup Gradle
-      uses: gradle/actions/setup-gradle@v3
-    - name: Build with Gradle
-      run: ./gradlew build javadoc

.gitmodules

@@ -1,6 +0,0 @@
-[submodule "lib/Posty"]
-	path = lib/Posty
-	url = git@github.com:FairportRobotics/Posty.git
-[submodule "lib/RoboLib"]
-	path = lib/RoboLib
-	url = git@github.com:FairportRobotics/RoboLib

.vscode/launch.json

@@ -1,21 +0,0 @@
-{
-  // Use IntelliSense to learn about possible attributes.
-  // Hover to view descriptions of existing attributes.
-  // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387
-  "version": "0.2.0",
-  "configurations": [
-
-    {
-      "type": "wpilib",
-      "name": "WPILib Desktop Debug",
-      "request": "launch",
-      "desktop": true,
-    },
-    {
-      "type": "wpilib",
-      "name": "WPILib roboRIO Debug",
-      "request": "launch",
-      "desktop": false,
-    }
-  ]
-}

.vscode/settings.json

@@ -1,29 +0,0 @@
-{
-  "java.configuration.updateBuildConfiguration": "automatic",
-  "java.server.launchMode": "Standard",
-  "files.exclude": {
-    "**/.git": true,
-    "**/.svn": true,
-    "**/.hg": true,
-    "**/CVS": true,
-    "**/.DS_Store": true,
-    "bin/": true,
-    "**/.classpath": true,
-    "**/.project": true,
-    "**/.settings": true,
-    "**/.factorypath": true,
-    "**/*~": true
-  },
-  "java.test.config": [
-    {
-      "name": "WPIlibUnitTests",
-      "workingDirectory": "${workspaceFolder}/build/jni/release",
-      "vmargs": [ "-Djava.library.path=${workspaceFolder}/build/jni/release" ],
-      "env": {
-        "LD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release" ,
-        "DYLD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release"
-      }
-    },
-  ],
-  "java.test.defaultConfig": "WPIlibUnitTests"
-}

.wpilib/wpilib_preferences.json

@@ -1,6 +0,0 @@
-{
-    "enableCppIntellisense": false,
-    "currentLanguage": "java",
-    "projectYear": "2026",
-    "teamNumber": 578
-}

LimelightHelpers.py

@@ -0,0 +1,81 @@
+"""
+LimelightHelpers.py – stub for the community limelightlib-python library.
+
+Replace this file with the real LimelightHelpers.py from:
+  https://github.com/LimelightVision/limelightlib-python
+
+The following methods must be verified to exist in the real library before
+any Limelight-dependent code is run on the robot (see PLAN.txt section 3):
+  - getBotPoseEstimate_wpiBlue_MegaTag2()
+  - SetIMUMode()
+  - getTargetPose_CameraSpace()
+  - SetRobotOrientation()
+  - SetFiducialIDFiltersOverride()
+  - setPipelineIndex()
+  - getTV()
+"""
+
+from __future__ import annotations
+
+from dataclasses import dataclass, field
+
+from wpimath.geometry import Pose2d
+
+
+@dataclass
+class PoseEstimate:
+    pose: Pose2d = field(default_factory=Pose2d)
+    timestampSeconds: float = 0.0
+    tagCount: int = 0
+    isMegaTag2: bool = False
+
+
+# ── Pose estimation ────────────────────────────────────────────────────────────
+
+def getBotPoseEstimate_wpiBlue_MegaTag2(limelightName: str) -> PoseEstimate | None:
+    """Return None (stub — no camera connected in sim)."""
+    return None
+
+
+def getBotPoseEstimate_wpiBlue(limelightName: str) -> PoseEstimate | None:
+    return None
+
+
+# ── Robot orientation / IMU ────────────────────────────────────────────────────
+
+def SetRobotOrientation(
+    limelightName: str,
+    yaw: float,
+    yawRate: float,
+    pitch: float,
+    pitchRate: float,
+    roll: float,
+    rollRate: float,
+) -> None:
+    pass
+
+
+def SetIMUMode(limelightName: str, mode: int) -> None:
+    pass
+
+
+# ── Target information ─────────────────────────────────────────────────────────
+
+def getTV(limelightName: str) -> bool:
+    """Return False — no targets visible in stub."""
+    return False
+
+
+def getTargetPose_CameraSpace(limelightName: str) -> list[float]:
+    """Return empty list — no target in stub."""
+    return []
+
+
+# ── Pipeline / filter control ──────────────────────────────────────────────────
+
+def setPipelineIndex(limelightName: str, pipelineIndex: int) -> None:
+    pass
+
+
+def SetFiducialIDFiltersOverride(limelightName: str, validIDs: list[int]) -> None:
+    pass

TODOs.md

@@ -1,57 +1,57 @@
-# Top Priority (for manual shooting functionality by RIT)
+# Top Priority (for manual shooting functionality by RIT)
-
+
-These are the jobs to be completed first, to ensure we have minimal functionality for RIT
+These are the jobs to be completed first, to ensure we have minimal functionality for RIT
-
+
-- Add subsystems to robot
+- Add subsystems to robot
-    - Add subsystems to the RobotContainer X
+    - Add subsystems to the RobotContainer X
-
+
-- Updates to subsystems X
+- Updates to subsystems X
-    - Update launcher wheel to a Neo/SparkMax X
+    - Update launcher wheel to a Neo/SparkMax X
-
+
-- Define basic commands for manual fire mode
+- Define basic commands for manual fire mode
-    - Y/Top button: Deploy the intake (and run the rollers) X
+    - Y/Top button: Deploy the intake (and run the rollers) X
-        - Could be a single command
+        - Could be a single command
-        - Could be 2 commands in sequence
+        - Could be 2 commands in sequence
-
+
-    - B/Right button: Reverse the intake X
+    - B/Right button: Reverse the intake X
-        - Unclear if this should touch the deploy motor. ATM suggest leaving the deploy motor alone
+        - Unclear if this should touch the deploy motor. ATM suggest leaving the deploy motor alone
-
+
-    - A/Bottom button: Retract the intake (and stop the rollers) X
+    - A/Bottom button: Retract the intake (and stop the rollers) X
-
+
-    - D-pad: Set deflector to pre-programmed angle
+    - D-pad: Set deflector to pre-programmed angle
-        - Suggestion: Implement this once, and make the angle an member variable of the command X
+        - Suggestion: Implement this once, and make the angle an member variable of the command X
-
+
-    - Both Triggers: Spin up flywheel (if not already spun up) and then start the kicker. X
+    - Both Triggers: Spin up flywheel (if not already spun up) and then start the kicker. X
-        - Re: Sensing flywheel speed
+        - Re: Sensing flywheel speed
-            - Could be a time offset (i.e. wait for N seconds for the motor to spin up)
+            - Could be a time offset (i.e. wait for N seconds for the motor to spin up)
-            - Ideally, check the encoder speed (will need the encoders for auto launch)
+            - Ideally, check the encoder speed (will need the encoders for auto launch)
-
+
-- Define unique IDs and channels for all devices in Constants.java
+- Define unique IDs and channels for all devices in Constants.java
-
+
-- Test all motors and reverse directions if required
+- Test all motors and reverse directions if required
-    - Direction flags are present in Constants.java Particular values of concern are marked with TODO comments
+    - Direction flags are present in Constants.java Particular values of concern are marked with TODO comments
-
+
-# Secondary priority
+# Secondary priority
-
+
-These are to be completed after the top priority tasks. Currently, they focus on getting automatic targeting working.
+These are to be completed after the top priority tasks. Currently, they focus on getting automatic targeting working.
-
+
-- Integrate limelight positioning
+- Integrate limelight positioning
-
+
-- Integrate automatic aiming
+- Integrate automatic aiming
-
+
-- Advanced movement functions:
+- Advanced movement functions:
-    - L Bumper: "Nudging" mode:
+    - L Bumper: "Nudging" mode:
-        - On press: 
+        - On press: 
-            - Briefly move the robot forward then stop
+            - Briefly move the robot forward then stop
-            - Trigger nudge at 1 to 2 hz (to be determined, control via constant)
+            - Trigger nudge at 1 to 2 hz (to be determined, control via constant)
-            - Duty cycle (i.e. what percentage of the time the robot is actively moving) is undefined. Control via constant
+            - Duty cycle (i.e. what percentage of the time the robot is actively moving) is undefined. Control via constant
-        - On release:
+        - On release:
-            - Return to normal driving mode
+            - Return to normal driving mode
-
+
-    - R Bumper: "Trench Maneuver"
+    - R Bumper: "Trench Maneuver"
-        - On button press/enter:
+        - On button press/enter:
-            - Move deflector to lowest position
+            - Move deflector to lowest position
-            - Deploy the intake, no need to start the rollers if not started
+            - Deploy the intake, no need to start the rollers if not started
-        - On button release/exit:
+        - On button release/exit:
-            - Return the deflector to its previous state
+            - Return the deflector to its previous state
-            - Restore intake to previous state
+            - Restore intake to previous state

WPILib-License.md

@@ -1,24 +0,0 @@
-Copyright (c) 2009-2026 FIRST and other WPILib contributors
-All rights reserved.
-
-Redistribution and use in source and binary forms, with or without
-modification, are permitted provided that the following conditions are met:
-   * Redistributions of source code must retain the above copyright
-     notice, this list of conditions and the following disclaimer.
-   * Redistributions in binary form must reproduce the above copyright
-     notice, this list of conditions and the following disclaimer in the
-     documentation and/or other materials provided with the distribution.
-   * Neither the name of FIRST, WPILib, nor the names of other WPILib
-     contributors may be used to endorse or promote products derived from
-     this software without specific prior written permission.
-
-THIS SOFTWARE IS PROVIDED BY FIRST AND OTHER WPILIB CONTRIBUTORS "AS IS" AND
-ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
-WARRANTIES OF MERCHANTABILITY NONINFRINGEMENT AND FITNESS FOR A PARTICULAR
-PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL FIRST OR CONTRIBUTORS BE LIABLE FOR
-ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
-(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
-ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
-(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
-SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

autonomous/basic_auto.py

@@ -0,0 +1,38 @@
+"""
+autonomous/basic_auto.py
+
+Fallback MagicBot AutonomousStateMachine used when PathPlanner is unavailable.
+Deploys the intake, homes the turret, then stops.
+"""
+
+import magicbot
+from magicbot.state_machine import state, timed_state
+
+from components.drive import DriveComponent
+from components.intake import IntakeComponent
+from components.turret import TurretComponent
+
+
+class BasicAuto(magicbot.AutonomousStateMachine):
+    """Simple fallback autonomous: deploy intake → home turret → idle."""
+
+    MODE_NAME = "Basic Auto"
+    DEFAULT = False
+
+    # MagicBot injects these
+    drive: DriveComponent
+    intake: IntakeComponent
+    turret: TurretComponent
+
+    @timed_state(first=True, duration=2.0, next_state="home_turret")
+    def deploy_intake(self) -> None:
+        self.intake.deploy()
+
+    @timed_state(duration=3.0, next_state="idle")
+    def home_turret(self) -> None:
+        self.intake.stop_deploy()
+        self.turret.home_turret()
+
+    @state
+    def idle(self) -> None:
+        self.drive.stop_drive()

autonomous/chassis_align_shoot.py

@@ -0,0 +1,158 @@
+"""
+autonomous/chassis_align_shoot.py
+
+commands2.Command that replicates AutoShootCommandChassisTurret.java.
+
+Uses a PID controller to camera-centre the robot on the HUB AprilTags and
+dead-reckoning as a fallback.  Fires the kicker when centred and launcher is
+up to speed.
+"""
+
+import math
+
+import commands2
+from robotpy_apriltag import AprilTagField, AprilTagFieldLayout
+from wpilib import DriverStation
+from wpimath.controller import PIDController
+from wpimath.geometry import Pose3d
+
+import constants
+import LimelightHelpers
+import utils
+from components.drive import DriveComponent
+from components.hopper import HopperComponent
+from components.turret import TurretComponent
+
+
+class ChassisAlignShootCommand(commands2.Command):
+    """
+    Aligns the robot chassis and fires the launcher using camera feedback.
+    Registered with PathPlanner as "ChassisAlignShoot".
+    """
+
+    _field_tags = AprilTagFieldLayout.loadField(AprilTagField.k2026RebuiltWelded)
+
+    def __init__(
+        self,
+        drive: DriveComponent,
+        hopper: HopperComponent,
+        turret: TurretComponent,
+    ) -> None:
+        super().__init__()
+        self._drive = drive
+        self._hopper = hopper
+        self._turret = turret
+
+        self._tag_filters: list[int] = []
+        self._closest_tag: Pose3d | None = None
+
+        # Camera centering PID (units: degrees)
+        self._camera_controller = PIDController(0.15, 0.0, 0.0)
+        self._camera_controller.setTolerance(0.2)
+        self._camera_controller.setSetpoint(0)
+
+        # Dead-reckoning fallback PID (units: radians)
+        self._dead_reckoning_controller = PIDController(math.pi * 0.1, 0, 0.0)
+        self._dead_reckoning_controller.setTolerance(0.2)
+        self._dead_reckoning_controller.enableContinuousInput(-math.pi, math.pi)
+
+        self._chassis_rotate_speed = math.pi * 0.1
+
+    def initialize(self) -> None:
+        alliance = DriverStation.getAlliance()
+        if alliance == DriverStation.Alliance.kBlue:
+            self._tag_filters = constants.CameraConstants.IDFilters.BLUE_HUB_SHOOTING_IDS
+        else:
+            self._tag_filters = constants.CameraConstants.IDFilters.RED_HUB_SHOOTING_IDS
+
+        self._turret.set_launcher(3000)
+        self._turret.set_target_elevation(constants.ShooterConstants.DEFLECTOR_STORED_ANGLE)
+        self._hopper.spindexer_on()
+        # Warm-up the camera PID
+        self._camera_controller.calculate(-100)
+
+        # Switch to HUB-tracking pipeline
+        LimelightHelpers.setPipelineIndex(
+            constants.CameraConstants.BACK_CAMERA_NAME,
+            constants.CameraConstants.BACK_HUB_TRACKING_3D_PIPELINE_NUMBER,
+        )
+        LimelightHelpers.SetFiducialIDFiltersOverride(
+            constants.CameraConstants.BACK_CAMERA_NAME, self._tag_filters
+        )
+
+        # Find the closest HUB AprilTag for dead-reckoning
+        bot_pose = self._drive.get_bot_pose()
+        tag_poses: list[Pose3d] = [
+            pose
+            for tag_id in self._tag_filters
+            if (pose := self._field_tags.getTagPose(tag_id)) is not None
+        ]
+        if tag_poses:
+            # Find nearest tag by distance
+            def _dist(tag: Pose3d) -> float:
+                delta = bot_pose.translation().toTranslation2d() - tag.translation().toTranslation2d()
+                return math.hypot(delta.X(), delta.Y())
+            self._closest_tag = min(tag_poses, key=_dist)
+
+            delta = (
+                bot_pose.translation().toTranslation2d()
+                - self._closest_tag.translation().toTranslation2d()
+            )
+            self._dead_reckoning_controller.setSetpoint(
+                math.atan2(delta.Y(), delta.X())
+            )
+
+    def execute(self) -> None:
+        has_target = LimelightHelpers.getTV(constants.CameraConstants.BACK_CAMERA_NAME)
+
+        if not has_target:
+            # ── Dead-reckoning fallback ──────────────────────────────────
+            # (Chassis rotation was commented out in Java source; preserved here)
+            # bot_pose = self._drive.get_bot_pose()
+            # self._drive.rotate_chassis(
+            #     -self._dead_reckoning_controller.calculate(
+            #         bot_pose.rotation().toRotation2d().radians()
+            #     )
+            # )
+            pass
+        else:
+            # ── Camera centering ─────────────────────────────────────────
+            targets_pose = LimelightHelpers.getTargetPose_CameraSpace(
+                constants.CameraConstants.BACK_CAMERA_NAME
+            )
+            if targets_pose and len(targets_pose) >= 3:
+                distance_z = targets_pose[2]
+                tx = targets_pose[0]
+
+                turret_angle = (
+                    utils.calculate_turret_angle_from_camera_tag_distance(distance_z)
+                    * constants.ShooterConstants.TURRET_GEAR_RATIO
+                )
+                self._turret.set_turret_motor_rotation(-turret_angle)
+
+                self._drive.rotate_chassis(
+                    -self._camera_controller.calculate(tx)
+                )
+
+        # Fire when centred and launcher is up to speed
+        if (
+            self._turret.is_launcher_up_to_speed()
+            and self._camera_controller.atSetpoint()
+        ):
+            self._hopper.feed_kicker()
+        else:
+            pass  # Waiting to fire
+
+    def isFinished(self) -> bool:
+        return False
+
+    def end(self, interrupted: bool) -> None:
+        self._turret.set_launcher(0)
+        self._hopper.stop_kicker()
+        self._hopper.spindexer_off()
+        self._drive.stop_drive()
+
+        LimelightHelpers.setPipelineIndex(
+            constants.CameraConstants.BACK_CAMERA_NAME,
+            constants.CameraConstants.BACK_LOCALIZATION_PIPELINE_NUMBER,
+        )

autonomous/turret_shoot.py

@@ -0,0 +1,85 @@
+"""
+autonomous/turret_shoot.py
+
+commands2.Command that replicates AutoTurretShootCommand.java.
+
+Uses distance-based ballistics (Utils.get_hood_angle/launcher_rpm_for_distance)
+to set the turret launcher speed and hood angle, then fires once ready.
+"""
+
+import commands2
+from wpilib import DriverStation
+
+import constants
+import utils
+from components.drive import DriveComponent
+from components.hopper import HopperComponent
+from components.intake import IntakeComponent
+from components.turret import TurretComponent
+
+
+class TurretShootCommand(commands2.Command):
+    """
+    Distance-based auto-shoot command.
+    Registered with PathPlanner as "TurretShoot".
+    """
+
+    _WAIT_SECONDS: float = 1.5
+
+    def __init__(
+        self,
+        hopper: HopperComponent,
+        turret: TurretComponent,
+        intake: IntakeComponent,
+        drive: DriveComponent,
+    ) -> None:
+        super().__init__()
+        self._hopper = hopper
+        self._turret = turret
+        self._intake = intake
+        self._drive = drive
+
+        self._wait_timer = commands2.WaitCommand(self._WAIT_SECONDS)
+        self._wait_started = False
+
+    def initialize(self) -> None:
+        self._hopper.spindexer_on()
+        self._wait_timer = commands2.WaitCommand(self._WAIT_SECONDS)
+        self._wait_timer.schedule()
+        self._wait_started = True
+
+    def execute(self) -> None:
+        turret_target = self._turret.get_turret_target_pose()
+        bot_pose = self._drive.get_bot_pose()
+
+        if turret_target is not None:
+            target_2d = turret_target.toPose2d().translation()
+            bot_2d = bot_pose.translation().toTranslation2d()
+            distance = target_2d.distance(bot_2d)
+
+            self._turret.set_target_elevation(utils.get_hood_angle_for_distance(distance))
+            self._turret.set_launcher(utils.get_launcher_rpm_for_distance(distance))
+
+        if self._turret.is_launcher_up_to_speed() and self._wait_timer.isFinished():
+            self._hopper.feed_kicker()
+        else:
+            self._hopper.stop_kicker()
+
+    def isFinished(self) -> bool:
+        return False
+
+    def end(self, interrupted: bool) -> None:
+        if self._wait_started:
+            self._wait_timer.cancel()
+
+        self._hopper.spindexer_off()
+        self._hopper.stop_kicker()
+        self._turret.set_launcher(0)
+        self._turret.set_target_elevation(constants.ShooterConstants.DEFLECTOR_STORED_ANGLE)
+
+        # Restore turret target to hub for the current alliance
+        alliance = DriverStation.getAlliance()
+        if alliance == DriverStation.Alliance.kBlue:
+            self._turret.set_turret_target_pose(constants.FieldPoses.BLUE_HUB_POSE)
+        elif alliance == DriverStation.Alliance.kRed:
+            self._turret.set_turret_target_pose(constants.FieldPoses.RED_HUB_POSE)

build.gradle

@@ -1,141 +0,0 @@
-plugins {
-    id "java"
-    id "edu.wpi.first.GradleRIO" version "2026.2.1"
-}
-
-java {
-    sourceCompatibility = JavaVersion.VERSION_17
-    targetCompatibility = JavaVersion.VERSION_17
-}
-
-def ROBOT_MAIN_CLASS = "frc.robot.Main"
-
-// Define my targets (RoboRIO) and artifacts (deployable files)
-// This is added by GradleRIO's backing project DeployUtils.
-deploy {
-    targets {
-        roborio(getTargetTypeClass('RoboRIO')) {
-            // Team number is loaded either from the .wpilib/wpilib_preferences.json
-            // or from command line. If not found an exception will be thrown.
-            // You can use getTeamOrDefault(team) instead of getTeamNumber if you
-            // want to store a team number in this file.
-            team = project.frc.getTeamNumber()
-            debug = project.frc.getDebugOrDefault(false)
-
-            artifacts {
-                // First part is artifact name, 2nd is artifact type
-                // getTargetTypeClass is a shortcut to get the class type using a string
-
-                frcJava(getArtifactTypeClass('FRCJavaArtifact')) {
-                }
-
-                // Static files artifact
-                frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) {
-                    files = project.fileTree('src/main/deploy')
-                    directory = '/home/lvuser/deploy'
-                    deleteOldFiles = false // Change to true to delete files on roboRIO that no
-                                           // longer exist in deploy directory of this project
-                }
-            }
-        }
-    }
-}
-
-def deployArtifact = deploy.targets.roborio.artifacts.frcJava
-
-// Set to true to use debug for all targets including JNI, which will drastically impact
-// performance.
-wpi.java.debugJni = false
-
-// Set this to true to enable desktop support.
-def includeDesktopSupport = true
-
-// Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries.
-// Also defines JUnit 5.
-dependencies {
-    annotationProcessor wpi.java.deps.wpilibAnnotations()
-    implementation wpi.java.deps.wpilib()
-    implementation wpi.java.vendor.java()
-
-    roborioDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.roborio)
-    roborioDebug wpi.java.vendor.jniDebug(wpi.platforms.roborio)
-
-    roborioRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.roborio)
-    roborioRelease wpi.java.vendor.jniRelease(wpi.platforms.roborio)
-
-    nativeDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.desktop)
-    nativeDebug wpi.java.vendor.jniDebug(wpi.platforms.desktop)
-    simulationDebug wpi.sim.enableDebug()
-
-    nativeRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.desktop)
-    nativeRelease wpi.java.vendor.jniRelease(wpi.platforms.desktop)
-    simulationRelease wpi.sim.enableRelease()
-
-    testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1'
-    testRuntimeOnly 'org.junit.platform:junit-platform-launcher'
-}
-
-test {
-    useJUnitPlatform()
-    systemProperty 'junit.jupiter.extensions.autodetection.enabled', 'true'
-}
-
-// Simulation configuration (e.g. environment variables).
-wpi.sim.addGui().defaultEnabled = true
-wpi.sim.addDriverstation()
-
-// Setting up my Jar File. In this case, adding all libraries into the main jar ('fat jar')
-// in order to make them all available at runtime. Also adding the manifest so WPILib
-// knows where to look for our Robot Class.
-jar {
-    from { configurations.runtimeClasspath.collect { it.isDirectory() ? it : zipTree(it) } }
-    from('src') { into 'backup/src' }
-    from('vendordeps') { into 'backup/vendordeps' }
-    from('build.gradle') { into 'backup' }
-    manifest edu.wpi.first.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS)
-    duplicatesStrategy = DuplicatesStrategy.INCLUDE
-}
-
-// Configure jar and deploy tasks
-deployArtifact.jarTask = jar
-wpi.java.configureExecutableTasks(jar)
-wpi.java.configureTestTasks(test)
-
-def saveCommitTask = tasks.register("saveCommit") {
-    println "Current Version: " + getCheckedOutGitCommitHash();
-
-    def deployFolder = "$projectDir/src/main/deploy/"
-    def commitFile = new FileWriter(deployFolder + "versionfile.txt");
-    doLast {
-        commitFile.write(getCheckedOutGitCommitHash() + " - " + new Date().toString());
-        commitFile.close();
-        println "versionfile.txt written";
-    }
-}
-
-def getCheckedOutGitCommitHash() {
-    def gitFolder = "$projectDir/.git/"
-    def takeFromHash = 12
-    /*
-     * '.git/HEAD' contains either
-     *      in case of detached head: the currently checked out commit hash
-     *      otherwise: a reference to a file containing the current commit hash
-     */
-    def head = new File(gitFolder + "HEAD").text.split(":") // .git/HEAD
-    def isCommit = head.length == 1 // e5a7c79edabbf7dd39888442df081b1c9d8e88fd
-    // def isRef = head.length > 1     // ref: refs/heads/master
-
-    if(isCommit) return head[0].trim().take(takeFromHash) // e5a7c79edabb
-
-    def refHead = new File(gitFolder + head[1].trim()) // .git/refs/heads/master
-    refHead.text.trim().take takeFromHash
-}
-
-// Configure string concat to always inline compile
-tasks.withType(JavaCompile) {
-    options.compilerArgs.add '-XDstringConcat=inline'
-    dependsOn saveCommitTask
-}
-
-
-apply from: 'libs.gradle'

components/controller.py

@@ -0,0 +1,359 @@
+"""
+components/controller.py – MagicBot-compatible Xbox controller wrapper.
+
+XboxController wraps wpilib.XboxController to provide:
+  - Deadband-corrected joystick axes
+  - Edge-detection helpers (was_pressed / pressed) for every button
+  - Integration with MagicBot's component injection system (``port`` attribute)
+"""
+
+import wpilib
+
+
+class XboxController:
+    """
+    MagicBot component that wraps a wpilib.XboxController.
+
+    MagicBot injects the ``port`` attribute before ``setup()`` is called,
+    so declare it at class level — its value is overridden per-robot in
+    ``MyRobot`` via the ``driver_controller_port`` prefixed variable.
+
+    Joystick values are deadband-corrected and cached each loop in
+    ``execute()`` so all code within a single loop sees consistent axis values.
+
+    Button helpers follow two patterns:
+      - ``*_pressed()``      – True while the button is held down.
+      - ``*_was_pressed()``  – True on the loop after the button was released
+                               (one-shot, resets immediately after reading).
+    """
+
+    #: Whether to apply deadband correction to raw joystick values.
+    correct_for_deadband: bool = True
+    #: Deadband radius; inputs within ±deadband map to 0.
+    deadband: float = 0.1
+    #: USB port number; injected by MagicBot from ``driver_controller_port``.
+    port: int
+    #: Cached left-stick X axis value (deadband-corrected).
+    left_x: float = 0.0
+    #: Cached left-stick Y axis value (deadband-corrected).
+    left_y: float = 0.0
+    #: Cached right-stick X axis value (deadband-corrected).
+    right_x: float = 0.0
+    #: Cached right-stick Y axis value (deadband-corrected).
+    right_y: float = 0.0
+
+    def setup(self) -> None:
+        """
+        Setup the Xbox controller.
+        This method is called once when the robot is initialized.
+        """
+        self.this_controller = wpilib.XboxController(self.port)
+        controller_buttons = [
+            "A",
+            "B",
+            "X",
+            "Y",
+            "DPad_Up",
+            "DPad_Down",
+            "DPad_Left",
+            "DPad_Right",
+            "Left_Bumper",
+            "Right_Bumper",
+            "Left_Trigger",
+            "Right_Trigger",
+            "Start",
+            "Back",
+        ]
+        self.button_is_pressed = {button: False for button in controller_buttons}
+        self.button_was_pressed = {button: False for button in controller_buttons}
+
+    def _button_pressed(self, button_name: str, pressed: bool) -> bool:
+        """
+        Generic method to check if a button is pressed.
+        :param button_name: The name of the button to check.
+        :param pressed: The current state of the button (True if pressed, False otherwise).
+        """
+        if self.button_is_pressed[button_name] and not pressed:
+            self.button_was_pressed[button_name] = True
+        self.button_is_pressed[button_name] = pressed
+        return pressed
+
+    def _button_was_pressed(self, button_name: str) -> bool:
+        """
+        Generic method to check if a button was pressed since the last check.
+        :param button_name: The name of the button to check.
+        :return: True if the button was pressed since the last check, False otherwise.
+        """
+        if self.button_was_pressed[button_name]:
+            self.button_was_pressed[button_name] = False
+            return True
+        return False
+
+    def _corrected_joystick_value(self, raw_value: float) -> float:
+        """
+        Apply deadband correction to a joystick value.
+        :param raw_value: The raw joystick value.
+        :return: The corrected joystick value.
+        """
+        if not self.correct_for_deadband:
+            return raw_value
+        if abs(raw_value) < self.deadband:
+            return 0.0
+        return (raw_value / abs(raw_value)) * (
+            (abs(raw_value) - self.deadband) / (1 - self.deadband)
+        )
+
+    def execute(self) -> None:
+        """
+        Capture the current state of all buttons to track presses.
+        This method should be called at the end of each control loop iteration.
+        """
+        self.a_button_pressed()
+        self.b_button_pressed()
+        self.x_button_pressed()
+        self.y_button_pressed()
+        self.dpad_up_pressed()
+        self.dpad_down_pressed()
+        self.dpad_left_pressed()
+        self.dpad_right_pressed()
+        self.left_bumper_pressed()
+        self.right_bumper_pressed()
+        self.left_trigger_pressed()
+        self.right_trigger_pressed()
+        self.start_button_pressed()
+        self.back_button_pressed()
+        self.left_y = self._corrected_joystick_value(self.this_controller.getLeftY())
+        self.left_x = self._corrected_joystick_value(self.this_controller.getLeftX())
+        self.right_y = self._corrected_joystick_value(self.this_controller.getRightY())
+        self.right_x = self._corrected_joystick_value(self.this_controller.getRightX())
+
+    def a_button_pressed(self) -> bool:
+        """
+        Check if the A button is pressed.
+
+        :return: True if the A button is pressed, False otherwise.
+        """
+        return self._button_pressed("A", self.this_controller.getAButton())
+
+    def a_button_was_pressed(self) -> bool:
+        """
+        Check if the A button was pressed since the last check.
+
+        :return: True if the A button was pressed since the last check, False otherwise.
+        """
+        return self._button_was_pressed("A")
+
+    def b_button_pressed(self) -> bool:
+        """
+        Check if the B button is pressed.
+        :return: True if the B button is pressed, False otherwise.
+        """
+        return self._button_pressed("B", self.this_controller.getBButton())
+
+    def b_button_was_pressed(self) -> bool:
+        """
+        Check if the B button was pressed since the last check.
+
+        :return: True if the B button was pressed since the last check, False otherwise.
+        """
+        return self._button_was_pressed("B")
+
+    def x_button_pressed(self) -> bool:
+        """
+        Check if the X button is pressed.
+
+        :return: True if the X button is pressed, False otherwise.
+        """
+        return self._button_pressed("X", self.this_controller.getXButton())
+
+    def x_button_was_pressed(self) -> bool:
+        """
+        Check if the X button was pressed since the last check.
+
+        :return: True if the X button was pressed since the last check, False otherwise.
+        """
+        return self._button_was_pressed("X")
+
+    def y_button_pressed(self) -> bool:
+        """
+        Check if the Y button is pressed.
+
+        :return: True if the Y button is pressed, False otherwise.
+        """
+        return self._button_pressed("Y", self.this_controller.getYButton())
+
+    def y_button_was_pressed(self) -> bool:
+        """
+        Check if the Y button was pressed since the last check.
+
+        :return: True if the Y button was pressed since the last check, False otherwise.
+        """
+        return self._button_was_pressed("Y")
+
+    def dpad_up_pressed(self) -> bool:
+        """
+        Check if the D-pad is pressed up.
+
+        :return: True if the D-pad is pressed up, False otherwise.
+        """
+        return self._button_pressed("DPad_Up", self.this_controller.getPOV() == 0)
+
+    def dpad_up_was_pressed(self) -> bool:
+        """
+        Check if the D-pad was pressed up since the last check.
+
+        :return: True if the the D-pad was pressed up since the last check, False otherwise.
+        """
+        return self._button_was_pressed("DPad_Up")
+
+    def dpad_down_pressed(self) -> bool:
+        """
+        Check if the D-pad is pressed down.
+
+        :return: True if the D-pad is pressed down, False otherwise.
+        """
+        return self._button_pressed("DPad_Down", self.this_controller.getPOV() == 180)
+
+    def dpad_down_was_pressed(self) -> bool:
+        """
+        Check if the D-pad was pressed down since the last check.
+
+        :return: True if the the D-pad was pressed down since the last check, False otherwise.
+        """
+        return self._button_was_pressed("DPad_Down")
+
+    def dpad_left_pressed(self) -> bool:
+        """
+        Check if the D-pad is pressed left.
+
+        :return: True if the D-pad is pressed left, False otherwise.
+        """
+        return self._button_pressed("DPad_Left", self.this_controller.getPOV() == 270)
+
+    def dpad_left_was_pressed(self) -> bool:
+        """
+        Check if the D-pad was pressed left since the last check.
+
+        :return: True if the the D-pad was pressed left since the last check, False otherwise.
+        """
+        return self._button_was_pressed("DPad_Left")
+
+    def dpad_right_pressed(self) -> bool:
+        """
+        Check if the D-pad is pressed right.
+
+        :return: True if the D-pad is pressed right, False otherwise.
+        """
+        return self._button_pressed("DPad_Right", self.this_controller.getPOV() == 90)
+
+    def dpad_right_was_pressed(self) -> bool:
+        """
+        Check if the D-pad was pressed right since the last check.
+
+        :return: True if the the D-pad was pressed right since the last check, False otherwise.
+        """
+        return self._button_was_pressed("DPad_Right")
+
+    def left_bumper_pressed(self) -> bool:
+        """
+        Check if the left bumper is pressed.
+
+        :return: True if the left bumper is pressed, False otherwise.
+        """
+        return self._button_pressed("Left_Bumper", self.this_controller.getLeftBumper())
+
+    def left_bumper_was_pressed(self) -> bool:
+        """
+        Check if the left bumper was pressed since the last check.
+
+        :return: True if the left bumper was pressed since the last check, False otherwise.
+        """
+        return self._button_was_pressed("Left_Bumper")
+
+    def right_bumper_pressed(self) -> bool:
+        """
+        Check if the right bumper is pressed.
+
+        :return: True if the right bumper is pressed, False otherwise.
+        """
+        return self._button_pressed(
+            "Right_Bumper", self.this_controller.getRightBumper()
+        )
+
+    def right_bumper_was_pressed(self) -> bool:
+        """
+        Check if the right bumper was pressed since the last check.
+
+        :return: True if the right bumper was pressed since the last check, False otherwise.
+        """
+        return self._button_was_pressed("Right_Bumper")
+
+    def left_trigger_pressed(self) -> float:
+        """
+        Get the value of the left trigger.
+
+        :return: The value of the left trigger, ranging from 0.0 to 1.0.
+        """
+        self._button_pressed(
+            "Left_Trigger", self.this_controller.getLeftTriggerAxis() >= 0.5
+        )
+        return self.this_controller.getLeftTriggerAxis()
+
+    def left_trigger_was_pressed(self) -> bool:
+        """
+        Check if the left trigger was pressed since the last check.
+
+        :return: True if the left trigger was pressed since the last check, False otherwise.
+        """
+        return self._button_was_pressed("Left_Trigger")
+
+    def right_trigger_pressed(self) -> float:
+        """
+        Get the value of the right trigger.
+
+        :return: The value of the right trigger, ranging from 0.0 to 1.0.
+        """
+        self._button_pressed(
+            "Right_Trigger", self.this_controller.getRightTriggerAxis() >= 0.5
+        )
+        return self.this_controller.getRightTriggerAxis()
+
+    def right_trigger_was_pressed(self) -> bool:
+        """
+        Check if the right trigger was pressed since the last check.
+
+        :return: True if the right trigger was pressed since the last check, False otherwise.
+        """
+        return self._button_was_pressed("Right_Trigger")
+
+    def start_button_pressed(self) -> bool:
+        """
+        Check if the start button is pressed.
+
+        :return: True if the start button is pressed, False otherwise.
+        """
+        return self._button_pressed("Start", self.this_controller.getStartButton())
+
+    def start_button_was_pressed(self) -> bool:
+        """
+        Check if the start button was pressed since the last check.
+
+        :return: True if the start button was pressed since the last check, False otherwise.
+        """
+        return self._button_was_pressed("Start")
+
+    def back_button_pressed(self) -> bool:
+        """
+        Check if the back button is pressed.
+
+        :return: True if the back button is pressed, False otherwise.
+        """
+        return self._button_pressed("Back", self.this_controller.getBackButton())
+
+    def back_button_was_pressed(self) -> bool:
+        """
+        Check if the back button was pressed since the last check.
+
+        :return: True if the back button was pressed since the last check, False otherwise.
+        """
+        return self._button_was_pressed("Back")

components/drive.py

@@ -0,0 +1,316 @@
+"""
+components/drive.py – DriveSubsystem ported to MagicBot component.
+
+Owns four SwerveModule instances.  All swerve math uses standard wpimath.
+Integrates with PathPlannerLib and Limelight vision.
+"""
+
+import math
+import os
+
+import magicbot
+from pathplannerlib.auto import AutoBuilder
+from pathplannerlib.config import PIDConstants, RobotConfig
+from pathplannerlib.controller import PPHolonomicDriveController
+from pathplannerlib.logging import PathPlannerLogging
+from phoenix6.hardware import Pigeon2
+from wpilib import DriverStation, RobotState, SendableChooser, SmartDashboard, getDeployDirectory
+from wpimath.estimator import SwerveDrive4PoseEstimator
+from wpimath.geometry import Pose2d, Pose3d
+from wpimath.kinematics import (
+    ChassisSpeeds,
+    SwerveDrive4Kinematics,
+)
+
+import constants
+import LimelightHelpers
+from components.swerve_module import SwerveModule
+
+
+class DriveComponent:
+    """
+    MagicBot component that drives four swerve modules and fuses vision
+    measurements via a SwerveDrive4PoseEstimator.
+    """
+
+    def setup(self) -> None:
+        # ── Swerve modules ─────────────────────────────────────────────────
+        self._front_left = SwerveModule(
+            name="Front Left",
+            drive_id=constants.SwerveDriveIDs.FRONT_LEFT_DRIVE_ID,
+            steer_id=constants.SwerveDriveIDs.FRONT_LEFT_STEER_ID,
+            encoder_id=constants.SwerveDriveIDs.FRONT_LEFT_ENCODER_ID,
+            steer_offset_rotations=constants.SwerveDriveOffsets.FRONT_LEFT_MOTOR_OFFSET,
+            drive_inverted=False,
+            canbus="Drive",
+        )
+        self._front_right = SwerveModule(
+            name="Front Right",
+            drive_id=constants.SwerveDriveIDs.FRONT_RIGHT_DRIVE_ID,
+            steer_id=constants.SwerveDriveIDs.FRONT_RIGHT_STEER_ID,
+            encoder_id=constants.SwerveDriveIDs.FRONT_RIGHT_ENCODER_ID,
+            steer_offset_rotations=constants.SwerveDriveOffsets.FRONT_RIGHT_MOTOR_OFFSET,
+            drive_inverted=True,   # Front Right is inverted
+            canbus="Drive",
+        )
+        self._back_left = SwerveModule(
+            name="Back Left",
+            drive_id=constants.SwerveDriveIDs.BACK_LEFT_DRIVE_ID,
+            steer_id=constants.SwerveDriveIDs.BACK_LEFT_STEER_ID,
+            encoder_id=constants.SwerveDriveIDs.BACK_LEFT_ENCODER_ID,
+            steer_offset_rotations=constants.SwerveDriveOffsets.BACK_LEFT_MOTOR_OFFSET,
+            drive_inverted=False,
+            canbus="Drive",
+        )
+        self._back_right = SwerveModule(
+            name="Back Right",
+            drive_id=constants.SwerveDriveIDs.BACK_RIGHT_DRIVE_ID,
+            steer_id=constants.SwerveDriveIDs.BACK_RIGHT_STEER_ID,
+            encoder_id=constants.SwerveDriveIDs.BACK_RIGHT_ENCODER_ID,
+            steer_offset_rotations=constants.SwerveDriveOffsets.BACK_RIGHT_MOTOR_OFFSET,
+            drive_inverted=True,   # Back Right is inverted
+            canbus="Drive",
+        )
+        self._modules = [
+            self._front_left,
+            self._front_right,
+            self._back_left,
+            self._back_right,
+        ]
+
+        # ── Kinematics ─────────────────────────────────────────────────────
+        self._kinematics = SwerveDrive4Kinematics(
+            constants.SwerveDriveModuleLocations.FRONT_LEFT,
+            constants.SwerveDriveModuleLocations.FRONT_RIGHT,
+            constants.SwerveDriveModuleLocations.BACK_LEFT,
+            constants.SwerveDriveModuleLocations.BACK_RIGHT,
+        )
+
+        # ── Gyro ───────────────────────────────────────────────────────────
+        self._pigeon = Pigeon2(constants.ExtraIDEntities.PIGEON_ID, "Drive")
+
+        # ── Pose estimator ─────────────────────────────────────────────────
+        self._pose_estimator = SwerveDrive4PoseEstimator(
+            self._kinematics,
+            self._pigeon.getRotation2d(),
+            self._module_positions(),
+            Pose2d(),
+        )
+        # 3 std-dev values: (x_std, y_std, heading_std)
+        self._pose_estimator.setVisionMeasurementStdDevs((0.7, 0.7, 0.1))
+
+        # ── Desired chassis speeds (reset each loop) ────────────────────────
+        self._desired_speeds = ChassisSpeeds()
+
+        # ── PathPlanner target pose (for @feedback) ────────────────────────
+        self._pp_target_pose = Pose2d()
+
+        # ── PathPlanner / AutoBuilder ──────────────────────────────────────
+        # _DriveSubsystemAdapter is imported from robot.py at runtime to
+        # avoid circular imports; it is passed in via inject_drive_adapter().
+        self._drive_adapter = None
+        self._auto_chooser: SendableChooser | None = None
+
+    def inject_drive_adapter(self, adapter, robot_config: RobotConfig) -> None:
+        """
+        Called by robot.py after all objects are constructed.
+        Finalises AutoBuilder configuration and builds the auto chooser.
+        """
+        AutoBuilder.configure(
+            self.get_robot_pose_2d,                   # pose supplier
+            self.set_pose,                            # pose resetter
+            self.get_robot_relative_speeds,           # speeds supplier
+            lambda speeds, ff: self.set_chassis_speeds(speeds),  # consumer (no negation)
+            PPHolonomicDriveController(
+                PIDConstants(5, 0, 0),
+                PIDConstants(math.pi, 0, 0),
+            ),
+            robot_config,
+            self._is_red_alliance,                    # flip-path predicate
+            adapter,                                  # dummy Subsystem
+        )
+
+        # Build auto chooser manually so a bad path file skips that auto instead
+        # of crashing the entire chooser (PathPlanner throws ZeroDivisionError on
+        # paths with consecutive rotation targets at the same point).
+        autos_dir = os.path.join(getDeployDirectory(), "pathplanner", "autos")
+        self._auto_chooser = SendableChooser()
+        self._auto_chooser.setDefaultOption("Do Nothing", None)
+        if os.path.isdir(autos_dir):
+            for filename in os.listdir(autos_dir):
+                auto_name = filename.removesuffix(".auto")
+                try:
+                    self._auto_chooser.addOption(auto_name, AutoBuilder.buildAuto(auto_name))
+                except Exception as build_err:
+                    SmartDashboard.putString(f"Auto/{auto_name} error", str(build_err))
+        SmartDashboard.putData("Auto Chooser", self._auto_chooser)
+
+        PathPlannerLogging.setLogTargetPoseCallback(
+            lambda pose: setattr(self, "_pp_target_pose", pose)
+        )
+
+    # ── Public drive API ───────────────────────────────────────────────────────
+
+    def set_chassis_speeds_field_relative(
+        self, vx: float, vy: float, omega: float
+    ) -> None:
+        """
+        Convert joystick inputs (with quadratic scaling) to robot-relative
+        ChassisSpeeds and store them to be applied in execute().
+        """
+        scaled_vx = vx * abs(vx) * constants.RobotChassisLimits.MAX_ROBOT_LINEAR_VELOCITY
+        scaled_vy = vy * abs(vy) * constants.RobotChassisLimits.MAX_ROBOT_LINEAR_VELOCITY
+        scaled_omega = omega * abs(omega) * constants.RobotChassisLimits.MAX_ROBOT_ANGULAR_VELOCITY
+
+        self._desired_speeds = ChassisSpeeds.fromFieldRelativeSpeeds(
+            scaled_vx,
+            scaled_vy,
+            scaled_omega,
+            self._pigeon.getRotation2d(),
+        )
+
+    def set_chassis_speeds(self, speeds: ChassisSpeeds) -> None:
+        """Direct robot-relative speed input (used by PathPlanner consumer)."""
+        self._desired_speeds = speeds
+
+    def stop_drive(self) -> None:
+        self._desired_speeds = ChassisSpeeds()
+
+    def rotate_chassis(self, rot_speed: float) -> None:
+        self._desired_speeds = ChassisSpeeds.fromRobotRelativeSpeeds(
+            0, 0, rot_speed, self._pigeon.getRotation2d()
+        )
+
+    def get_bot_pose(self) -> Pose3d:
+        return Pose3d(self._pose_estimator.getEstimatedPosition())
+
+    def get_robot_pose_2d(self) -> Pose2d:
+        return self._pose_estimator.getEstimatedPosition()
+
+    def get_robot_relative_speeds(self) -> ChassisSpeeds:
+        states = self._module_states()
+        return self._kinematics.toChassisSpeeds(states)
+
+    def set_pose(self, pose: Pose2d) -> None:
+        self._pose_estimator.resetPosition(
+            self._pigeon.getRotation2d(),
+            self._module_positions(),
+            pose,
+        )
+
+    def set_brake_mode(self, brake: bool) -> None:
+        from phoenix6.configs import MotorOutputConfigs
+        from phoenix6.signals import NeutralModeValue
+        mode = NeutralModeValue.BRAKE if brake else NeutralModeValue.COAST
+        for module in self._modules:
+            cfg = MotorOutputConfigs().with_neutral_mode(mode)
+            module._drive_motor.configurator.apply(cfg)
+            module._steer_motor.configurator.apply(cfg)
+
+    def get_auto_command(self):
+        if self._auto_chooser is not None:
+            return self._auto_chooser.getSelected()
+        return None
+
+    # ── @feedback properties ───────────────────────────────────────────────────
+
+    @magicbot.feedback
+    def robot_x(self) -> float:
+        return self._pose_estimator.getEstimatedPosition().X()
+
+    @magicbot.feedback
+    def robot_y(self) -> float:
+        return self._pose_estimator.getEstimatedPosition().Y()
+
+    @magicbot.feedback
+    def robot_heading_degrees(self) -> float:
+        return self._pose_estimator.getEstimatedPosition().rotation().degrees()
+
+    @magicbot.feedback
+    def pathplanner_target_x(self) -> float:
+        return self._pp_target_pose.X()
+
+    @magicbot.feedback
+    def pathplanner_target_y(self) -> float:
+        return self._pp_target_pose.Y()
+
+    # ── MagicBot lifecycle ─────────────────────────────────────────────────────
+
+    def execute(self) -> None:
+        # Apply desired speeds through kinematics
+        module_states = self._kinematics.toSwerveModuleStates(self._desired_speeds)
+
+        # Desaturate wheel speeds to enforce linear velocity limit
+        SwerveDrive4Kinematics.desaturateWheelSpeeds(
+            module_states,
+            constants.RobotChassisLimits.MAX_ROBOT_LINEAR_VELOCITY,
+        )
+
+        # Send states to modules
+        for module, state in zip(self._modules, module_states):
+            module.set_desired_state(state)
+
+        # Reset desired speeds so the robot stops if nothing sets them this loop
+        self._desired_speeds = ChassisSpeeds()
+
+        # Update pose estimator with gyro + module positions
+        self._pose_estimator.update(
+            self._pigeon.getRotation2d(),
+            self._module_positions(),
+        )
+
+        heading_deg = self.get_robot_pose_2d().rotation().degrees()
+
+        # Update Limelight orientation so MegaTag2 knows where the robot is facing
+        LimelightHelpers.SetRobotOrientation(
+            constants.CameraConstants.FRONT_CAMERA_NAME, heading_deg, 0, 0, 0, 0, 0
+        )
+        LimelightHelpers.SetRobotOrientation(
+            constants.CameraConstants.BACK_CAMERA_NAME, heading_deg, 0, 0, 0, 0, 0
+        )
+
+        # IMU mode: 1 when disabled (external), 4 when enabled (fused)
+        imu_mode = 1 if RobotState.isDisabled() else 4
+        LimelightHelpers.SetIMUMode(constants.CameraConstants.FRONT_CAMERA_NAME, imu_mode)
+        LimelightHelpers.SetIMUMode(constants.CameraConstants.BACK_CAMERA_NAME, imu_mode)
+
+        # MegaTag2 vision fusion
+        front_estimate = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(
+            constants.CameraConstants.FRONT_CAMERA_NAME
+        )
+        back_estimate = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(
+            constants.CameraConstants.BACK_CAMERA_NAME
+        )
+
+        if front_estimate is not None and front_estimate.tagCount >= 2:
+            self._pose_estimator.addVisionMeasurement(
+                front_estimate.pose,
+                front_estimate.timestampSeconds,
+            )
+
+        if back_estimate is not None and back_estimate.tagCount >= 2:
+            self._pose_estimator.addVisionMeasurement(
+                back_estimate.pose,
+                back_estimate.timestampSeconds,
+            )
+
+        # Publish per-module telemetry
+        for module in self._modules:
+            module.publish_telemetry()
+
+    # ── Helpers ────────────────────────────────────────────────────────────────
+
+    def _module_positions(self):
+        fl, fr, bl, br = self._modules
+        return (fl.get_position(), fr.get_position(), bl.get_position(), br.get_position())
+
+    def _module_states(self):
+        fl, fr, bl, br = self._modules
+        return (fl.get_state(), fr.get_state(), bl.get_state(), br.get_state())
+
+    @staticmethod
+    def _is_red_alliance() -> bool:
+        alliance = DriverStation.getAlliance()
+        if alliance:
+            return alliance == DriverStation.Alliance.kRed
+        return False

components/hopper.py

@@ -0,0 +1,82 @@
+"""
+components/hopper.py – HopperSubsystem ported to MagicBot component.
+
+Hardware:
+  - TalonFX  spindexer motor
+  - TalonFX  kicker motor
+"""
+
+import magicbot
+from phoenix6.configs import MotorOutputConfigs, Slot0Configs, TalonFXConfiguration
+from phoenix6.hardware import TalonFX
+
+import constants
+
+
+class HopperComponent:
+    """MagicBot component that controls the hopper (spindexer + kicker)."""
+
+    def setup(self) -> None:
+        """
+        MagicBot lifecycle hook — instantiate and configure all hopper hardware.
+
+        Called once by MagicBot after all component objects are created but
+        before the robot enters any enabled mode.
+        """
+        # ── Spindexer ──────────────────────────────────────────────────────
+        self._spindexer_motor = TalonFX(constants.HopperConstants.SPINDEXER_MOTOR_ID)
+        spindexer_cfg = TalonFXConfiguration()
+        spindexer_cfg.motor_output = MotorOutputConfigs().with_inverted(
+            constants.HopperConstants.SPINDEXER_MOTOR_DIRECTION
+        )
+        spindexer_cfg.slot0 = Slot0Configs().with_k_p(0).with_k_i(0).with_k_d(0).with_k_s(0)
+        self._spindexer_motor.configurator.apply(spindexer_cfg)
+
+        # ── Kicker ─────────────────────────────────────────────────────────
+        self._kicker_motor = TalonFX(constants.HopperConstants.KICKER_MOTOR_ID)
+        kicker_cfg = TalonFXConfiguration()
+        kicker_cfg.motor_output = MotorOutputConfigs().with_inverted(
+            constants.HopperConstants.KICKER_MOTOR_DIRECTION
+        )
+        kicker_cfg.slot0 = Slot0Configs().with_k_p(0).with_k_i(0).with_k_d(0).with_k_s(0)
+        self._kicker_motor.configurator.apply(kicker_cfg)
+
+        self._kicker_velocity = self._kicker_motor.get_velocity()
+
+    # ── Public API ─────────────────────────────────────────────────────────────
+
+    def feed_kicker(self) -> None:
+        """Drive the kicker motor forward at 75 % output to push a note into the launcher."""
+        self._kicker_motor.set(0.75)
+
+    def reverse_kicker(self) -> None:
+        """Drive the kicker motor in reverse at 50 % output to un-jam a stuck note."""
+        self._kicker_motor.set(-0.5)
+
+    def stop_kicker(self) -> None:
+        """Stop the kicker motor."""
+        self._kicker_motor.stop_motor()
+
+    def spindexer_on(self) -> None:
+        """Drive the spindexer forward at 75 % output to advance notes toward the kicker."""
+        self._spindexer_motor.set(0.75)
+
+    def spindexer_reverse(self) -> None:
+        """Drive the spindexer in reverse at 75 % output to clear a jam."""
+        self._spindexer_motor.set(-0.75)
+
+    def spindexer_off(self) -> None:
+        """Stop the spindexer motor."""
+        self._spindexer_motor.stop_motor()
+
+    # ── Telemetry ──────────────────────────────────────────────────────────────
+
+    @magicbot.feedback
+    def kicker_speed_rps(self) -> float:
+        return self._kicker_velocity.refresh().value_as_double
+
+    # ── MagicBot lifecycle ─────────────────────────────────────────────────────
+
+    def execute(self) -> None:
+        # No periodic logic required; all control is driven by external callers.
+        pass

components/intake.py

@@ -0,0 +1,84 @@
+"""
+components/intake.py – IntakeSubsystem ported to MagicBot component.
+
+Hardware:
+  - TalonFX         intake motor (Phoenix 6)
+  - WPI_TalonSRX    deploy motor (Phoenix 5) with forward/reverse limit switches
+"""
+
+import wpilib
+from phoenix5 import LimitSwitchNormal, LimitSwitchSource, WPI_TalonSRX
+from phoenix6.configs import MotorOutputConfigs, TalonFXConfiguration
+from phoenix6.hardware import TalonFX
+
+import constants
+
+
+class IntakeComponent:
+    """MagicBot component that controls the intake and deploy mechanism."""
+
+    def setup(self) -> None:
+        # ── Intake motor (Phoenix 6 TalonFX) ───────────────────────────────
+        self._intake_motor = TalonFX(constants.IntakeConstants.INTAKE_MOTOR_ID)
+        intake_cfg = TalonFXConfiguration()
+        intake_cfg.motor_output = MotorOutputConfigs().with_inverted(
+            constants.IntakeConstants.INTAKE_MOTOR_DIRECTION
+        )
+        self._intake_motor.configurator.apply(intake_cfg)
+
+        # ── Deploy motor (Phoenix 5 WPI_TalonSRX) ──────────────────────────
+        self._deploy_motor = WPI_TalonSRX(constants.IntakeConstants.DEPLOY_MOTOR_ID)
+        self._deploy_motor.configForwardLimitSwitchSource(
+            LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen
+        )
+        self._deploy_motor.configReverseLimitSwitchSource(
+            LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen
+        )
+        self._deploy_motor.setInverted(constants.IntakeConstants.DEPLOY_MOTOR_INVERTED)
+
+        # Controller reference — injected by robot.py after setup
+        self._controller: wpilib.XboxController | None = None
+
+    # ── Public API ─────────────────────────────────────────────────────────────
+
+    def set_controller(self, controller: wpilib.XboxController) -> None:
+        """Inject the driver Xbox controller for rumble feedback."""
+        self._controller = controller
+
+    def deploy(self) -> None:
+        """Run the deploy motor forward briefly (call once; timing handled externally)."""
+        self._deploy_motor.set(1.0)
+
+    def stop_deploy(self) -> None:
+        self._deploy_motor.stopMotor()
+
+    def reset_deploy(self) -> None:
+        """Retract the intake deploy mechanism."""
+        self._deploy_motor.set(-1.0)
+
+    def set_speed(self, speed: float) -> None:
+        self._intake_motor.set(speed)
+
+    def stop_intake(self) -> None:
+        self._intake_motor.stopMotor()
+        if self._controller is not None:
+            self._controller.setRumble(wpilib.XboxController.RumbleType.kBothRumble, 0.0)
+
+    def intake(self) -> None:
+        """Toggle intake on/off with controller rumble feedback."""
+        if self._intake_motor.get() == 0:
+            self.set_speed(-0.5)
+            if self._controller is not None:
+                self._controller.setRumble(wpilib.XboxController.RumbleType.kBothRumble, 0.5)
+        else:
+            self._intake_motor.stopMotor()
+            if self._controller is not None:
+                self._controller.setRumble(wpilib.XboxController.RumbleType.kBothRumble, 0.0)
+
+    def reverse_intake(self) -> None:
+        self.set_speed(0.5)
+
+    # ── MagicBot lifecycle ─────────────────────────────────────────────────────
+
+    def execute(self) -> None:
+        pass

components/swerve_module.py

@@ -0,0 +1,155 @@
+"""
+components/swerve_module.py – Plain Python class (NOT a MagicBot component).
+
+Four instances are owned by DriveComponent. Encapsulates all per-module hardware:
+  - phoenix6 TalonFX  drive motor
+  - phoenix6 TalonFX  steer motor
+  - phoenix6 CANcoder
+"""
+
+import math
+
+import ntcore
+from phoenix6.configs import (
+    CANcoderConfiguration,
+    FeedbackConfigs,
+    MagnetSensorConfigs,
+    MotorOutputConfigs,
+    Slot0Configs,
+    TalonFXConfiguration,
+)
+from phoenix6.controls import PositionVoltage, VelocityVoltage
+from phoenix6.hardware import CANcoder, TalonFX
+from phoenix6.signals import (
+    FeedbackSensorSourceValue,
+    InvertedValue,
+    SensorDirectionValue,
+)
+from wpimath.geometry import Rotation2d
+from wpimath.kinematics import SwerveModulePosition, SwerveModuleState
+
+# Physical constants shared by all modules
+GEAR_RATIO: float = 8.14
+WHEEL_DIAMETER_METERS: float = 0.1016  # 4 inches
+WHEEL_CIRCUMFERENCE_METERS: float = WHEEL_DIAMETER_METERS * 3.14159265358979
+
+
+class SwerveModule:
+    """
+    Plain Python class (not a MagicBot component).
+    Encapsulates a single swerve module: drive motor, steer motor, CANcoder.
+    """
+
+    def __init__(
+        self,
+        name: str,
+        drive_id: int,
+        steer_id: int,
+        encoder_id: int,
+        steer_offset_rotations: float,
+        drive_inverted: bool,
+        canbus: str = "Drive",
+    ) -> None:
+        self._name = name
+
+        # ── CANcoder ───────────────────────────────────────────────────────
+        self._encoder = CANcoder(encoder_id, canbus)
+        encoder_cfg = CANcoderConfiguration()
+        encoder_cfg.magnet_sensor = (
+            MagnetSensorConfigs()
+            .with_magnet_offset(steer_offset_rotations)
+            .with_sensor_direction(SensorDirectionValue.COUNTER_CLOCKWISE_POSITIVE)
+        )
+        self._encoder.configurator.apply(encoder_cfg)
+
+        # ── Drive motor (TalonFX) ──────────────────────────────────────────
+        self._drive_motor = TalonFX(drive_id, canbus)
+        drive_cfg = TalonFXConfiguration()
+        drive_cfg.slot0 = (
+            Slot0Configs()
+            .with_k_p(0.1)
+            .with_k_i(0.0)
+            .with_k_d(0.01)
+            .with_k_v(0.2)
+        )
+        drive_cfg.motor_output = MotorOutputConfigs().with_inverted(
+            InvertedValue.CLOCKWISE_POSITIVE if drive_inverted else InvertedValue.COUNTER_CLOCKWISE_POSITIVE
+        )
+        self._drive_motor.configurator.apply(drive_cfg)
+        self._drive_velocity_signal = self._drive_motor.get_velocity()
+        self._drive_position_signal = self._drive_motor.get_position()
+
+        # ── Steer motor (TalonFX) ──────────────────────────────────────────
+        self._steer_motor = TalonFX(steer_id, canbus)
+        steer_cfg = TalonFXConfiguration()
+        steer_cfg.slot0 = (
+            Slot0Configs()
+            .with_k_p(20)
+            .with_k_i(0)
+            .with_k_d(0.0)
+            .with_k_v(0)
+        )
+        # Use the CANcoder as the absolute feedback source for the steer motor
+        steer_cfg.feedback = (
+            FeedbackConfigs()
+            .with_feedback_remote_sensor_id(encoder_id)
+            .with_feedback_sensor_source(FeedbackSensorSourceValue.REMOTE_CANCODER)
+        )
+        self._steer_motor.configurator.apply(steer_cfg)
+        self._steer_position_signal = self._steer_motor.get_position()
+
+        # Controls
+        self._drive_control = VelocityVoltage(0)
+        self._steer_control = PositionVoltage(0)
+
+        # Desired state (for telemetry)
+        self._desired_state = SwerveModuleState(0, Rotation2d())
+
+        # ── NetworkTables telemetry entries ────────────────────────────────
+        inst = ntcore.NetworkTableInstance.getDefault()
+        tbl = inst.getTable(f"SwerveModules/{name}")
+        self._nt_desired_speed = tbl.getDoubleTopic("desired_speed_mps").publish()
+        self._nt_desired_angle = tbl.getDoubleTopic("desired_angle_deg").publish()
+        self._nt_actual_speed  = tbl.getDoubleTopic("actual_speed_mps").publish()
+        self._nt_actual_angle  = tbl.getDoubleTopic("actual_angle_deg").publish()
+
+    # ── Public API ─────────────────────────────────────────────────────────────
+
+    def set_desired_state(self, state: SwerveModuleState) -> None:
+        """Optimize the state and apply drive velocity + steer position controls."""
+        state.optimize(self._get_steer_angle())  # mutates in-place; returns None in WPILib 2026
+        self._desired_state = state
+
+        # Drive: convert m/s → rotations/s
+        drive_rps = (state.speed / WHEEL_CIRCUMFERENCE_METERS) * GEAR_RATIO
+        self._drive_motor.set_control(self._drive_control.with_velocity(drive_rps))
+
+        # Steer: position in rotations (CANcoder feedback is in rotations)
+        steer_rotations = state.angle.radians() / (2 * math.pi)
+        self._steer_motor.set_control(self._steer_control.with_position(steer_rotations))
+
+    def get_state(self) -> SwerveModuleState:
+        """Return current speed (m/s) and angle from motor signals."""
+        velocity_rps = self._drive_velocity_signal.refresh().value_as_double
+        speed_mps = (velocity_rps / GEAR_RATIO) * WHEEL_CIRCUMFERENCE_METERS
+        return SwerveModuleState(speed_mps, self._get_steer_angle())
+
+    def get_position(self) -> SwerveModulePosition:
+        """Return current distance (m) and angle for odometry update."""
+        position_rotations = self._drive_position_signal.refresh().value_as_double
+        distance_meters = (position_rotations / GEAR_RATIO) * WHEEL_CIRCUMFERENCE_METERS
+        return SwerveModulePosition(distance_meters, self._get_steer_angle())
+
+    def publish_telemetry(self) -> None:
+        """Write module telemetry to NetworkTables. Called each loop by DriveComponent."""
+        actual = self.get_state()
+        self._nt_desired_speed.set(self._desired_state.speed)
+        self._nt_desired_angle.set(self._desired_state.angle.degrees())
+        self._nt_actual_speed.set(actual.speed)
+        self._nt_actual_angle.set(actual.angle.degrees())
+
+    # ── Helpers ────────────────────────────────────────────────────────────────
+
+    def _get_steer_angle(self) -> Rotation2d:
+        rotations = self._steer_position_signal.refresh().value_as_double
+        return Rotation2d.fromRotations(rotations)

components/turret.py

@@ -0,0 +1,233 @@
+"""
+components/turret.py – TurretSubsystem ported to MagicBot component.
+
+Hardware:
+  - TalonFX   turret motor (Phoenix 6)
+  - SparkMax   launcher motor (REV, brushless)
+  - DigitalInput  limit switch
+  - Servo         hood
+"""
+
+import enum
+import math
+
+import magicbot
+import wpilib
+from phoenix6.configs import (
+    CurrentLimitsConfigs,
+    FeedbackConfigs,
+    MotorOutputConfigs,
+    Slot0Configs,
+    SoftwareLimitSwitchConfigs,
+    TalonFXConfiguration,
+)
+from phoenix6.controls import PositionVoltage
+from phoenix6.hardware import TalonFX
+from phoenix6.signals import InvertedValue
+from rev import (
+    ClosedLoopSlot,
+    FeedbackSensor,
+    PersistMode,
+    ResetMode,
+    SparkBase,
+    SparkLowLevel,
+    SparkMax,
+    SparkMaxConfig,
+)
+from wpimath.geometry import Pose3d, Rotation2d
+
+import constants
+
+
+class TurretState(enum.Enum):
+    INIT = enum.auto()
+    HOMING = enum.auto()
+    READY = enum.auto()
+
+
+class TurretComponent:
+    """MagicBot component that controls the turret, launcher, and hood."""
+
+    def setup(self) -> None:
+        # ── State ──────────────────────────────────────────────────────────
+        self._turret_state = TurretState.INIT
+        self._turret_offset_rotations: float = 0.0
+        self._forward_limit: float = 0.0
+        self._reverse_limit: float = 0.0
+        self._launcher_setpoint_rpm: float = 0.0
+
+        # ── Limit switch ───────────────────────────────────────────────────
+        self._turret_limit_switch = wpilib.DigitalInput(
+            constants.ShooterConstants.TURRET_LIMIT_CHANNEL
+        )
+
+        # ── Turret motor (TalonFX) ─────────────────────────────────────────
+        self._turret_motor = TalonFX(constants.ShooterConstants.TURRET_MOTOR_ID)
+        turret_cfg = TalonFXConfiguration()
+        turret_cfg.slot0 = (
+            Slot0Configs()
+            .with_k_p(6)
+            .with_k_i(0)
+            .with_k_d(0.3)
+            .with_k_s(0)
+        )
+        turret_cfg.motor_output = MotorOutputConfigs().with_inverted(
+            InvertedValue.CLOCKWISE_POSITIVE
+        )
+        turret_cfg.current_limits = CurrentLimitsConfigs().with_stator_current_limit(50)
+        turret_cfg.software_limit_switch = (
+            SoftwareLimitSwitchConfigs()
+            .with_forward_soft_limit_enable(False)
+            .with_reverse_soft_limit_enable(False)
+        )
+        turret_cfg.feedback = FeedbackConfigs().with_feedback_rotor_offset(0)
+        self._turret_motor.configurator.apply(turret_cfg)
+
+        self._turret_position_signal = self._turret_motor.get_position()
+        self._turret_error_signal = self._turret_motor.get_closed_loop_error()
+        self._turret_control = PositionVoltage(0)
+
+        # ── Launcher motor (SparkMax / REV NEO) ────────────────────────────
+        self._launcher_motor = SparkMax(
+            constants.ShooterConstants.LAUNCHER_MOTOR_ID,
+            SparkLowLevel.MotorType.kBrushless,
+        )
+        launcher_cfg = SparkMaxConfig()
+        # Apply equivalent of REV_NEO preset manually
+        launcher_cfg.inverted(constants.ShooterConstants.LAUNCHER_MOTOR_INVERTED)
+        launcher_cfg.voltageCompensation(10)
+        launcher_cfg.closedLoop.P(0.0002).I(0.000001).D(0.0005)
+        launcher_cfg.closedLoop.feedForward.kS(0.2).kV(0.0).kA(0.0)
+        launcher_cfg.closedLoop.setFeedbackSensor(FeedbackSensor.kPrimaryEncoder)
+        launcher_cfg.closedLoop.positionWrappingEnabled(False)
+        self._launcher_motor.configure(
+            launcher_cfg,
+            ResetMode.kResetSafeParameters,
+            PersistMode.kPersistParameters,
+        )
+        self._launcher_controller = self._launcher_motor.getClosedLoopController()
+
+        # ── Hood servo ─────────────────────────────────────────────────────
+        self._hood = wpilib.Servo(constants.ShooterConstants.HOOD_SERVO_CHANNEL)
+
+        self._turret_target_pose: Pose3d = Pose3d()
+
+    # ── Public API ─────────────────────────────────────────────────────────────
+
+    def set_launcher(self, speed_rpm: float) -> None:
+        self._launcher_setpoint_rpm = speed_rpm
+        if speed_rpm == 0:
+            self._launcher_motor.stopMotor()
+            return
+        self._launcher_controller.setSetpoint(
+            speed_rpm, SparkBase.ControlType.kVelocity, ClosedLoopSlot.kSlot0
+        )
+
+    def is_launcher_up_to_speed(self) -> bool:
+        # Currently always returns True (same as Java source)
+        return True
+
+    def set_target_elevation(self, elev: float) -> None:
+        pos = abs((elev - 300) / 300)
+        self._hood.set(pos)
+
+    def home_turret(self) -> None:
+        if self._turret_state != TurretState.INIT:
+            return
+        self._turret_motor.set(constants.ShooterConstants.HOMING_SPEED)
+        self._turret_state = TurretState.HOMING
+
+    def is_turret_ready(self) -> bool:
+        return self._turret_state == TurretState.READY
+
+    def get_turret_angle_robot_relative(self) -> float:
+        return self._get_turret_motor_position() / constants.ShooterConstants.TURRET_GEAR_RATIO
+
+    def set_turret_target_pose(self, target_pose: Pose3d) -> None:
+        self._turret_target_pose = target_pose
+
+    def get_turret_target_pose(self) -> Pose3d:
+        return self._turret_target_pose
+
+    def set_turret_field_relative(self, robot_pose, pos: float) -> None:
+        """Set turret angle field-relative, accounting for robot heading."""
+        if self._turret_state != TurretState.READY:
+            return
+
+        corrected_pos = -pos  # Flip angle to match CCW positive
+        robot_rotations = robot_pose.rotation().toRotation2d().radians() / (2 * math.pi)
+        turret_angle = (
+            Rotation2d.fromRotations(corrected_pos)
+            + Rotation2d.fromRotations(robot_rotations)
+            - Rotation2d.fromRotations(0.5)
+        )
+        self.set_turret_robot_relative(turret_angle.radians() / (2 * math.pi))
+
+    def set_turret_robot_relative(self, pos: float) -> None:
+        if self._turret_state != TurretState.READY:
+            return
+        corrected_pos = -pos  # Flip angle to match CCW positive
+        self.set_turret_motor_rotation(
+            corrected_pos * constants.ShooterConstants.TURRET_GEAR_RATIO
+        )
+
+    def set_turret_motor_rotation(self, pos: float) -> None:
+        pos_with_offset = pos + self._turret_offset_rotations
+        self._turret_motor.set_control(self._turret_control.with_position(pos_with_offset))
+
+    def is_turret_at_target(self) -> bool:
+        return abs(self._turret_error_signal.refresh().value_as_double) <= 0.1
+
+    # ── Feedback properties ────────────────────────────────────────────────────
+
+    @magicbot.feedback
+    def turret_state(self) -> str:
+        return self._turret_state.name
+
+    @magicbot.feedback
+    def turret_angle_robot_relative(self) -> float:
+        return self.get_turret_angle_robot_relative()
+
+    @magicbot.feedback
+    def turret_position_error(self) -> float:
+        return self._turret_error_signal.refresh().value_as_double
+
+    @magicbot.feedback
+    def launcher_speed_rpm(self) -> float:
+        return self._launcher_motor.getEncoder().getVelocity()
+
+    @magicbot.feedback
+    def turret_limit_switch(self) -> bool:
+        return self._turret_limit_switch.get()
+
+    # ── MagicBot lifecycle ─────────────────────────────────────────────────────
+
+    def execute(self) -> None:
+        # Homing state machine
+        if self._turret_state == TurretState.HOMING:
+            if self._turret_limit_switch.get():  # Switch tripped → homed
+                self._turret_motor.stopMotor()
+                raw_pos = self._turret_position_signal.refresh().value_as_double
+                if wpilib.RobotBase.isReal():
+                    self._turret_offset_rotations = raw_pos - 1.323
+                else:
+                    self._turret_offset_rotations = 0.0
+
+                self._turret_state = TurretState.READY
+                self._forward_limit = 1.5 + self._turret_offset_rotations
+                self._reverse_limit = -1.5 + self._turret_offset_rotations
+
+                # Enable software limits now that homing is complete
+                limit_cfg = (
+                    SoftwareLimitSwitchConfigs()
+                    .with_forward_soft_limit_enable(True)
+                    .with_forward_soft_limit_threshold(self._forward_limit)
+                    .with_reverse_soft_limit_enable(True)
+                    .with_reverse_soft_limit_threshold(self._reverse_limit)
+                )
+                self._turret_motor.configurator.apply(limit_cfg)
+
+    # ── Helpers ────────────────────────────────────────────────────────────────
+
+    def _get_turret_motor_position(self) -> float:
+        return self._turret_position_signal.refresh().value_as_double - self._turret_offset_rotations

constants.py

@@ -0,0 +1,202 @@
+"""
+constants.py – robot-wide constants for Castor 2026 (Python/MagicBot port).
+
+All Java "public static final" fields become Python class-level attributes.
+"""
+
+import enum
+import math
+
+from phoenix6.signals import InvertedValue
+from robotpy_apriltag import AprilTagField, AprilTagFieldLayout
+from wpimath.geometry import (
+    Pose3d,
+    Rotation3d,
+    Transform3d,
+    Translation2d,
+    Translation3d,
+)
+
+
+class OperatorConstants:
+    CONTROLLER_PORT: int = 0
+
+
+class CameraConstants:
+
+    class IDFilters:
+        RED_HUB_SHOOTING_IDS: list[int] = [8, 9, 10, 11]
+        BLUE_HUB_SHOOTING_IDS: list[int] = [27, 26, 25, 24]
+
+    BACK_CAMERA_NAME: str = "limelight-back"
+    BACK_LOCALIZATION_PIPELINE_NUMBER: int = 0
+    BACK_HUB_TRACKING_2D_PIPELINE_NUMBER: int = 1
+    BACK_HUB_TRACKING_3D_PIPELINE_NUMBER: int = 2
+
+    FRONT_CAMERA_NAME: str = "limelight-front"
+    FRONT_LOCALIZATION_PIPELINE_NUMBER: int = 0
+
+    BLUE_ZONE_COOR: list[float] = [0, 3.5]
+    RED_ZONE_COOR: list[float] = [13, 16]
+    NEUTRAL_ZONE_COOR: list[float] = [5.5, 11]
+    NEUTRAL1_ZONE_COOR: list[float] = [5.5, 11, 0, 3]
+    NEUTRAL2_ZONE_COOR: list[float] = [5.5, 11, 5, 8]
+    BLUE1_ZONE_COOR: list[float] = [0, 3.5, 0, 3]
+    BLUE2_ZONE_COOR: list[float] = [0, 3.5, 5, 8]
+    RED1_ZONE_COOR: list[float] = [13, 16, 0, 3]
+    RED2_ZONE_COOR: list[float] = [13, 16, 5, 8]
+
+
+# Swerve encoder offsets as of 3/2/26
+class SwerveDriveOffsets:
+    FRONT_LEFT_MOTOR_OFFSET: float = -0.336426
+    FRONT_RIGHT_MOTOR_OFFSET: float = 0.108887
+    BACK_LEFT_MOTOR_OFFSET: float = -0.492432
+    BACK_RIGHT_MOTOR_OFFSET: float = 0.470215
+
+
+# CAN ID numbering convention:
+#   1x = Front Left, 2x = Front Right, 3x = Back Left, 4x = Back Right
+#   Single-digit IDs are non-motor devices (e.g. Pigeon2)
+class SwerveDriveIDs:
+    FRONT_LEFT_STEER_ID: int = 11
+    FRONT_LEFT_DRIVE_ID: int = 12
+    FRONT_LEFT_ENCODER_ID: int = 13
+
+    FRONT_RIGHT_STEER_ID: int = 21
+    FRONT_RIGHT_DRIVE_ID: int = 22
+    FRONT_RIGHT_ENCODER_ID: int = 23
+
+    BACK_LEFT_STEER_ID: int = 31
+    BACK_LEFT_DRIVE_ID: int = 32
+    BACK_LEFT_ENCODER_ID: int = 33
+
+    BACK_RIGHT_STEER_ID: int = 41
+    BACK_RIGHT_DRIVE_ID: int = 42
+    BACK_RIGHT_ENCODER_ID: int = 43
+
+
+class RobotChassisLimits:
+    # Max linear velocity (m/s)
+    MAX_ROBOT_LINEAR_VELOCITY: float = 3.0
+    # Max linear acceleration (m/s²)
+    MAX_ROBOT_LINEAR_VELOCITY_ACCEL: float = 2.0
+    # Max angular velocity (rad/s)
+    MAX_ROBOT_ANGULAR_VELOCITY: float = math.pi
+    # Max angular acceleration (rad/s²)
+    MAX_ROBOT_ANGULAR_VELOCITY_ACCEL: float = math.pi
+
+
+class ExtraIDEntities:
+    PIGEON_ID: int = 1
+
+
+class IntakeConstants:
+    DEPLOY_MOTOR_ID: int = 17
+    DEPLOY_MOTOR_INVERTED: bool = True  # TODO: Test this
+
+    INTAKE_MOTOR_ID: int = 2
+    INTAKE_MOTOR_DIRECTION: InvertedValue = InvertedValue.CLOCKWISE_POSITIVE  # TODO: Test this
+
+
+class ShooterConstants:
+    LAUNCHER_MOTOR_ID: int = 3
+    LAUNCHER_MOTOR_INVERTED: bool = False
+
+    TURRET_MOTOR_ID: int = 4
+    TURRET_MOTOR_DIRECTION: InvertedValue = InvertedValue.CLOCKWISE_POSITIVE
+    TURRET_LIMIT_CHANNEL: int = 9
+
+    # Plain floats (degrees) replacing edu.wpi.first.units.Degrees.of(x)
+    LIMIT_AZIMUTH_POS: float = 165.0   # degrees
+    LIMIT_AZIMUTH_NEG: float = 5.0     # degrees
+
+    HOMING_SPEED: float = 0.05
+    TURRET_GEAR_RATIO: float = 6.0
+
+    HOOD_SERVO_CHANNEL: int = 0
+    HOOD_SERVO_INVERTED: bool = False   # TODO: Test this
+    HOOD_ENCODER_ID: int = 4
+
+    TARGET_ELEVATION_DIF: float = 1.0   # degrees
+    LIMIT_ELEVATION_POS: float = 90.0   # degrees
+    LIMIT_ELEVATION_NEG: float = 0.0    # degrees
+
+    DEFLECTOR_STORED_ANGLE: float = 0.0
+    DEFLECTOR_SET_ANGLE1: float = 100.0
+    DEFLECTOR_SET_ANGLE2: float = 200.0
+    DEFLECTOR_SET_ANGLE3: float = 300.0
+    DEFLECTOR_SERVO_RATIO: float = 1.0
+
+
+class HopperConstants:
+    KICKER_MOTOR_ID: int = 9
+    KICKER_MOTOR_DIRECTION: InvertedValue = InvertedValue.CLOCKWISE_POSITIVE
+
+    SPINDEXER_MOTOR_ID: int = 8
+    SPINDEXER_MOTOR_DIRECTION: InvertedValue = InvertedValue.COUNTER_CLOCKWISE_POSITIVE
+
+
+# ── Swerve module Translation2d locations ────────────────────────────────────
+# Robot is 22" × 22"; modules are at the corners.
+# Half of 22" = 11" = 0.2794 m from robot centre to each module.
+# WPILib convention: +X is forward, +Y is to the left.
+class SwerveDriveModuleLocations:
+    _HALF: float = 0.2794  # metres (11 inches)
+
+    FRONT_LEFT:  Translation2d = Translation2d( _HALF,  _HALF)
+    FRONT_RIGHT: Translation2d = Translation2d( _HALF, -_HALF)
+    BACK_LEFT:   Translation2d = Translation2d(-_HALF,  _HALF)
+    BACK_RIGHT:  Translation2d = Translation2d(-_HALF, -_HALF)
+
+
+# ── April-tag field layout (loaded once at import time) ───────────────────────
+_field_tags = AprilTagFieldLayout.loadField(AprilTagField.k2026RebuiltWelded)
+
+
+class FieldPoses:
+    BLUE_HUB_POSE: Pose3d = _field_tags.getTagPose(26).transformBy(
+        Transform3d(Translation3d(-0.5207, 0, 0), Rotation3d())
+    )
+    RED_HUB_POSE: Pose3d = _field_tags.getTagPose(10).transformBy(
+        Transform3d(Translation3d(-0.5207, 0, 0), Rotation3d())
+    )
+
+    # Passing targets — left/right from driver's perspective;
+    # origin at blue side of field; rotations are zero (don't matter).
+    BLUE_LEFT_ALLIANCE_ZONE_PASS_TARGET_POSE:  Pose3d = Pose3d(1.99,  6.35, 0, Rotation3d())
+    BLUE_RIGHT_ALLIANCE_ZONE_PASS_TARGET_POSE: Pose3d = Pose3d(1.99,  1.72, 0, Rotation3d())
+    BLUE_LEFT_NEUTRAL_ZONE_PASS_TARGET_POSE:   Pose3d = Pose3d(8.27,  6.35, 0, Rotation3d())
+    BLUE_RIGHT_NEUTRAL_ZONE_PASS_TARGET_POSE:  Pose3d = Pose3d(8.27,  1.72, 0, Rotation3d())
+
+    RED_LEFT_ALLIANCE_ZONE_PASS_TARGET_POSE:   Pose3d = Pose3d(14.55, 1.72, 0, Rotation3d())
+    RED_RIGHT_ALLIANCE_ZONE_PASS_TARGET_POSE:  Pose3d = Pose3d(14.55, 6.35, 0, Rotation3d())
+    RED_LEFT_NEUTRAL_ZONE_PASS_TARGET_POSE:    Pose3d = Pose3d(8.27,  1.72, 0, Rotation3d())
+    RED_RIGHT_NEUTRAL_ZONE_PASS_TARGET_POSE:   Pose3d = Pose3d(8.27,  6.35, 0, Rotation3d())
+
+
+# ── Shooting-region boundary lines ───────────────────────────────────────────
+class ShootingRegionDimensions:
+    BLUE_ALLIANCE_ZONE_REGION_X_MAX: float = 3.5
+    RED_ALLIANCE_ZONE_REGION_X_MIN:  float = 13.0
+    NEUTRAL_ZONE_REGION_X_MIN:       float = 5.5
+    NEUTRAL_ZONE_REGION_X_MAX:       float = 11.0
+    BLUE_RIGHT_REGION_Y_MAX:         float = 3.0
+    RED_LEFT_REGION_Y_MAX:           float = 3.0
+    BLUE_LEFT_REGION_Y_MIN:          float = 5.0
+    RED_RIGHT_REGION_Y_MIN:          float = 5.0
+
+
+# ── Shooting regions (Java enum → Python enum.Enum) ───────────────────────────
+class ShootingRegion(enum.Enum):
+    NON_SHOOTING_REGION               = enum.auto()
+    BLUE_OWN_ALLIANCE_ZONE            = enum.auto()
+    BLUE_LEFT_NEUTRAL_ZONE            = enum.auto()
+    BLUE_RIGHT_NEUTRAL_ZONE           = enum.auto()
+    BLUE_LEFT_OPPONENT_ALLIANCE_ZONE  = enum.auto()
+    BLUE_RIGHT_OPPONENT_ALLIANCE_ZONE = enum.auto()
+    RED_OWN_ALLIANCE_ZONE             = enum.auto()
+    RED_LEFT_NEUTRAL_ZONE             = enum.auto()
+    RED_RIGHT_NEUTRAL_ZONE            = enum.auto()
+    RED_LEFT_OPPONENT_ALLIANCE_ZONE   = enum.auto()
+    RED_RIGHT_OPPONENT_ALLIANCE_ZONE  = enum.auto()

deploy/example.txt

@@ -1,3 +1,3 @@
-Files placed in this directory will be deployed to the RoboRIO into the
+Files placed in this directory will be deployed to the RoboRIO into the
-'deploy' directory in the home folder. Use the 'Filesystem.getDeployDirectory' wpilib function
+'deploy' directory in the home folder. Use the 'Filesystem.getDeployDirectory' wpilib function
 to get a proper path relative to the deploy directory.

deploy/pathplanner/autos/1. Hub to Shoot.auto

@@ -1,88 +1,88 @@
-{
+{
-  "version": "2025.0",
+  "version": "2025.0",
-  "command": {
+  "command": {
-    "type": "sequential",
+    "type": "sequential",
-    "data": {
+    "data": {
-      "commands": [
+      "commands": [
-        {
+        {
-          "type": "path",
+          "type": "path",
-          "data": {
+          "data": {
-            "pathName": "1.1 Hub to Shoot"
+            "pathName": "1.1 Hub to Shoot"
-          }
+          }
-        },
+        },
-        {
+        {
-          "type": "deadline",
+          "type": "deadline",
-          "data": {
+          "data": {
-            "commands": [
+            "commands": [
-              {
+              {
-                "type": "named",
+                "type": "named",
-                "data": {
+                "data": {
-                  "name": "ChassisAlignShoot"
+                  "name": "ChassisAlignShoot"
-                }
+                }
-              },
+              },
-              {
+              {
-                "type": "wait",
+                "type": "wait",
-                "data": {
+                "data": {
-                  "waitTime": 10.0
+                  "waitTime": 10.0
-                }
+                }
-              }
+              }
-            ]
+            ]
-          }
+          }
-        },
+        },
-        {
+        {
-          "type": "path",
+          "type": "path",
-          "data": {
+          "data": {
-            "pathName": "1.2 Shoot to Depot Path"
+            "pathName": "1.2 Shoot to Depot Path"
-          }
+          }
-        },
+        },
-        {
+        {
-          "type": "deadline",
+          "type": "deadline",
-          "data": {
+          "data": {
-            "commands": [
+            "commands": [
-              {
+              {
-                "type": "named",
+                "type": "named",
-                "data": {
+                "data": {
-                  "name": "RunIntake"
+                  "name": "RunIntake"
-                }
+                }
-              },
+              },
-              {
+              {
-                "type": "wait",
+                "type": "wait",
-                "data": {
+                "data": {
-                  "waitTime": 10.0
+                  "waitTime": 10.0
-                }
+                }
-              }
+              }
-            ]
+            ]
-          }
+          }
-        },
+        },
-        {
+        {
-          "type": "path",
+          "type": "path",
-          "data": {
+          "data": {
-            "pathName": "1.3 Depot to Shoot Path"
+            "pathName": "1.3 Depot to Shoot Path"
-          }
+          }
-        },
+        },
-        {
+        {
-          "type": "deadline",
+          "type": "deadline",
-          "data": {
+          "data": {
-            "commands": [
+            "commands": [
-              {
+              {
-                "type": "named",
+                "type": "named",
-                "data": {
+                "data": {
-                  "name": "ChassisAlignShoot"
+                  "name": "ChassisAlignShoot"
-                }
+                }
-              },
+              },
-              {
+              {
-                "type": "wait",
+                "type": "wait",
-                "data": {
+                "data": {
-                  "waitTime": 10.0
+                  "waitTime": 10.0
-                }
+                }
-              }
+              }
-            ]
+            ]
-          }
+          }
-        }
+        }
-      ]
+      ]
-    }
+    }
-  },
+  },
-  "resetOdom": true,
+  "resetOdom": true,
-  "folder": null,
+  "folder": null,
-  "choreoAuto": false
+  "choreoAuto": false
 }

deploy/pathplanner/autos/9. Chaos Monkey.auto

@@ -1,25 +1,25 @@
-{
+{
-  "version": "2025.0",
+  "version": "2025.0",
-  "command": {
+  "command": {
-    "type": "sequential",
+    "type": "sequential",
-    "data": {
+    "data": {
-      "commands": [
+      "commands": [
-        {
+        {
-          "type": "path",
+          "type": "path",
-          "data": {
+          "data": {
-            "pathName": "9.1 Chaos Monkey Path"
+            "pathName": "9.1 Chaos Monkey Path"
-          }
+          }
-        },
+        },
-        {
+        {
-          "type": "named",
+          "type": "named",
-          "data": {
+          "data": {
-            "name": "Shoot preload"
+            "name": "Shoot preload"
-          }
+          }
-        }
+        }
-      ]
+      ]
-    }
+    }
-  },
+  },
-  "resetOdom": true,
+  "resetOdom": true,
-  "folder": null,
+  "folder": null,
-  "choreoAuto": false
+  "choreoAuto": false
 }

deploy/pathplanner/autos/Depot. Hub.auto

@@ -1,25 +1,25 @@
-{
+{
-  "version": "2025.0",
+  "version": "2025.0",
-  "command": {
+  "command": {
-    "type": "sequential",
+    "type": "sequential",
-    "data": {
+    "data": {
-      "commands": [
+      "commands": [
-        {
+        {
-          "type": "path",
+          "type": "path",
-          "data": {
+          "data": {
-            "pathName": "S. Hub. shoot 8. E-Hub"
+            "pathName": "S. Hub. shoot 8. E-Hub"
-          }
+          }
-        },
+        },
-        {
+        {
-          "type": "path",
+          "type": "path",
-          "data": {
+          "data": {
-            "pathName": "M. Depot. "
+            "pathName": "M. Depot. "
-          }
+          }
-        }
+        }
-      ]
+      ]
-    }
+    }
-  },
+  },
-  "resetOdom": true,
+  "resetOdom": true,
-  "folder": null,
+  "folder": null,
-  "choreoAuto": false
+  "choreoAuto": false
 }

deploy/pathplanner/autos/Depot. L-Tr..auto

@@ -1,25 +1,25 @@
-{
+{
-  "version": "2025.0",
+  "version": "2025.0",
-  "command": {
+  "command": {
-    "type": "sequential",
+    "type": "sequential",
-    "data": {
+    "data": {
-      "commands": [
+      "commands": [
-        {
+        {
-          "type": "path",
+          "type": "path",
-          "data": {
+          "data": {
-            "pathName": "S. L-Tre. Preload."
+            "pathName": "S. L-Tre. Preload."
-          }
+          }
-        },
+        },
-        {
+        {
-          "type": "path",
+          "type": "path",
-          "data": {
+          "data": {
-            "pathName": "M. Depot. "
+            "pathName": "M. Depot. "
-          }
+          }
-        }
+        }
-      ]
+      ]
-    }
+    }
-  },
+  },
-  "resetOdom": true,
+  "resetOdom": true,
-  "folder": null,
+  "folder": null,
-  "choreoAuto": false
+  "choreoAuto": false
 }

deploy/pathplanner/autos/Depot. R-Tr.auto

@@ -1,25 +1,25 @@
-{
+{
-  "version": "2025.0",
+  "version": "2025.0",
-  "command": {
+  "command": {
-    "type": "sequential",
+    "type": "sequential",
-    "data": {
+    "data": {
-      "commands": [
+      "commands": [
-        {
+        {
-          "type": "path",
+          "type": "path",
-          "data": {
+          "data": {
-            "pathName": "S. R-Trench. E-Hub"
+            "pathName": "S. R-Trench. E-Hub"
-          }
+          }
-        },
+        },
-        {
+        {
-          "type": "path",
+          "type": "path",
-          "data": {
+          "data": {
-            "pathName": "M. Depot. "
+            "pathName": "M. Depot. "
-          }
+          }
-        }
+        }
-      ]
+      ]
-    }
+    }
-  },
+  },
-  "resetOdom": true,
+  "resetOdom": true,
-  "folder": null,
+  "folder": null,
-  "choreoAuto": false
+  "choreoAuto": false
 }

deploy/pathplanner/autos/HUB_START.auto

@@ -1,19 +1,19 @@
-{
+{
-  "version": "2025.0",
+  "version": "2025.0",
-  "command": {
+  "command": {
-    "type": "sequential",
+    "type": "sequential",
-    "data": {
+    "data": {
-      "commands": [
+      "commands": [
-        {
+        {
-          "type": "path",
+          "type": "path",
-          "data": {
+          "data": {
-            "pathName": "HUB_START"
+            "pathName": "HUB_START"
-          }
+          }
-        }
+        }
-      ]
+      ]
-    }
+    }
-  },
+  },
-  "resetOdom": true,
+  "resetOdom": true,
-  "folder": null,
+  "folder": null,
-  "choreoAuto": false
+  "choreoAuto": false
 }

deploy/pathplanner/autos/LEFT_TRENCH_START.auto

@@ -1,19 +1,19 @@
-{
+{
-  "version": "2025.0",
+  "version": "2025.0",
-  "command": {
+  "command": {
-    "type": "sequential",
+    "type": "sequential",
-    "data": {
+    "data": {
-      "commands": [
+      "commands": [
-        {
+        {
-          "type": "path",
+          "type": "path",
-          "data": {
+          "data": {
-            "pathName": "LEFT_TRENCH_START"
+            "pathName": "LEFT_TRENCH_START"
-          }
+          }
-        }
+        }
-      ]
+      ]
-    }
+    }
-  },
+  },
-  "resetOdom": true,
+  "resetOdom": true,
-  "folder": null,
+  "folder": null,
-  "choreoAuto": false
+  "choreoAuto": false
 }

deploy/pathplanner/autos/RIGHT_TRENCH_START.auto

@@ -1,19 +1,19 @@
-{
+{
-  "version": "2025.0",
+  "version": "2025.0",
-  "command": {
+  "command": {
-    "type": "sequential",
+    "type": "sequential",
-    "data": {
+    "data": {
-      "commands": [
+      "commands": [
-        {
+        {
-          "type": "path",
+          "type": "path",
-          "data": {
+          "data": {
-            "pathName": "RIGHT_TRENCH_START"
+            "pathName": "RIGHT_TRENCH_START"
-          }
+          }
-        }
+        }
-      ]
+      ]
-    }
+    }
-  },
+  },
-  "resetOdom": true,
+  "resetOdom": true,
-  "folder": null,
+  "folder": null,
-  "choreoAuto": false
+  "choreoAuto": false
 }

deploy/pathplanner/autos/Relay. L-Tr..auto

@@ -1,19 +1,19 @@
-{
+{
-  "version": "2025.0",
+  "version": "2025.0",
-  "command": {
+  "command": {
-    "type": "sequential",
+    "type": "sequential",
-    "data": {
+    "data": {
-      "commands": [
+      "commands": [
-        {
+        {
-          "type": "path",
+          "type": "path",
-          "data": {
+          "data": {
-            "pathName": "Relay. L-Tr."
+            "pathName": "Relay. L-Tr."
-          }
+          }
-        }
+        }
-      ]
+      ]
-    }
+    }
-  },
+  },
-  "resetOdom": true,
+  "resetOdom": true,
-  "folder": null,
+  "folder": null,
-  "choreoAuto": false
+  "choreoAuto": false
 }

deploy/pathplanner/autos/Relay. R-Tr..auto

@@ -1,19 +1,19 @@
-{
+{
-  "version": "2025.0",
+  "version": "2025.0",
-  "command": {
+  "command": {
-    "type": "sequential",
+    "type": "sequential",
-    "data": {
+    "data": {
-      "commands": [
+      "commands": [
-        {
+        {
-          "type": "path",
+          "type": "path",
-          "data": {
+          "data": {
-            "pathName": "Relay. R-Tr."
+            "pathName": "Relay. R-Tr."
-          }
+          }
-        }
+        }
-      ]
+      ]
-    }
+    }
-  },
+  },
-  "resetOdom": true,
+  "resetOdom": true,
-  "folder": null,
+  "folder": null,
-  "choreoAuto": false
+  "choreoAuto": false
 }

deploy/pathplanner/autos/Shoot 8. Hub.auto

@@ -1,38 +1,38 @@
-{
+{
-  "version": "2025.0",
+  "version": "2025.0",
-  "command": {
+  "command": {
-    "type": "sequential",
+    "type": "sequential",
-    "data": {
+    "data": {
-      "commands": [
+      "commands": [
-        {
+        {
-          "type": "path",
+          "type": "path",
-          "data": {
+          "data": {
-            "pathName": "S. Hub. shoot 8. E-Hub"
+            "pathName": "S. Hub. shoot 8. E-Hub"
-          }
+          }
-        },
+        },
-        {
+        {
-          "type": "deadline",
+          "type": "deadline",
-          "data": {
+          "data": {
-            "commands": [
+            "commands": [
-              {
+              {
-                "type": "wait",
+                "type": "wait",
-                "data": {
+                "data": {
-                  "waitTime": 10.0
+                  "waitTime": 10.0
-                }
+                }
-              },
+              },
-              {
+              {
-                "type": "named",
+                "type": "named",
-                "data": {
+                "data": {
-                  "name": "TurretShoot"
+                  "name": "TurretShoot"
-                }
+                }
-              }
+              }
-            ]
+            ]
-          }
+          }
-        }
+        }
-      ]
+      ]
-    }
+    }
-  },
+  },
-  "resetOdom": true,
+  "resetOdom": true,
-  "folder": null,
+  "folder": null,
-  "choreoAuto": false
+  "choreoAuto": false
 }

deploy/pathplanner/autos/Shoot 8. L-Tr..auto

@@ -1,19 +1,19 @@
-{
+{
-  "version": "2025.0",
+  "version": "2025.0",
-  "command": {
+  "command": {
-    "type": "sequential",
+    "type": "sequential",
-    "data": {
+    "data": {
-      "commands": [
+      "commands": [
-        {
+        {
-          "type": "path",
+          "type": "path",
-          "data": {
+          "data": {
-            "pathName": "S. L-Tre. Preload."
+            "pathName": "S. L-Tre. Preload."
-          }
+          }
-        }
+        }
-      ]
+      ]
-    }
+    }
-  },
+  },
-  "resetOdom": true,
+  "resetOdom": true,
-  "folder": null,
+  "folder": null,
-  "choreoAuto": false
+  "choreoAuto": false
 }

deploy/pathplanner/autos/Shoot 8. R-Tr..auto

@@ -1,19 +1,19 @@
-{
+{
-  "version": "2025.0",
+  "version": "2025.0",
-  "command": {
+  "command": {
-    "type": "sequential",
+    "type": "sequential",
-    "data": {
+    "data": {
-      "commands": [
+      "commands": [
-        {
+        {
-          "type": "path",
+          "type": "path",
-          "data": {
+          "data": {
-            "pathName": "S. R-Trench. E-Hub"
+            "pathName": "S. R-Trench. E-Hub"
-          }
+          }
-        }
+        }
-      ]
+      ]
-    }
+    }
-  },
+  },
-  "resetOdom": true,
+  "resetOdom": true,
-  "folder": null,
+  "folder": null,
-  "choreoAuto": false
+  "choreoAuto": false
 }

deploy/pathplanner/paths/1.1 Hub to Shoot Path.path

@@ -1,58 +1,58 @@
-{
+{
-  "version": "2025.0",
+  "version": "2025.0",
-  "waypoints": [
+  "waypoints": [
-    {
+    {
-      "anchor": {
+      "anchor": {
-        "x": 3.5,
+        "x": 3.5,
-        "y": 4.0
+        "y": 4.0
-      },
+      },
-      "prevControl": null,
+      "prevControl": null,
-      "nextControl": {
+      "nextControl": {
-        "x": 3.0,
+        "x": 3.0,
-        "y": 4.0
+        "y": 4.0
-      },
+      },
-      "isLocked": false,
+      "isLocked": false,
-      "linkedName": null
+      "linkedName": null
-    },
+    },
-    {
+    {
-      "anchor": {
+      "anchor": {
-        "x": 2.0,
+        "x": 2.0,
-        "y": 4.0
+        "y": 4.0
-      },
+      },
-      "prevControl": {
+      "prevControl": {
-        "x": 2.5,
+        "x": 2.5,
-        "y": 4.0
+        "y": 4.0
-      },
+      },
-      "nextControl": null,
+      "nextControl": null,
-      "isLocked": false,
+      "isLocked": false,
-      "linkedName": null
+      "linkedName": null
-    }
+    }
-  ],
+  ],
-  "rotationTargets": [],
+  "rotationTargets": [],
-  "constraintZones": [],
+  "constraintZones": [],
-  "pointTowardsZones": [],
+  "pointTowardsZones": [],
-  "eventMarkers": [],
+  "eventMarkers": [],
-  "globalConstraints": {
+  "globalConstraints": {
-    "maxVelocity": 1.5,
+    "maxVelocity": 1.5,
-    "maxAcceleration": 1.5,
+    "maxAcceleration": 1.5,
-    "maxAngularVelocity": 540.0,
+    "maxAngularVelocity": 540.0,
-    "maxAngularAcceleration": 720.0,
+    "maxAngularAcceleration": 720.0,
-    "nominalVoltage": 12.0,
+    "nominalVoltage": 12.0,
-    "unlimited": false
+    "unlimited": false
-  },
+  },
-  "goalEndState": {
+  "goalEndState": {
-    "velocity": 0,
+    "velocity": 0,
-    "rotation": -177.9047474356738
+    "rotation": -177.9047474356738
-  },
+  },
-  "reversed": false,
+  "reversed": false,
-  "folder": null,
+  "folder": null,
-  "idealStartingState": {
+  "idealStartingState": {
-    "velocity": 0,
+    "velocity": 0,
-<<<<<<< HEAD:src/main/deploy/pathplanner/paths/Hub to Depot.path
+<<<<<<< HEAD:src/main/deploy/pathplanner/paths/Hub to Depot.path
-    "rotation": -179.99999999992184
+    "rotation": -179.99999999992184
-=======
+=======
-    "rotation": -179.69626968995792
+    "rotation": -179.69626968995792
->>>>>>> c2fa27aa5cc22124b930645e802b8176658edb7a:src/main/deploy/pathplanner/paths/1.1 Hub to Shoot Path.path
+>>>>>>> c2fa27aa5cc22124b930645e802b8176658edb7a:src/main/deploy/pathplanner/paths/1.1 Hub to Shoot Path.path
-  },
+  },
-  "useDefaultConstraints": false
+  "useDefaultConstraints": false
 }

deploy/pathplanner/paths/1.1 Hub to Shoot.path

@@ -1,54 +1,54 @@
-{
+{
-  "version": "2025.0",
+  "version": "2025.0",
-  "waypoints": [
+  "waypoints": [
-    {
+    {
-      "anchor": {
+      "anchor": {
-        "x": 3.5,
+        "x": 3.5,
-        "y": 4.0
+        "y": 4.0
-      },
+      },
-      "prevControl": null,
+      "prevControl": null,
-      "nextControl": {
+      "nextControl": {
-        "x": 2.75,
+        "x": 2.75,
-        "y": 4.0
+        "y": 4.0
-      },
+      },
-      "isLocked": false,
+      "isLocked": false,
-      "linkedName": null
+      "linkedName": null
-    },
+    },
-    {
+    {
-      "anchor": {
+      "anchor": {
-        "x": 2.0,
+        "x": 2.0,
-        "y": 4.0
+        "y": 4.0
-      },
+      },
-      "prevControl": {
+      "prevControl": {
-        "x": 2.75,
+        "x": 2.75,
-        "y": 4.0
+        "y": 4.0
-      },
+      },
-      "nextControl": null,
+      "nextControl": null,
-      "isLocked": false,
+      "isLocked": false,
-      "linkedName": null
+      "linkedName": null
-    }
+    }
-  ],
+  ],
-  "rotationTargets": [],
+  "rotationTargets": [],
-  "constraintZones": [],
+  "constraintZones": [],
-  "pointTowardsZones": [],
+  "pointTowardsZones": [],
-  "eventMarkers": [],
+  "eventMarkers": [],
-  "globalConstraints": {
+  "globalConstraints": {
-    "maxVelocity": 3.0,
+    "maxVelocity": 3.0,
-    "maxAcceleration": 3.0,
+    "maxAcceleration": 3.0,
-    "maxAngularVelocity": 360.0,
+    "maxAngularVelocity": 360.0,
-    "maxAngularAcceleration": 720.0,
+    "maxAngularAcceleration": 720.0,
-    "nominalVoltage": 12.0,
+    "nominalVoltage": 12.0,
-    "unlimited": false
+    "unlimited": false
-  },
+  },
-  "goalEndState": {
+  "goalEndState": {
-    "velocity": 0,
+    "velocity": 0,
-    "rotation": 177.4888349270938
+    "rotation": 177.4888349270938
-  },
+  },
-  "reversed": false,
+  "reversed": false,
-  "folder": null,
+  "folder": null,
-  "idealStartingState": {
+  "idealStartingState": {
-    "velocity": 0,
+    "velocity": 0,
-    "rotation": -179.0393266954795
+    "rotation": -179.0393266954795
-  },
+  },
-  "useDefaultConstraints": true
+  "useDefaultConstraints": true
 }

deploy/pathplanner/paths/1.2 Shoot to Depot Path.path

@@ -1,54 +1,54 @@
-{
+{
-  "version": "2025.0",
+  "version": "2025.0",
-  "waypoints": [
+  "waypoints": [
-    {
+    {
-      "anchor": {
+      "anchor": {
-        "x": 2.0,
+        "x": 2.0,
-        "y": 4.0
+        "y": 4.0
-      },
+      },
-      "prevControl": null,
+      "prevControl": null,
-      "nextControl": {
+      "nextControl": {
-        "x": 2.2144854970378125,
+        "x": 2.2144854970378125,
-        "y": 3.8554998234204434
+        "y": 3.8554998234204434
-      },
+      },
-      "isLocked": false,
+      "isLocked": false,
-      "linkedName": null
+      "linkedName": null
-    },
+    },
-    {
+    {
-      "anchor": {
+      "anchor": {
-        "x": 1.0,
+        "x": 1.0,
-        "y": 6.0
+        "y": 6.0
-      },
+      },
-      "prevControl": {
+      "prevControl": {
-        "x": 1.2166471375173105,
+        "x": 1.2166471375173105,
-        "y": 5.875244167248359
+        "y": 5.875244167248359
-      },
+      },
-      "nextControl": null,
+      "nextControl": null,
-      "isLocked": false,
+      "isLocked": false,
-      "linkedName": null
+      "linkedName": null
-    }
+    }
-  ],
+  ],
-  "rotationTargets": [],
+  "rotationTargets": [],
-  "constraintZones": [],
+  "constraintZones": [],
-  "pointTowardsZones": [],
+  "pointTowardsZones": [],
-  "eventMarkers": [],
+  "eventMarkers": [],
-  "globalConstraints": {
+  "globalConstraints": {
-    "maxVelocity": 3.0,
+    "maxVelocity": 3.0,
-    "maxAcceleration": 3.0,
+    "maxAcceleration": 3.0,
-    "maxAngularVelocity": 360.0,
+    "maxAngularVelocity": 360.0,
-    "maxAngularAcceleration": 720.0,
+    "maxAngularAcceleration": 720.0,
-    "nominalVoltage": 12.0,
+    "nominalVoltage": 12.0,
-    "unlimited": false
+    "unlimited": false
-  },
+  },
-  "goalEndState": {
+  "goalEndState": {
-    "velocity": 0,
+    "velocity": 0,
-    "rotation": -178.09121133641756
+    "rotation": -178.09121133641756
-  },
+  },
-  "reversed": false,
+  "reversed": false,
-  "folder": null,
+  "folder": null,
-  "idealStartingState": {
+  "idealStartingState": {
-    "velocity": 0,
+    "velocity": 0,
-    "rotation": 0.4910281334858283
+    "rotation": 0.4910281334858283
-  },
+  },
-  "useDefaultConstraints": true
+  "useDefaultConstraints": true
 }

deploy/pathplanner/paths/1.3 Depot to Shoot Path.path

@@ -1,54 +1,54 @@
-{
+{
-  "version": "2025.0",
+  "version": "2025.0",
-  "waypoints": [
+  "waypoints": [
-    {
+    {
-      "anchor": {
+      "anchor": {
-        "x": 1.0,
+        "x": 1.0,
-        "y": 6.0
+        "y": 6.0
-      },
+      },
-      "prevControl": null,
+      "prevControl": null,
-      "nextControl": {
+      "nextControl": {
-        "x": 1.453699105174549,
+        "x": 1.453699105174549,
-        "y": 6.084627986890304
+        "y": 6.084627986890304
-      },
+      },
-      "isLocked": false,
+      "isLocked": false,
-      "linkedName": null
+      "linkedName": null
-    },
+    },
-    {
+    {
-      "anchor": {
+      "anchor": {
-        "x": 2.0,
+        "x": 2.0,
-        "y": 4.0
+        "y": 4.0
-      },
+      },
-      "prevControl": {
+      "prevControl": {
-        "x": 2.266577414275921,
+        "x": 2.266577414275921,
-        "y": 3.829411518660614
+        "y": 3.829411518660614
-      },
+      },
-      "nextControl": null,
+      "nextControl": null,
-      "isLocked": false,
+      "isLocked": false,
-      "linkedName": null
+      "linkedName": null
-    }
+    }
-  ],
+  ],
-  "rotationTargets": [],
+  "rotationTargets": [],
-  "constraintZones": [],
+  "constraintZones": [],
-  "pointTowardsZones": [],
+  "pointTowardsZones": [],
-  "eventMarkers": [],
+  "eventMarkers": [],
-  "globalConstraints": {
+  "globalConstraints": {
-    "maxVelocity": 1.5,
+    "maxVelocity": 1.5,
-    "maxAcceleration": 1.5,
+    "maxAcceleration": 1.5,
-    "maxAngularVelocity": 100.0,
+    "maxAngularVelocity": 100.0,
-    "maxAngularAcceleration": 100.0,
+    "maxAngularAcceleration": 100.0,
-    "nominalVoltage": 12.0,
+    "nominalVoltage": 12.0,
-    "unlimited": false
+    "unlimited": false
-  },
+  },
-  "goalEndState": {
+  "goalEndState": {
-    "velocity": 0,
+    "velocity": 0,
-    "rotation": -179.02897806892076
+    "rotation": -179.02897806892076
-  },
+  },
-  "reversed": false,
+  "reversed": false,
-  "folder": null,
+  "folder": null,
-  "idealStartingState": {
+  "idealStartingState": {
-    "velocity": 0,
+    "velocity": 0,
-    "rotation": -179.69626968995792
+    "rotation": -179.69626968995792
-  },
+  },
-  "useDefaultConstraints": false
+  "useDefaultConstraints": false
 }

deploy/pathplanner/paths/9.1 Chaos Monkey Path.path

@@ -1,102 +1,102 @@
-{
+{
-  "version": "2025.0",
+  "version": "2025.0",
-  "waypoints": [
+  "waypoints": [
-    {
+    {
-      "anchor": {
+      "anchor": {
-        "x": 3.5184925161825644,
+        "x": 3.5184925161825644,
-        "y": 7.5345006466771025
+        "y": 7.5345006466771025
-      },
+      },
-      "prevControl": null,
+      "prevControl": null,
-      "nextControl": {
+      "nextControl": {
-        "x": 4.489963675146704,
+        "x": 4.489963675146704,
-        "y": 7.219438447522688
+        "y": 7.219438447522688
-      },
+      },
-      "isLocked": false,
+      "isLocked": false,
-      "linkedName": null
+      "linkedName": null
-    },
+    },
-    {
+    {
-      "anchor": {
+      "anchor": {
-        "x": 7.6,
+        "x": 7.6,
-        "y": 6.812200117368956
+        "y": 6.812200117368956
-      },
+      },
-      "prevControl": {
+      "prevControl": {
-        "x": 7.532870181778978,
+        "x": 7.532870181778978,
-        "y": 7.8099443669167
+        "y": 7.8099443669167
-      },
+      },
-      "nextControl": {
+      "nextControl": {
-        "x": 7.650893142287796,
+        "x": 7.650893142287796,
-        "y": 6.055780073270545
+        "y": 6.055780073270545
-      },
+      },
-      "isLocked": false,
+      "isLocked": false,
-      "linkedName": null
+      "linkedName": null
-    },
+    },
-    {
+    {
-      "anchor": {
+      "anchor": {
-        "x": 7.6,
+        "x": 7.6,
-        "y": 0.9617601315610045
+        "y": 0.9617601315610045
-      },
+      },
-      "prevControl": {
+      "prevControl": {
-        "x": 7.654625343431328,
+        "x": 7.654625343431328,
-        "y": 1.468826273060325
+        "y": 1.468826273060325
-      },
+      },
-      "nextControl": {
+      "nextControl": {
-        "x": 7.527819365080411,
+        "x": 7.527819365080411,
-        "y": 0.2917349428743933
+        "y": 0.2917349428743933
-      },
+      },
-      "isLocked": false,
+      "isLocked": false,
-      "linkedName": null
+      "linkedName": null
-    },
+    },
-    {
+    {
-      "anchor": {
+      "anchor": {
-        "x": 0.5,
+        "x": 0.5,
-        "y": 0.6692650602433468
+        "y": 0.6692650602433468
-      },
+      },
-      "prevControl": {
+      "prevControl": {
-        "x": 1.096874747061515,
+        "x": 1.096874747061515,
-        "y": 0.6440356562159093
+        "y": 0.6440356562159093
-      },
+      },
-      "nextControl": null,
+      "nextControl": null,
-      "isLocked": false,
+      "isLocked": false,
-      "linkedName": null
+      "linkedName": null
-    }
+    }
-  ],
+  ],
-  "rotationTargets": [
+  "rotationTargets": [
-    {
+    {
-      "waypointRelativePos": 1.2332261810856255,
+      "waypointRelativePos": 1.2332261810856255,
-      "rotationDegrees": -90.0
+      "rotationDegrees": -90.0
-    },
+    },
-    {
+    {
-      "waypointRelativePos": 1.7516108262639798,
+      "waypointRelativePos": 1.7516108262639798,
-      "rotationDegrees": 0.0
+      "rotationDegrees": 0.0
-    }
+    }
-  ],
+  ],
-  "constraintZones": [],
+  "constraintZones": [],
-  "pointTowardsZones": [],
+  "pointTowardsZones": [],
-  "eventMarkers": [
+  "eventMarkers": [
-    {
+    {
-      "name": "",
+      "name": "",
-      "waypointRelativePos": 0,
+      "waypointRelativePos": 0,
-      "endWaypointRelativePos": null,
+      "endWaypointRelativePos": null,
-      "command": null
+      "command": null
-    }
+    }
-  ],
+  ],
-  "globalConstraints": {
+  "globalConstraints": {
-    "maxVelocity": 3.0,
+    "maxVelocity": 3.0,
-    "maxAcceleration": 3.0,
+    "maxAcceleration": 3.0,
-    "maxAngularVelocity": 360.0,
+    "maxAngularVelocity": 360.0,
-    "maxAngularAcceleration": 720.0,
+    "maxAngularAcceleration": 720.0,
-    "nominalVoltage": 12.0,
+    "nominalVoltage": 12.0,
-    "unlimited": false
+    "unlimited": false
-  },
+  },
-  "goalEndState": {
+  "goalEndState": {
-    "velocity": 0,
+    "velocity": 0,
-    "rotation": -1.2282908533168928
+    "rotation": -1.2282908533168928
-  },
+  },
-  "reversed": false,
+  "reversed": false,
-  "folder": null,
+  "folder": null,
-  "idealStartingState": {
+  "idealStartingState": {
-    "velocity": 0,
+    "velocity": 0,
-    "rotation": -0.1533900770684255
+    "rotation": -0.1533900770684255
-  },
+  },
-  "useDefaultConstraints": true
+  "useDefaultConstraints": true
 }

deploy/pathplanner/paths/HUB_START.path

@@ -1,54 +1,54 @@
-{
+{
-  "version": "2025.0",
+  "version": "2025.0",
-  "waypoints": [
+  "waypoints": [
-    {
+    {
-      "anchor": {
+      "anchor": {
-        "x": 3.519556041182133,
+        "x": 3.519556041182133,
-        "y": 4.059804750409042
+        "y": 4.059804750409042
-      },
+      },
-      "prevControl": null,
+      "prevControl": null,
-      "nextControl": {
+      "nextControl": {
-        "x": 3.769556041182133,
+        "x": 3.769556041182133,
-        "y": 4.059804750409042
+        "y": 4.059804750409042
-      },
+      },
-      "isLocked": false,
+      "isLocked": false,
-      "linkedName": null
+      "linkedName": null
-    },
+    },
-    {
+    {
-      "anchor": {
+      "anchor": {
-        "x": 3.519556041182133,
+        "x": 3.519556041182133,
-        "y": 4.059804750409042
+        "y": 4.059804750409042
-      },
+      },
-      "prevControl": {
+      "prevControl": {
-        "x": 3.269556041182133,
+        "x": 3.269556041182133,
-        "y": 4.059804750409042
+        "y": 4.059804750409042
-      },
+      },
-      "nextControl": null,
+      "nextControl": null,
-      "isLocked": false,
+      "isLocked": false,
-      "linkedName": null
+      "linkedName": null
-    }
+    }
-  ],
+  ],
-  "rotationTargets": [],
+  "rotationTargets": [],
-  "constraintZones": [],
+  "constraintZones": [],
-  "pointTowardsZones": [],
+  "pointTowardsZones": [],
-  "eventMarkers": [],
+  "eventMarkers": [],
-  "globalConstraints": {
+  "globalConstraints": {
-    "maxVelocity": 3.0,
+    "maxVelocity": 3.0,
-    "maxAcceleration": 3.0,
+    "maxAcceleration": 3.0,
-    "maxAngularVelocity": 360.0,
+    "maxAngularVelocity": 360.0,
-    "maxAngularAcceleration": 720.0,
+    "maxAngularAcceleration": 720.0,
-    "nominalVoltage": 12.0,
+    "nominalVoltage": 12.0,
-    "unlimited": false
+    "unlimited": false
-  },
+  },
-  "goalEndState": {
+  "goalEndState": {
-    "velocity": 0,
+    "velocity": 0,
-    "rotation": 0.0
+    "rotation": 0.0
-  },
+  },
-  "reversed": false,
+  "reversed": false,
-  "folder": null,
+  "folder": null,
-  "idealStartingState": {
+  "idealStartingState": {
-    "velocity": 0,
+    "velocity": 0,
-    "rotation": 0.0
+    "rotation": 0.0
-  },
+  },
-  "useDefaultConstraints": true
+  "useDefaultConstraints": true
 }

deploy/pathplanner/paths/LEFT_TRENCH_START.path

@@ -1,54 +1,54 @@
-{
+{
-  "version": "2025.0",
+  "version": "2025.0",
-  "waypoints": [
+  "waypoints": [
-    {
+    {
-      "anchor": {
+      "anchor": {
-        "x": 4.0,
+        "x": 4.0,
-        "y": 7.423545951843262
+        "y": 7.423545951843262
-      },
+      },
-      "prevControl": null,
+      "prevControl": null,
-      "nextControl": {
+      "nextControl": {
-        "x": 4.25,
+        "x": 4.25,
-        "y": 7.423545951843262
+        "y": 7.423545951843262
-      },
+      },
-      "isLocked": false,
+      "isLocked": false,
-      "linkedName": null
+      "linkedName": null
-    },
+    },
-    {
+    {
-      "anchor": {
+      "anchor": {
-        "x": 4.0,
+        "x": 4.0,
-        "y": 7.423545951843262
+        "y": 7.423545951843262
-      },
+      },
-      "prevControl": {
+      "prevControl": {
-        "x": 3.75,
+        "x": 3.75,
-        "y": 7.423545951843262
+        "y": 7.423545951843262
-      },
+      },
-      "nextControl": null,
+      "nextControl": null,
-      "isLocked": false,
+      "isLocked": false,
-      "linkedName": null
+      "linkedName": null
-    }
+    }
-  ],
+  ],
-  "rotationTargets": [],
+  "rotationTargets": [],
-  "constraintZones": [],
+  "constraintZones": [],
-  "pointTowardsZones": [],
+  "pointTowardsZones": [],
-  "eventMarkers": [],
+  "eventMarkers": [],
-  "globalConstraints": {
+  "globalConstraints": {
-    "maxVelocity": 3.0,
+    "maxVelocity": 3.0,
-    "maxAcceleration": 3.0,
+    "maxAcceleration": 3.0,
-    "maxAngularVelocity": 360.0,
+    "maxAngularVelocity": 360.0,
-    "maxAngularAcceleration": 720.0,
+    "maxAngularAcceleration": 720.0,
-    "nominalVoltage": 12.0,
+    "nominalVoltage": 12.0,
-    "unlimited": false
+    "unlimited": false
-  },
+  },
-  "goalEndState": {
+  "goalEndState": {
-    "velocity": 0,
+    "velocity": 0,
-    "rotation": 0.0
+    "rotation": 0.0
-  },
+  },
-  "reversed": false,
+  "reversed": false,
-  "folder": null,
+  "folder": null,
-  "idealStartingState": {
+  "idealStartingState": {
-    "velocity": 0,
+    "velocity": 0,
-    "rotation": 0.0
+    "rotation": 0.0
-  },
+  },
-  "useDefaultConstraints": true
+  "useDefaultConstraints": true
 }

deploy/pathplanner/paths/M. Depot. .path

@@ -1,107 +1,107 @@
-{
+{
-  "version": "2025.0",
+  "version": "2025.0",
-  "waypoints": [
+  "waypoints": [
-    {
+    {
-      "anchor": {
+      "anchor": {
-        "x": 3.4727349397590364,
+        "x": 3.4727349397590364,
-        "y": 4.013144578313253
+        "y": 4.013144578313253
-      },
+      },
-      "prevControl": null,
+      "prevControl": null,
-      "nextControl": {
+      "nextControl": {
-        "x": 3.2818883684167575,
+        "x": 3.2818883684167575,
-        "y": 4.174630138679796
+        "y": 4.174630138679796
-      },
+      },
-      "isLocked": false,
+      "isLocked": false,
-      "linkedName": null
+      "linkedName": null
-    },
+    },
-    {
+    {
-      "anchor": {
+      "anchor": {
-        "x": 0.751734939759036,
+        "x": 0.751734939759036,
-        "y": 4.942
+        "y": 4.942
-      },
+      },
-      "prevControl": {
+      "prevControl": {
-        "x": 1.975638554216867,
+        "x": 1.975638554216867,
-        "y": 6.362602409638553
+        "y": 6.362602409638553
-      },
+      },
-      "nextControl": {
+      "nextControl": {
-        "x": 0.40641026098477545,
+        "x": 0.40641026098477545,
-        "y": 4.54117671213702
+        "y": 4.54117671213702
-      },
+      },
-      "isLocked": false,
+      "isLocked": false,
-      "linkedName": null
+      "linkedName": null
-    },
+    },
-    {
+    {
-      "anchor": {
+      "anchor": {
-        "x": 0.751734939759036,
+        "x": 0.751734939759036,
-        "y": 7.138469879518072
+        "y": 7.138469879518072
-      },
+      },
-      "prevControl": {
+      "prevControl": {
-        "x": 0.41241207330734936,
+        "x": 0.41241207330734936,
-        "y": 6.876706525398199
+        "y": 6.876706525398199
-      },
+      },
-      "nextControl": {
+      "nextControl": {
-        "x": 1.1342048192771081,
+        "x": 1.1342048192771081,
-        "y": 7.433518072289157
+        "y": 7.433518072289157
-      },
+      },
-      "isLocked": false,
+      "isLocked": false,
-      "linkedName": null
+      "linkedName": null
-    },
+    },
-    {
+    {
-      "anchor": {
+      "anchor": {
-        "x": 3.4727349397590364,
+        "x": 3.4727349397590364,
-        "y": 4.013144578313253
+        "y": 4.013144578313253
-      },
+      },
-      "prevControl": {
+      "prevControl": {
-        "x": 2.161409638554217,
+        "x": 2.161409638554217,
-        "y": 4.1989156626506015
+        "y": 4.1989156626506015
-      },
+      },
-      "nextControl": null,
+      "nextControl": null,
-      "isLocked": false,
+      "isLocked": false,
-      "linkedName": null
+      "linkedName": null
-    }
+    }
-  ],
+  ],
-  "rotationTargets": [
+  "rotationTargets": [
-    {
+    {
-      "waypointRelativePos": 1.0308661417322806,
+      "waypointRelativePos": 1.0308661417322806,
-      "rotationDegrees": 90.0
+      "rotationDegrees": 90.0
-    },
+    },
-    {
+    {
-      "waypointRelativePos": 1.7754330708661374,
+      "waypointRelativePos": 1.7754330708661374,
-      "rotationDegrees": 90.0
+      "rotationDegrees": 90.0
-    }
+    }
-  ],
+  ],
-  "constraintZones": [],
+  "constraintZones": [],
-  "pointTowardsZones": [],
+  "pointTowardsZones": [],
-  "eventMarkers": [
+  "eventMarkers": [
-    {
+    {
-      "name": "Shoot 8",
+      "name": "Shoot 8",
-      "waypointRelativePos": 3.0,
+      "waypointRelativePos": 3.0,
-      "endWaypointRelativePos": null,
+      "endWaypointRelativePos": null,
-      "command": {
+      "command": {
-        "type": "named",
+        "type": "named",
-        "data": {
+        "data": {
-          "name": "Shoot preload"
+          "name": "Shoot preload"
-        }
+        }
-      }
+      }
-    }
+    }
-  ],
+  ],
-  "globalConstraints": {
+  "globalConstraints": {
-    "maxVelocity": 3.0,
+    "maxVelocity": 3.0,
-    "maxAcceleration": 3.0,
+    "maxAcceleration": 3.0,
-    "maxAngularVelocity": 360.0,
+    "maxAngularVelocity": 360.0,
-    "maxAngularAcceleration": 720.0,
+    "maxAngularAcceleration": 720.0,
-    "nominalVoltage": 12.0,
+    "nominalVoltage": 12.0,
-    "unlimited": false
+    "unlimited": false
-  },
+  },
-  "goalEndState": {
+  "goalEndState": {
-    "velocity": 0,
+    "velocity": 0,
-    "rotation": 180.0
+    "rotation": 180.0
-  },
+  },
-  "reversed": false,
+  "reversed": false,
-  "folder": null,
+  "folder": null,
-  "idealStartingState": {
+  "idealStartingState": {
-    "velocity": 0,
+    "velocity": 0,
-    "rotation": 180.0
+    "rotation": 180.0
-  },
+  },
-  "useDefaultConstraints": true
+  "useDefaultConstraints": true
 }

deploy/pathplanner/paths/RIGHT_TRENCH_START.path

@@ -1,54 +1,54 @@
-{
+{
-  "version": "2025.0",
+  "version": "2025.0",
-  "waypoints": [
+  "waypoints": [
-    {
+    {
-      "anchor": {
+      "anchor": {
-        "x": 4.0,
+        "x": 4.0,
-        "y": 0.6236959156237152
+        "y": 0.6236959156237152
-      },
+      },
-      "prevControl": null,
+      "prevControl": null,
-      "nextControl": {
+      "nextControl": {
-        "x": 4.25,
+        "x": 4.25,
-        "y": 0.6236959156237152
+        "y": 0.6236959156237152
-      },
+      },
-      "isLocked": false,
+      "isLocked": false,
-      "linkedName": null
+      "linkedName": null
-    },
+    },
-    {
+    {
-      "anchor": {
+      "anchor": {
-        "x": 4.0,
+        "x": 4.0,
-        "y": 0.6236959156237152
+        "y": 0.6236959156237152
-      },
+      },
-      "prevControl": {
+      "prevControl": {
-        "x": 3.75,
+        "x": 3.75,
-        "y": 0.6236959156237152
+        "y": 0.6236959156237152
-      },
+      },
-      "nextControl": null,
+      "nextControl": null,
-      "isLocked": false,
+      "isLocked": false,
-      "linkedName": null
+      "linkedName": null
-    }
+    }
-  ],
+  ],
-  "rotationTargets": [],
+  "rotationTargets": [],
-  "constraintZones": [],
+  "constraintZones": [],
-  "pointTowardsZones": [],
+  "pointTowardsZones": [],
-  "eventMarkers": [],
+  "eventMarkers": [],
-  "globalConstraints": {
+  "globalConstraints": {
-    "maxVelocity": 3.0,
+    "maxVelocity": 3.0,
-    "maxAcceleration": 3.0,
+    "maxAcceleration": 3.0,
-    "maxAngularVelocity": 360.0,
+    "maxAngularVelocity": 360.0,
-    "maxAngularAcceleration": 720.0,
+    "maxAngularAcceleration": 720.0,
-    "nominalVoltage": 12.0,
+    "nominalVoltage": 12.0,
-    "unlimited": false
+    "unlimited": false
-  },
+  },
-  "goalEndState": {
+  "goalEndState": {
-    "velocity": 0,
+    "velocity": 0,
-    "rotation": 0.0
+    "rotation": 0.0
-  },
+  },
-  "reversed": false,
+  "reversed": false,
-  "folder": null,
+  "folder": null,
-  "idealStartingState": {
+  "idealStartingState": {
-    "velocity": 0,
+    "velocity": 0,
-    "rotation": 0.0
+    "rotation": 0.0
-  },
+  },
-  "useDefaultConstraints": true
+  "useDefaultConstraints": true
 }

deploy/pathplanner/paths/Relay. L-Tr..path

@@ -1,193 +1,193 @@
-{
+{
-  "version": "2025.0",
+  "version": "2025.0",
-  "waypoints": [
+  "waypoints": [
-    {
+    {
-      "anchor": {
+      "anchor": {
-        "x": 3.5383012048192777,
+        "x": 3.5383012048192777,
-        "y": 7.4553734939759035
+        "y": 7.4553734939759035
-      },
+      },
-      "prevControl": null,
+      "prevControl": null,
-      "nextControl": {
+      "nextControl": {
-        "x": 4.538301204819281,
+        "x": 4.538301204819281,
-        "y": 7.4553734939759035
+        "y": 7.4553734939759035
-      },
+      },
-      "isLocked": false,
+      "isLocked": false,
-      "linkedName": null
+      "linkedName": null
-    },
+    },
-    {
+    {
-      "anchor": {
+      "anchor": {
-        "x": 7.67990361445783,
+        "x": 7.67990361445783,
-        "y": 7.269602409638554
+        "y": 7.269602409638554
-      },
+      },
-      "prevControl": {
+      "prevControl": {
-        "x": 7.475661648641639,
+        "x": 7.475661648641639,
-        "y": 7.413773209038217
+        "y": 7.413773209038217
-      },
+      },
-      "nextControl": {
+      "nextControl": {
-        "x": 7.884145580274021,
+        "x": 7.884145580274021,
-        "y": 7.125431610238891
+        "y": 7.125431610238891
-      },
+      },
-      "isLocked": false,
+      "isLocked": false,
-      "linkedName": null
+      "linkedName": null
-    },
+    },
-    {
+    {
-      "anchor": {
+      "anchor": {
-        "x": 7.67990361445783,
+        "x": 7.67990361445783,
-        "y": 1.0845180722891563
+        "y": 1.0845180722891563
-      },
+      },
-      "prevControl": {
+      "prevControl": {
-        "x": 7.619269708198755,
+        "x": 7.619269708198755,
-        "y": 1.3270536973254914
+        "y": 1.3270536973254914
-      },
+      },
-      "nextControl": {
+      "nextControl": {
-        "x": 7.743952438085447,
+        "x": 7.743952438085447,
-        "y": 0.828322777778649
+        "y": 0.828322777778649
-      },
+      },
-      "isLocked": false,
+      "isLocked": false,
-      "linkedName": null
+      "linkedName": null
-    },
+    },
-    {
+    {
-      "anchor": {
+      "anchor": {
-        "x": 6.641771084337348,
+        "x": 6.641771084337348,
-        "y": 0.9206024096385546
+        "y": 0.9206024096385546
-      },
+      },
-      "prevControl": {
+      "prevControl": {
-        "x": 6.662532454300786,
+        "x": 6.662532454300786,
-        "y": 1.1697388491997742
+        "y": 1.1697388491997742
-      },
+      },
-      "nextControl": {
+      "nextControl": {
-        "x": 6.62100971437391,
+        "x": 6.62100971437391,
-        "y": 0.6714659700773349
+        "y": 0.6714659700773349
-      },
+      },
-      "isLocked": false,
+      "isLocked": false,
-      "linkedName": null
+      "linkedName": null
-    },
+    },
-    {
+    {
-      "anchor": {
+      "anchor": {
-        "x": 6.663626506024096,
+        "x": 6.663626506024096,
-        "y": 7.19310843373494
+        "y": 7.19310843373494
-      },
+      },
-      "prevControl": {
+      "prevControl": {
-        "x": 6.649759001118466,
+        "x": 6.649759001118466,
-        "y": 6.943493345433587
+        "y": 6.943493345433587
-      },
+      },
-      "nextControl": {
+      "nextControl": {
-        "x": 6.677494010929727,
+        "x": 6.677494010929727,
-        "y": 7.442723522036293
+        "y": 7.442723522036293
-      },
+      },
-      "isLocked": false,
+      "isLocked": false,
-      "linkedName": null
+      "linkedName": null
-    },
+    },
-    {
+    {
-      "anchor": {
+      "anchor": {
-        "x": 5.876831325301204,
+        "x": 5.876831325301204,
-        "y": 7.19310843373494
+        "y": 7.19310843373494
-      },
+      },
-      "prevControl": {
+      "prevControl": {
-        "x": 6.125590622853701,
+        "x": 6.125590622853701,
-        "y": 7.217984363490191
+        "y": 7.217984363490191
-      },
+      },
-      "nextControl": {
+      "nextControl": {
-        "x": 5.628072027748707,
+        "x": 5.628072027748707,
-        "y": 7.168232503979689
+        "y": 7.168232503979689
-      },
+      },
-      "isLocked": false,
+      "isLocked": false,
-      "linkedName": null
+      "linkedName": null
-    },
+    },
-    {
+    {
-      "anchor": {
+      "anchor": {
-        "x": 5.745698795180723,
+        "x": 5.745698795180723,
-        "y": 0.9206024096385546
+        "y": 0.9206024096385546
-      },
+      },
-      "prevControl": {
+      "prevControl": {
-        "x": 5.857502194055715,
+        "x": 5.857502194055715,
-        "y": 1.1442092073885324
+        "y": 1.1442092073885324
-      },
+      },
-      "nextControl": null,
+      "nextControl": null,
-      "isLocked": false,
+      "isLocked": false,
-      "linkedName": null
+      "linkedName": null
-    }
+    }
-  ],
+  ],
-  "rotationTargets": [
+  "rotationTargets": [
-    {
+    {
-      "waypointRelativePos": 0.5,
+      "waypointRelativePos": 0.5,
-      "rotationDegrees": 0.0
+      "rotationDegrees": 0.0
-    },
+    },
-    {
+    {
-      "waypointRelativePos": 1.0672199170124472,
+      "waypointRelativePos": 1.0672199170124472,
-      "rotationDegrees": -90.0
+      "rotationDegrees": -90.0
-    },
+    },
-    {
+    {
-      "waypointRelativePos": 2.0863070539419084,
+      "waypointRelativePos": 2.0863070539419084,
-      "rotationDegrees": -90.0
+      "rotationDegrees": -90.0
-    },
+    },
-    {
+    {
-      "waypointRelativePos": 2.962655601659752,
+      "waypointRelativePos": 2.962655601659752,
-      "rotationDegrees": 90.0
+      "rotationDegrees": 90.0
-    },
+    },
-    {
+    {
-      "waypointRelativePos": 3.9983402489627107,
+      "waypointRelativePos": 3.9983402489627107,
-      "rotationDegrees": 90.0
+      "rotationDegrees": 90.0
-    },
+    },
-    {
+    {
-      "waypointRelativePos": 5.02406639004149,
+      "waypointRelativePos": 5.02406639004149,
-      "rotationDegrees": -90.0
+      "rotationDegrees": -90.0
-    }
+    }
-  ],
+  ],
-  "constraintZones": [],
+  "constraintZones": [],
-  "pointTowardsZones": [],
+  "pointTowardsZones": [],
-  "eventMarkers": [
+  "eventMarkers": [
-    {
+    {
-      "name": "Shoot Headed R-Tr",
+      "name": "Shoot Headed R-Tr",
-      "waypointRelativePos": 1.0302729528535988,
+      "waypointRelativePos": 1.0302729528535988,
-      "endWaypointRelativePos": 2.2094292803970026,
+      "endWaypointRelativePos": 2.2094292803970026,
-      "command": {
+      "command": {
-        "type": "named",
+        "type": "named",
-        "data": {
+        "data": {
-          "name": "Shoot Headed R-Tr"
+          "name": "Shoot Headed R-Tr"
-        }
+        }
-      }
+      }
-    },
+    },
-    {
+    {
-      "name": "Shoot headed L-Tr",
+      "name": "Shoot headed L-Tr",
-      "waypointRelativePos": 3.1503722084366608,
+      "waypointRelativePos": 3.1503722084366608,
-      "endWaypointRelativePos": 4.281885856079398,
+      "endWaypointRelativePos": 4.281885856079398,
-      "command": {
+      "command": {
-        "type": "named",
+        "type": "named",
-        "data": {
+        "data": {
-          "name": "Shoot headed L-Tr"
+          "name": "Shoot headed L-Tr"
-        }
+        }
-      }
+      }
-    },
+    },
-    {
+    {
-      "name": "Shoot Headed R-Tr",
+      "name": "Shoot Headed R-Tr",
-      "waypointRelativePos": 5.067990074441674,
+      "waypointRelativePos": 5.067990074441674,
-      "endWaypointRelativePos": 6.0,
+      "endWaypointRelativePos": 6.0,
-      "command": {
+      "command": {
-        "type": "named",
+        "type": "named",
-        "data": {
+        "data": {
-          "name": "Shoot Headed R-Tr"
+          "name": "Shoot Headed R-Tr"
-        }
+        }
-      }
+      }
-    }
+    }
-  ],
+  ],
-  "globalConstraints": {
+  "globalConstraints": {
-    "maxVelocity": 3.0,
+    "maxVelocity": 3.0,
-    "maxAcceleration": 3.0,
+    "maxAcceleration": 3.0,
-    "maxAngularVelocity": 360.0,
+    "maxAngularVelocity": 360.0,
-    "maxAngularAcceleration": 720.0,
+    "maxAngularAcceleration": 720.0,
-    "nominalVoltage": 12.0,
+    "nominalVoltage": 12.0,
-    "unlimited": false
+    "unlimited": false
-  },
+  },
-  "goalEndState": {
+  "goalEndState": {
-    "velocity": 0,
+    "velocity": 0,
-    "rotation": -90.0
+    "rotation": -90.0
-  },
+  },
-  "reversed": false,
+  "reversed": false,
-  "folder": null,
+  "folder": null,
-  "idealStartingState": {
+  "idealStartingState": {
-    "velocity": 0,
+    "velocity": 0,
-    "rotation": 0.0
+    "rotation": 0.0
-  },
+  },
-  "useDefaultConstraints": true
+  "useDefaultConstraints": true
 }

deploy/pathplanner/paths/Relay. R-Tr..path

@@ -1,197 +1,197 @@
-{
+{
-  "version": "2025.0",
+  "version": "2025.0",
-  "waypoints": [
+  "waypoints": [
-    {
+    {
-      "anchor": {
+      "anchor": {
-        "x": 3.4618072289156623,
+        "x": 3.4618072289156623,
-        "y": 0.680192771084337
+        "y": 0.680192771084337
-      },
+      },
-      "prevControl": null,
+      "prevControl": null,
-      "nextControl": {
+      "nextControl": {
-        "x": 4.4618072289156725,
+        "x": 4.4618072289156725,
-        "y": 0.680192771084337
+        "y": 0.680192771084337
-      },
+      },
-      "isLocked": false,
+      "isLocked": false,
-      "linkedName": null
+      "linkedName": null
-    },
+    },
-    {
+    {
-      "anchor": {
+      "anchor": {
-        "x": 7.395783132530121,
+        "x": 7.395783132530121,
-        "y": 0.8768915662650603
+        "y": 0.8768915662650603
-      },
+      },
-      "prevControl": {
+      "prevControl": {
-        "x": 6.47916753759839,
+        "x": 6.47916753759839,
-        "y": 0.6593895606880393
+        "y": 0.6593895606880393
-      },
+      },
-      "nextControl": {
+      "nextControl": {
-        "x": 8.040518072289156,
+        "x": 8.040518072289156,
-        "y": 1.029879518072289
+        "y": 1.029879518072289
-      },
+      },
-      "isLocked": false,
+      "isLocked": false,
-      "linkedName": null
+      "linkedName": null
-    },
+    },
-    {
+    {
-      "anchor": {
+      "anchor": {
-        "x": 7.668975903614458,
+        "x": 7.668975903614458,
-        "y": 7.138469879518072
+        "y": 7.138469879518072
-      },
+      },
-      "prevControl": {
+      "prevControl": {
-        "x": 7.691609840125085,
+        "x": 7.691609840125085,
-        "y": 7.387443181134999
+        "y": 7.387443181134999
-      },
+      },
-      "nextControl": {
+      "nextControl": {
-        "x": 7.64634196710383,
+        "x": 7.64634196710383,
-        "y": 6.889496577901146
+        "y": 6.889496577901146
-      },
+      },
-      "isLocked": false,
+      "isLocked": false,
-      "linkedName": null
+      "linkedName": null
-    },
+    },
-    {
+    {
-      "anchor": {
+      "anchor": {
-        "x": 6.761975903614458,
+        "x": 6.761975903614458,
-        "y": 7.138469879518072
+        "y": 7.138469879518072
-      },
+      },
-      "prevControl": {
+      "prevControl": {
-        "x": 7.011975903614458,
+        "x": 7.011975903614458,
-        "y": 7.138469879518072
+        "y": 7.138469879518072
-      },
+      },
-      "nextControl": {
+      "nextControl": {
-        "x": 6.511975903614458,
+        "x": 6.511975903614458,
-        "y": 7.138469879518072
+        "y": 7.138469879518072
-      },
+      },
-      "isLocked": false,
+      "isLocked": false,
-      "linkedName": null
+      "linkedName": null
-    },
+    },
-    {
+    {
-      "anchor": {
+      "anchor": {
-        "x": 6.761975903614458,
+        "x": 6.761975903614458,
-        "y": 1.029879518072289
+        "y": 1.029879518072289
-      },
+      },
-      "prevControl": {
+      "prevControl": {
-        "x": 7.011975903614458,
+        "x": 7.011975903614458,
-        "y": 1.029879518072289
+        "y": 1.029879518072289
-      },
+      },
-      "nextControl": {
+      "nextControl": {
-        "x": 6.511975903614458,
+        "x": 6.511975903614458,
-        "y": 1.029879518072289
+        "y": 1.029879518072289
-      },
+      },
-      "isLocked": false,
+      "isLocked": false,
-      "linkedName": null
+      "linkedName": null
-    },
+    },
-    {
+    {
-      "anchor": {
+      "anchor": {
-        "x": 5.8440481927710834,
+        "x": 5.8440481927710834,
-        "y": 1.029879518072289
+        "y": 1.029879518072289
-      },
+      },
-      "prevControl": {
+      "prevControl": {
-        "x": 6.020824888067715,
+        "x": 6.020824888067715,
-        "y": 0.8531028227756465
+        "y": 0.8531028227756465
-      },
+      },
-      "nextControl": {
+      "nextControl": {
-        "x": 5.667271497474452,
+        "x": 5.667271497474452,
-        "y": 1.2066562133689316
+        "y": 1.2066562133689316
-      },
+      },
-      "isLocked": false,
+      "isLocked": false,
-      "linkedName": null
+      "linkedName": null
-    },
+    },
-    {
+    {
-      "anchor": {
+      "anchor": {
-        "x": 5.8440481927710834,
+        "x": 5.8440481927710834,
-        "y": 6.9964096385542165
+        "y": 6.9964096385542165
-      },
+      },
-      "prevControl": {
+      "prevControl": {
-        "x": 5.885147939597421,
+        "x": 5.885147939597421,
-        "y": 6.74981115759618
+        "y": 6.74981115759618
-      },
+      },
-      "nextControl": null,
+      "nextControl": null,
-      "isLocked": false,
+      "isLocked": false,
-      "linkedName": null
+      "linkedName": null
-    }
+    }
-  ],
+  ],
-  "rotationTargets": [
+  "rotationTargets": [
-    {
+    {
-      "waypointRelativePos": 0.5,
+      "waypointRelativePos": 0.5,
-      "rotationDegrees": 0.0
+      "rotationDegrees": 0.0
-    },
+    },
-    {
+    {
-      "waypointRelativePos": 1.0406639004149303,
+      "waypointRelativePos": 1.0406639004149303,
-      "rotationDegrees": 90.0
+      "rotationDegrees": 90.0
-    },
+    },
-    {
+    {
-      "waypointRelativePos": 1.917012448132766,
+      "waypointRelativePos": 1.917012448132766,
-      "rotationDegrees": 90.0
+      "rotationDegrees": 90.0
-    },
+    },
-    {
+    {
-      "waypointRelativePos": 3.0904564315352623,
+      "waypointRelativePos": 3.0904564315352623,
-      "rotationDegrees": -90.0
+      "rotationDegrees": -90.0
-    },
+    },
-    {
+    {
-      "waypointRelativePos": 3.91867219917012,
+      "waypointRelativePos": 3.91867219917012,
-      "rotationDegrees": -90.0
+      "rotationDegrees": -90.0
-    },
+    },
-    {
+    {
-      "waypointRelativePos": 5.063900414937716,
+      "waypointRelativePos": 5.063900414937716,
-      "rotationDegrees": 90.0
+      "rotationDegrees": 90.0
-    },
+    },
-    {
+    {
-      "waypointRelativePos": 6.0,
+      "waypointRelativePos": 6.0,
-      "rotationDegrees": 90.0
+      "rotationDegrees": 90.0
-    }
+    }
-  ],
+  ],
-  "constraintZones": [],
+  "constraintZones": [],
-  "pointTowardsZones": [],
+  "pointTowardsZones": [],
-  "eventMarkers": [
+  "eventMarkers": [
-    {
+    {
-      "name": "Shoot headed L-Tr",
+      "name": "Shoot headed L-Tr",
-      "waypointRelativePos": 1.0302729528536159,
+      "waypointRelativePos": 1.0302729528536159,
-      "endWaypointRelativePos": 2.4357320099255366,
+      "endWaypointRelativePos": 2.4357320099255366,
-      "command": {
+      "command": {
-        "type": "named",
+        "type": "named",
-        "data": {
+        "data": {
-          "name": "Shoot headed L-Tr"
+          "name": "Shoot headed L-Tr"
-        }
+        }
-      }
+      }
-    },
+    },
-    {
+    {
-      "name": "Shoot Headed R-Tr",
+      "name": "Shoot Headed R-Tr",
-      "waypointRelativePos": 2.9836228287841102,
+      "waypointRelativePos": 2.9836228287841102,
-      "endWaypointRelativePos": 4.103225806451647,
+      "endWaypointRelativePos": 4.103225806451647,
-      "command": {
+      "command": {
-        "type": "named",
+        "type": "named",
-        "data": {
+        "data": {
-          "name": "Shoot Headed R-Tr"
+          "name": "Shoot Headed R-Tr"
-        }
+        }
-      }
+      }
-    },
+    },
-    {
+    {
-      "name": "Shoot headed L-Tr",
+      "name": "Shoot headed L-Tr",
-      "waypointRelativePos": 5.079900744416814,
+      "waypointRelativePos": 5.079900744416814,
-      "endWaypointRelativePos": 6.0,
+      "endWaypointRelativePos": 6.0,
-      "command": {
+      "command": {
-        "type": "named",
+        "type": "named",
-        "data": {
+        "data": {
-          "name": "Shoot headed L-Tr"
+          "name": "Shoot headed L-Tr"
-        }
+        }
-      }
+      }
-    }
+    }
-  ],
+  ],
-  "globalConstraints": {
+  "globalConstraints": {
-    "maxVelocity": 3.0,
+    "maxVelocity": 3.0,
-    "maxAcceleration": 3.0,
+    "maxAcceleration": 3.0,
-    "maxAngularVelocity": 360.0,
+    "maxAngularVelocity": 360.0,
-    "maxAngularAcceleration": 720.0,
+    "maxAngularAcceleration": 720.0,
-    "nominalVoltage": 12.0,
+    "nominalVoltage": 12.0,
-    "unlimited": false
+    "unlimited": false
-  },
+  },
-  "goalEndState": {
+  "goalEndState": {
-    "velocity": 0,
+    "velocity": 0,
-    "rotation": 90.0
+    "rotation": 90.0
-  },
+  },
-  "reversed": false,
+  "reversed": false,
-  "folder": null,
+  "folder": null,
-  "idealStartingState": {
+  "idealStartingState": {
-    "velocity": 0,
+    "velocity": 0,
-    "rotation": 0.0
+    "rotation": 0.0
-  },
+  },
-  "useDefaultConstraints": true
+  "useDefaultConstraints": true
 }

deploy/pathplanner/paths/S. Hub. shoot 8. E-Hub.path

@@ -1,87 +1,87 @@
-{
+{
-  "version": "2025.0",
+  "version": "2025.0",
-  "waypoints": [
+  "waypoints": [
-    {
+    {
-      "anchor": {
+      "anchor": {
-        "x": 3.48366265060241,
+        "x": 3.48366265060241,
-        "y": 4.00221686746988
+        "y": 4.00221686746988
-      },
+      },
-      "prevControl": null,
+      "prevControl": null,
-      "nextControl": {
+      "nextControl": {
-        "x": 3.2365697261897557,
+        "x": 3.2365697261897557,
-        "y": 4.040231163533364
+        "y": 4.040231163533364
-      },
+      },
-      "isLocked": false,
+      "isLocked": false,
-      "linkedName": null
+      "linkedName": null
-    },
+    },
-    {
+    {
-      "anchor": {
+      "anchor": {
-        "x": 2.084915662650602,
+        "x": 2.084915662650602,
-        "y": 4.00221686746988
+        "y": 4.00221686746988
-      },
+      },
-      "prevControl": {
+      "prevControl": {
-        "x": 1.084915662650602,
+        "x": 1.084915662650602,
-        "y": 4.00221686746988
+        "y": 4.00221686746988
-      },
+      },
-      "nextControl": {
+      "nextControl": {
-        "x": 3.084915662650602,
+        "x": 3.084915662650602,
-        "y": 4.00221686746988
+        "y": 4.00221686746988
-      },
+      },
-      "isLocked": false,
+      "isLocked": false,
-      "linkedName": null
+      "linkedName": null
-    },
+    },
-    {
+    {
-      "anchor": {
+      "anchor": {
-        "x": 2.677383349903843,
+        "x": 2.677383349903843,
-        "y": 4.00221686746988
+        "y": 4.00221686746988
-      },
+      },
-      "prevControl": {
+      "prevControl": {
-        "x": 2.4274139067638107,
+        "x": 2.4274139067638107,
-        "y": 4.006125511257969
+        "y": 4.006125511257969
-      },
+      },
-      "nextControl": null,
+      "nextControl": null,
-      "isLocked": false,
+      "isLocked": false,
-      "linkedName": null
+      "linkedName": null
-    }
+    }
-  ],
+  ],
-  "rotationTargets": [
+  "rotationTargets": [
-    {
+    {
-      "waypointRelativePos": 0.7905511811023622,
+      "waypointRelativePos": 0.7905511811023622,
-      "rotationDegrees": 180.0
+      "rotationDegrees": 180.0
-    }
+    }
-  ],
+  ],
-  "constraintZones": [],
+  "constraintZones": [],
-  "pointTowardsZones": [],
+  "pointTowardsZones": [],
-  "eventMarkers": [
+  "eventMarkers": [
-    {
+    {
-      "name": "Shoot 8",
+      "name": "Shoot 8",
-      "waypointRelativePos": 2.0,
+      "waypointRelativePos": 2.0,
-      "endWaypointRelativePos": null,
+      "endWaypointRelativePos": null,
-      "command": {
+      "command": {
-        "type": "named",
+        "type": "named",
-        "data": {
+        "data": {
-          "name": "Shoot preload"
+          "name": "Shoot preload"
-        }
+        }
-      }
+      }
-    }
+    }
-  ],
+  ],
-  "globalConstraints": {
+  "globalConstraints": {
-    "maxVelocity": 3.0,
+    "maxVelocity": 3.0,
-    "maxAcceleration": 3.0,
+    "maxAcceleration": 3.0,
-    "maxAngularVelocity": 360.0,
+    "maxAngularVelocity": 360.0,
-    "maxAngularAcceleration": 720.0,
+    "maxAngularAcceleration": 720.0,
-    "nominalVoltage": 12.0,
+    "nominalVoltage": 12.0,
-    "unlimited": false
+    "unlimited": false
-  },
+  },
-  "goalEndState": {
+  "goalEndState": {
-    "velocity": 0,
+    "velocity": 0,
-    "rotation": 180.0
+    "rotation": 180.0
-  },
+  },
-  "reversed": false,
+  "reversed": false,
-  "folder": null,
+  "folder": null,
-  "idealStartingState": {
+  "idealStartingState": {
-    "velocity": 0,
+    "velocity": 0,
-    "rotation": 0.0
+    "rotation": 0.0
-  },
+  },
-  "useDefaultConstraints": true
+  "useDefaultConstraints": true
 }

deploy/pathplanner/paths/S. L-Tre. Preload..path

@@ -1,82 +1,82 @@
-{
+{
-  "version": "2025.0",
+  "version": "2025.0",
-  "waypoints": [
+  "waypoints": [
-    {
+    {
-      "anchor": {
+      "anchor": {
-        "x": 3.48366265060241,
+        "x": 3.48366265060241,
-        "y": 7.466301204819277
+        "y": 7.466301204819277
-      },
+      },
-      "prevControl": null,
+      "prevControl": null,
-      "nextControl": {
+      "nextControl": {
-        "x": 3.3666907204976515,
+        "x": 3.3666907204976515,
-        "y": 7.245354225732512
+        "y": 7.245354225732512
-      },
+      },
-      "isLocked": false,
+      "isLocked": false,
-      "linkedName": null
+      "linkedName": null
-    },
+    },
-    {
+    {
-      "anchor": {
+      "anchor": {
-        "x": 2.1723373493975906,
+        "x": 2.1723373493975906,
-        "y": 4.00221686746988
+        "y": 4.00221686746988
-      },
+      },
-      "prevControl": {
+      "prevControl": {
-        "x": 1.8772891566265066,
+        "x": 1.8772891566265066,
-        "y": 4.024072289156626
+        "y": 4.024072289156626
-      },
+      },
-      "nextControl": {
+      "nextControl": {
-        "x": 2.421654288011768,
+        "x": 2.421654288011768,
-        "y": 3.9837489460910525
+        "y": 3.9837489460910525
-      },
+      },
-      "isLocked": false,
+      "isLocked": false,
-      "linkedName": null
+      "linkedName": null
-    },
+    },
-    {
+    {
-      "anchor": {
+      "anchor": {
-        "x": 2.1723373493975906,
+        "x": 2.1723373493975906,
-        "y": 4.00221686746988
+        "y": 4.00221686746988
-      },
+      },
-      "prevControl": {
+      "prevControl": {
-        "x": 1.8226506024096385,
+        "x": 1.8226506024096385,
-        "y": 3.9912891566265065
+        "y": 3.9912891566265065
-      },
+      },
-      "nextControl": null,
+      "nextControl": null,
-      "isLocked": false,
+      "isLocked": false,
-      "linkedName": null
+      "linkedName": null
-    }
+    }
-  ],
+  ],
-  "rotationTargets": [],
+  "rotationTargets": [],
-  "constraintZones": [],
+  "constraintZones": [],
-  "pointTowardsZones": [],
+  "pointTowardsZones": [],
-  "eventMarkers": [
+  "eventMarkers": [
-    {
+    {
-      "name": "Shoot 8",
+      "name": "Shoot 8",
-      "waypointRelativePos": 2.0,
+      "waypointRelativePos": 2.0,
-      "endWaypointRelativePos": null,
+      "endWaypointRelativePos": null,
-      "command": {
+      "command": {
-        "type": "named",
+        "type": "named",
-        "data": {
+        "data": {
-          "name": "Shoot preload"
+          "name": "Shoot preload"
-        }
+        }
-      }
+      }
-    }
+    }
-  ],
+  ],
-  "globalConstraints": {
+  "globalConstraints": {
-    "maxVelocity": 3.0,
+    "maxVelocity": 3.0,
-    "maxAcceleration": 3.0,
+    "maxAcceleration": 3.0,
-    "maxAngularVelocity": 360.0,
+    "maxAngularVelocity": 360.0,
-    "maxAngularAcceleration": 720.0,
+    "maxAngularAcceleration": 720.0,
-    "nominalVoltage": 12.0,
+    "nominalVoltage": 12.0,
-    "unlimited": false
+    "unlimited": false
-  },
+  },
-  "goalEndState": {
+  "goalEndState": {
-    "velocity": 0,
+    "velocity": 0,
-    "rotation": 180.0
+    "rotation": 180.0
-  },
+  },
-  "reversed": false,
+  "reversed": false,
-  "folder": null,
+  "folder": null,
-  "idealStartingState": {
+  "idealStartingState": {
-    "velocity": 0,
+    "velocity": 0,
-    "rotation": 0.0
+    "rotation": 0.0
-  },
+  },
-  "useDefaultConstraints": true
+  "useDefaultConstraints": true
 }

deploy/pathplanner/paths/S. R-Trench. E-Hub.path

@@ -1,87 +1,87 @@
-{
+{
-  "version": "2025.0",
+  "version": "2025.0",
-  "waypoints": [
+  "waypoints": [
-    {
+    {
-      "anchor": {
+      "anchor": {
-        "x": 3.5273734939759036,
+        "x": 3.5273734939759036,
-        "y": 0.5272048192771093
+        "y": 0.5272048192771093
-      },
+      },
-      "prevControl": null,
+      "prevControl": null,
-      "nextControl": {
+      "nextControl": {
-        "x": 2.478313253012048,
+        "x": 2.478313253012048,
-        "y": 0.8003975903614468
+        "y": 0.8003975903614468
-      },
+      },
-      "isLocked": false,
+      "isLocked": false,
-      "linkedName": null
+      "linkedName": null
-    },
+    },
-    {
+    {
-      "anchor": {
+      "anchor": {
-        "x": 2.128626506024096,
+        "x": 2.128626506024096,
-        "y": 3.892939759036145
+        "y": 3.892939759036145
-      },
+      },
-      "prevControl": {
+      "prevControl": {
-        "x": 1.9085087310435527,
+        "x": 1.9085087310435527,
-        "y": 3.77441480327739
+        "y": 3.77441480327739
-      },
+      },
-      "nextControl": {
+      "nextControl": {
-        "x": 2.3487442810046395,
+        "x": 2.3487442810046395,
-        "y": 4.0114647147949
+        "y": 4.0114647147949
-      },
+      },
-      "isLocked": false,
+      "isLocked": false,
-      "linkedName": null
+      "linkedName": null
-    },
+    },
-    {
+    {
-      "anchor": {
+      "anchor": {
-        "x": 3.473,
+        "x": 3.473,
-        "y": 4.013
+        "y": 4.013
-      },
+      },
-      "prevControl": {
+      "prevControl": {
-        "x": 3.2270325224750227,
+        "x": 3.2270325224750227,
-        "y": 4.057721359549996
+        "y": 4.057721359549996
-      },
+      },
-      "nextControl": null,
+      "nextControl": null,
-      "isLocked": false,
+      "isLocked": false,
-      "linkedName": null
+      "linkedName": null
-    }
+    }
-  ],
+  ],
-  "rotationTargets": [
+  "rotationTargets": [
-    {
+    {
-      "waypointRelativePos": 0.8686614173228185,
+      "waypointRelativePos": 0.8686614173228185,
-      "rotationDegrees": 180.0
+      "rotationDegrees": 180.0
-    }
+    }
-  ],
+  ],
-  "constraintZones": [],
+  "constraintZones": [],
-  "pointTowardsZones": [],
+  "pointTowardsZones": [],
-  "eventMarkers": [
+  "eventMarkers": [
-    {
+    {
-      "name": "Shoot 8",
+      "name": "Shoot 8",
-      "waypointRelativePos": 2.0,
+      "waypointRelativePos": 2.0,
-      "endWaypointRelativePos": null,
+      "endWaypointRelativePos": null,
-      "command": {
+      "command": {
-        "type": "named",
+        "type": "named",
-        "data": {
+        "data": {
-          "name": "Shoot preload"
+          "name": "Shoot preload"
-        }
+        }
-      }
+      }
-    }
+    }
-  ],
+  ],
-  "globalConstraints": {
+  "globalConstraints": {
-    "maxVelocity": 3.0,
+    "maxVelocity": 3.0,
-    "maxAcceleration": 3.0,
+    "maxAcceleration": 3.0,
-    "maxAngularVelocity": 360.0,
+    "maxAngularVelocity": 360.0,
-    "maxAngularAcceleration": 720.0,
+    "maxAngularAcceleration": 720.0,
-    "nominalVoltage": 12.0,
+    "nominalVoltage": 12.0,
-    "unlimited": false
+    "unlimited": false
-  },
+  },
-  "goalEndState": {
+  "goalEndState": {
-    "velocity": 0,
+    "velocity": 0,
-    "rotation": 180.0
+    "rotation": 180.0
-  },
+  },
-  "reversed": false,
+  "reversed": false,
-  "folder": null,
+  "folder": null,
-  "idealStartingState": {
+  "idealStartingState": {
-    "velocity": 0,
+    "velocity": 0,
-    "rotation": 0.0
+    "rotation": 0.0
-  },
+  },
-  "useDefaultConstraints": true
+  "useDefaultConstraints": true
 }

deploy/pathplanner/paths/Spin.path

@@ -1,91 +1,91 @@
-{
+{
-  "version": "2025.0",
+  "version": "2025.0",
-  "waypoints": [
+  "waypoints": [
-    {
+    {
-      "anchor": {
+      "anchor": {
-        "x": 3.5164457831325304,
+        "x": 3.5164457831325304,
-        "y": 0.5599879518072293
+        "y": 0.5599879518072293
-      },
+      },
-      "prevControl": null,
+      "prevControl": null,
-      "nextControl": {
+      "nextControl": {
-        "x": 2.716570175215442,
+        "x": 2.716570175215442,
-        "y": 2.6055674399847426
+        "y": 2.6055674399847426
-      },
+      },
-      "isLocked": false,
+      "isLocked": false,
-      "linkedName": null
+      "linkedName": null
-    },
+    },
-    {
+    {
-      "anchor": {
+      "anchor": {
-        "x": 3.5164457831325304,
+        "x": 3.5164457831325304,
-        "y": 3.958506024096385
+        "y": 3.958506024096385
-      },
+      },
-      "prevControl": {
+      "prevControl": {
-        "x": 3.3576574884732078,
+        "x": 3.3576574884732078,
-        "y": 2.7315170387830205
+        "y": 2.7315170387830205
-      },
+      },
-      "nextControl": null,
+      "nextControl": null,
-      "isLocked": false,
+      "isLocked": false,
-      "linkedName": null
+      "linkedName": null
-    }
+    }
-  ],
+  ],
-  "rotationTargets": [
+  "rotationTargets": [
-    {
+    {
-      "waypointRelativePos": 0.24787401574803541,
+      "waypointRelativePos": 0.24787401574803541,
-      "rotationDegrees": 90.0
+      "rotationDegrees": 90.0
-    },
+    },
-    {
+    {
-      "waypointRelativePos": 0.29574803149607193,
+      "waypointRelativePos": 0.29574803149607193,
-      "rotationDegrees": -90.0
+      "rotationDegrees": -90.0
-    },
+    },
-    {
+    {
-      "waypointRelativePos": 0.3398425196850417,
+      "waypointRelativePos": 0.3398425196850417,
-      "rotationDegrees": 90.0
+      "rotationDegrees": 90.0
-    },
+    },
-    {
+    {
-      "waypointRelativePos": 0.38267716535433216,
+      "waypointRelativePos": 0.38267716535433216,
-      "rotationDegrees": -90.0
+      "rotationDegrees": -90.0
-    },
+    },
-    {
+    {
-      "waypointRelativePos": 0.4305511811023681,
+      "waypointRelativePos": 0.4305511811023681,
-      "rotationDegrees": -90.0
+      "rotationDegrees": -90.0
-    },
+    },
-    {
+    {
-      "waypointRelativePos": 0.465826771653542,
+      "waypointRelativePos": 0.465826771653542,
-      "rotationDegrees": 90.0
+      "rotationDegrees": 90.0
-    },
+    },
-    {
+    {
-      "waypointRelativePos": 0.5099212598425222,
+      "waypointRelativePos": 0.5099212598425222,
-      "rotationDegrees": -90.0
+      "rotationDegrees": -90.0
-    },
+    },
-    {
+    {
-      "waypointRelativePos": 0.5363779527559113,
+      "waypointRelativePos": 0.5363779527559113,
-      "rotationDegrees": 90.0
+      "rotationDegrees": 90.0
-    },
+    },
-    {
+    {
-      "waypointRelativePos": 0.5565354330708714,
+      "waypointRelativePos": 0.5565354330708714,
-      "rotationDegrees": -90.0
+      "rotationDegrees": -90.0
-    }
+    }
-  ],
+  ],
-  "constraintZones": [],
+  "constraintZones": [],
-  "pointTowardsZones": [],
+  "pointTowardsZones": [],
-  "eventMarkers": [],
+  "eventMarkers": [],
-  "globalConstraints": {
+  "globalConstraints": {
-    "maxVelocity": 3.0,
+    "maxVelocity": 3.0,
-    "maxAcceleration": 3.0,
+    "maxAcceleration": 3.0,
-    "maxAngularVelocity": 360.0,
+    "maxAngularVelocity": 360.0,
-    "maxAngularAcceleration": 720.0,
+    "maxAngularAcceleration": 720.0,
-    "nominalVoltage": 12.0,
+    "nominalVoltage": 12.0,
-    "unlimited": false
+    "unlimited": false
-  },
+  },
-  "goalEndState": {
+  "goalEndState": {
-    "velocity": 0,
+    "velocity": 0,
-    "rotation": 0.0
+    "rotation": 0.0
-  },
+  },
-  "reversed": false,
+  "reversed": false,
-  "folder": null,
+  "folder": null,
-  "idealStartingState": {
+  "idealStartingState": {
-    "velocity": 0,
+    "velocity": 0,
-    "rotation": 0.0
+    "rotation": 0.0
-  },
+  },
-  "useDefaultConstraints": true
+  "useDefaultConstraints": true
 }

deploy/pathplanner/settings.json

@@ -1,32 +1,32 @@
-{
+{
-  "robotWidth": 0.8763,
+  "robotWidth": 0.8763,
-  "robotLength": 0.8763,
+  "robotLength": 0.8763,
-  "holonomicMode": true,
+  "holonomicMode": true,
-  "pathFolders": [],
+  "pathFolders": [],
-  "autoFolders": [],
+  "autoFolders": [],
-  "defaultMaxVel": 3.0,
+  "defaultMaxVel": 3.0,
-  "defaultMaxAccel": 3.0,
+  "defaultMaxAccel": 3.0,
-  "defaultMaxAngVel": 360.0,
+  "defaultMaxAngVel": 360.0,
-  "defaultMaxAngAccel": 720.0,
+  "defaultMaxAngAccel": 720.0,
-  "defaultNominalVoltage": 12.0,
+  "defaultNominalVoltage": 12.0,
-  "robotMass": 44.4521,
+  "robotMass": 44.4521,
-  "robotMOI": 6.883,
+  "robotMOI": 6.883,
-  "robotTrackwidth": 0.546,
+  "robotTrackwidth": 0.546,
-  "driveWheelRadius": 0.051,
+  "driveWheelRadius": 0.051,
-  "driveGearing": 8.14,
+  "driveGearing": 8.14,
-  "maxDriveSpeed": 3.9,
+  "maxDriveSpeed": 3.9,
-  "driveMotorType": "krakenX60",
+  "driveMotorType": "krakenX60",
-  "driveCurrentLimit": 60.0,
+  "driveCurrentLimit": 60.0,
-  "wheelCOF": 1.2,
+  "wheelCOF": 1.2,
-  "flModuleX": 0.273,
+  "flModuleX": 0.273,
-  "flModuleY": 0.273,
+  "flModuleY": 0.273,
-  "frModuleX": 0.273,
+  "frModuleX": 0.273,
-  "frModuleY": -0.273,
+  "frModuleY": -0.273,
-  "blModuleX": -0.273,
+  "blModuleX": -0.273,
-  "blModuleY": 0.273,
+  "blModuleY": 0.273,
-  "brModuleX": -0.273,
+  "brModuleX": -0.273,
-  "brModuleY": -0.273,
+  "brModuleY": -0.273,
-  "bumperOffsetX": 0.0,
+  "bumperOffsetX": 0.0,
-  "bumperOffsetY": 0.0,
+  "bumperOffsetY": 0.0,
-  "robotFeatures": []
+  "robotFeatures": []
 }

genie.py

@@ -0,0 +1,37 @@
+"""
+genie.py – Thin MagicRobot subclass that integrates commands2.CommandScheduler.
+
+GenieRobot bridges MagicBot's component-injection lifecycle with the
+commands2/PathPlannerLib command-based pattern by running the
+CommandScheduler once per robot loop iteration.
+"""
+
+import commands2
+import magicbot
+
+
+class GenieRobot(magicbot.MagicRobot):
+    """
+    MagicRobot subclass that runs the commands2 CommandScheduler each loop.
+
+    Inherit from this instead of MagicRobot whenever PathPlannerLib commands
+    or other commands2 commands need to execute.
+    """
+
+    def robotPeriodic(self) -> None:
+        """Run the CommandScheduler every loop, then delegate to MagicRobot."""
+        commands2.CommandScheduler.getInstance().run()
+        return super().robotPeriodic()
+
+    def scheduleCommand(self, command: commands2.Command) -> None:
+        """
+        Schedule a commands2 Command to run via the CommandScheduler.
+        Used to kick off PathPlanner autos and named commands.
+
+        :param command: The command to schedule.
+        """
+        commands2.CommandScheduler.getInstance().schedule(command)
+
+    def disabledPeriodic(self) -> None:
+        """Called periodically while the robot is disabled. No-op by default."""
+        pass

gradle.properties

@@ -1,4 +0,0 @@
-frcYear = 2026
-wpiVersion = 2026.2.1
-
-ctreVersion = 26.1.0

gradle/wrapper/gradle-wrapper.properties

@@ -1,7 +0,0 @@
-distributionBase=GRADLE_USER_HOME
-distributionPath=permwrapper/dists
-distributionUrl=https\://services.gradle.org/distributions/gradle-8.11-bin.zip
-networkTimeout=10000
-validateDistributionUrl=true
-zipStoreBase=GRADLE_USER_HOME
-zipStorePath=permwrapper/dists

gradlew

@@ -1,252 +0,0 @@
-#!/bin/sh
-
-#
-# Copyright © 2015-2021 the original authors.
-#
-# Licensed under the Apache License, Version 2.0 (the "License");
-# you may not use this file except in compliance with the License.
-# You may obtain a copy of the License at
-#
-#      https://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS,
-# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
-# limitations under the License.
-#
-# SPDX-License-Identifier: Apache-2.0
-#
-
-##############################################################################
-#
-#   Gradle start up script for POSIX generated by Gradle.
-#
-#   Important for running:
-#
-#   (1) You need a POSIX-compliant shell to run this script. If your /bin/sh is
-#       noncompliant, but you have some other compliant shell such as ksh or
-#       bash, then to run this script, type that shell name before the whole
-#       command line, like:
-#
-#           ksh Gradle
-#
-#       Busybox and similar reduced shells will NOT work, because this script
-#       requires all of these POSIX shell features:
-#         * functions;
-#         * expansions «$var», «${var}», «${var:-default}», «${var+SET}»,
-#           «${var#prefix}», «${var%suffix}», and «$( cmd )»;
-#         * compound commands having a testable exit status, especially «case»;
-#         * various built-in commands including «command», «set», and «ulimit».
-#
-#   Important for patching:
-#
-#   (2) This script targets any POSIX shell, so it avoids extensions provided
-#       by Bash, Ksh, etc; in particular arrays are avoided.
-#
-#       The "traditional" practice of packing multiple parameters into a
-#       space-separated string is a well documented source of bugs and security
-#       problems, so this is (mostly) avoided, by progressively accumulating
-#       options in "$@", and eventually passing that to Java.
-#
-#       Where the inherited environment variables (DEFAULT_JVM_OPTS, JAVA_OPTS,
-#       and GRADLE_OPTS) rely on word-splitting, this is performed explicitly;
-#       see the in-line comments for details.
-#
-#       There are tweaks for specific operating systems such as AIX, CygWin,
-#       Darwin, MinGW, and NonStop.
-#
-#   (3) This script is generated from the Groovy template
-#       https://github.com/gradle/gradle/blob/HEAD/platforms/jvm/plugins-application/src/main/resources/org/gradle/api/internal/plugins/unixStartScript.txt
-#       within the Gradle project.
-#
-#       You can find Gradle at https://github.com/gradle/gradle/.
-#
-##############################################################################
-
-# Attempt to set APP_HOME
-
-# Resolve links: $0 may be a link
-app_path=$0
-
-# Need this for daisy-chained symlinks.
-while
-    APP_HOME=${app_path%"${app_path##*/}"}  # leaves a trailing /; empty if no leading path
-    [ -h "$app_path" ]
-do
-    ls=$( ls -ld "$app_path" )
-    link=${ls#*' -> '}
-    case $link in             #(
-      /*)   app_path=$link ;; #(
-      *)    app_path=$APP_HOME$link ;;
-    esac
-done
-
-# This is normally unused
-# shellcheck disable=SC2034
-APP_BASE_NAME=${0##*/}
-# Discard cd standard output in case $CDPATH is set (https://github.com/gradle/gradle/issues/25036)
-APP_HOME=$( cd -P "${APP_HOME:-./}" > /dev/null && printf '%s
-' "$PWD" ) || exit
-
-# Use the maximum available, or set MAX_FD != -1 to use that value.
-MAX_FD=maximum
-
-warn () {
-    echo "$*"
-} >&2
-
-die () {
-    echo
-    echo "$*"
-    echo
-    exit 1
-} >&2
-
-# OS specific support (must be 'true' or 'false').
-cygwin=false
-msys=false
-darwin=false
-nonstop=false
-case "$( uname )" in                #(
-  CYGWIN* )         cygwin=true  ;; #(
-  Darwin* )         darwin=true  ;; #(
-  MSYS* | MINGW* )  msys=true    ;; #(
-  NONSTOP* )        nonstop=true ;;
-esac
-
-CLASSPATH=$APP_HOME/gradle/wrapper/gradle-wrapper.jar
-
-
-# Determine the Java command to use to start the JVM.
-if [ -n "$JAVA_HOME" ] ; then
-    if [ -x "$JAVA_HOME/jre/sh/java" ] ; then
-        # IBM's JDK on AIX uses strange locations for the executables
-        JAVACMD=$JAVA_HOME/jre/sh/java
-    else
-        JAVACMD=$JAVA_HOME/bin/java
-    fi
-    if [ ! -x "$JAVACMD" ] ; then
-        die "ERROR: JAVA_HOME is set to an invalid directory: $JAVA_HOME
-
-Please set the JAVA_HOME variable in your environment to match the
-location of your Java installation."
-    fi
-else
-    JAVACMD=java
-    if ! command -v java >/dev/null 2>&1
-    then
-        die "ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH.
-
-Please set the JAVA_HOME variable in your environment to match the
-location of your Java installation."
-    fi
-fi
-
-# Increase the maximum file descriptors if we can.
-if ! "$cygwin" && ! "$darwin" && ! "$nonstop" ; then
-    case $MAX_FD in #(
-      max*)
-        # In POSIX sh, ulimit -H is undefined. That's why the result is checked to see if it worked.
-        # shellcheck disable=SC2039,SC3045
-        MAX_FD=$( ulimit -H -n ) ||
-            warn "Could not query maximum file descriptor limit"
-    esac
-    case $MAX_FD in  #(
-      '' | soft) :;; #(
-      *)
-        # In POSIX sh, ulimit -n is undefined. That's why the result is checked to see if it worked.
-        # shellcheck disable=SC2039,SC3045
-        ulimit -n "$MAX_FD" ||
-            warn "Could not set maximum file descriptor limit to $MAX_FD"
-    esac
-fi
-
-# Collect all arguments for the java command, stacking in reverse order:
-#   * args from the command line
-#   * the main class name
-#   * -classpath
-#   * -D...appname settings
-#   * --module-path (only if needed)
-#   * DEFAULT_JVM_OPTS, JAVA_OPTS, and GRADLE_OPTS environment variables.
-
-# For Cygwin or MSYS, switch paths to Windows format before running java
-if "$cygwin" || "$msys" ; then
-    APP_HOME=$( cygpath --path --mixed "$APP_HOME" )
-    CLASSPATH=$( cygpath --path --mixed "$CLASSPATH" )
-
-    JAVACMD=$( cygpath --unix "$JAVACMD" )
-
-    # Now convert the arguments - kludge to limit ourselves to /bin/sh
-    for arg do
-        if
-            case $arg in                                #(
-              -*)   false ;;                            # don't mess with options #(
-              /?*)  t=${arg#/} t=/${t%%/*}              # looks like a POSIX filepath
-                    [ -e "$t" ] ;;                      #(
-              *)    false ;;
-            esac
-        then
-            arg=$( cygpath --path --ignore --mixed "$arg" )
-        fi
-        # Roll the args list around exactly as many times as the number of
-        # args, so each arg winds up back in the position where it started, but
-        # possibly modified.
-        #
-        # NB: a `for` loop captures its iteration list before it begins, so
-        # changing the positional parameters here affects neither the number of
-        # iterations, nor the values presented in `arg`.
-        shift                   # remove old arg
-        set -- "$@" "$arg"      # push replacement arg
-    done
-fi
-
-
-# Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script.
-DEFAULT_JVM_OPTS='"-Xmx64m" "-Xms64m"'
-
-# Collect all arguments for the java command:
-#   * DEFAULT_JVM_OPTS, JAVA_OPTS, JAVA_OPTS, and optsEnvironmentVar are not allowed to contain shell fragments,
-#     and any embedded shellness will be escaped.
-#   * For example: A user cannot expect ${Hostname} to be expanded, as it is an environment variable and will be
-#     treated as '${Hostname}' itself on the command line.
-
-set -- \
-        "-Dorg.gradle.appname=$APP_BASE_NAME" \
-        -classpath "$CLASSPATH" \
-        org.gradle.wrapper.GradleWrapperMain \
-        "$@"
-
-# Stop when "xargs" is not available.
-if ! command -v xargs >/dev/null 2>&1
-then
-    die "xargs is not available"
-fi
-
-# Use "xargs" to parse quoted args.
-#
-# With -n1 it outputs one arg per line, with the quotes and backslashes removed.
-#
-# In Bash we could simply go:
-#
-#   readarray ARGS < <( xargs -n1 <<<"$var" ) &&
-#   set -- "${ARGS[@]}" "$@"
-#
-# but POSIX shell has neither arrays nor command substitution, so instead we
-# post-process each arg (as a line of input to sed) to backslash-escape any
-# character that might be a shell metacharacter, then use eval to reverse
-# that process (while maintaining the separation between arguments), and wrap
-# the whole thing up as a single "set" statement.
-#
-# This will of course break if any of these variables contains a newline or
-# an unmatched quote.
-#
-
-eval "set -- $(
-        printf '%s\n' "$DEFAULT_JVM_OPTS $JAVA_OPTS $GRADLE_OPTS" |
-        xargs -n1 |
-        sed ' s~[^-[:alnum:]+,./:=@_]~\\&~g; ' |
-        tr '\n' ' '
-    )" '"$@"'
-
-exec "$JAVACMD" "$@"

gradlew.bat

@@ -1,94 +0,0 @@
-@rem
-@rem Copyright 2015 the original author or authors.
-@rem
-@rem Licensed under the Apache License, Version 2.0 (the "License");
-@rem you may not use this file except in compliance with the License.
-@rem You may obtain a copy of the License at
-@rem
-@rem      https://www.apache.org/licenses/LICENSE-2.0
-@rem
-@rem Unless required by applicable law or agreed to in writing, software
-@rem distributed under the License is distributed on an "AS IS" BASIS,
-@rem WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-@rem See the License for the specific language governing permissions and
-@rem limitations under the License.
-@rem
-@rem SPDX-License-Identifier: Apache-2.0
-@rem
-
-@if "%DEBUG%"=="" @echo off
-@rem ##########################################################################
-@rem
-@rem  Gradle startup script for Windows
-@rem
-@rem ##########################################################################
-
-@rem Set local scope for the variables with windows NT shell
-if "%OS%"=="Windows_NT" setlocal
-
-set DIRNAME=%~dp0
-if "%DIRNAME%"=="" set DIRNAME=.
-@rem This is normally unused
-set APP_BASE_NAME=%~n0
-set APP_HOME=%DIRNAME%
-
-@rem Resolve any "." and ".." in APP_HOME to make it shorter.
-for %%i in ("%APP_HOME%") do set APP_HOME=%%~fi
-
-@rem Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script.
-set DEFAULT_JVM_OPTS="-Xmx64m" "-Xms64m"
-
-@rem Find java.exe
-if defined JAVA_HOME goto findJavaFromJavaHome
-
-set JAVA_EXE=java.exe
-%JAVA_EXE% -version >NUL 2>&1
-if %ERRORLEVEL% equ 0 goto execute
-
-echo. 1>&2
-echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. 1>&2
-echo. 1>&2
-echo Please set the JAVA_HOME variable in your environment to match the 1>&2
-echo location of your Java installation. 1>&2
-
-goto fail
-
-:findJavaFromJavaHome
-set JAVA_HOME=%JAVA_HOME:"=%
-set JAVA_EXE=%JAVA_HOME%/bin/java.exe
-
-if exist "%JAVA_EXE%" goto execute
-
-echo. 1>&2
-echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME% 1>&2
-echo. 1>&2
-echo Please set the JAVA_HOME variable in your environment to match the 1>&2
-echo location of your Java installation. 1>&2
-
-goto fail
-
-:execute
-@rem Setup the command line
-
-set CLASSPATH=%APP_HOME%\gradle\wrapper\gradle-wrapper.jar
-
-
-@rem Execute Gradle
-"%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" org.gradle.wrapper.GradleWrapperMain %*
-
-:end
-@rem End local scope for the variables with windows NT shell
-if %ERRORLEVEL% equ 0 goto mainEnd
-
-:fail
-rem Set variable GRADLE_EXIT_CONSOLE if you need the _script_ return code instead of
-rem the _cmd.exe /c_ return code!
-set EXIT_CODE=%ERRORLEVEL%
-if %EXIT_CODE% equ 0 set EXIT_CODE=1
-if not ""=="%GRADLE_EXIT_CONSOLE%" exit %EXIT_CODE%
-exit /b %EXIT_CODE%
-
-:mainEnd
-if "%OS%"=="Windows_NT" endlocal
-
-:omega

lib/Posty

@@ -1 +0,0 @@
-Subproject commit f3e38dc7ec323ba5cca8293a6df8de677d067d4d

lib/RoboLib

@@ -1 +0,0 @@
-Subproject commit c0478b01a526712b8308ac329017857177a3c876

libs.gradle

@@ -1,11 +0,0 @@
-build{
-    dependsOn ':lib:Posty:lib:build'
-    dependsOn ':lib:RoboLib:lib:build'
-}
-
-dependencies{
-    // Team 578 libraries
-    implementation project(':lib:Posty:lib')
-    implementation project(':lib:RoboLib:lib')
-}
-

pyproject.toml

@@ -0,0 +1,36 @@
+#
+# Use this configuration file to control what RobotPy packages are installed
+# on your RoboRIO
+#
+
+[tool.robotpy]
+
+# Version of robotpy this project depends on
+robotpy_version = "2026.2.2"
+
+# Which optional robotpy components should be installed?
+# -> equivalent to `pip install robotpy[extra1, ...]
+components = [
+    #"all",
+    "apriltag",
+    "commands2",            # Needed for PathPlanner + GenieRobot pattern
+    # "cscore",
+    # "romi",
+    # "sim",
+    # "xrp",
+]
+
+# Other pip packages to install, such as vendor packages (each element
+# is equivalent to a line in requirements.txt)
+requires = [
+    "robotpy-rev",              # SparkMax (launcher motor)
+    "robotpy-ctre",             # WPI_TalonSRX (deploy motor, Phoenix 5), TalonFX (drive, turret, hopper), CANcoder, Pigeon2
+    "robotpy-pathplannerlib",   # Path following
+]
+
+[tool.ruff]
+line-length = 120
+target-version = "py312"
+
+[tool.ruff.lint]
+select = ["E", "F", "W", "I"]   # pycodestyle, pyflakes, isort

robot.py

@@ -0,0 +1,164 @@
+"""
+robot.py – Main robot file for Castor 2026 (Python/MagicBot port).
+
+MyRobot extends GenieRobot (which extends MagicRobot and also runs the
+CommandScheduler each loop so PathPlannerLib commands work seamlessly).
+"""
+
+import commands2
+import wpilib
+from pathplannerlib.auto import NamedCommands
+from pathplannerlib.config import RobotConfig
+
+import constants
+from autonomous.chassis_align_shoot import ChassisAlignShootCommand
+from autonomous.turret_shoot import TurretShootCommand
+from components.controller import XboxController
+from components.drive import DriveComponent
+from components.hopper import HopperComponent
+from components.intake import IntakeComponent
+from components.turret import TurretComponent
+from genie import GenieRobot
+
+
+class _DriveSubsystemAdapter(commands2.Subsystem):
+    """
+    Satisfies AutoBuilder.configure()'s subsystem requirement parameter.
+    DriveComponent is not a commands2.Subsystem, so this adapter bridges the gap.
+    """
+
+    pass
+
+
+class MyRobot(GenieRobot):
+    # ── MagicBot component declarations (framework auto-injects instances) ──
+    driver_controller: XboxController
+    # Prefixed variable: MagicBot strips "driver_controller_" and injects
+    # `port` into the XboxController component before its setup() runs.
+    driver_controller_port: int = constants.OperatorConstants.CONTROLLER_PORT
+    drive: DriveComponent
+    hopper: HopperComponent
+    intake: IntakeComponent
+    turret: TurretComponent
+
+    def createObjects(self) -> None:
+        """
+        Called by MagicBot before any component setup() methods.
+        Class-level prefixed variables (e.g. driver_controller_port) are
+        injected into components automatically after this method returns.
+        """
+        # Tracks the active A/X turret-shoot command so it can be cancelled
+        self._shoot_cmd: TurretShootCommand | None = None
+
+    def _post_setup(self) -> None:
+        """
+        Called after all component setup() methods have run (from robotInit).
+        Wire button bindings and PathPlanner named commands here because
+        component objects are fully constructed at this point.
+        """
+        # Pass the underlying wpilib controller to intake for rumble feedback
+        self.intake.set_controller(self.driver_controller.this_controller)
+
+        # ── PathPlanner named commands ─────────────────────────────────────
+        NamedCommands.registerCommand(
+            "ChassisAlignShoot",
+            ChassisAlignShootCommand(self.drive, self.hopper, self.turret),
+        )
+        NamedCommands.registerCommand(
+            "TurretShoot",
+            TurretShootCommand(self.hopper, self.turret, self.intake, self.drive),
+        )
+        NamedCommands.registerCommand(
+            "RunIntake",
+            commands2.FunctionalCommand(
+                onInit=self.intake.intake,
+                onExecute=lambda: None,
+                onEnd=lambda interrupted: self.intake.stop_intake(),
+                isFinished=lambda: False,
+            ),
+        )
+        # Aliases used in PathPlanner auto files
+        NamedCommands.registerCommand(
+            "Shoot preload",
+            TurretShootCommand(self.hopper, self.turret, self.intake, self.drive),
+        )
+        NamedCommands.registerCommand(
+            "Shoot Headed R-Tr",
+            ChassisAlignShootCommand(self.drive, self.hopper, self.turret),
+        )
+        NamedCommands.registerCommand(
+            "Shoot headed L-Tr",
+            ChassisAlignShootCommand(self.drive, self.hopper, self.turret),
+        )
+
+        # ── AutoBuilder / auto chooser ─────────────────────────────────────
+        try:
+            robot_config = RobotConfig.fromGUISettings()
+            drive_adapter = _DriveSubsystemAdapter()
+            self.drive.inject_drive_adapter(drive_adapter, robot_config)
+        except Exception as e:
+            print(f"[robot.py] AutoBuilder configuration failed: {e}")
+
+    def teleopPeriodic(self) -> None:
+        # ── Drive ──────────────────────────────────────────────────────────
+        self.drive.set_chassis_speeds_field_relative(
+            -self.driver_controller.left_y,
+            -self.driver_controller.left_x,
+            -self.driver_controller.right_x,
+        )
+
+        # ── One-shot actions (fire when button is released) ────────────────
+        if self.driver_controller.left_bumper_was_pressed():
+            self.turret.home_turret()
+
+        if self.driver_controller.right_bumper_was_pressed():
+            self.intake.intake()
+
+        if self.driver_controller.y_button_was_pressed():
+            self.scheduleCommand(
+                commands2.SequentialCommandGroup(
+                    commands2.InstantCommand(self.intake.deploy),
+                    commands2.WaitCommand(2.0),
+                    commands2.InstantCommand(self.intake.stop_deploy),
+                )
+            )
+
+        # ── Held actions ───────────────────────────────────────────────────
+        if self.driver_controller.start_button_pressed():
+            self.intake.reset_deploy()
+
+        if self.driver_controller.b_button_pressed():
+            self.intake.reverse_intake()
+            self.hopper.reverse_kicker()
+
+        # A or X held → schedule turret-shoot command; cancel when released
+        shooting = (
+            self.driver_controller.a_button_pressed()
+            or self.driver_controller.x_button_pressed()
+        )
+        if shooting:
+            if self._shoot_cmd is None or self._shoot_cmd.isFinished():
+                self._shoot_cmd = TurretShootCommand(
+                    self.hopper, self.turret, self.intake, self.drive
+                )
+                self.scheduleCommand(self._shoot_cmd)
+        elif self._shoot_cmd is not None and not self._shoot_cmd.isFinished():
+            self._shoot_cmd.cancel()
+            self._shoot_cmd = None
+
+    def autonomousInit(self) -> None:
+        auto_cmd = self.drive.get_auto_command()
+        if auto_cmd is not None:
+            self.scheduleCommand(auto_cmd)
+
+    def disabledInit(self) -> None:
+        self.drive.set_brake_mode(True)
+
+    def robotInit(self) -> None:
+        super().robotInit()
+        self.drive.set_brake_mode(False)
+        self._post_setup()
+
+
+if __name__ == "__main__":
+    wpilib.run(MyRobot)

settings.gradle

@@ -1,33 +0,0 @@
-import org.gradle.internal.os.OperatingSystem
-
-pluginManagement {
-    repositories {
-        mavenLocal()
-        gradlePluginPortal()
-        String frcYear = '2026'
-        File frcHome
-        if (OperatingSystem.current().isWindows()) {
-            String publicFolder = System.getenv('PUBLIC')
-            if (publicFolder == null) {
-                publicFolder = "C:\\Users\\Public"
-            }
-            def homeRoot = new File(publicFolder, "wpilib")
-            frcHome = new File(homeRoot, frcYear)
-        } else {
-            def userFolder = System.getProperty("user.home")
-            def homeRoot = new File(userFolder, "wpilib")
-            frcHome = new File(homeRoot, frcYear)
-        }
-        def frcHomeMaven = new File(frcHome, 'maven')
-        maven {
-            name = 'frcHome'
-            url = frcHomeMaven
-        }
-    }
-}
-
-Properties props = System.getProperties();
-props.setProperty("org.gradle.internal.native.headers.unresolved.dependencies.ignore", "true");
-
-include ':lib:Posty:lib'
-include ':lib:RoboLib:lib'

simgui-ds.json

@@ -1,105 +0,0 @@
-{
-  "Keyboard 0 Settings": {
-    "window": {
-      "visible": true
-    }
-  },
-  "keyboardJoysticks": [
-    {
-      "axisConfig": [
-        {
-          "decKey": 65,
-          "incKey": 68
-        },
-        {
-          "decKey": 83,
-          "incKey": 87
-        },
-        {
-          "decayRate": 0.009999999776482582,
-          "keyRate": 0.009999999776482582
-        },
-        {},
-        {
-          "decKey": 81,
-          "incKey": 69
-        }
-      ],
-      "axisCount": 5,
-      "buttonCount": 4,
-      "buttonKeys": [
-        90,
-        88,
-        67,
-        86
-      ],
-      "povConfig": [
-        {
-          "key0": 328,
-          "key135": 323,
-          "key180": 322,
-          "key225": 321,
-          "key270": 324,
-          "key315": 327,
-          "key45": 329,
-          "key90": 326
-        }
-      ],
-      "povCount": 1
-    },
-    {
-      "axisConfig": [
-        {
-          "decKey": 74,
-          "incKey": 76
-        },
-        {
-          "decKey": 73,
-          "incKey": 75
-        }
-      ],
-      "axisCount": 2,
-      "buttonCount": 4,
-      "buttonKeys": [
-        77,
-        44,
-        46,
-        47
-      ],
-      "povCount": 0
-    },
-    {
-      "axisConfig": [
-        {
-          "decKey": 263,
-          "incKey": 262
-        },
-        {
-          "decKey": 265,
-          "incKey": 264
-        }
-      ],
-      "axisCount": 2,
-      "buttonCount": 6,
-      "buttonKeys": [
-        260,
-        268,
-        266,
-        261,
-        269,
-        267
-      ],
-      "povCount": 0
-    },
-    {
-      "axisCount": 0,
-      "buttonCount": 0,
-      "povCount": 0
-    }
-  ],
-  "robotJoysticks": [
-    {
-      "guid": "Keyboard0"
-    }
-  ]
-}

src/main/java/frc/robot/Constants.java

@@ -1,220 +0,0 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-package frc.robot;
-
-import com.ctre.phoenix6.signals.InvertedValue;
-
-import edu.wpi.first.apriltag.AprilTagFieldLayout;
-import edu.wpi.first.apriltag.AprilTagFields;
-import edu.wpi.first.math.geometry.Pose3d;
-import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Rotation3d;
-import edu.wpi.first.math.geometry.Transform2d;
-import edu.wpi.first.math.geometry.Transform3d;
-import edu.wpi.first.units.measure.Angle;
-
-/**
- * The Constants class provides a convenient place for teams to hold robot-wide
- * numerical or boolean constants. This class should not be used for any other
- * purpose. All constants should be declared globally (i.e. public static). Do
- * not put anything functional in this class.
- *
- * <p>
- * It is advised to statically import this class (or one of its inner classes)
- * wherever the constants are needed, to reduce verbosity.
- */
-public final class Constants {
-
-    public static class OperatorConstants {
-
-        public static final int kDriverControllerPort = 0;
-    }
-
-    public static class CameraConstanst {
-
-        public static class IDFilters {
-
-            public static final int[] RED_HUB_SHOOTING_IDS = new int[]{8, 9, 10, 11};
-            public static final int[] BLUE_HUB_SHOOTING_IDS = new int[]{27, 26, 25, 24};
-        }
-
-        public static final String BACK_CAMERA_NAME = "limelight-back";
-        public static final int BACK_LOCALIZATION_PIPELINE_NUMBER = 0;
-        public static final int BACK_HUB_TRACKING_2D_PIPELINE_NUMBER = 1;
-        public static final int BACK_HUB_TRACKING_3D_PIPELINE_NUMBER = 2;
-
-        public static final String FRONT_CAMERA_NAME = "limelight-front";
-        public static final int FRONT_LOCALIZATION_PIPELINE_NUMBER = 0;
-        public static final double[] BLUE_ZONE_COOR = new double[]{0, 3.5};
-        public static final double[] RED_ZONE_COOR = new double[]{13, 16};
-        public static final double[] NEUTRAL_ZONE_COOR = new double[]{5.5, 11};
-        public static final double[] NEUTRAL1_ZONE_COOR = new double[]{5.5, 11, 0, 3};
-        public static final double[] NEUTRAL2_ZONE_COOR = new double[]{5.5, 11, 5, 8};
-        public static final double[] BLUE1_ZONE_COOR = new double[]{0, 3.5, 0, 3};
-        public static final double[] BLUE2_ZONE_COOR = new double[]{0, 3.5, 5, 8};
-        public static final double[] RED1_ZONE_COOR = new double[]{13, 16, 0, 3};
-        public static final double[] RED2_ZONE_COOR = new double[]{13, 16, 5, 8};
-
-    }
-
-    public static class AdvantageKitConstants {
-
-        public enum RobotType {
-            SIM,
-            REAL,
-            REPLAY
-        }
-        public static final RobotType CURRENT_MODE = RobotType.REAL;
-    }
-    // Swerve offsets as of 3/2/26
-
-    public static class SwerveDriveOffsets {
-
-        public static final float FRONT_LEFT_MOTOR_OFFSET = -0.336426f;
-        public static final float FRONT_RIGHT_MOTOR_OFFSET = 0.108887f;
-        public static final float BACK_LEFT_MOTOR_OFFSET = -0.492432f;
-        public static final float BACK_RIGHT_MOTOR_OFFSET = 0.470215f;
-    }
-
-    /*
-   * Numbers starting with 1 (E.g. 11, 12, 13) are in the Front Left of the robot
-   * Numbers starting with 2 (E.g. 21, 22, 23) are in the Front Right of the robot
-   * Numbers starting with 3 (E.g. 31, 32, 33) are in the Back Left of the robot
-   * Numbers starting with 4 (E.g. 41, 42, 43) are in the Back Right of the robot
-   * Laters numbers will be added for more subsystems
-   * Single diget numbers (E.g. 1, 2, 3) are extra, non-motor, pieces that require IDs, such as the Pigeon
-     */
-    public static class SwerveDriveIDs {
-
-        public static final int FRONT_LEFT_STEER_ID = 11;
-        public static final int FRONT_LEFT_DRIVE_ID = 12;
-        public static final int FRONT_LEFT_ENCODER_ID = 13;
-        public static final int FRONT_RIGHT_STEER_ID = 21;
-        public static final int FRONT_RIGHT_DRIVE_ID = 22;
-        public static final int FRONT_RIGHT_ENCODER_ID = 23;
-        public static final int BACK_LEFT_STEER_ID = 31;
-        public static final int BACK_LEFT_DRIVE_ID = 32;
-        public static final int BACK_LEFT_ENCODER_ID = 33;
-        public static final int BACK_RIGHT_STEER_ID = 41;
-        public static final int BACK_RIGHT_DRIVE_ID = 42;
-        public static final int BACK_RIGHT_ENCODER_ID = 43;
-    }
-
-    public static class RobotChassisLimits {
-        // Max velocity in meters per second
-
-        public static final double MAX_ROBOT_LINEAR_VELOCITY = 3;
-        // In meters per second squared
-        public static final double MAX_ROBOT_LINEAR_VELOCITY_ACCEL = 2;
-
-        // In radians per second
-        public static final double MAX_ROBOT_ANGULAR_VELOCITY = Math.PI;
-        // In radians per second squared
-        public static final double MAX_ROBOT_ANGULAR_VELOCITY_ACCEL = Math.PI;
-    }
-
-    //Bird
-    public static class ExtraIDEntities {
-
-        public static final int PIGEON_ID = 1;
-    }
-
-    public static class IntakeConstants {
-
-        public static final int DEPLOY_MOTOR_ID = 17;
-        public static final boolean DEPLOY_MOTOR_INVERTED = true; // TODO: Test this
-
-        public static final int INTAKE_MOTOR_ID = 2;
-        public static final InvertedValue INTAKE_MOTOR_DIRECTION = InvertedValue.Clockwise_Positive; // TODO: Test this
-    }
-
-    public static class ShooterConstants {
-
-        public static final int LAUNCHER_MOTOR_ID = 3;
-        public static final boolean LAUNCHER_MOTOR_INVERTED = false;
-
-        public static final int TURRET_MOTOR_ID = 4;
-        public static final InvertedValue TURRET_MOTOR_DIRECTION = InvertedValue.CounterClockwise_Positive;
-        public static final int TURRET_LIMIT_CHANNEL = 9;
-        public static final Angle LIMIT_AXIMUTH_POS = edu.wpi.first.units.Units.Degrees.of(165);
-        public static final Angle LIMIT_AXIMUTH_NEG = edu.wpi.first.units.Units.Degrees.of(5);
-        public static final double HOMING_SPEED = .05;
-        public static final double TURRET_GEAR_RATIO = 6;
-
-        public static final int HOOD_SERVO_CHANNEL = 0;
-        public static final boolean HOOD_SERVO_INVERTED = false; // TODO: Test this
-        public static final int HOOD_ENCODER_ID = 4;
-        public static final Angle TARGET_ELEVATION_DIF = edu.wpi.first.units.Units.Degrees.of(1);
-        public static final Angle LIMIT_ELEVATION_POS = edu.wpi.first.units.Units.Degrees.of(90);
-        public static final Angle LIMIT_ELEVATION_NEG = edu.wpi.first.units.Units.Degrees.of(0);
-        public static final double DEFLECTOR_STORED_ANGLE = 0;
-        public static final double DEFLECTOR_SET_ANGLE1 = 100;
-        public static final double DEFLECTOR_SET_ANGLE2 = 200;
-        public static final double DEFLECTOR_SET_ANGLE3 = 300;
-        public static final double DEFLECTOR_SERVO_RATIO = 1.0;
-    }
-
-    public static class HopperConstants {
-
-        public static final int KICKER_MOTOR_ID = 9;
-        public static final InvertedValue KICKER_MOTOR_DIRECTION = InvertedValue.Clockwise_Positive;
-
-        public static final int SPINDEXER_MOTOR_ID = 8;
-        public static final InvertedValue SPINDEXER_MOTOR_DIRECTION = InvertedValue.CounterClockwise_Positive;
-    }
-
-    public static class FieldPoses {
-
-        private static AprilTagFieldLayout fieldTags = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltWelded);
-
-        public static Pose3d BLUE_HUB_POSE = fieldTags.getTagPose(26).get().transformBy(new Transform3d(new Transform2d(-0.5207, 0, Rotation2d.kZero)));
-        public static Pose3d RED_HUB_POSE = fieldTags.getTagPose(10).get().transformBy(new Transform3d(new Transform2d(-0.5207, 0, Rotation2d.kZero)));
-
-        // These are the field targets for passing fuel to the Alliance Zone or to the Neutral Zone from the opponent's side.
-        // Left/Right are from driver's perspective. Coordinates assume origin at blue side of field.
-        // Rotation are zero because they don't matter.
-        public static Pose3d BLUE_LEFT_ALLIANCE_ZONE_PASS_TARGET_POSE = new Pose3d(1.99, 6.35, 0, new Rotation3d());
-        public static Pose3d BLUE_RIGHT_ALLIANCE_ZONE_PASS_TARGET_POSE = new Pose3d(1.99, 1.72, 0, new Rotation3d());
-        public static Pose3d BLUE_LEFT_NEUTRAL_ZONE_PASS_TARGET_POSE = new Pose3d(8.27, 6.35, 0, new Rotation3d());
-        public static Pose3d BLUE_RIGHT_NEUTRAL_ZONE_PASS_TARGET_POSE = new Pose3d(8.27, 1.72, 0, new Rotation3d());
-
-        public static Pose3d RED_LEFT_ALLIANCE_ZONE_PASS_TARGET_POSE = new Pose3d(14.55, 1.72, 0, new Rotation3d());
-        public static Pose3d RED_RIGHT_ALLIANCE_ZONE_PASS_TARGET_POSE = new Pose3d(14.55, 6.35, 0, new Rotation3d());
-        public static Pose3d RED_LEFT_NEUTRAL_ZONE_PASS_TARGET_POSE = new Pose3d(8.27, 1.72, 0, new Rotation3d());
-        public static Pose3d RED_RIGHT_NEUTRAL_ZONE_PASS_TARGET_POSE = new Pose3d(8.27, 6.35, 0, new Rotation3d());
-
-
-    }
-
-    // These represent the dividing lines for the regions in our shooting strategy.
-    public static class ShootingRegionDimensions {
-
-        public static double BLUE_ALLIANCE_ZONE_REGION_X_MAX = 3.5;
-        public static double RED_ALLIANCE_ZONE_REGION_X_MIN = 13.0;
-        public static double NEUTRAL_ZONE_REGION_X_MIN = 5.5;
-        public static double NEUTRAL_ZONE_REGION_X_MAX = 11.0;
-        public static double BLUE_RIGHT_REGION_Y_MAX = 3.0;
-        public static double RED_LEFT_REGION_Y_MAX = 3.0;               // same meaning as the previous one, but from red perspective
-        public static double BLUE_LEFT_REGION_Y_MIN = 5.0;
-        public static double RED_RIGHT_REGION_Y_MIN = 5.0;              // same meaning as the previous one, but from red perspective
-
-    }
-
-    // Regions that define the AutoTurret shooting behaviors.
-    // Color ID in the name reflects robot's alliance since region definitions vary by alliance.
-    // Left/Right are from driver's perspective.
-    public enum ShootingRegion {
-        NON_SHOOTING_REGION,                // Any part of the field that's not in one of the regions
-        BLUE_OWN_ALLIANCE_ZONE,
-        BLUE_LEFT_NEUTRAL_ZONE,
-        BLUE_RIGHT_NEUTRAL_ZONE,
-        BLUE_LEFT_OPPONENT_ALLIANCE_ZONE,
-        BLUE_RIGHT_OPPONENT_ALLIANCE_ZONE,
-        RED_OWN_ALLIANCE_ZONE,
-        RED_LEFT_NEUTRAL_ZONE,
-        RED_RIGHT_NEUTRAL_ZONE,
-        RED_LEFT_OPPONENT_ALLIANCE_ZONE,
-        RED_RIGHT_OPPONENT_ALLIANCE_ZONE        
-    }
-}

src/main/java/frc/robot/Main.java

@@ -1,25 +0,0 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
-package frc.robot;
-
-import edu.wpi.first.wpilibj.RobotBase;
-
-/**
- * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
- * you are doing, do not modify this file except to change the parameter class to the startRobot
- * call.
- */
-public final class Main {
-  private Main() {}
-
-  /**
-   * Main initialization function. Do not perform any initialization here.
-   *
-   * <p>If you change your main robot class, change the parameter type.
-   */
-  public static void main(String... args) {
-    RobotBase.startRobot(Robot::new);
-  }
-}

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

@@ -1,188 +0,0 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
-package frc.robot;
-
-import java.io.File;
-import java.io.FileNotFoundException;
-import java.util.Scanner;
-
-import org.fairportrobotics.frc.posty.PostyManager;
-import org.littletonrobotics.junction.LoggedRobot;
-import org.littletonrobotics.junction.Logger;
-import org.littletonrobotics.junction.networktables.NT4Publisher;
-import org.littletonrobotics.junction.wpilog.WPILOGWriter;
-
-import com.ctre.phoenix6.swerve.SwerveModule;
-
-import edu.wpi.first.wpilibj.Filesystem;
-import edu.wpi.first.wpilibj.PowerDistribution;
-import edu.wpi.first.wpilibj.PowerDistribution.ModuleType;
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.CommandScheduler;
-
-/**
- * The VM is configured to automatically run this class, and to call the
- * functions corresponding to
- * each mode, as described in the TimedRobot documentation. If you change the
- * name of this class or
- * the package after creating this project, you must also update the
- * build.gradle file in the
- * project.
- */
-public class Robot extends LoggedRobot {
-  private Command m_autonomousCommand;
-
-  private RobotContainer m_robotContainer;
-
-  public static String getVersion() {
-    try {
-      Scanner scan = new Scanner(new File(Filesystem.getDeployDirectory().getAbsolutePath() + "/versionfile.txt"));
-      String ver = scan.nextLine();
-      scan.close();
-
-      return ver;
-    } catch (FileNotFoundException e) {
-      e.printStackTrace();
-    }
-    return "";
-  }
-
-  /**
-   * This function is run when the robot is first started up and should be used
-   * for any
-   * initialization code.
-   */
-  @Override
-  public void robotInit() {
-
-    String swVersion = getVersion();
-    Logger.recordMetadata("ProjectName", "MyProject"); // Set a metadata value
-    Logger.recordMetadata("Software Version", swVersion);
-
-    if (isReal()) {
-      Logger.addDataReceiver(new WPILOGWriter()); // Log to a USB stick ("/U/logs")
-      Logger.addDataReceiver(new NT4Publisher()); // Publish data to NetworkTables
-      new PowerDistribution(1, ModuleType.kRev); // Enables power distribution logging
-    } else {
-      setUseTiming(false); // Run as fast as possible
-      // String logPath = LogFileUtil.findReplayLog(); // Pull the replay log from AdvantageScope (or prompt the user)
-      // Logger.setReplaySource(new WPILOGReader(logPath)); // Read replay log
-      // Logger.addDataReceiver(new WPILOGWriter(LogFileUtil.addPathSuffix(logPath, "_sim"))); // Save outputs to a new log
-      Logger.addDataReceiver(new NT4Publisher()); // Publish data to NetworkTables
-    }
-
-    // Logger.disableDeterministicTimestamps() // See "Deterministic Timestamps" in
-    // the "Understanding Data Flow" page
-    Logger.start(); // Start logging! No more data receivers, replay sources, or metadata values may
-                    // be added.
-
-    // Print out software version
-    Logger.recordOutput("Software Version", swVersion);
-    System.out.println("Robot Software Version: " + swVersion);
-
-    // Instantiate our RobotContainer. This will perform all our button bindings,
-    // and put our
-    // autonomous chooser on the dashboard.
-    m_robotContainer = new RobotContainer();
-
-    // Collect Posty tests
-    PostyManager.getInstance().collectTests();
-
-    // Run all POST tests after robot has been initialized
-    PostyManager.getInstance().runAllPOSTs();
-
-
-    m_robotContainer.driveSubsystem.driveSystem.setBrakeMode(false);
-  }
-
-  /**
-   * This function is called every 20 ms, no matter the mode. Use this for items
-   * like diagnostics
-   * that you want ran during disabled, autonomous, teleoperated and test.
-   *
-   * <p>
-   * This runs after the mode specific periodic functions, but before LiveWindow
-   * and
-   * SmartDashboard integrated updating.
-   */
-  @Override
-  public void robotPeriodic() {
-    // Runs the Scheduler. This is responsible for polling buttons, adding
-    // newly-scheduled
-    // commands, running already-scheduled commands, removing finished or
-    // interrupted commands,
-    // and running subsystem periodic() methods. This must be called from the
-    // robot's periodic
-    // block in order for anything in the Command-based framework to work.
-    CommandScheduler.getInstance().run();
-  }
-
-  /** This function is called once each time the robot enters Disabled mode. */
-  @Override
-  public void disabledInit() {
-    m_robotContainer.driveSubsystem.driveSystem.setBrakeMode(true);
-  }
-
-  @Override
-  public void disabledPeriodic() {
-  }
-
-  /**
-   * This autonomous runs the autonomous command selected by your
-   * {@link RobotContainer} class.
-   */
-  @Override
-  public void autonomousInit() {
-    m_autonomousCommand = m_robotContainer.getAutonomousCommand();
-
-    // schedule the autonomous command (example)
-    if (m_autonomousCommand != null) {
-      CommandScheduler.getInstance().schedule(m_autonomousCommand);
-    }
-  }
-
-  /** This function is called periodically during autonomous. */
-  @Override
-  public void autonomousPeriodic() {
-  }
-
-  @Override
-  public void teleopInit() {
-    // This makes sure that the autonomous stops running when
-    // teleop starts running. If you want the autonomous to
-    // continue until interrupted by another command, remove
-    // this line or comment it out.
-    if (m_autonomousCommand != null) {
-      m_autonomousCommand.cancel();
-    }
-
-  }
-
-  /** This function is called periodically during operator control. */
-  @Override
-  public void teleopPeriodic() {
-  }
-
-  @Override
-  public void testInit() {
-    // Cancels all running commands at the start of test mode.
-    CommandScheduler.getInstance().cancelAll();
-  }
-
-  /** This function is called periodically during test mode. */
-  @Override
-  public void testPeriodic() {
-  }
-
-  /** This function is called once when the robot is first started up. */
-  @Override
-  public void simulationInit() {
-  }
-
-  /** This function is called periodically whilst in simulation. */
-  @Override
-  public void simulationPeriodic() {
-  }
-}

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

@@ -1,103 +0,0 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-package frc.robot;
-
-import com.pathplanner.lib.auto.NamedCommands;
-
-import edu.wpi.first.wpilibj.GenericHID.RumbleType;
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.Commands;
-import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
-import edu.wpi.first.wpilibj2.command.button.Trigger;
-import frc.robot.Constants.OperatorConstants;
-import frc.robot.commands.AutoShootCommandChassisTurret;
-import frc.robot.commands.AutoTurretCommand;
-import frc.robot.commands.AutoTurretShootCommand;
-import frc.robot.commands.Reverse;
-import frc.robot.commands.SetDeflectorCommand;
-import frc.robot.subsystems.DriveSubsystem;
-import frc.robot.subsystems.HopperSubsystem;
-import frc.robot.subsystems.IntakeSubsystem;
-import frc.robot.subsystems.TurretSubsystem;
-
-/**
- * This class is where the bulk of the robot should be declared. Since
- * Command-based is a "declarative" paradigm, very little robot logic should
- * actually be handled in the {@link Robot} periodic methods (other than the
- * scheduler calls). Instead, the structure of the robot (including subsystems,
- * commands, and trigger mappings) should be declared here.
- */
-public class RobotContainer {
-    // The robot's subsystems and commands are defined here...
-    // Replace with CommandPS4Controller or CommandJoystick if needed
-
-    private final CommandXboxController m_driverController
-            = new CommandXboxController(OperatorConstants.kDriverControllerPort);
-
-    // private final CommandPS4Controller m_driverController = new CommandPS4Controller(OperatorConstants.kDriverControllerPort);
-    public final DriveSubsystem driveSubsystem = new DriveSubsystem(m_driverController);
-
-    public final IntakeSubsystem intakeSubsystem = new IntakeSubsystem();
-
-    public final TurretSubsystem turretSubsystem = new TurretSubsystem();
-
-    public final HopperSubsystem hopperSubsystem = new HopperSubsystem();
-
-    /**
-     * The container for the robot. Contains subsystems, OI devices, and
-     * commands.
-     */
-    public RobotContainer() {
-
-        // turretSubsystem.setDefaultCommand(new AutoTurretCommand(turretSubsystem, driveSubsystem));
-        // turretSubsystem.homeTurret();
-        // Configure the trigger bindings
-        configureBindings();
-
-        NamedCommands.registerCommand("ChassisAlignShoot", new AutoShootCommandChassisTurret(driveSubsystem, hopperSubsystem, turretSubsystem));
-        NamedCommands.registerCommand("RunIntake", intakeSubsystem.intake(m_driverController.getHID()));
-        NamedCommands.registerCommand("TurretShoot", new AutoTurretShootCommand(hopperSubsystem, turretSubsystem, intakeSubsystem, driveSubsystem));
-
-        turretSubsystem.setDefaultCommand(new AutoTurretCommand(turretSubsystem, driveSubsystem));
-    }
-
-    /**
-     * Use this method to define your trigger->command mappings. Triggers can be
-     * created via the
-     * {@link Trigger#Trigger(java.util.function.BooleanSupplier)} constructor
-     * with an arbitrary predicate, or via the named factories in {@link
-     * edu.wpi.first.wpilibj2.command.button.CommandGenericHID}'s subclasses for {@link
-     * CommandXboxController Xbox}/{@link edu.wpi.first.wpilibj2.command.button.CommandPS4Controller
-     * PS4} controllers or {@link edu.wpi.first.wpilibj2.command.button.CommandJoystick Flight
-     * joysticks}.
-     *
-     */
-    private void configureBindings() {
-        // m_driverController.povDown().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_STORED_ANGLE));
-        // m_driverController.povLeft().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE1));
-        // m_driverController.povRight().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE3));
-        // m_driverController.povUp().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE2));
-
-        //m_driverController.back().whileTrue(new AutoShootCommandChassisTurretPathPlanner(driveSubsystem, hopperSubsystem, turretSubsystem));
-        m_driverController.leftBumper().onTrue(Commands.runOnce(() -> turretSubsystem.homeTurret(), turretSubsystem));
-        m_driverController.rightBumper().onTrue(Commands.parallel(intakeSubsystem.intake(m_driverController.getHID())));
-        m_driverController.start().whileTrue(intakeSubsystem.resetDeploy());
-        m_driverController.a().whileTrue((new AutoTurretShootCommand(hopperSubsystem, turretSubsystem, intakeSubsystem, driveSubsystem)));
-        m_driverController.x().whileTrue((new AutoTurretShootCommand(hopperSubsystem, turretSubsystem, intakeSubsystem, driveSubsystem)));
-        m_driverController.b().whileTrue(new Reverse(intakeSubsystem, hopperSubsystem, m_driverController.getHID()));
-        m_driverController.y().onTrue(intakeSubsystem.deploy());
-    }
-
-    /**
-     * Use this to pass the autonomous command to the main {@link Robot} class.
-     *
-     * @return the command to run in autonomous
-     */
-    public Command getAutonomousCommand() {
-        // An example command will be run in autonomous
-        return Commands.parallel(intakeSubsystem.deploy(), Commands.run(() -> {
-            turretSubsystem.homeTurret();
-        }, turretSubsystem), driveSubsystem.getAutoCommand());
-    }
-}

src/main/java/frc/robot/Utils.java

@@ -1,173 +0,0 @@
-package frc.robot;
-
-import edu.wpi.first.wpilibj.DriverStation.Alliance;
-import frc.robot.Constants;
-import frc.robot.Constants.ShootingRegion;
-import frc.robot.Constants.ShootingRegionDimensions;
-
-import org.littletonrobotics.junction.Logger;
-
-import edu.wpi.first.math.geometry.Pose3d;
-import edu.wpi.first.units.measure.Angle;
-
-public class Utils {
-    
-    /**
-     * Calculates the needed angle of the turret for a distance from the HUB.
-     * @param distanceToTag
-     * @return number of rotations of the turret, robot relative
-     */
-    public static double calculateTurretAngleFromCameraTagDistance(double distanceToTag){
-
-        // Calculate turret Angle
-        double cameraDistanceToTarget = distanceToTag + .6;
-        double cameraToShootDistance = 0.26035;
-        // a^2 + b^2 = c^2
-        double shooterDistanceToTarget = Math
-                .sqrt(Math.pow(cameraDistanceToTarget, 2) + Math.pow(cameraToShootDistance, 2));
-        double angleAtShooter = Math.acos(cameraToShootDistance / shooterDistanceToTarget);
-        double robotTurretAngle = (Math.PI / 2) - angleAtShooter;
-        double robotTurretRotations = (robotTurretAngle / Math.PI);
-
-        return robotTurretRotations;
-    }
-
-
-
-    /**
-     * Determines the region for the robot based on its location and alliance.
-     * Assumes the robot is a 27" square. Thresholds based on observations from simulator.
-     * @param robotPose: the robot's pose
-     * @param alliance: the red/blue alliance of the robot, as a sanity check
-     * @return the robot's shooting region
-     */
-    public static ShootingRegion findRobotShootingRegion(Pose3d robotPose, Alliance alliance){
-        ShootingRegion region = Constants.ShootingRegion.NON_SHOOTING_REGION;   // the default return value if robot isn't in a valid region
-        double robotX = robotPose.getX();
-        double robotY = robotPose.getY();
-
-        // Receommended: Add a sanity check for invalid robot poses.
-
-        if (alliance == Alliance.Blue) {
-            if (robotX < Constants.ShootingRegionDimensions.BLUE_ALLIANCE_ZONE_REGION_X_MAX) {
-                region = Constants.ShootingRegion.BLUE_OWN_ALLIANCE_ZONE;
-                }
-            else if (robotX > Constants.ShootingRegionDimensions.NEUTRAL_ZONE_REGION_X_MIN &&
-                     robotX < Constants.ShootingRegionDimensions.NEUTRAL_ZONE_REGION_X_MAX &&
-                     robotY > Constants.ShootingRegionDimensions.BLUE_LEFT_REGION_Y_MIN) {
-                        region = Constants.ShootingRegion.BLUE_LEFT_NEUTRAL_ZONE;
-                     }
-            else if (robotX > Constants.ShootingRegionDimensions.NEUTRAL_ZONE_REGION_X_MIN &&
-                     robotX < Constants.ShootingRegionDimensions.NEUTRAL_ZONE_REGION_X_MAX &&
-                     robotY < Constants.ShootingRegionDimensions.BLUE_RIGHT_REGION_Y_MAX) {
-                        region = Constants.ShootingRegion.BLUE_RIGHT_NEUTRAL_ZONE;                
-                     }
-            else if (robotX > Constants.ShootingRegionDimensions.RED_ALLIANCE_ZONE_REGION_X_MIN &&
-                     robotY > Constants.ShootingRegionDimensions.BLUE_LEFT_REGION_Y_MIN) {
-                        region = Constants.ShootingRegion.BLUE_LEFT_OPPONENT_ALLIANCE_ZONE;
-                     }
-            else if (robotX > Constants.ShootingRegionDimensions.RED_ALLIANCE_ZONE_REGION_X_MIN &&
-                     robotY < Constants.ShootingRegionDimensions.BLUE_RIGHT_REGION_Y_MAX) {
-                        region = Constants.ShootingRegion.BLUE_RIGHT_OPPONENT_ALLIANCE_ZONE;
-                     }
-        }
-        else if (alliance == Alliance.Red) {
-            if (robotX > Constants.ShootingRegionDimensions.RED_ALLIANCE_ZONE_REGION_X_MIN) {
-                region = Constants.ShootingRegion.RED_OWN_ALLIANCE_ZONE;
-                }
-            else if (robotX > Constants.ShootingRegionDimensions.NEUTRAL_ZONE_REGION_X_MIN &&
-                     robotX < Constants.ShootingRegionDimensions.NEUTRAL_ZONE_REGION_X_MAX &&
-                     robotY < Constants.ShootingRegionDimensions.RED_LEFT_REGION_Y_MAX) {
-                        region = Constants.ShootingRegion.RED_LEFT_NEUTRAL_ZONE;
-                     }
-            else if (robotX > Constants.ShootingRegionDimensions.NEUTRAL_ZONE_REGION_X_MIN &&
-                     robotX < Constants.ShootingRegionDimensions.NEUTRAL_ZONE_REGION_X_MAX &&
-                     robotY > Constants.ShootingRegionDimensions.RED_RIGHT_REGION_Y_MIN) {
-                        region = Constants.ShootingRegion.RED_RIGHT_NEUTRAL_ZONE;
-                     }
-            else if (robotX < Constants.ShootingRegionDimensions.BLUE_ALLIANCE_ZONE_REGION_X_MAX &&
-                     robotY < Constants.ShootingRegionDimensions.RED_LEFT_REGION_Y_MAX) {
-                        region = Constants.ShootingRegion.RED_LEFT_OPPONENT_ALLIANCE_ZONE;
-                     }
-            else if (robotX > Constants.ShootingRegionDimensions.RED_ALLIANCE_ZONE_REGION_X_MIN &&
-                     robotY > Constants.ShootingRegionDimensions.RED_RIGHT_REGION_Y_MIN) {
-                        region = Constants.ShootingRegion.RED_RIGHT_OPPONENT_ALLIANCE_ZONE;
-                     }
-        }
-
-        Logger.recordOutput("Utils-ShootingRegion", region);
-
-        return region;
-    }
-
-    public static double getHoodAngleForDistance(double distanceInMeters){
-        if(distanceInMeters >= 4){
-            return 200;
-        }
-        return 0;
-    }
-
-    public static double getLauncherRPMForDistance(double distanceInMeters){ 
-        return Math.min((667.557*distanceInMeters) + 1500.699, 5000);
-    }
-
-    /**
-     * Determines the appropriate shooting target for the robot based on its alliance
-     * and its current region on the field. There are 10 possible targets:
-     * Scoring hub, 2x passing from Neutral Zone, 2x passing from opponent's Alliance Zone,
-     * and these exist for both red and blue alliances.
-     * Right/left are from driver's perspective.
-     * @param region: the identifier of the robot's current field region
-     * @param alliance: the red/blue alliance of the robot, as a sanity check
-     * @return the shooting target for a robot with the provided properties. Returns all-zero
-     * Pose3d object if the region/alliance combination is invalid.
-     */
-    public static Pose3d findShootingTarget(ShootingRegion region, Alliance alliance){
-
-        Pose3d shootingTarget = new Pose3d();
-        if (alliance == Alliance.Blue) {
-            switch (region) {
-                case BLUE_OWN_ALLIANCE_ZONE:
-                    shootingTarget = Constants.FieldPoses.BLUE_HUB_POSE;
-                    break;
-                case BLUE_LEFT_NEUTRAL_ZONE:
-                    shootingTarget = Constants.FieldPoses.BLUE_LEFT_ALLIANCE_ZONE_PASS_TARGET_POSE;
-                    break;
-                case BLUE_RIGHT_NEUTRAL_ZONE:
-                    shootingTarget = Constants.FieldPoses.BLUE_RIGHT_ALLIANCE_ZONE_PASS_TARGET_POSE;
-                    break;
-                case BLUE_LEFT_OPPONENT_ALLIANCE_ZONE:
-                    shootingTarget = Constants.FieldPoses.BLUE_LEFT_NEUTRAL_ZONE_PASS_TARGET_POSE;
-                    break;
-                case BLUE_RIGHT_OPPONENT_ALLIANCE_ZONE:
-                    shootingTarget = Constants.FieldPoses.BLUE_RIGHT_NEUTRAL_ZONE_PASS_TARGET_POSE;
-                    break;
-                default:
-                    break;
-            }
-        }
-        else if (alliance == Alliance.Red) {
-            switch (region) {
-                case RED_OWN_ALLIANCE_ZONE:
-                    shootingTarget = Constants.FieldPoses.RED_HUB_POSE;
-                    break;
-                case RED_LEFT_NEUTRAL_ZONE:
-                    shootingTarget = Constants.FieldPoses.RED_LEFT_ALLIANCE_ZONE_PASS_TARGET_POSE;
-                    break;
-                case RED_RIGHT_NEUTRAL_ZONE:
-                    shootingTarget = Constants.FieldPoses.RED_RIGHT_ALLIANCE_ZONE_PASS_TARGET_POSE;
-                    break;
-                case RED_LEFT_OPPONENT_ALLIANCE_ZONE:
-                    shootingTarget = Constants.FieldPoses.RED_LEFT_NEUTRAL_ZONE_PASS_TARGET_POSE;
-                    break;
-                case RED_RIGHT_OPPONENT_ALLIANCE_ZONE:
-                    shootingTarget = Constants.FieldPoses.RED_RIGHT_NEUTRAL_ZONE_PASS_TARGET_POSE;
-                    break;
-                default:
-                    break;
-            }
-        }
-        return shootingTarget;
-    }
-
-}

src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java

@@ -1,154 +0,0 @@
-package frc.robot.commands;
-
-import java.util.Arrays;
-import java.util.List;
-
-import org.fairportrobotics.frc.robolib.vision.limelight.LimelightHelpers;
-import org.littletonrobotics.junction.Logger;
-
-import edu.wpi.first.apriltag.AprilTagFieldLayout;
-import edu.wpi.first.apriltag.AprilTagFields;
-import edu.wpi.first.math.controller.PIDController;
-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.geometry.Rotation3d;
-import edu.wpi.first.math.geometry.Transform2d;
-import edu.wpi.first.math.geometry.Transform3d;
-import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.math.geometry.Translation3d;
-import edu.wpi.first.units.Units;
-import edu.wpi.first.units.measure.Angle;
-import edu.wpi.first.wpilibj.DriverStation;
-import edu.wpi.first.wpilibj.DriverStation.Alliance;
-import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.Constants;
-import frc.robot.Utils;
-import frc.robot.subsystems.DriveSubsystem;
-import frc.robot.subsystems.HopperSubsystem;
-import frc.robot.subsystems.TurretSubsystem;
-
-public class AutoShootCommandChassisTurret extends Command{
-    
-    // For Chassis Control
-    private DriveSubsystem driveSubsystem;
-    
-    private HopperSubsystem hopperSubsystem;
-    private TurretSubsystem turretSubsystem;
-
-    private PIDController cameraAutoCenterController;
-    private PIDController deadreckoningAutoCenterController;
-
-    private int[] tagFilters;
-
-    private AprilTagFieldLayout fieldTags = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltWelded);
-
-    private Pose3d closestTag;
-
-    private final double chassisRotateSpeed = Math.PI * 0.1;
-
-    public AutoShootCommandChassisTurret(DriveSubsystem driveSubsystem, HopperSubsystem hopperSubsystem, TurretSubsystem turretSubsystem)
-    {
-        this.driveSubsystem = driveSubsystem;
-        this.hopperSubsystem = hopperSubsystem;
-        this.turretSubsystem = turretSubsystem;
-
-        this.addRequirements(driveSubsystem, hopperSubsystem, turretSubsystem);
-
-        cameraAutoCenterController = new PIDController(0.15, 0.0, 0.0);
-        cameraAutoCenterController.setTolerance(0.2); // Units degrees
-        cameraAutoCenterController.setSetpoint(0);
-
-
-        deadreckoningAutoCenterController = new PIDController(Math.PI * 0.1, 0, 0.0);
-        deadreckoningAutoCenterController.setTolerance(0.2); // Units is rotations
-        deadreckoningAutoCenterController.enableContinuousInput(-Math.PI, Math.PI);
-    }
-
-    @Override
-    public void initialize() {
-        DriverStation.getAlliance().ifPresent((aliance) -> {
-            if (aliance == Alliance.Blue) {
-                tagFilters = Constants.CameraConstanst.IDFilters.BLUE_HUB_SHOOTING_IDS;
-            } else {
-                tagFilters = Constants.CameraConstanst.IDFilters.RED_HUB_SHOOTING_IDS;
-            }
-        });
-
-        turretSubsystem.setLauncher(3000);
-        turretSubsystem.setTargetElevation(Constants.ShooterConstants.DEFLECTOR_STORED_ANGLE);
-        hopperSubsystem.spindexerOn();
-        cameraAutoCenterController.calculate(-100);
-
-        // Change to HUB tracking pipeline
-        LimelightHelpers.setPipelineIndex(Constants.CameraConstanst.BACK_CAMERA_NAME, Constants.CameraConstanst.BACK_HUB_TRACKING_3D_PIPELINE_NUMBER);
-        LimelightHelpers.SetFiducialIDFiltersOverride(Constants.CameraConstanst.BACK_CAMERA_NAME, tagFilters);
-
-        Pose3d botPose = driveSubsystem.getBotPose();
-        // Find closest target to us
-        List<Pose3d> tagPoses = Arrays.stream(tagFilters).mapToObj(tagId -> fieldTags.getTagPose(tagId).get()).toList();
-        closestTag = botPose.nearest(tagPoses);
-        Logger.recordOutput("AutoAlign-ClosestAprilTagPose", closestTag);
-
-        Translation2d delta = botPose.getTranslation().toTranslation2d().minus(closestTag.getTranslation().toTranslation2d());
-        Logger.recordOutput("AutoAlign-ClosestAprilTagTransform", delta);
-
-        deadreckoningAutoCenterController.setSetpoint(delta.getAngle().plus(Rotation2d.kZero).getRadians());
-    }
-
-    @Override
-    public void execute() {
-        // Need to compute shoot speed and angle
-
-        Logger.recordOutput("AutoAlign-CenteringError", cameraAutoCenterController.getError());
-
-        // No april tag in camera view. Rotate towards our theoretical closest tag
-        if(!LimelightHelpers.getTV(Constants.CameraConstanst.BACK_CAMERA_NAME)){ 
-            Logger.recordOutput("AutoAlignState-Tracking", "Deadreckoning");
-
-            Pose3d botPose = driveSubsystem.getBotPose();
-            //driveSubsystem.rotateChassis(-deadreckoningAutoCenterController.calculate(botPose.getRotation().toRotation2d().getRadians()));
-        }
-        else // We have an april tag, center it to the camera frame
-        {
-            Logger.recordOutput("AutoAlignState-Tracking", "Camera");
-            // [tx, ty, tz, pitch, yaw, roll]
-            double[] targetsPose = LimelightHelpers.getTargetPose_CameraSpace(Constants.CameraConstanst.BACK_CAMERA_NAME);
-            if(targetsPose.length >= 1){
-                Logger.recordOutput("AutoAlign-Target Distance", targetsPose[2]);
-
-                // Calculate turret Angle
-                double turretAngle = Utils.calculateTurretAngleFromCameraTagDistance(targetsPose[2]) * Constants.ShooterConstants.TURRET_GEAR_RATIO;
-                turretSubsystem.setTurretMotorRotation(-turretAngle);
-
-                driveSubsystem.rotateChassis(-cameraAutoCenterController.calculate(targetsPose[0])); // This may need to be negated
-            }
-        }
-
-        // If robot is centered on HUB and shooter is up to speed. Run the kicker
-        if(turretSubsystem.isLauncherUpToSpeed() && cameraAutoCenterController.atSetpoint()){
-            Logger.recordOutput("AutoAlignState-Shoot", "FIRE!");
-            hopperSubsystem.feedKicker();
-        }else{
-            Logger.recordOutput("AutoAlignState-Shoot", "Waiting");
-        }
-    }
-
-    @Override
-    public boolean isFinished() {
-        return false;
-    }
-
-    @Override
-    public void end(boolean interrupted) {
-        turretSubsystem.setLauncher(0);
-
-        hopperSubsystem.stopKicker();
-        hopperSubsystem.spindexerOff();
-
-        driveSubsystem.stopDrive();
-
-        LimelightHelpers.setPipelineIndex(Constants.CameraConstanst.BACK_CAMERA_NAME, Constants.CameraConstanst.BACK_LOCALIZATION_PIPELINE_NUMBER);
-    }
-
-}

src/main/java/frc/robot/commands/AutoShootCommandChassisTurretPathPlanner.java

@@ -1,130 +0,0 @@
-package frc.robot.commands;
-
-import java.util.Arrays;
-import java.util.List;
-
-import org.fairportrobotics.frc.robolib.vision.limelight.LimelightHelpers;
-import org.littletonrobotics.junction.Logger;
-
-import com.pathplanner.lib.auto.AutoBuilder;
-import com.pathplanner.lib.path.PathConstraints;
-import edu.wpi.first.apriltag.AprilTagFieldLayout;
-import edu.wpi.first.apriltag.AprilTagFields;
-import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Pose3d;
-import edu.wpi.first.math.geometry.Translation2d;
-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.CommandScheduler;
-import frc.robot.Constants;
-import frc.robot.subsystems.DriveSubsystem;
-import frc.robot.subsystems.HopperSubsystem;
-import frc.robot.subsystems.TurretSubsystem;
-
-public class AutoShootCommandChassisTurretPathPlanner extends Command {
-
-    // For Chassis Control
-    private DriveSubsystem driveSubsystem;
-
-    private HopperSubsystem hopperSubsystem;
-    private TurretSubsystem turretSubsystem;
-
-    private int[] tagFilters;
-
-    private AprilTagFieldLayout fieldTags = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltWelded);
-
-    private Pose3d closestTag;
-    private Pose2d targetPPPose;
-
-    private Command pathFollowCommand;
-
-    public AutoShootCommandChassisTurretPathPlanner(DriveSubsystem driveSubsystem, HopperSubsystem hopperSubsystem,
-            TurretSubsystem turretSubsystem) {
-        this.driveSubsystem = driveSubsystem;
-        this.hopperSubsystem = hopperSubsystem;
-        this.turretSubsystem = turretSubsystem;
-
-        this.addRequirements(driveSubsystem, hopperSubsystem, turretSubsystem);
-
-    }
-
-    @Override
-    public void initialize() {
-
-        DriverStation.getAlliance().ifPresent((aliance) -> {
-            Pose3d targetHub;
-            if (aliance == Alliance.Blue) {
-                tagFilters = Constants.CameraConstanst.IDFilters.BLUE_HUB_SHOOTING_IDS;
-                targetHub = Constants.FieldPoses.BLUE_HUB_POSE;
-            } else {
-                tagFilters = Constants.CameraConstanst.IDFilters.RED_HUB_SHOOTING_IDS;
-                targetHub = Constants.FieldPoses.RED_HUB_POSE;
-            }
-
-            Pose3d botPose = driveSubsystem.getBotPose();
-            // Find closest target to us
-            List<Pose3d> tagPoses = Arrays.stream(tagFilters).mapToObj(tagId -> fieldTags.getTagPose(tagId).get())
-                    .toList();
-            closestTag = botPose.nearest(tagPoses);
-            Logger.recordOutput("AutoAlignPP-ClosestAprilTagPose", closestTag);
-
-            Translation2d delta = botPose.getTranslation().toTranslation2d()
-                    .minus(closestTag.getTranslation().toTranslation2d());
-            Logger.recordOutput("AutoAlignPP-ClosestAprilTagTransform", delta);
-
-            // Add 180 because we want the back of the robot to face the HUB
-            // deadreckoningAutoCenterController.setSetpoint(delta.getAngle().plus(Rotation2d.k180deg).getRadians());
-
-            // Translate back 1 meter from HUB
-            Translation2d newTrans = targetHub.getTranslation().toTranslation2d()
-                    .plus(new Translation2d(1.5, delta.getAngle()));
-            targetPPPose = new Pose2d(newTrans, delta.getAngle());
-            Logger.recordOutput("AutoAlignPP-PPTargetPose", targetPPPose);
-
-            pathFollowCommand = AutoBuilder.pathfindToPose(targetPPPose,
-                    new PathConstraints(Constants.RobotChassisLimits.MAX_ROBOT_LINEAR_VELOCITY,
-                            Constants.RobotChassisLimits.MAX_ROBOT_LINEAR_VELOCITY_ACCEL,
-                            Constants.RobotChassisLimits.MAX_ROBOT_ANGULAR_VELOCITY,
-                            Constants.RobotChassisLimits.MAX_ROBOT_LINEAR_VELOCITY_ACCEL));
-
-            CommandScheduler.getInstance().schedule(pathFollowCommand);
-        });
-
-        turretSubsystem.setLauncher(5500);
-        hopperSubsystem.spindexerOn();
-
-    }
-
-    @Override
-    public void execute() {
-
-        // If robot is centered on HUB and shooter is up to speed. Run the kicker
-        if (turretSubsystem.isLauncherUpToSpeed() && pathFollowCommand.isFinished()) {
-            Logger.recordOutput("AutoAlignState-Shoot", "FIRE!");
-            hopperSubsystem.feedKicker();
-        } else {
-            Logger.recordOutput("AutoAlignState-Shoot", "Waiting");
-        }
-    }
-
-    @Override
-    public boolean isFinished() {
-        return false;
-    }
-
-    @Override
-    public void end(boolean interrupted) {
-        CommandScheduler.getInstance().cancel(pathFollowCommand);
-        turretSubsystem.setLauncher(0);
-
-        hopperSubsystem.stopKicker();
-        hopperSubsystem.spindexerOff();
-
-        driveSubsystem.stopDrive();
-
-        LimelightHelpers.setPipelineIndex(Constants.CameraConstanst.BACK_CAMERA_NAME,
-                Constants.CameraConstanst.BACK_LOCALIZATION_PIPELINE_NUMBER);
-    }
-
-}

src/main/java/frc/robot/commands/AutoTurretCommand.java

@@ -1,96 +0,0 @@
-package frc.robot.commands;
-
-import org.littletonrobotics.junction.Logger;
-
-import edu.wpi.first.apriltag.AprilTagFieldLayout;
-import edu.wpi.first.apriltag.AprilTagFields;
-import edu.wpi.first.math.geometry.Pose3d;
-import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Rotation3d;
-import edu.wpi.first.math.geometry.Transform2d;
-import edu.wpi.first.math.geometry.Transform3d;
-import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.math.geometry.Translation3d;
-import edu.wpi.first.units.Units;
-import edu.wpi.first.units.measure.Angle;
-import edu.wpi.first.wpilibj.DriverStation;
-import edu.wpi.first.wpilibj.DriverStation.Alliance;
-import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.Constants;
-import frc.robot.Constants.ShootingRegion;
-import frc.robot.Utils;
-import frc.robot.subsystems.DriveSubsystem;
-import frc.robot.subsystems.TurretSubsystem;
-
-public class AutoTurretCommand extends Command {
-
-    private final Translation2d CENTER_TURRET_TO_ROBOT = new Translation2d(-0.1335024, -0.1824736);
-
-    private TurretSubsystem turretSubsystem;
-    private DriveSubsystem driveSubsystem;
-
-    private Alliance alliance;
-
-    public AutoTurretCommand(TurretSubsystem turretSubsystem, DriveSubsystem driveSubsystem) {
-        this.turretSubsystem = turretSubsystem;
-        this.driveSubsystem = driveSubsystem;
-
-        addRequirements(turretSubsystem); // Not adding drive as we don't wan't the drive command to be de-scheduled
-    }
-
-    @Override
-    public void initialize() {
-        // Perform auto-home
-        turretSubsystem.homeTurret();
-
-        DriverStation.getAlliance().ifPresent((aliance) -> {
-            if (aliance == Alliance.Blue) {
-                turretSubsystem.setTurretTargetPose(Constants.FieldPoses.BLUE_HUB_POSE);
-                aliance = Alliance.Blue;
-            } else {
-                turretSubsystem.setTurretTargetPose(Constants.FieldPoses.RED_HUB_POSE);
-                aliance = Alliance.Red;
-            }
-        });
-
-    }
-
-    @Override
-    public void execute() {
-
-        ShootingRegion shootingRegion = Utils.findRobotShootingRegion(driveSubsystem.getBotPose(), DriverStation.getAlliance().get());
-
-        if(shootingRegion != ShootingRegion.NON_SHOOTING_REGION){
-            Pose3d targetPose = Utils.findShootingTarget(shootingRegion, DriverStation.getAlliance().get());
-
-            turretSubsystem.setTurretTargetPose(targetPose);
-        }
-
-        Logger.recordOutput("AutoTurret-ScoringTarget", turretSubsystem.getTurretTargetPose());
-
-        Pose3d botPose = driveSubsystem.getBotPose();
-        Pose3d turretPose = botPose.plus(new Transform3d(new Translation3d(CENTER_TURRET_TO_ROBOT),
-                new Rotation3d(Rotation2d.fromRotations(turretSubsystem.getTurretAngleRobotRelative() + 0.5))));
-        Logger.recordOutput("AutoTurret-TurretPose", turretPose);
-
-        Translation2d targetTranslation = turretSubsystem.getTurretTargetPose().getTranslation().toTranslation2d().minus(turretPose.getTranslation().toTranslation2d());
-        Logger.recordOutput("AutoTurret-Translation", targetTranslation);
-
-        if(turretSubsystem.isTurretReady()){
-            turretSubsystem.setTurretFieldRelative(botPose, targetTranslation.getAngle().getRotations());
-            // turretSubsystem.setTurretRobotRelative(0.25);
-            // turretSubsystem.setTurretMotorRotation(0);
-        }
-
-    }
-
-    @Override
-    public boolean isFinished() {
-        return false;
-    }
-
-    @Override
-    public void end(boolean interrupted) {
-    }
-
-}

src/main/java/frc/robot/commands/AutoTurretShootCommand.java

@@ -1,90 +0,0 @@
-package frc.robot.commands;
-
-import org.littletonrobotics.junction.Logger;
-
-import edu.wpi.first.math.geometry.Pose3d;
-import edu.wpi.first.units.Units;
-import edu.wpi.first.units.measure.Angle;
-import edu.wpi.first.wpilibj.DriverStation;
-import edu.wpi.first.wpilibj.DriverStation.Alliance;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.CommandScheduler;
-import edu.wpi.first.wpilibj2.command.Commands;
-import frc.robot.Constants;
-import frc.robot.Utils;
-import frc.robot.subsystems.DriveSubsystem;
-import frc.robot.subsystems.HopperSubsystem;
-import frc.robot.subsystems.IntakeSubsystem;
-import frc.robot.subsystems.TurretSubsystem;
-
-public class AutoTurretShootCommand extends Command{
-
-    private HopperSubsystem hopperSubsystem;
-    private TurretSubsystem turretSubsystem;
-    private IntakeSubsystem intakeSubsystem;
-    private DriveSubsystem driveSubsystem;
-
-    private Command waitCommand = Commands.waitSeconds(1.5);
-
-    private Alliance alliance;
-    public AutoTurretShootCommand(HopperSubsystem hopperSubsystem, TurretSubsystem turretSubsystem, IntakeSubsystem intakeSubsystem, DriveSubsystem driveSubsystem){
-        this.hopperSubsystem = hopperSubsystem;
-        this.turretSubsystem = turretSubsystem;
-        this.intakeSubsystem = intakeSubsystem;
-        this.driveSubsystem = driveSubsystem;
-
-        addRequirements(hopperSubsystem, intakeSubsystem);
-    }
-    
-    @Override
-    public void initialize() {
-        hopperSubsystem.spindexerOn();
-        CommandScheduler.getInstance().schedule(waitCommand);
-        alliance = DriverStation.getAlliance().get();
-        Logger.recordOutput("AutoTurretShoot-State", "SHOOTING");
-    }
-
-    @Override
-    public void execute() {
-        double distance = turretSubsystem.getTurretTargetPose().toPose2d().getTranslation().getDistance(driveSubsystem.getBotPose().getTranslation().toTranslation2d());
-        Logger.recordOutput("AutoTurretShoot-DistanceToTarget(Meters)", distance);
-
-        turretSubsystem.setTargetElevation(Utils.getHoodAngleForDistance(distance));
-        turretSubsystem.setLauncher(Utils.getLauncherRPMForDistance(distance));
-
-        if(turretSubsystem.isLauncherUpToSpeed() && waitCommand.isFinished()){
-            hopperSubsystem.feedKicker();
-        }else{
-            hopperSubsystem.stopKicker();
-        }
-    }
-
-    @Override
-    public boolean isFinished() {
-        return false;
-    }
-
-    @Override
-    public void end(boolean interrupted) {
-        waitCommand.cancel();
-        hopperSubsystem.spindexerOff();
-        hopperSubsystem.stopKicker();
-
-        turretSubsystem.setLauncher(0);
-        turretSubsystem.setTargetElevation(Constants.ShooterConstants.DEFLECTOR_STORED_ANGLE);
-
-        Logger.recordOutput("AutoTurretShoot-State", "NOT SHOOTING");
-
-        DriverStation.getAlliance().ifPresent((aliance) -> {
-            if (aliance == Alliance.Blue) {
-                turretSubsystem.setTurretTargetPose(Constants.FieldPoses.BLUE_HUB_POSE);
-            } else {
-                turretSubsystem.setTurretTargetPose(Constants.FieldPoses.RED_HUB_POSE);
-            }
-        });
-
-        // turretSubsystem.setTurretMotorRotation(-0.34); // Return to 0 after MIAMI VALLEY
-    }
-
-}

src/main/java/frc/robot/commands/Autos.java

@@ -1,12 +0,0 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
-package frc.robot.commands;
-
-public final class Autos {
-
-  private Autos() {
-    throw new UnsupportedOperationException("This is a utility class!");
-  }
-}

src/main/java/frc/robot/commands/ExampleCommand.java

@@ -1,43 +0,0 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
-package frc.robot.commands;
-
-import frc.robot.subsystems.ExampleSubsystem;
-import edu.wpi.first.wpilibj2.command.Command;
-
-/** An example command that uses an example subsystem. */
-public class ExampleCommand extends Command {
-  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
-  private final ExampleSubsystem m_subsystem;
-
-  /**
-   * Creates a new ExampleCommand.
-   *
-   * @param subsystem The subsystem used by this command.
-   */
-  public ExampleCommand(ExampleSubsystem subsystem) {
-    m_subsystem = subsystem;
-    // Use addRequirements() here to declare subsystem dependencies.
-    addRequirements(subsystem);
-  }
-
-  // Called when the command is initially scheduled.
-  @Override
-  public void initialize() {}
-
-  // Called every time the scheduler runs while the command is scheduled.
-  @Override
-  public void execute() {}
-
-  // Called once the command ends or is interrupted.
-  @Override
-  public void end(boolean interrupted) {}
-
-  // Returns true when the command should end.
-  @Override
-  public boolean isFinished() {
-    return false;
-  }
-}

src/main/java/frc/robot/commands/Reverse.java

@@ -1,57 +0,0 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-package frc.robot.commands;
-
-import edu.wpi.first.wpilibj.XboxController;
-import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.subsystems.HopperSubsystem;
-import frc.robot.subsystems.IntakeSubsystem;
-
-/**
- * An example command that uses an example subsystem.
- */
-public class Reverse extends Command {
-
-    private IntakeSubsystem intakeSubsystem;
-    private HopperSubsystem hopperSubsystem;
-    private XboxController m_XboxController;
-
-    /**
-     * Creates a new ExampleCommand.
-     *
-     * @param subsystem The subsystem used by this command.
-     */
-    public Reverse(IntakeSubsystem intakeSubsystem, HopperSubsystem hopperSubsystem, XboxController m_XboxController) {
-        this.intakeSubsystem = intakeSubsystem;
-        this.hopperSubsystem = hopperSubsystem;
-        this.m_XboxController = m_XboxController;
-    }
-
-    // Called when the command is initially scheduled.
-    @Override
-    public void initialize() {
-    }
-
-    // Called every time the scheduler runs while the command is scheduled.
-    @Override
-    public void execute() {
-        intakeSubsystem.reverseIntake();
-        hopperSubsystem.spindexerReverse();
-        hopperSubsystem.reverseKicker();
-    }
-
-    // Called once the command ends or is interrupted.
-    @Override
-    public void end(boolean interrupted) {
-        hopperSubsystem.stopKicker();
-        hopperSubsystem.spindexerOff();
-        intakeSubsystem.stopIntake(m_XboxController);
-    }
-
-    // Returns true when the command should end.
-    @Override
-    public boolean isFinished() {
-        return false;
-    }
-}

src/main/java/frc/robot/commands/SetDeflectorCommand.java

@@ -1,31 +0,0 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
-package frc.robot.commands;
-
-import frc.robot.subsystems.TurretSubsystem;
-import edu.wpi.first.wpilibj2.command.Command;
-
-/** An example command that uses an example subsystem. **/
-public class SetDeflectorCommand extends Command {
-  private final TurretSubsystem m_subsystem;
-  private double deployAngle;
-
-  public SetDeflectorCommand(TurretSubsystem subsystem, double deployAngle) {
-    m_subsystem = subsystem;
-    this.deployAngle = deployAngle;
-  }
-
-  // Called when the command is initially scheduled.
-  @Override
-  public void initialize() {
-    m_subsystem.setTargetElevation(deployAngle);
-  }
-  
-  // Returns true when the command should end.
-  @Override
-  public boolean isFinished() {
-    return true;
-  }
-}

src/main/java/frc/robot/commands/ZoneCheck.java

@@ -1,86 +0,0 @@
-package frc.robot.commands;
-
-import org.fairportrobotics.frc.robolib.vision.limelight.LimelightHelpers;
-import org.littletonrobotics.junction.Logger;
-
-import edu.wpi.first.apriltag.AprilTagFieldLayout;
-import edu.wpi.first.apriltag.AprilTagFields;
-import edu.wpi.first.wpilibj.DriverStation;
-import edu.wpi.first.wpilibj.DriverStation.Alliance;
-import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.Constants;
-import frc.robot.subsystems.DriveSubsystem;
-
-public class ZoneCheck extends Command {
-    private Zone zone;
-    private DriveSubsystem driveSubsystem;
-
-    public ZoneCheck(DriveSubsystem driveSubsystem)
-    {
-        this.driveSubsystem = driveSubsystem;
-    }
-
-    public enum Zone {
-        Blue,
-        Red,
-        Neutral,
-        INIT;
-    }
-
-    @Override
-    public void initialize() {
-        zone = Zone.INIT;
-    }
-
-    @Override
-    public void execute() {
-        getZone();
-        Logger.recordOutput("FieldZone", zone);
-        DriverStation.getAlliance().ifPresent((aliance) -> {
-            if (aliance == Alliance.Blue) {
-                switch (zone) {
-                    case Blue:
-                        System.out.println("BLUE");
-                            break;
-                    case Neutral:
-                        System.out.println("NEUTRAL");
-                            break;
-                    case Red:
-                        System.out.println("RED");
-                            break;
-                    default:
-                        System.out.println("****");
-                }
-            } else {
-                switch (zone) {
-                    case Blue:
-                        System.out.println("BLUE");
-                            break;
-                    case Neutral:
-                        System.out.println("NEUTRAL");
-                            break;
-                    case Red:
-                        System.out.println("RED");
-                            break;
-                    default:
-                        System.out.println("****");
-                }
-            }
-        });
-    }
-
-    @Override
-    public boolean isFinished() {
-        return false;
-    }
-
-    private void getZone() {
-        if (driveSubsystem.getBotPose().getX() >= Constants.CameraConstanst.RED_ZONE_COOR[0]) {
-            zone = Zone.Red;
-        } else if (driveSubsystem.getBotPose().getX() >= Constants.CameraConstanst.NEUTRAL_ZONE_COOR[0]) {
-            zone = Zone.Neutral;
-        } else {
-            zone = Zone.Blue;
-        }
-    }
-}

src/main/java/frc/robot/subsystems/DriveSubsystem.java

@@ -1,280 +0,0 @@
-package frc.robot.subsystems;
-
-import static org.fairportrobotics.frc.posty.assertions.Assertions.assertThat;
-
-import org.fairportrobotics.frc.posty.TestableSubsystem;
-import org.fairportrobotics.frc.posty.test.PostTest;
-import org.fairportrobotics.frc.robolib.drivesystems.swerve.SwerveBuilder;
-import org.fairportrobotics.frc.robolib.drivesystems.swerve.SwerveDriveSystem;
-import org.fairportrobotics.frc.robolib.drivesystems.swerve.SwerveModule;
-import org.fairportrobotics.frc.robolib.vision.limelight.LimelightHelpers;
-import org.littletonrobotics.junction.Logger;
-
-import com.pathplanner.lib.auto.AutoBuilder;
-import com.pathplanner.lib.config.PIDConstants;
-import com.pathplanner.lib.config.RobotConfig;
-import com.pathplanner.lib.controllers.PPHolonomicDriveController;
-import com.pathplanner.lib.util.PathPlannerLogging;
-
-import edu.wpi.first.math.VecBuilder;
-import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Pose3d;
-import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.math.kinematics.ChassisSpeeds;
-import edu.wpi.first.wpilibj.DriverStation;
-import edu.wpi.first.wpilibj.RobotState;
-import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.Commands;
-import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
-import frc.robot.Constants;
-
-public class DriveSubsystem extends TestableSubsystem {
-
-    private SendableChooser<Command> autoChooser;
-
-    private SwerveBuilder swerveBuilder = new SwerveBuilder();
-    public SwerveDriveSystem driveSystem;
-
-    private double driveP = 0.1;
-    private double driveI = 0.0;
-    private double driveD = 0.01;
-    private double driveKV = 0.2;
-
-    private double steerP = 20;
-    private double steerI = 0;
-    private double steerD = 0.0;
-    private double steerKV = 0;
-
-    public DriveSubsystem(CommandXboxController xboxController) {
-
-        driveSystem = swerveBuilder
-                .withCanbusName("Drive")
-                .withPigeonId(Constants.ExtraIDEntities.PIGEON_ID)
-                .withMaxLinearVelocity(Constants.RobotChassisLimits.MAX_ROBOT_LINEAR_VELOCITY)
-                .withMaxAngularVelocity(Constants.RobotChassisLimits.MAX_ROBOT_ANGULAR_VELOCITY)
-                .withSwerveModule(
-                        swerveBuilder.new SwerveModuleBuilder()
-                                .withDriveMotorId(Constants.SwerveDriveIDs.FRONT_LEFT_DRIVE_ID)
-                                .withDriveKP(driveP)
-                                .withDriveKI(driveI)
-                                .withDriveKD(driveD)
-                                .withDriveKV(driveKV)
-                                .withSteerMotorId(Constants.SwerveDriveIDs.FRONT_LEFT_STEER_ID)
-                                .withSteerKP(steerP)
-                                .withSteerKI(steerI)
-                                .withSteerKD(steerD)
-                                .withSteerKV(steerKV)
-                                // If the offsets don't work, contact Tyler W or Dom G
-                                .withSteerOffset(Constants.SwerveDriveOffsets.FRONT_LEFT_MOTOR_OFFSET)
-                                .withSteerEncoderId(Constants.SwerveDriveIDs.FRONT_LEFT_ENCODER_ID)
-                                .withModuleLocation(new Translation2d(1, 1))
-                                .withModuleName("Front Left")
-                                .withGearRatio(8.14)
-                                .withWheelDiameter(0.1016)
-                                .build())
-                .withSwerveModule(
-                        swerveBuilder.new SwerveModuleBuilder()
-                                .withDriveMotorId(Constants.SwerveDriveIDs.FRONT_RIGHT_DRIVE_ID)
-                                .withDriveKP(driveP)
-                                .withDriveKI(driveI)
-                                .withDriveKD(driveD)
-                                .withDriveKV(driveKV)
-                                .withDriveInverted()
-                                .withSteerMotorId(Constants.SwerveDriveIDs.FRONT_RIGHT_STEER_ID)
-                                .withSteerKP(steerP)
-                                .withSteerKI(steerI)
-                                .withSteerKD(steerD)
-                                .withSteerKV(steerKV)
-                                .withSteerOffset(Constants.SwerveDriveOffsets.FRONT_RIGHT_MOTOR_OFFSET)
-                                .withSteerEncoderId(Constants.SwerveDriveIDs.FRONT_RIGHT_ENCODER_ID)
-                                .withModuleLocation(new Translation2d(1, -1))
-                                .withModuleName("Front Right")
-                                .withGearRatio(8.14)
-                                .withWheelDiameter(0.1016)
-                                .build())
-                .withSwerveModule(
-                        swerveBuilder.new SwerveModuleBuilder()
-                                .withDriveMotorId(Constants.SwerveDriveIDs.BACK_LEFT_DRIVE_ID)
-                                .withDriveKP(driveP)
-                                .withDriveKI(driveI)
-                                .withDriveKD(driveD)
-                                .withDriveKV(driveKV)
-                                .withSteerMotorId(Constants.SwerveDriveIDs.BACK_LEFT_STEER_ID)
-                                .withSteerKP(steerP)
-                                .withSteerKI(steerI)
-                                .withSteerKD(steerD)
-                                .withSteerKV(steerKV)
-                                .withSteerOffset(Constants.SwerveDriveOffsets.BACK_LEFT_MOTOR_OFFSET)
-                                .withSteerEncoderId(Constants.SwerveDriveIDs.BACK_LEFT_ENCODER_ID)
-                                .withModuleLocation(new Translation2d(-1, 1))
-                                .withModuleName("Back Left")
-                                .withGearRatio(8.14)
-                                .withWheelDiameter(0.1016)
-                                .build())
-                .withSwerveModule(
-                        swerveBuilder.new SwerveModuleBuilder()
-                                .withDriveMotorId(Constants.SwerveDriveIDs.BACK_RIGHT_DRIVE_ID)
-                                .withDriveKP(driveP)
-                                .withDriveKI(driveI)
-                                .withDriveKD(driveD)
-                                .withDriveKV(driveKV)
-                                .withDriveInverted()
-                                .withSteerMotorId(Constants.SwerveDriveIDs.BACK_RIGHT_STEER_ID)
-                                .withSteerKP(steerP)
-                                .withSteerKI(steerI)
-                                .withSteerKD(steerD)
-                                .withSteerKV(steerKV)
-                                .withSteerOffset(Constants.SwerveDriveOffsets.BACK_RIGHT_MOTOR_OFFSET)
-                                .withSteerEncoderId(Constants.SwerveDriveIDs.BACK_RIGHT_ENCODER_ID)
-                                .withModuleLocation(new Translation2d(-1, -1))
-                                .withModuleName("Back Right")
-                                .withGearRatio(8.14)
-                                .withWheelDiameter(0.1016)
-                                .build())
-                .build();
-
-        // Set vision measurement confidence values
-        driveSystem.getPoseEstimator().setVisionMeasurementStdDevs(VecBuilder.fill(0.7, 0.7, 0.7, 0.1));
-
-        this.setDefaultCommand(Commands.run(new Runnable() {
-
-            @Override
-            public void run() {
-                driveSystem.setChassisSpeedsFromJoystickFieldRelative(
-                        -(xboxController.getLeftY() * Math.abs(xboxController.getLeftY())),
-                        -(xboxController.getLeftX() * Math.abs(xboxController.getLeftX())),
-                        -(xboxController.getRawAxis(4) * Math.abs(xboxController.getRawAxis(4))));
-
-                Logger.recordOutput("RequestedSwerveState", driveSystem.getRequestedModuleStates());
-                Logger.recordOutput("ActualSwerveState", driveSystem.getActualModuleStates());
-            }
-
-        }, this));
-
-        RobotConfig config;
-        try {
-            config = RobotConfig.fromGUISettings();
-
-            AutoBuilder.configure(
-                    driveSystem::getRobotPose2d,
-                    driveSystem::setPose2d,
-                    driveSystem::getRobotRelativeSpeeds,
-                    (speeds, feedforwards) -> {driveSystem.setChassisSpeed(ChassisSpeeds.fromRobotRelativeSpeeds(-speeds.vxMetersPerSecond, -speeds.vyMetersPerSecond, -speeds.omegaRadiansPerSecond, driveSystem.getRobotHeading()), new Translation2d());},
-                    new PPHolonomicDriveController(
-                            new PIDConstants(5, 0, 0),
-                            new PIDConstants(Math.PI, 0, 0)),
-                    config,
-                    () -> {
-                        var alliance = DriverStation.getAlliance();
-                        if (alliance.isPresent()) {
-                            return alliance.get() == DriverStation.Alliance.Red;
-                        }
-                        return false;
-                    },
-                    this);
-
-            autoChooser = AutoBuilder.buildAutoChooser();
-            autoChooser.setDefaultOption("Do Nothing", Commands.none());
-            SmartDashboard.putData("Auto Chooser", autoChooser);
-
-            // PathPlannerLogging.setLogActivePathCallback((List<Pose2d> pose) -> {});
-            PathPlannerLogging.setLogTargetPoseCallback((Pose2d targetPose) -> { Logger.recordOutput("PathPlanner-TargetPose", targetPose);});
-
-        } catch (Exception e) {
-            e.printStackTrace();
-        }
-
-        
-
-    }
-
-    @Override
-    public void periodic() {
-        super.periodic();
-        driveSystem.periodic();
-
-        Logger.recordOutput("RobotPose", driveSystem.getRobotPose3d());
-
-        LimelightHelpers.SetRobotOrientation(Constants.CameraConstanst.FRONT_CAMERA_NAME, driveSystem.getRobotPose2d().getRotation().getDegrees(), 0, 0, 0, 0, 0);
-        LimelightHelpers.SetRobotOrientation(Constants.CameraConstanst.BACK_CAMERA_NAME, driveSystem.getRobotPose2d().getRotation().getDegrees(), 0, 0, 0, 0, 0);
-
-        // MegaTag 2 impl
-        if(RobotState.isDisabled()){
-            LimelightHelpers.SetIMUMode(Constants.CameraConstanst.FRONT_CAMERA_NAME, 1);
-            LimelightHelpers.SetIMUMode(Constants.CameraConstanst.BACK_CAMERA_NAME, 1);
-        }else{
-            LimelightHelpers.SetIMUMode(Constants.CameraConstanst.FRONT_CAMERA_NAME, 4);
-            LimelightHelpers.SetIMUMode(Constants.CameraConstanst.BACK_CAMERA_NAME, 4);
-        }
-
-        LimelightHelpers.PoseEstimate frontTag = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(Constants.CameraConstanst.FRONT_CAMERA_NAME);
-        LimelightHelpers.PoseEstimate backTag = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(Constants.CameraConstanst.BACK_CAMERA_NAME);
-
-        if(frontTag.tagCount >= 2){
-            driveSystem.getPoseEstimator().addVisionMeasurement(new Pose3d(frontTag.pose), frontTag.timestampSeconds);
-            Logger.recordOutput("FrontPose-Estimate", frontTag.pose);
-            Logger.recordOutput("FrontPose-IsMegaTag2", frontTag.isMegaTag2);
-        }
-
-        if(backTag.tagCount >= 2){
-            driveSystem.getPoseEstimator().addVisionMeasurement(new Pose3d(backTag.pose), backTag.timestampSeconds);
-            Logger.recordOutput("BackPose-Estimate", backTag.pose);
-            Logger.recordOutput("BackPose-IsMegaTag2", frontTag.isMegaTag2);
-        }
-
-
-        // MegaTag 1 Impl
-        // LimelightHelpers.PoseEstimate frontCameraPose = LimelightHelpers.getBotPoseEstimate_wpiBlue(Constants.CameraConstanst.FRONT_CAMERA_NAME);
-        // if (frontCameraPose.tagCount >= 2) {
-        //     driveSystem.getPoseEstimator().addVisionMeasurement(new Pose3d(frontCameraPose.pose), frontCameraPose.timestampSeconds);
-        //     Logger.recordOutput("FrontPoseEstimate", frontCameraPose.pose);
-        // }
-        // LimelightHelpers.PoseEstimate backCameraPose = LimelightHelpers.getBotPoseEstimate_wpiBlue(Constants.CameraConstanst.BACK_CAMERA_NAME);
-        // if (backCameraPose.tagCount >= 2) {
-        //     driveSystem.getPoseEstimator().addVisionMeasurement(new Pose3d(backCameraPose.pose), backCameraPose.timestampSeconds);
-        //     Logger.recordOutput("BackPoseEstimate", backCameraPose.pose);
-        // }
-    }
-
-    @Override
-    public void simulationPeriodic() {
-        super.simulationPeriodic();
-        driveSystem.simulationPeriodic();
-    }
-
-    public Command getAutoCommand() {
-        return autoChooser.getSelected();
-    }
-
-    public void stopDrive() {
-        driveSystem.setChassisSpeed(ChassisSpeeds.fromRobotRelativeSpeeds(0, 0, 0, driveSystem.getGyro().getRotation2d()), Translation2d.kZero);
-    }
-
-    public void rotateChassis(double rotSpeed) {
-        driveSystem.setChassisSpeed(ChassisSpeeds.fromRobotRelativeSpeeds(0, 0, rotSpeed, driveSystem.getGyro().getRotation2d()), Translation2d.kZero);
-    }
-
-    public Pose3d getBotPose() {
-        return driveSystem.getRobotPose3d();
-    }
-
-    @PostTest()
-    public void DriveSubsystem_CanDevicesConnected() {
-
-        SwerveModule[] modules = driveSystem.getModules();
-
-        for (int i = 0; i < modules.length; i++) {
-            assertThat(modules[i].getDriveMotor().isConnected())
-                    .as(modules[i].getModuleName() + " drive motor is not connected").isTrue();
-            assertThat(modules[i].getSteerMotor().isConnected())
-                    .as(modules[i].getModuleName() + " steer motor is not connected").isTrue();
-            assertThat(modules[i].getSteerEncoder().isConnected())
-                    .as(modules[i].getModuleName() + " CANCoder is not connected").isTrue();
-        }
-
-        assertThat(driveSystem.getGyro().isConnected()).as("Pigeon is not connected").isTrue();
-    }
-
-}

src/main/java/frc/robot/subsystems/ExampleSubsystem.java

@@ -1,60 +0,0 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
-package frc.robot.subsystems;
-
-import org.fairportrobotics.frc.posty.TestableSubsystem;
-import org.fairportrobotics.frc.posty.test.PostTest;
-import static org.fairportrobotics.frc.posty.assertions.Assertions.*;
-
-import edu.wpi.first.wpilibj2.command.Command;
-
-public class ExampleSubsystem extends TestableSubsystem {
-  /** Creates a new ExampleSubsystem. */
-  public ExampleSubsystem() {}
-
-  /**
-   * Example command factory method.
-   *
-   * @return a command
-   */
-  public Command exampleMethodCommand() {
-    // Inline construction of command goes here.
-    // Subsystem::RunOnce implicitly requires `this` subsystem.
-    return runOnce(
-        () -> {
-          /* one-time action goes here */
-        });
-  }
-
-  /**
-   * An example method querying a boolean state of the subsystem (for example, a digital sensor).
-   *
-   * @return value of some boolean subsystem state, such as a digital sensor.
-   */
-  public boolean exampleCondition() {
-    // Query some boolean state, such as a digital sensor.
-    return false;
-  }
-
-  @Override
-  public void periodic() {
-    // This method will be called once per scheduler run
-  }
-
-  @Override
-  public void simulationPeriodic() {
-    // This method will be called once per scheduler run during simulation
-  }
-
-  @PostTest(name = "A friendly name", enabled = true)
-  public void myFailingPostTest(){
-    assertThat(true).isFalse();
-  }
-
-  @PostTest(enabled = true)
-  public void myPassingPostTest(){
-    assertThat("Hello World").endWith("World");
-  }
-}

src/main/java/frc/robot/subsystems/HopperSubsystem.java

@@ -1,114 +0,0 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-package frc.robot.subsystems;
-
-import org.fairportrobotics.frc.posty.TestableSubsystem;
-import static org.fairportrobotics.frc.posty.assertions.Assertions.assertThat;
-import org.fairportrobotics.frc.posty.test.PostTest;
-import org.littletonrobotics.junction.Logger;
-
-import com.ctre.phoenix6.StatusSignal;
-import com.ctre.phoenix6.configs.MotorOutputConfigs;
-import com.ctre.phoenix6.configs.Slot0Configs;
-import com.ctre.phoenix6.configs.TalonFXConfiguration;
-import com.ctre.phoenix6.controls.VelocityVoltage;
-import com.ctre.phoenix6.hardware.TalonFX;
-
-import edu.wpi.first.units.Units;
-import edu.wpi.first.units.measure.AngularVelocity;
-import edu.wpi.first.wpilibj.GenericHID.RumbleType;
-import edu.wpi.first.wpilibj.XboxController;
-import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.Constants;
-
-public class HopperSubsystem extends TestableSubsystem {
-
-    private TalonFX spindexerMotor;
-
-    private TalonFX kickerMotor;
-    private VelocityVoltage kickerControl = new VelocityVoltage(0);
-    private StatusSignal<AngularVelocity> kickerVelocity;
-
-    /**
-     * Creates a new HopperSubsystem.
-     */
-    public HopperSubsystem() {
-        spindexerMotor = new TalonFX(Constants.HopperConstants.SPINDEXER_MOTOR_ID);
-        spindexerMotor.getConfigurator().apply(
-                new TalonFXConfiguration()
-                        .withMotorOutput(
-                                new MotorOutputConfigs()
-                                        .withInverted(Constants.HopperConstants.SPINDEXER_MOTOR_DIRECTION))
-                        .withSlot0(
-                                new Slot0Configs()
-                                        .withKP(0)
-                                        .withKI(0)
-                                        .withKD(0)
-                                        .withKS(0)));
-
-        kickerMotor = new TalonFX(Constants.HopperConstants.KICKER_MOTOR_ID);
-        kickerMotor.getConfigurator().apply(
-                new TalonFXConfiguration()
-                        .withMotorOutput(
-                                new MotorOutputConfigs()
-                                        .withInverted(Constants.HopperConstants.KICKER_MOTOR_DIRECTION)
-                        )
-                        .withSlot0(new Slot0Configs()
-                                .withKP(0)
-                                .withKI(0)
-                                .withKD(0)
-                                .withKS(0)
-                        ));
-        kickerVelocity = kickerMotor.getVelocity();
-    }
-
-    public void feedKicker() {
-        //kickerMotor.setControl(kickerControl.withVelocity(1000));
-        kickerMotor.set(0.75);
-    }
-
-    public void reverseKicker() {
-        //kickerMotor.setControl(kickerControl.withVelocity(-500));
-        kickerMotor.set(-0.5);
-    }
-
-    public void stopKicker() {
-        kickerMotor.stopMotor();
-    }
-
-    public void spindexerOn() {
-        spindexerMotor.set(0.75);
-    }
-
-    public void spindexerReverse() {
-        spindexerMotor.set(-0.75);
-    }
-
-    public void spindexerOff() {
-        spindexerMotor.stopMotor();
-    }
-
-    public Command spinny() {
-        return this.runOnce(() -> {
-            if (spindexerMotor.get() == 0) {
-                spindexerOn();
-            } else {
-                spindexerOff();
-            }
-        });
-    }
-
-
-    @Override
-    public void periodic() {
-        Logger.recordOutput("HopperSubsystem-KickerSpeed(Rotations/sec)", kickerVelocity.refresh().getValue().in(Units.RotationsPerSecond));
-    }
-
-    @PostTest(enabled = true)
-    public void hopperSystem_CANDevicesConnected() {
-        assertThat(kickerMotor.isConnected()).as("Kicker motor not connected!").isTrue();
-        assertThat(spindexerMotor.isConnected()).as("Spindexer motor not connected!").isTrue();
-    }
-
-}

src/main/java/frc/robot/subsystems/IntakeSubsystem.java

@@ -1,93 +0,0 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-package frc.robot.subsystems;
-
-import org.fairportrobotics.frc.posty.TestableSubsystem;
-import static org.fairportrobotics.frc.posty.assertions.Assertions.assertThat;
-import org.fairportrobotics.frc.posty.test.PostTest;
-import static org.fairportrobotics.frc.robolib.motors.Utils.SetMotorDirection;
-
-import com.ctre.phoenix.motorcontrol.LimitSwitchNormal;
-import com.ctre.phoenix.motorcontrol.LimitSwitchSource;
-import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
-import com.ctre.phoenix6.hardware.TalonFX;
-
-import edu.wpi.first.wpilibj.XboxController;
-import edu.wpi.first.wpilibj.GenericHID.RumbleType;
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.Commands;
-import frc.robot.Constants;
-
-public class IntakeSubsystem extends TestableSubsystem {
-
-    /**
-     * Creates a new ExampleSubsystem.
-     */
-
-    private TalonFX intakeMotor;
-    private WPI_TalonSRX deployMotor;
-
-    public IntakeSubsystem() {
-        intakeMotor = new TalonFX(Constants.IntakeConstants.INTAKE_MOTOR_ID);
-        SetMotorDirection(intakeMotor, Constants.IntakeConstants.INTAKE_MOTOR_DIRECTION);
-
-        deployMotor = new WPI_TalonSRX(Constants.IntakeConstants.DEPLOY_MOTOR_ID);
-        deployMotor.configForwardLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen);
-        deployMotor.configReverseLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen);
-        deployMotor.setInverted(Constants.IntakeConstants.DEPLOY_MOTOR_INVERTED);
-    }
-
-    public Command deploy() {
-        return Commands.sequence(
-                Commands.deadline(
-                        Commands.waitSeconds(2),
-                        this.runOnce(
-                                () -> {
-                                    deployMotor.set(1);
-                                } // Bump this speed up if intake doesn't deploy
-                        )
-                ),
-                this.runOnce(() -> deployMotor.stopMotor())
-        );
-    }
-
-    public Command resetDeploy() {
-        return this.runEnd(() -> deployMotor.set(-1), () -> deployMotor.stopMotor());
-    }
-
-    public void setSpeed(double speed) {
-        intakeMotor.set(speed);
-    }
-
-    public Command stopIntake(XboxController m_XboxController) {
-        return this.runOnce(() -> {
-            intakeMotor.stopMotor();
-            m_XboxController.setRumble(RumbleType.kBothRumble, .0);
-        });
-    }
-
-    public Command intake(XboxController m_XboxController) {
-        return this.runOnce(() -> {
-            if (intakeMotor.get() == 0) {
-                setSpeed(-.5);
-                m_XboxController.setRumble(RumbleType.kBothRumble, .5);
-            } else {
-                intakeMotor.stopMotor();
-                m_XboxController.setRumble(RumbleType.kBothRumble, .0);
-            }
-        });
-    }
-
-    public Command reverseIntake() {
-        return this.runEnd(() -> setSpeed(.5), () -> {
-            intakeMotor.stopMotor();
-        });
-    }
-
-    @PostTest(enabled = true)
-    public void IntakeSubsystem_CANDevicesConnected() {
-        assertThat(intakeMotor.isConnected()).as("Intake motor not connected!").isTrue();
-        assertThat(deployMotor.isAlive()).as("Deploy controller not connected!").isTrue();
-    }
-}

src/main/java/frc/robot/subsystems/TurretSubsystem.java

@@ -1,318 +0,0 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
-package frc.robot.subsystems;
-
-import org.fairportrobotics.frc.posty.TestableSubsystem;
-import org.fairportrobotics.frc.posty.test.PostTest;
-import org.littletonrobotics.junction.Logger;
-import org.littletonrobotics.junction.ConsoleSource.RoboRIO;
-
-import static org.fairportrobotics.frc.posty.assertions.Assertions.*;
-
-import com.ctre.phoenix6.controls.PositionVoltage;
-import com.ctre.phoenix6.StatusSignal;
-import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
-import com.ctre.phoenix6.configs.FeedbackConfigs;
-import com.ctre.phoenix6.configs.MotorOutputConfigs;
-import com.ctre.phoenix6.configs.Slot0Configs;
-import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs;
-import com.ctre.phoenix6.configs.TalonFXConfiguration;
-import com.ctre.phoenix6.hardware.TalonFX;
-import com.ctre.phoenix6.signals.ForwardLimitValue;
-import com.ctre.phoenix6.signals.InvertedValue;
-import com.ctre.phoenix6.signals.ReverseLimitValue;
-import com.ctre.phoenix6.sim.TalonFXSimState;
-import com.revrobotics.PersistMode;
-import com.revrobotics.ResetMode;
-import com.revrobotics.spark.ClosedLoopSlot;
-import com.revrobotics.spark.FeedbackSensor;
-import com.revrobotics.spark.SparkClosedLoopController;
-import com.revrobotics.spark.SparkMax;
-import com.revrobotics.spark.SparkBase.ControlType;
-import com.revrobotics.spark.SparkLowLevel.MotorType;
-import com.revrobotics.spark.config.SparkMaxConfig;
-
-import frc.robot.Constants;
-import frc.robot.Robot;
-import edu.wpi.first.math.geometry.Pose3d;
-import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.units.AngleUnit;
-import edu.wpi.first.units.Units;
-import edu.wpi.first.units.measure.Angle;
-import edu.wpi.first.wpilibj.DigitalInput;
-import edu.wpi.first.wpilibj.Servo;
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.Commands;
-
-public class TurretSubsystem extends TestableSubsystem {
-  public enum TurretState {
-    INIT,
-    HOMING,
-    READY;
-  }
-
-  private final boolean LOGGING = true;
-  private TurretState turretState;
-  private DigitalInput turretLimitSwitch;
-  private TalonFX turretMotor;
-  private PositionVoltage turretControlMode = new PositionVoltage(0);
-  private TalonFXSimState turretSim;
-  private StatusSignal<Angle> turretMotorPosition;
-  private StatusSignal<ForwardLimitValue> turretMotorForwardLimit;
-  private StatusSignal<ReverseLimitValue> turretMotorReverseLimit;
-  private StatusSignal<Double> turretPositionError;
-  private double forwardLimit = 0;
-  private double reverseLimit = 0;
-
-  private SparkMax launcherMotor;
-  private SparkClosedLoopController launcherControler;
-
-  private double turretOffsetInRotations = 0;
-
-  private Servo hood;
-
-  private Pose3d turretTarget;
-
-  public TurretSubsystem() {
-
-    // Turret configuration
-    turretLimitSwitch = new DigitalInput(Constants.ShooterConstants.TURRET_LIMIT_CHANNEL);
-    turretMotor = new TalonFX(Constants.ShooterConstants.TURRET_MOTOR_ID);
-    turretMotor.getConfigurator().apply(
-      new TalonFXConfiguration()
-        .withSlot0(
-          new Slot0Configs()
-            .withKP(6)
-            .withKI(0)
-            .withKD(0.3)
-            .withKS(0)
-        )
-        .withMotorOutput(
-          new MotorOutputConfigs()
-            .withInverted(InvertedValue.Clockwise_Positive)
-        )
-        .withCurrentLimits(
-          new CurrentLimitsConfigs()
-            .withStatorCurrentLimit(50)
-        )
-        .withSoftwareLimitSwitch(new SoftwareLimitSwitchConfigs().withForwardSoftLimitEnable(false).withReverseSoftLimitEnable(false))
-        .withFeedback(
-          new FeedbackConfigs()
-            .withFeedbackRotorOffset(0)
-        )
-    );
-    turretMotorPosition = turretMotor.getPosition();
-    turretMotorForwardLimit = turretMotor.getForwardLimit();
-    turretMotorReverseLimit = turretMotor.getReverseLimit();
-    turretPositionError = turretMotor.getClosedLoopError();
-
-    turretSim = turretMotor.getSimState();
-
-    // Launcher configuration
-    launcherMotor = new SparkMax(Constants.ShooterConstants.LAUNCHER_MOTOR_ID, MotorType.kBrushless);
-    SparkMaxConfig config = new SparkMaxConfig().apply((SparkMaxConfig) SparkMaxConfig.Presets.REV_NEO);
-    config.inverted(Constants.ShooterConstants.LAUNCHER_MOTOR_INVERTED);
-    config.voltageCompensation(10);
-    config.closedLoop.p(0.0002).i(0.000001).d(0.0005); // I = 0.0000001
-    config.closedLoop.feedForward.kS(0.2).kV(0.000).kA(0.000);
-    // config.closedLoop.allowedClosedLoopError(10, ClosedLoopSlot.kSlot0);
-    config.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder);
-    config.closedLoop.positionWrappingEnabled(false);
-    // config.closedLoopRampRate(0.5);
-    // config.smartCurrentLimit(5, 50);
-    launcherMotor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
-    launcherControler = launcherMotor.getClosedLoopController();
-
-    // Hood configuration
-    hood = new Servo(Constants.ShooterConstants.HOOD_SERVO_CHANNEL);
-    turretState = TurretState.INIT;
-  }
-
-  public void setLauncher(double speed) {
-    if(speed == 0)
-    {
-        launcherMotor.stopMotor();
-        return;
-    }
-    launcherControler.setSetpoint(speed, ControlType.kVelocity, ClosedLoopSlot.kSlot0);
-  }
-
-  public boolean isLauncherUpToSpeed() {
-    // Valid if velcity is within 10% of setpoint.
-    // May need to lower this
-  return true;
-    // return Math.abs(launcherControler.getSetpoint() - launcherMotor.getEncoder().getVelocity()) >= launcherControler.getSetpoint() * 0.03;
-  }
-
-  public void setTargetElevation(double elev)
-  {
-    double pos = Math.abs((elev - 300)/300);
-    Logger.recordOutput("ShooterHood-Angle", pos);
-    hood.set(pos);
-  }
-
-  public Command revSpeedCommand()
-  {
-    return this.runOnce(() -> setLauncher(-.5));
-  }
-
-  public Command setShooter(int speedRPM){
-    return Commands.runEnd(() -> {
-      setLauncher(speedRPM);
-    }, () -> {
-      launcherMotor.stopMotor();
-    }, this);
-  }
-
-  public void homeTurret()
-  {
-    if(turretState != TurretState.INIT){
-      return; 
-    }
-    turretMotor.set(Constants.ShooterConstants.HOMING_SPEED);
-    turretState = TurretState.HOMING;
-  }
-
-  public boolean isTurretReady(){
-    return turretState == TurretState.READY;
-  }
-
-  public void turretRotateDir(double speed)
-  {
-    turretMotor.set(speed);
-  }
-
-  public double getTurretAngleRobotRelative()
-  {
-    return getTurretMotorPosition() / Constants.ShooterConstants.TURRET_GEAR_RATIO;
-  }
-
-  public void setTurretTargetPose(Pose3d targetPose){
-    turretTarget = targetPose;
-  }
-
-  public Pose3d getTurretTargetPose(){
-    return turretTarget;
-  }
-
-  private double getTurretMotorPosition(){
-    return turretMotorPosition.refresh().getValueAsDouble() - turretOffsetInRotations;
-  }
-
-  @Override
-  public void periodic() {
-    if (turretState == TurretState.HOMING) {
-      if (turretLimitSwitch.get()) { // Turret has tripped the switch
-        turretMotor.stopMotor();
-        turretOffsetInRotations = (turretMotorPosition.refresh().getValueAsDouble() - 1.323);
-        if (!Robot.isReal()) {
-          turretOffsetInRotations = 0; // Flip turret around
-        }
-        turretState = TurretState.READY;
-        forwardLimit = (1.5 + turretOffsetInRotations);
-        reverseLimit = (-1.5 + turretOffsetInRotations);
-        Logger.recordOutput("TurretSubsystem-TurretForwardLimit", forwardLimit);
-        Logger.recordOutput("TurretSubsystem-TurretReverseLimit", reverseLimit);
-        turretMotor.getConfigurator().apply(
-            new SoftwareLimitSwitchConfigs()
-                .withForwardSoftLimitEnable(true)
-                .withForwardSoftLimitThreshold(forwardLimit)
-                .withReverseSoftLimitEnable(true)
-                .withReverseSoftLimitThreshold(reverseLimit));
-        // setTurretRobotRelative(0);
-        // setTurretMotorRotation(-0.34); // Return to 0 after MIAMI VALLEY
-      }
-    }
-
-    if (LOGGING) {
-      Logger.recordOutput("TurretSubsystem-TurretLimitSwitch", turretLimitSwitch.get());
-      Logger.recordOutput("TurretSubsystem-Launcher Speed (RPM)", launcherMotor.getEncoder().getVelocity());
-      Logger.recordOutput("TurretSubsystem-TurretState", turretState);
-      Logger.recordOutput("TurretSubsystem-TurretPositionRobotRelative", getTurretAngleRobotRelative());
-      Logger.recordOutput("TurretSubsystem-TurretMotorPositionWithOffset", getTurretMotorPosition());
-      Logger.recordOutput("TurretSubsystem-TurretMotorPositionRaw",
-          turretMotorPosition.refresh().getValue().in(Units.Rotations));
-      Logger.recordOutput("TurretSubsystem-TurretRequestedPosRaw",
-          turretControlMode.getPositionMeasure().in(Units.Rotations));
-      Logger.recordOutput("TurretSubsystem-TurretRequestedPos", getRequestedPosTurretRelative());
-      Logger.recordOutput("TurretSubsystem-TurretOffset", turretOffsetInRotations);
-      Logger.recordOutput("TurretSubsystem-TurretForwardLimitHit", turretMotorForwardLimit.refresh().getValue());
-      Logger.recordOutput("TurretSubsystem-TurretReverseLimitHit", turretMotorReverseLimit.refresh().getValue());
-      Logger.recordOutput("TurretSubsystem-TurretPositionError", turretPositionError.refresh().getValueAsDouble());
-      Logger.recordOutput("TurretSubsystem-LauncherSetpoint(RPM)", launcherControler.getSetpoint());
-    }
-
-  }
-
-  public void setTurretFieldRelative(Pose3d robotPose, double pos)
-  {
-    if(turretState != TurretState.READY)
-    {
-      // Not ready to set angle
-      return;
-    }
-
-    double correctedPos = -pos; // Flip angle to match CCW positive. Add 0.5 to flip around
-
-    double robotRotations = robotPose.getRotation().toRotation2d().getRotations();
-
-    Rotation2d turretAngle = Rotation2d.fromRotations(correctedPos).plus(Rotation2d.fromRotations(robotRotations)).minus(Rotation2d.fromRotations(0.5));
-
-    Logger.recordOutput("TurretSubsystem-TurretFieldTarget", turretAngle.getRotations());
-
-    setTurretRobotRelative(((turretAngle.getRotations())));
-  }
-
-  public void setTurretRobotRelative(double pos)
-  {
-    if(turretState != TurretState.READY)
-    {
-      // Not ready to set angle
-      return;
-    }
-
-    double correctedPos = -pos; // Flip angle to match CCW positive
-
-    setTurretMotorRotation(correctedPos * Constants.ShooterConstants.TURRET_GEAR_RATIO);
-  }
-
-  public void setTurretMotorRotation(double pos){
-    double posWithOffset = pos + turretOffsetInRotations;
-
-    Logger.recordOutput("Blah", posWithOffset);
-
-    if(Robot.isSimulation()){
-      if(posWithOffset >= forwardLimit){
-        turretSim.setForwardLimit(true);
-      } else {
-        turretSim.setForwardLimit(false);
-      }
-
-      if(posWithOffset <= reverseLimit){
-        turretSim.setReverseLimit(true);
-      } else {
-        turretSim.setReverseLimit(false);
-      }
-    }
-
-    turretMotor.setControl(turretControlMode.withPosition(posWithOffset));
-    turretSim.setRawRotorPosition(-posWithOffset);
-  }
-
-  public boolean isTurretAtTarget(){
-    return Math.abs(turretPositionError.refresh().getValueAsDouble()) <= 0.1;
-  }
-
-  private double getRequestedPosTurretRelative() {
-    return (turretControlMode.getPositionMeasure().in(Units.Rotations) - turretOffsetInRotations) / Constants.ShooterConstants.TURRET_GEAR_RATIO;
-  }
-
-  @PostTest()
-  public void TurretSubsystem_CANDevicesConnected(){
-    assertThat(launcherMotor.getFaults().can).as("Launcher motor not connected!").isFalse();
-    assertThat(turretMotor.isConnected()).as("Turret motor not connected!").isTrue();
-  }
-
-}

utils.py

@@ -0,0 +1,167 @@
+"""
+utils.py – pure math utilities for Castor 2026 (Python/MagicBot port).
+
+Direct translation of Utils.java. No framework dependencies.
+"""
+
+import math
+
+from wpilib import DriverStation
+from wpimath.geometry import Pose3d
+
+from constants import FieldPoses, ShootingRegion, ShootingRegionDimensions
+
+
+def calculate_turret_angle_from_camera_tag_distance(distance_to_tag: float) -> float:
+    """
+    Calculates the turret rotation (in rotations, robot-relative) needed
+    to aim at the HUB given the camera's measured distance to the tag.
+
+    :param distance_to_tag: camera distance to the AprilTag (metres)
+    :return: turret angle in rotations (robot-relative)
+    """
+    camera_distance_to_target = distance_to_tag + 0.6
+    camera_to_shoot_distance = 0.26035
+
+    # Pythagorean theorem
+    shooter_distance_to_target = math.sqrt(
+        camera_distance_to_target**2 + camera_to_shoot_distance**2
+    )
+    angle_at_shooter = math.acos(camera_to_shoot_distance / shooter_distance_to_target)
+    robot_turret_angle = (math.pi / 2) - angle_at_shooter
+    robot_turret_rotations = robot_turret_angle / math.pi
+
+    return robot_turret_rotations
+
+
+def find_robot_shooting_region(
+    robot_pose: Pose3d,
+    alliance: DriverStation.Alliance,
+) -> ShootingRegion:
+    """
+    Determines the shooting region for the robot based on its pose and alliance.
+
+    :param robot_pose: current robot pose
+    :param alliance: robot's alliance (Blue or Red)
+    :return: the matching ShootingRegion enum value
+    """
+    region = ShootingRegion.NON_SHOOTING_REGION
+    robot_x = robot_pose.X()
+    robot_y = robot_pose.Y()
+
+    # Local aliases to keep condition lines under 120 chars
+    D = ShootingRegionDimensions
+    in_neutral_x = D.NEUTRAL_ZONE_REGION_X_MIN < robot_x < D.NEUTRAL_ZONE_REGION_X_MAX
+
+    if alliance == DriverStation.Alliance.kBlue:
+        if robot_x < ShootingRegionDimensions.BLUE_ALLIANCE_ZONE_REGION_X_MAX:
+            region = ShootingRegion.BLUE_OWN_ALLIANCE_ZONE
+        elif (
+            in_neutral_x
+            and robot_y > ShootingRegionDimensions.BLUE_LEFT_REGION_Y_MIN
+        ):
+            region = ShootingRegion.BLUE_LEFT_NEUTRAL_ZONE
+        elif (
+            in_neutral_x
+            and robot_y < ShootingRegionDimensions.BLUE_RIGHT_REGION_Y_MAX
+        ):
+            region = ShootingRegion.BLUE_RIGHT_NEUTRAL_ZONE
+        elif (
+            robot_x > ShootingRegionDimensions.RED_ALLIANCE_ZONE_REGION_X_MIN
+            and robot_y > ShootingRegionDimensions.BLUE_LEFT_REGION_Y_MIN
+        ):
+            region = ShootingRegion.BLUE_LEFT_OPPONENT_ALLIANCE_ZONE
+        elif (
+            robot_x > ShootingRegionDimensions.RED_ALLIANCE_ZONE_REGION_X_MIN
+            and robot_y < ShootingRegionDimensions.BLUE_RIGHT_REGION_Y_MAX
+        ):
+            region = ShootingRegion.BLUE_RIGHT_OPPONENT_ALLIANCE_ZONE
+
+    elif alliance == DriverStation.Alliance.kRed:
+        if robot_x > ShootingRegionDimensions.RED_ALLIANCE_ZONE_REGION_X_MIN:
+            region = ShootingRegion.RED_OWN_ALLIANCE_ZONE
+        elif (
+            in_neutral_x
+            and robot_y < ShootingRegionDimensions.RED_LEFT_REGION_Y_MAX
+        ):
+            region = ShootingRegion.RED_LEFT_NEUTRAL_ZONE
+        elif (
+            in_neutral_x
+            and robot_y > ShootingRegionDimensions.RED_RIGHT_REGION_Y_MIN
+        ):
+            region = ShootingRegion.RED_RIGHT_NEUTRAL_ZONE
+        elif (
+            robot_x < ShootingRegionDimensions.BLUE_ALLIANCE_ZONE_REGION_X_MAX
+            and robot_y < ShootingRegionDimensions.RED_LEFT_REGION_Y_MAX
+        ):
+            region = ShootingRegion.RED_LEFT_OPPONENT_ALLIANCE_ZONE
+        elif (
+            robot_x > ShootingRegionDimensions.RED_ALLIANCE_ZONE_REGION_X_MIN
+            and robot_y > ShootingRegionDimensions.RED_RIGHT_REGION_Y_MIN
+        ):
+            region = ShootingRegion.RED_RIGHT_OPPONENT_ALLIANCE_ZONE
+
+    return region
+
+
+def get_hood_angle_for_distance(distance_meters: float) -> float:
+    """
+    Returns the hood angle for a given distance.
+
+    :param distance_meters: distance to target in metres
+    :return: hood servo angle
+    """
+    if distance_meters >= 4:
+        return 200.0
+    return 0.0
+
+
+def get_launcher_rpm_for_distance(distance_meters: float) -> float:
+    """
+    Returns the launcher RPM for a given distance, capped at 5000 RPM.
+
+    :param distance_meters: distance to target in metres
+    :return: launcher RPM
+    """
+    return min((667.557 * distance_meters) + 1500.699, 5000.0)
+
+
+def find_shooting_target(
+    region: ShootingRegion,
+    alliance: DriverStation.Alliance,
+) -> Pose3d:
+    """
+    Determines the appropriate shooting target for the robot's current region
+    and alliance.
+
+    :param region: the robot's current ShootingRegion
+    :param alliance: the robot's alliance (Blue or Red)
+    :return: target Pose3d, or all-zero Pose3d if region/alliance is invalid
+    """
+    target = Pose3d()
+
+    if alliance == DriverStation.Alliance.kBlue:
+        if region == ShootingRegion.BLUE_OWN_ALLIANCE_ZONE:
+            target = FieldPoses.BLUE_HUB_POSE
+        elif region == ShootingRegion.BLUE_LEFT_NEUTRAL_ZONE:
+            target = FieldPoses.BLUE_LEFT_ALLIANCE_ZONE_PASS_TARGET_POSE
+        elif region == ShootingRegion.BLUE_RIGHT_NEUTRAL_ZONE:
+            target = FieldPoses.BLUE_RIGHT_ALLIANCE_ZONE_PASS_TARGET_POSE
+        elif region == ShootingRegion.BLUE_LEFT_OPPONENT_ALLIANCE_ZONE:
+            target = FieldPoses.BLUE_LEFT_NEUTRAL_ZONE_PASS_TARGET_POSE
+        elif region == ShootingRegion.BLUE_RIGHT_OPPONENT_ALLIANCE_ZONE:
+            target = FieldPoses.BLUE_RIGHT_NEUTRAL_ZONE_PASS_TARGET_POSE
+
+    elif alliance == DriverStation.Alliance.kRed:
+        if region == ShootingRegion.RED_OWN_ALLIANCE_ZONE:
+            target = FieldPoses.RED_HUB_POSE
+        elif region == ShootingRegion.RED_LEFT_NEUTRAL_ZONE:
+            target = FieldPoses.RED_LEFT_ALLIANCE_ZONE_PASS_TARGET_POSE
+        elif region == ShootingRegion.RED_RIGHT_NEUTRAL_ZONE:
+            target = FieldPoses.RED_RIGHT_ALLIANCE_ZONE_PASS_TARGET_POSE
+        elif region == ShootingRegion.RED_LEFT_OPPONENT_ALLIANCE_ZONE:
+            target = FieldPoses.RED_LEFT_NEUTRAL_ZONE_PASS_TARGET_POSE
+        elif region == ShootingRegion.RED_RIGHT_OPPONENT_ALLIANCE_ZONE:
+            target = FieldPoses.RED_RIGHT_NEUTRAL_ZONE_PASS_TARGET_POSE
+
+    return target

vendordeps/AdvantageKit.json

@@ -1,35 +0,0 @@
-{
-    "fileName": "AdvantageKit.json",
-    "name": "AdvantageKit",
-    "version": "26.0.2",
-    "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003",
-    "frcYear": "2026",
-    "mavenUrls": [
-        "https://frcmaven.wpi.edu/artifactory/littletonrobotics-mvn-release/"
-    ],
-    "jsonUrl": "https://github.com/Mechanical-Advantage/AdvantageKit/releases/latest/download/AdvantageKit.json",
-    "javaDependencies": [
-        {
-            "groupId": "org.littletonrobotics.akit",
-            "artifactId": "akit-java",
-            "version": "26.0.2"
-        }
-    ],
-    "jniDependencies": [
-        {
-            "groupId": "org.littletonrobotics.akit",
-            "artifactId": "akit-wpilibio",
-            "version": "26.0.2",
-            "skipInvalidPlatforms": false,
-            "isJar": false,
-            "validPlatforms": [
-                "linuxathena",
-                "linuxx86-64",
-                "linuxarm64",
-                "osxuniversal",
-                "windowsx86-64"
-            ]
-        }
-    ],
-    "cppDependencies": []
-}

vendordeps/ChoreoLib2026.json

@@ -1,44 +0,0 @@
-{
-    "fileName": "ChoreoLib2026.json",
-    "name": "ChoreoLib",
-    "version": "2026.0.2",
-    "uuid": "b5e23f0a-dac9-4ad2-8dd6-02767c520aca",
-    "frcYear": "2026",
-    "mavenUrls": [
-        "https://frcmaven.wpi.edu/artifactory/sleipnirgroup-mvn-release/",
-        "https://repo1.maven.org/maven2"
-    ],
-    "jsonUrl": "https://choreo.autos/lib/ChoreoLib2026.json",
-    "javaDependencies": [
-        {
-            "groupId": "choreo",
-            "artifactId": "ChoreoLib-java",
-            "version": "2026.0.2"
-        },
-        {
-            "groupId": "com.google.code.gson",
-            "artifactId": "gson",
-            "version": "2.11.0"
-        }
-    ],
-    "jniDependencies": [],
-    "cppDependencies": [
-        {
-            "groupId": "choreo",
-            "artifactId": "ChoreoLib-cpp",
-            "version": "2026.0.2",
-            "libName": "ChoreoLib",
-            "headerClassifier": "headers",
-            "sharedLibrary": false,
-            "skipInvalidPlatforms": true,
-            "binaryPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "osxuniversal",
-                "linuxathena",
-                "linuxarm32",
-                "linuxarm64"
-            ]
-        }
-    ]
-}

vendordeps/PathplannerLib-2026.1.2.json

@@ -1,38 +0,0 @@
-{
-    "fileName": "PathplannerLib-2026.1.2.json",
-    "name": "PathplannerLib",
-    "version": "2026.1.2",
-    "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786",
-    "frcYear": "2026",
-    "mavenUrls": [
-        "https://3015rangerrobotics.github.io/pathplannerlib/repo"
-    ],
-    "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json",
-    "javaDependencies": [
-        {
-            "groupId": "com.pathplanner.lib",
-            "artifactId": "PathplannerLib-java",
-            "version": "2026.1.2"
-        }
-    ],
-    "jniDependencies": [],
-    "cppDependencies": [
-        {
-            "groupId": "com.pathplanner.lib",
-            "artifactId": "PathplannerLib-cpp",
-            "version": "2026.1.2",
-            "libName": "PathplannerLib",
-            "headerClassifier": "headers",
-            "sharedLibrary": false,
-            "skipInvalidPlatforms": true,
-            "binaryPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "osxuniversal",
-                "linuxathena",
-                "linuxarm32",
-                "linuxarm64"
-            ]
-        }
-    ]
-}

vendordeps/Phoenix5-5.36.0.json

@@ -1,171 +0,0 @@
-{
-    "fileName": "Phoenix5-5.36.0.json",
-    "name": "CTRE-Phoenix (v5)",
-    "version": "5.36.0",
-    "frcYear": "2026",
-    "uuid": "ab676553-b602-441f-a38d-f1296eff6537",
-    "mavenUrls": [
-        "https://maven.ctr-electronics.com/release/"
-    ],
-    "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2026-latest.json",
-    "requires": [
-        {
-            "uuid": "e995de00-2c64-4df5-8831-c1441420ff19",
-            "errorMessage": "Phoenix 5 requires low-level libraries from Phoenix 6.  Please add the Phoenix 6 vendordep before adding Phoenix 5.",
-            "offlineFileName": "Phoenix6-frc2026-latest.json",
-            "onlineUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2026-latest.json"
-        }
-    ],
-    "conflictsWith": [
-        {
-            "uuid": "e7900d8d-826f-4dca-a1ff-182f658e98af",
-            "errorMessage": "Users must use the Phoenix 5 replay vendordep when using the Phoenix 6 replay vendordep.",
-            "offlineFileName": "Phoenix6-replay-frc2026-latest.json"
-        },
-        {
-            "uuid": "fbc886a4-2cec-40c0-9835-71086a8cc3df",
-            "errorMessage": "Users cannot have both the replay and regular Phoenix 5 vendordeps in their robot program.",
-            "offlineFileName": "Phoenix5-replay-frc2026-latest.json"
-        }
-    ],
-    "javaDependencies": [
-        {
-            "groupId": "com.ctre.phoenix",
-            "artifactId": "api-java",
-            "version": "5.36.0"
-        },
-        {
-            "groupId": "com.ctre.phoenix",
-            "artifactId": "wpiapi-java",
-            "version": "5.36.0"
-        }
-    ],
-    "jniDependencies": [
-        {
-            "groupId": "com.ctre.phoenix",
-            "artifactId": "cci",
-            "version": "5.36.0",
-            "isJar": false,
-            "skipInvalidPlatforms": true,
-            "validPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "linuxarm64",
-                "linuxathena"
-            ],
-            "simMode": "hwsim"
-        },
-        {
-            "groupId": "com.ctre.phoenix.sim",
-            "artifactId": "cci-sim",
-            "version": "5.36.0",
-            "isJar": false,
-            "skipInvalidPlatforms": true,
-            "validPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "linuxarm64",
-                "osxuniversal"
-            ],
-            "simMode": "swsim"
-        }
-    ],
-    "cppDependencies": [
-        {
-            "groupId": "com.ctre.phoenix",
-            "artifactId": "wpiapi-cpp",
-            "version": "5.36.0",
-            "libName": "CTRE_Phoenix_WPI",
-            "headerClassifier": "headers",
-            "sharedLibrary": true,
-            "skipInvalidPlatforms": true,
-            "binaryPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "linuxarm64",
-                "linuxathena"
-            ],
-            "simMode": "hwsim"
-        },
-        {
-            "groupId": "com.ctre.phoenix",
-            "artifactId": "api-cpp",
-            "version": "5.36.0",
-            "libName": "CTRE_Phoenix",
-            "headerClassifier": "headers",
-            "sharedLibrary": true,
-            "skipInvalidPlatforms": true,
-            "binaryPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "linuxarm64",
-                "linuxathena"
-            ],
-            "simMode": "hwsim"
-        },
-        {
-            "groupId": "com.ctre.phoenix",
-            "artifactId": "cci",
-            "version": "5.36.0",
-            "libName": "CTRE_PhoenixCCI",
-            "headerClassifier": "headers",
-            "sharedLibrary": true,
-            "skipInvalidPlatforms": true,
-            "binaryPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "linuxarm64",
-                "linuxathena"
-            ],
-            "simMode": "hwsim"
-        },
-        {
-            "groupId": "com.ctre.phoenix.sim",
-            "artifactId": "wpiapi-cpp-sim",
-            "version": "5.36.0",
-            "libName": "CTRE_Phoenix_WPISim",
-            "headerClassifier": "headers",
-            "sharedLibrary": true,
-            "skipInvalidPlatforms": true,
-            "binaryPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "linuxarm64",
-                "osxuniversal"
-            ],
-            "simMode": "swsim"
-        },
-        {
-            "groupId": "com.ctre.phoenix.sim",
-            "artifactId": "api-cpp-sim",
-            "version": "5.36.0",
-            "libName": "CTRE_PhoenixSim",
-            "headerClassifier": "headers",
-            "sharedLibrary": true,
-            "skipInvalidPlatforms": true,
-            "binaryPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "linuxarm64",
-                "osxuniversal"
-            ],
-            "simMode": "swsim"
-        },
-        {
-            "groupId": "com.ctre.phoenix.sim",
-            "artifactId": "cci-sim",
-            "version": "5.36.0",
-            "libName": "CTRE_PhoenixCCISim",
-            "headerClassifier": "headers",
-            "sharedLibrary": true,
-            "skipInvalidPlatforms": true,
-            "binaryPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "linuxarm64",
-                "osxuniversal"
-            ],
-            "simMode": "swsim"
-        }
-    ]
-}

vendordeps/Phoenix6-26.1.2.json

@@ -1,449 +0,0 @@
-{
-    "fileName": "Phoenix6-26.1.2.json",
-    "name": "CTRE-Phoenix (v6)",
-    "version": "26.1.2",
-    "frcYear": "2026",
-    "uuid": "e995de00-2c64-4df5-8831-c1441420ff19",
-    "mavenUrls": [
-        "https://maven.ctr-electronics.com/release/"
-    ],
-    "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2026-latest.json",
-    "conflictsWith": [
-        {
-            "uuid": "e7900d8d-826f-4dca-a1ff-182f658e98af",
-            "errorMessage": "Users can not have both the replay and regular Phoenix 6 vendordeps in their robot program.",
-            "offlineFileName": "Phoenix6-replay-frc2026-latest.json"
-        }
-    ],
-    "javaDependencies": [
-        {
-            "groupId": "com.ctre.phoenix6",
-            "artifactId": "wpiapi-java",
-            "version": "26.1.2"
-        }
-    ],
-    "jniDependencies": [
-        {
-            "groupId": "com.ctre.phoenix6",
-            "artifactId": "api-cpp",
-            "version": "26.1.2",
-            "isJar": false,
-            "skipInvalidPlatforms": true,
-            "validPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "linuxarm64",
-                "linuxathena"
-            ],
-            "simMode": "hwsim"
-        },
-        {
-            "groupId": "com.ctre.phoenix6",
-            "artifactId": "tools",
-            "version": "26.1.2",
-            "isJar": false,
-            "skipInvalidPlatforms": true,
-            "validPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "linuxarm64",
-                "linuxathena"
-            ],
-            "simMode": "hwsim"
-        },
-        {
-            "groupId": "com.ctre.phoenix6.sim",
-            "artifactId": "api-cpp-sim",
-            "version": "26.1.2",
-            "isJar": false,
-            "skipInvalidPlatforms": true,
-            "validPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "linuxarm64",
-                "osxuniversal"
-            ],
-            "simMode": "swsim"
-        },
-        {
-            "groupId": "com.ctre.phoenix6.sim",
-            "artifactId": "tools-sim",
-            "version": "26.1.2",
-            "isJar": false,
-            "skipInvalidPlatforms": true,
-            "validPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "linuxarm64",
-                "osxuniversal"
-            ],
-            "simMode": "swsim"
-        },
-        {
-            "groupId": "com.ctre.phoenix6.sim",
-            "artifactId": "simTalonSRX",
-            "version": "26.1.2",
-            "isJar": false,
-            "skipInvalidPlatforms": true,
-            "validPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "linuxarm64",
-                "osxuniversal"
-            ],
-            "simMode": "swsim"
-        },
-        {
-            "groupId": "com.ctre.phoenix6.sim",
-            "artifactId": "simVictorSPX",
-            "version": "26.1.2",
-            "isJar": false,
-            "skipInvalidPlatforms": true,
-            "validPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "linuxarm64",
-                "osxuniversal"
-            ],
-            "simMode": "swsim"
-        },
-        {
-            "groupId": "com.ctre.phoenix6.sim",
-            "artifactId": "simPigeonIMU",
-            "version": "26.1.2",
-            "isJar": false,
-            "skipInvalidPlatforms": true,
-            "validPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "linuxarm64",
-                "osxuniversal"
-            ],
-            "simMode": "swsim"
-        },
-        {
-            "groupId": "com.ctre.phoenix6.sim",
-            "artifactId": "simProTalonFX",
-            "version": "26.1.2",
-            "isJar": false,
-            "skipInvalidPlatforms": true,
-            "validPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "linuxarm64",
-                "osxuniversal"
-            ],
-            "simMode": "swsim"
-        },
-        {
-            "groupId": "com.ctre.phoenix6.sim",
-            "artifactId": "simProTalonFXS",
-            "version": "26.1.2",
-            "isJar": false,
-            "skipInvalidPlatforms": true,
-            "validPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "linuxarm64",
-                "osxuniversal"
-            ],
-            "simMode": "swsim"
-        },
-        {
-            "groupId": "com.ctre.phoenix6.sim",
-            "artifactId": "simProCANcoder",
-            "version": "26.1.2",
-            "isJar": false,
-            "skipInvalidPlatforms": true,
-            "validPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "linuxarm64",
-                "osxuniversal"
-            ],
-            "simMode": "swsim"
-        },
-        {
-            "groupId": "com.ctre.phoenix6.sim",
-            "artifactId": "simProPigeon2",
-            "version": "26.1.2",
-            "isJar": false,
-            "skipInvalidPlatforms": true,
-            "validPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "linuxarm64",
-                "osxuniversal"
-            ],
-            "simMode": "swsim"
-        },
-        {
-            "groupId": "com.ctre.phoenix6.sim",
-            "artifactId": "simProCANrange",
-            "version": "26.1.2",
-            "isJar": false,
-            "skipInvalidPlatforms": true,
-            "validPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "linuxarm64",
-                "osxuniversal"
-            ],
-            "simMode": "swsim"
-        },
-        {
-            "groupId": "com.ctre.phoenix6.sim",
-            "artifactId": "simProCANdi",
-            "version": "26.1.2",
-            "isJar": false,
-            "skipInvalidPlatforms": true,
-            "validPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "linuxarm64",
-                "osxuniversal"
-            ],
-            "simMode": "swsim"
-        },
-        {
-            "groupId": "com.ctre.phoenix6.sim",
-            "artifactId": "simProCANdle",
-            "version": "26.1.2",
-            "isJar": false,
-            "skipInvalidPlatforms": true,
-            "validPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "linuxarm64",
-                "osxuniversal"
-            ],
-            "simMode": "swsim"
-        }
-    ],
-    "cppDependencies": [
-        {
-            "groupId": "com.ctre.phoenix6",
-            "artifactId": "wpiapi-cpp",
-            "version": "26.1.2",
-            "libName": "CTRE_Phoenix6_WPI",
-            "headerClassifier": "headers",
-            "sharedLibrary": true,
-            "skipInvalidPlatforms": true,
-            "binaryPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "linuxarm64",
-                "linuxathena"
-            ],
-            "simMode": "hwsim"
-        },
-        {
-            "groupId": "com.ctre.phoenix6",
-            "artifactId": "tools",
-            "version": "26.1.2",
-            "libName": "CTRE_PhoenixTools",
-            "headerClassifier": "headers",
-            "sharedLibrary": true,
-            "skipInvalidPlatforms": true,
-            "binaryPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "linuxarm64",
-                "linuxathena"
-            ],
-            "simMode": "hwsim"
-        },
-        {
-            "groupId": "com.ctre.phoenix6.sim",
-            "artifactId": "wpiapi-cpp-sim",
-            "version": "26.1.2",
-            "libName": "CTRE_Phoenix6_WPISim",
-            "headerClassifier": "headers",
-            "sharedLibrary": true,
-            "skipInvalidPlatforms": true,
-            "binaryPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "linuxarm64",
-                "osxuniversal"
-            ],
-            "simMode": "swsim"
-        },
-        {
-            "groupId": "com.ctre.phoenix6.sim",
-            "artifactId": "tools-sim",
-            "version": "26.1.2",
-            "libName": "CTRE_PhoenixTools_Sim",
-            "headerClassifier": "headers",
-            "sharedLibrary": true,
-            "skipInvalidPlatforms": true,
-            "binaryPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "linuxarm64",
-                "osxuniversal"
-            ],
-            "simMode": "swsim"
-        },
-        {
-            "groupId": "com.ctre.phoenix6.sim",
-            "artifactId": "simTalonSRX",
-            "version": "26.1.2",
-            "libName": "CTRE_SimTalonSRX",
-            "headerClassifier": "headers",
-            "sharedLibrary": true,
-            "skipInvalidPlatforms": true,
-            "binaryPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "linuxarm64",
-                "osxuniversal"
-            ],
-            "simMode": "swsim"
-        },
-        {
-            "groupId": "com.ctre.phoenix6.sim",
-            "artifactId": "simVictorSPX",
-            "version": "26.1.2",
-            "libName": "CTRE_SimVictorSPX",
-            "headerClassifier": "headers",
-            "sharedLibrary": true,
-            "skipInvalidPlatforms": true,
-            "binaryPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "linuxarm64",
-                "osxuniversal"
-            ],
-            "simMode": "swsim"
-        },
-        {
-            "groupId": "com.ctre.phoenix6.sim",
-            "artifactId": "simPigeonIMU",
-            "version": "26.1.2",
-            "libName": "CTRE_SimPigeonIMU",
-            "headerClassifier": "headers",
-            "sharedLibrary": true,
-            "skipInvalidPlatforms": true,
-            "binaryPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "linuxarm64",
-                "osxuniversal"
-            ],
-            "simMode": "swsim"
-        },
-        {
-            "groupId": "com.ctre.phoenix6.sim",
-            "artifactId": "simProTalonFX",
-            "version": "26.1.2",
-            "libName": "CTRE_SimProTalonFX",
-            "headerClassifier": "headers",
-            "sharedLibrary": true,
-            "skipInvalidPlatforms": true,
-            "binaryPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "linuxarm64",
-                "osxuniversal"
-            ],
-            "simMode": "swsim"
-        },
-        {
-            "groupId": "com.ctre.phoenix6.sim",
-            "artifactId": "simProTalonFXS",
-            "version": "26.1.2",
-            "libName": "CTRE_SimProTalonFXS",
-            "headerClassifier": "headers",
-            "sharedLibrary": true,
-            "skipInvalidPlatforms": true,
-            "binaryPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "linuxarm64",
-                "osxuniversal"
-            ],
-            "simMode": "swsim"
-        },
-        {
-            "groupId": "com.ctre.phoenix6.sim",
-            "artifactId": "simProCANcoder",
-            "version": "26.1.2",
-            "libName": "CTRE_SimProCANcoder",
-            "headerClassifier": "headers",
-            "sharedLibrary": true,
-            "skipInvalidPlatforms": true,
-            "binaryPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "linuxarm64",
-                "osxuniversal"
-            ],
-            "simMode": "swsim"
-        },
-        {
-            "groupId": "com.ctre.phoenix6.sim",
-            "artifactId": "simProPigeon2",
-            "version": "26.1.2",
-            "libName": "CTRE_SimProPigeon2",
-            "headerClassifier": "headers",
-            "sharedLibrary": true,
-            "skipInvalidPlatforms": true,
-            "binaryPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "linuxarm64",
-                "osxuniversal"
-            ],
-            "simMode": "swsim"
-        },
-        {
-            "groupId": "com.ctre.phoenix6.sim",
-            "artifactId": "simProCANrange",
-            "version": "26.1.2",
-            "libName": "CTRE_SimProCANrange",
-            "headerClassifier": "headers",
-            "sharedLibrary": true,
-            "skipInvalidPlatforms": true,
-            "binaryPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "linuxarm64",
-                "osxuniversal"
-            ],
-            "simMode": "swsim"
-        },
-        {
-            "groupId": "com.ctre.phoenix6.sim",
-            "artifactId": "simProCANdi",
-            "version": "26.1.2",
-            "libName": "CTRE_SimProCANdi",
-            "headerClassifier": "headers",
-            "sharedLibrary": true,
-            "skipInvalidPlatforms": true,
-            "binaryPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "linuxarm64",
-                "osxuniversal"
-            ],
-            "simMode": "swsim"
-        },
-        {
-            "groupId": "com.ctre.phoenix6.sim",
-            "artifactId": "simProCANdle",
-            "version": "26.1.2",
-            "libName": "CTRE_SimProCANdle",
-            "headerClassifier": "headers",
-            "sharedLibrary": true,
-            "skipInvalidPlatforms": true,
-            "binaryPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "linuxarm64",
-                "osxuniversal"
-            ],
-            "simMode": "swsim"
-        }
-    ]
-}

vendordeps/REVLib.json

@@ -1,133 +0,0 @@
-{
-    "fileName": "REVLib.json",
-    "name": "REVLib",
-    "version": "2026.0.5",
-    "frcYear": "2026",
-    "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb",
-    "mavenUrls": [
-        "https://maven.revrobotics.com/"
-    ],
-    "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2026.json",
-    "javaDependencies": [
-        {
-            "groupId": "com.revrobotics.frc",
-            "artifactId": "REVLib-java",
-            "version": "2026.0.5"
-        }
-    ],
-    "jniDependencies": [
-        {
-            "groupId": "com.revrobotics.frc",
-            "artifactId": "REVLib-driver",
-            "version": "2026.0.5",
-            "skipInvalidPlatforms": true,
-            "isJar": false,
-            "validPlatforms": [
-                "windowsx86-64",
-                "linuxarm64",
-                "linuxx86-64",
-                "linuxathena",
-                "linuxarm32",
-                "osxuniversal"
-            ]
-        },
-        {
-            "groupId": "com.revrobotics.frc",
-            "artifactId": "RevLibBackendDriver",
-            "version": "2026.0.5",
-            "skipInvalidPlatforms": true,
-            "isJar": false,
-            "validPlatforms": [
-                "windowsx86-64",
-                "linuxarm64",
-                "linuxx86-64",
-                "linuxathena",
-                "linuxarm32",
-                "osxuniversal"
-            ]
-        },
-        {
-            "groupId": "com.revrobotics.frc",
-            "artifactId": "RevLibWpiBackendDriver",
-            "version": "2026.0.5",
-            "skipInvalidPlatforms": true,
-            "isJar": false,
-            "validPlatforms": [
-                "windowsx86-64",
-                "linuxarm64",
-                "linuxx86-64",
-                "linuxathena",
-                "linuxarm32",
-                "osxuniversal"
-            ]
-        }
-    ],
-    "cppDependencies": [
-        {
-            "groupId": "com.revrobotics.frc",
-            "artifactId": "REVLib-cpp",
-            "version": "2026.0.5",
-            "libName": "REVLib",
-            "headerClassifier": "headers",
-            "sharedLibrary": false,
-            "skipInvalidPlatforms": true,
-            "binaryPlatforms": [
-                "windowsx86-64",
-                "linuxarm64",
-                "linuxx86-64",
-                "linuxathena",
-                "linuxarm32",
-                "osxuniversal"
-            ]
-        },
-        {
-            "groupId": "com.revrobotics.frc",
-            "artifactId": "REVLib-driver",
-            "version": "2026.0.5",
-            "libName": "REVLibDriver",
-            "headerClassifier": "headers",
-            "sharedLibrary": false,
-            "skipInvalidPlatforms": true,
-            "binaryPlatforms": [
-                "windowsx86-64",
-                "linuxarm64",
-                "linuxx86-64",
-                "linuxathena",
-                "linuxarm32",
-                "osxuniversal"
-            ]
-        },
-        {
-            "groupId": "com.revrobotics.frc",
-            "artifactId": "RevLibBackendDriver",
-            "version": "2026.0.5",
-            "libName": "BackendDriver",
-            "sharedLibrary": true,
-            "skipInvalidPlatforms": true,
-            "binaryPlatforms": [
-                "windowsx86-64",
-                "linuxarm64",
-                "linuxx86-64",
-                "linuxathena",
-                "linuxarm32",
-                "osxuniversal"
-            ]
-        },
-        {
-            "groupId": "com.revrobotics.frc",
-            "artifactId": "RevLibWpiBackendDriver",
-            "version": "2026.0.5",
-            "libName": "REVLibWpi",
-            "sharedLibrary": true,
-            "skipInvalidPlatforms": true,
-            "binaryPlatforms": [
-                "windowsx86-64",
-                "linuxarm64",
-                "linuxx86-64",
-                "linuxathena",
-                "linuxarm32",
-                "osxuniversal"
-            ]
-        }
-    ]
-}

vendordeps/WPILibNewCommands.json

@@ -1,39 +0,0 @@
-{
-  "fileName": "WPILibNewCommands.json",
-  "name": "WPILib-New-Commands",
-  "version": "1.0.0",
-  "uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266",
-  "frcYear": "2026",
-  "mavenUrls": [],
-  "jsonUrl": "",
-  "javaDependencies": [
-    {
-      "groupId": "edu.wpi.first.wpilibNewCommands",
-      "artifactId": "wpilibNewCommands-java",
-      "version": "wpilib"
-    }
-  ],
-  "jniDependencies": [],
-  "cppDependencies": [
-    {
-      "groupId": "edu.wpi.first.wpilibNewCommands",
-      "artifactId": "wpilibNewCommands-cpp",
-      "version": "wpilib",
-      "libName": "wpilibNewCommands",
-      "headerClassifier": "headers",
-      "sourcesClassifier": "sources",
-      "sharedLibrary": true,
-      "skipInvalidPlatforms": true,
-      "binaryPlatforms": [
-        "linuxsystemcore",
-        "linuxathena",
-        "linuxarm32",
-        "linuxarm64",
-        "windowsx86-64",
-        "windowsx86",
-        "linuxx86-64",
-        "osxuniversal"
-      ]
-    }
-  ]
-}